Ejemplo n.º 1
0
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'
Ejemplo n.º 2
0
    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.