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.