#----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- dt=5e-3 @loopInThread def inc(): robot.device.increment(dt) updateComDisplay(robot.device,robot.dynamic.com) runner=inc() [go,stop,next,n]=loopShortcuts(runner) # --- PG --------------------------------------------------------- from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG pg = MetaPG(robot.dynamic) pg.plugZmp(robot.device) # ---- TASKS ------------------------------------------------------------------- # ---- WAIST TASK --- # This task fix the motion of the waist around the z axis, the roll and the pitch taskWaist=MetaTask6d('waist',robot.dynamic,'waist','waist') pg.plugWaistTask(taskWaist) taskWaist.task.controlGain.value = 5 taskWaist.feature.selec.value = '011100' # --- TASK COM --- # the x,y coords of the centor of mass are given by the pattern generator taskCom = MetaTaskKineCom(robot.dynamic,"compd") pg.plugComTask(taskCom) taskCom.feature.selec.value = '011'
dyn.acceleration.value = robotDim*(0.,) dyn.ffposition.unplug() dyn.ffvelocity.unplug() dyn.ffacceleration.unplug() dyn.setProperty('ComputeBackwardDynamics','true') dyn.setProperty('ComputeAccelerationCoM','true') robot.control.unplug() # --- PG --------------------------------------------------------- # --- PG --------------------------------------------------------- # --- PG --------------------------------------------------------- from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG pg = MetaPG(dyn) pg.plugZmp(robot) # ---- SOT --------------------------------------------------------------------- # ---- SOT --------------------------------------------------------------------- # ---- SOT --------------------------------------------------------------------- # The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient from dynamic_graph.sot.dyninv import SolverKine sot = SolverKine('sot') sot.setSize(robotDim) plug(sot.control,robot.control) # ---- TASKS ------------------------------------------------------------------- # ---- TASKS ------------------------------------------------------------------- # ---- TASKS -------------------------------------------------------------------
dyn.acceleration.value = robotDim*(0.,) dyn.ffposition.unplug() dyn.ffvelocity.unplug() dyn.ffacceleration.unplug() dyn.setProperty('ComputeBackwardDynamics','true') dyn.setProperty('ComputeAccelerationCoM','true') robot.control.unplug() # --- PG --------------------------------------------------------- # --- PG --------------------------------------------------------- # --- PG --------------------------------------------------------- from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG pg = MetaPG(dyn) pg.plugZmp(robot) # ---- SOT --------------------------------------------------------------------- # ---- SOT --------------------------------------------------------------------- # ---- SOT --------------------------------------------------------------------- # The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient from dynamic_graph.sot.dyninv import SolverKine def toList(sot): return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:]) SolverKine.toList = toList sot = SolverKine('sot') sot.setSize(robotDim) plug(sot.control,robot.control)
dyn.acceleration.value = robotDim * (0., ) dyn.ffposition.unplug() dyn.ffvelocity.unplug() dyn.ffacceleration.unplug() dyn.setProperty('ComputeBackwardDynamics', 'true') dyn.setProperty('ComputeAccelerationCoM', 'true') robot.control.unplug() # --- PG --------------------------------------------------------- # --- PG --------------------------------------------------------- # --- PG --------------------------------------------------------- from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG pg = MetaPG(dyn) pg.plugZmp(robot) # ---- SOT --------------------------------------------------------------------- # ---- SOT --------------------------------------------------------------------- # ---- SOT --------------------------------------------------------------------- # The solver SOTH of dyninv is used, but normally, the SOT solver should be sufficient from dynamic_graph.sot.dyninv import SolverKine sot = SolverKine('sot') sot.setSize(robotDim) plug(sot.control, robot.control) # ---- TASKS ------------------------------------------------------------------- # ---- TASKS ------------------------------------------------------------------- # ---- TASKS -------------------------------------------------------------------
jointRankPath = xmlDir + '/' + jointRankName[robotName] dyn = Dynamic("dyn") dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath) dyn.parse() dyn.inertiaRotor.value = inertiaRotor[robotName] dyn.gearRatio.value = gearRatio[robotName] plug(robot.state, dyn.position) dyn.velocity.value = robotDim * (0., ) dyn.acceleration.value = robotDim * (0., ) # --- PG --------------------------------------------------------- from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG pg = MetaPG(dyn) pg.plugZmp(robot) # ---- SOT --------------------------------------------------------------------- # The basic SOT solver would work too. sot = SolverKine('sot') sot.setSize(robotDim) plug(sot.control, robot.control) # ---- TASKS ------------------------------------------------------------------- # ---- WAIST TASK --- taskWaist = MetaTask6d('waist', dyn, 'waist', 'waist') pg.plugWaistTask(taskWaist) taskWaist.task.controlGain.value = 5 taskWaist.feature.selec.value = '011100'