# CoM Task
taskCom = MetaTaskDynCom(dyn,dt)
# Posture Task
taskPosture = MetaTaskDynPosture(dyn,dt)
# Angular position and velocity limits
taskLim = MetaTaskDynLimits(dyn,dt)
#-----------------------------------------------------------------------------
# --- Stack of tasks controller ---------------------------------------------
#-----------------------------------------------------------------------------
#sot = SolverDynReduced('sot')
sot = SolverMotionReduced('sot')
contact = AddContactHelper(sot)
sot.setSize(robotDim-6)
sot.breakFactor.value = 10
plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
plug(dyn.velocity,sot.velocity)
plug(sot.solution, robot.control)
plug(sot.acceleration,robot.acceleration)
#-----------------------------------------------------------------------------
# ---- CONTACT: Contact definition -------------------------------------------
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
# ------------------------------------------------------------------------------
# --- SOT Dyn OpSpaceH ---------------------------------------------------------
# ------------------------------------------------------------------------------
# sot = SolverDynReduced('sot')
# contact = AddContactHelper(sot)
sot = SolverMotionReduced('sot')
contact = AddContactPointsHelperNew(sot)
sot.setSize(robotDim-6)
sot.breakFactor.value = 10
plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
plug(dyn.velocity,sot.velocity)
plug(sot.solution, robot.control)
plug(sot.acceleration,robot.acceleration)
#--- ZMP ----------------
zmp = ZmpEstimator('zmp')
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')
robot.control.unplug()
# ------------------------------------------------------------------------------
# --- SOT Dyn OpSpaceH ---------------------------------------------------------
# ------------------------------------------------------------------------------
# SOT controller.
#sot = SolverOpSpace('sot')
#sot = SolverDynReduced('sot')
sot = SolverMotionReduced('sot')
sot.setSize(robotDim-6)
sot.breakFactor.value = 10
plug(dyn.inertiaReal,sot.matrixInertia)
plug(dyn.dynamicDrift,sot.dyndrift)
plug(dyn.velocity,sot.velocity)
#plug(sot.control,robot.control)
plug(sot.solution, robot.control)
plug(sot.acceleration,robot.acceleration)
#--- ZMP ----------------
zmp = ZmpEstimator('zmp')
zmp.declare(sot,dyn)
