bezier = Bezier('bezier') timeline = Timeline('timeline') spline = Spline('spline') bezier.controlPoints.value = ((0,0), (10,20)) plug(spline.out, bezier.t) plug(timeline.scaledTime, spline.input) timeline.timeMax.value = 100 from dynamic_graph.sot.dynamics.hrp2 import Hrp2Laas, Hrp2Jrl from dynamic_graph.sot.dynamics.solver import Solver robot = Hrp2Laas("robot") solver = Solver(robot) robot.device.before.addSignal('timeline.dummy') robot.device.before.addSignal('bezier.state') robot.device.zmp.value = (0.,0.,0.) s = ['left-ankle', 'right-ankle'] for i in s: robot.dynamic.signal(i).recompute(robot.dynamic.signal(i).time + 1) robot.features[i].reference.value = robot.dynamic.signal(i).value robot.features[i]._feature.selec.value = '111111' robot.tasks[i].controlGain.value = 180. robot.featureComDes.errorIN.value = robot.dynamic.com.value robot.featureComDes.selec.value = '111'
clt = None parser = OptionParser() parser.add_option("-d", "--display", action="store_true", dest="display", default=False, help="enable display using robotviewer") parser.add_option("-r", "--robot", action="store", dest="robot", default="Hrp2Laas", help="Specify which robot model to use") (options, args) = parser.parse_args() if options.display: if not hasRobotViewer: print "Failed to import robotviewer client library." clt = initRobotViewer() if not clt: print "Failed to initialize robotviewer client library." # Initialize the stack of tasks. robots = {"Hrp2Laas": Hrp2Laas, "Hrp2Jrl": Hrp2Jrl} if not options.robot in robots: raise RuntimeError( "invalid robot name (should by Hrp2Laas or Hrp2Jrl)") robot = robots[options.robot]("robot") solver = Solver(robot)
from dynamic_graph.sot.core import FeatureVisualPoint from dynamic_graph.sot.dynamics.hrp2 import Hrp2Laas from dynamic_graph.sot.dynamics.solver import Solver from dynamic_graph.sot.motion_planner import VispPointProjection from dynamic_graph.sot.reaching import CylindricalCubicInterpolationSE3 from dynamic_graph.ros import Ros I4 = reduce(lambda m, i: m + (i*(0.,)+(1.,)+ (3-i)*(0.,),), range(4), ()) # Prologue if __name__ == '__main__': robot = Hrp2Laas ('robot') # Initialize the zmp signal to the current com. _com = robot.dynamic.com.value robot.device.zmp.value = (_com[0], _com[1], 0.) solver = Solver(robot) s = ['left-ankle', 'right-ankle'] for i in s: robot.dynamic.signal(i).recompute(robot.dynamic.signal(i).time + 1) robot.features[i].reference.value = \ robot.dynamic.signal(i).value robot.features[i]._feature.selec.value = '111111' robot.tasks[i].controlGain.value = 180. robot.featureComDes.errorIN.value = robot.dynamic.com.value robot.featureComDes.selec.value = '111' robot.comTask.controlGain.value = 180. robot.balanceTask.controlGain.value = 180. # Push com and feet tasks.