# 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)