rank = robot.rankInConfiguration ['pr2/l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['pr2/r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/r_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_goal [rank:rank+3] = [-4.8, -4.6, 0.9]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_goal [rank:rank+4] = [0, 0, 0, 1]
# 2}}}

# Create the constraints. {{{2
graph.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', passiveJoints = 'pr2')
graph.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')

lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock', compType = 'Equality')
lockpr2 = ps.lockPlanarJoint ('pr2/base_joint', 'pr2_lock', compType = 'Equality')
lockboth = lockpr2[:]; lockboth.extend (lockbox)

locklhand = ['l_l_finger','l_r_finger'];
ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,])
# 2}}}

# Create the constraint graph. {{{2

# Create nodes and edges. {{{3
graph.createNode (['box', 'free'])
    if n.startswith ("baxter/left_"):
      jointNames['baxterLeftSide'].append (n)
    if n.startswith ("baxter/right_"):
      jointNames['baxterRightSide'].append (n)
  elif n.startswith ("box1"):
      jointNames["box1"].append (n)
  elif n.startswith ("box2"):
      jointNames["box2"].append (n)

ps.addPassiveDofs ('box1', jointNames ['box1'])
ps.addPassiveDofs ('box2', jointNames ['box2'])
ps.addPassiveDofs ('baxter', jointNames ['baxter'])
# 4}}}

# Create the grasps {{{4
cg.createGrasp ('l1_grasp', 'baxter/l_gripper', 'box1/handle')
cg.createPreGrasp ('l1_pregrasp', 'baxter/l_gripper', 'box1/handle')

cg.createGrasp ('l2_grasp', 'baxter/l_gripper', 'box2/handle')
cg.createPreGrasp ('l2_pregrasp', 'baxter/l_gripper', 'box2/handle')

cg.createGrasp ('r1_grasp', 'baxter/r_gripper', 'box1/handle2')
cg.createPreGrasp ('r1_pregrasp', 'baxter/r_gripper', 'box1/handle2')

cg.createGrasp ('r2_grasp', 'baxter/r_gripper', 'box2/handle2')
cg.createPreGrasp ('r2_pregrasp', 'baxter/r_gripper', 'box2/handle2')
# 4}}}

# Create placement constraints {{{4
robot.client.manipulation.problem.createPlacementConstraint (
  'box1_on_table', ['box1/box_surface'], ['table/pancake_table_table_top'])
Exemplo n.º 3
0
    if n.startswith ("baxter/left_"):
      jointNames['baxterLeftSide'].append (n)
    if n.startswith ("baxter/right_"):
      jointNames['baxterRightSide'].append (n)
  elif n.startswith ("box1"):
      jointNames["box1"].append (n)
  elif n.startswith ("box2"):
      jointNames["box2"].append (n)

ps.addPassiveDofs ('box1', jointNames ['box1'])
ps.addPassiveDofs ('box2', jointNames ['box2'])
ps.addPassiveDofs ('baxter', jointNames ['baxter'])
# 4}}}

# Create the grasps {{{4
cg.createGrasp ('grasp1', 'baxter/r_gripper', 'box1/handle2')
cg.createPreGrasp ('pregrasp1', 'baxter/r_gripper', 'box1/handle2')

cg.createGrasp ('grasp2', 'baxter/r_gripper', 'box2/handle2')
cg.createPreGrasp ('pregrasp2', 'baxter/r_gripper', 'box2/handle2')
# 4}}}

# Create placement constraints {{{4
robot.client.manipulation.problem.createPlacementConstraint (
  'box1_on_table', ['box1/box_surface',], ['table/pancake_table_table_top',])
robot.client.manipulation.problem.createPrePlacementConstraint (
  'box1_above_table', ['box1/box_surface',],
  ['table/pancake_table_table_top',], 0.05)
robot.client.manipulation.problem.createPlacementConstraint (
  'box2_on_table', ['box2/box_surface',], ['table/pancake_table_table_top',])
robot.client.manipulation.problem.createPrePlacementConstraint (
Exemplo n.º 4
0
robot.client.manipulation.problem.createPlacementConstraint(
    'box_placement', 'box/box_surface', 'kitchen_area/pancake_table_table_top')

jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['pr2'] = list()
jointNames['allButPR2LeftArm'] = list()
for n in jointNames['all']:
    if n.startswith("pr2"):
        jointNames['pr2'].append(n)
    if not n.startswith("pr2/l_gripper"):
        jointNames['allButPR2LeftArm'].append(n)

ps.addPassiveDofs('pr2', jointNames['pr2'])

cg.createGrasp('l_grasp', 'pr2/l_gripper', 'box/handle', 'pr2')

cg.createGrasp('r_grasp', 'pr2/r_gripper', 'box/handle2', 'pr2')

cg.createPreGrasp('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp('r_pregrasp', 'pr2/r_gripper', 'box/handle2')

lockbox = ps.lockFreeFlyerJoint('box/base_joint', 'box_lock')

locklhand = ['l_l_finger', 'l_r_finger']
ps.createLockedJoint('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5])
ps.createLockedJoint('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5])

lockrhand = ['r_l_finger', 'r_r_finger']
ps.createLockedJoint('r_l_finger', 'pr2/r_gripper_l_finger_joint', [0.5])
ps.createLockedJoint('r_r_finger', 'pr2/r_gripper_r_finger_joint', [0.5])
Exemplo n.º 5
0
robot.client.manipulation.problem.createPlacementConstraint(
    'box_placement', 'box', 'box/base_joint_SO3', 'box_surface',
    'pancake_table_table_top')

jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['pr2'] = list()
jointNames['allButPR2LeftArm'] = list()
for n in jointNames['all']:
    if n.startswith("pr2"):
        jointNames['pr2'].append(n)
    if not n.startswith("pr2/l_gripper"):
        jointNames['allButPR2LeftArm'].append(n)

cg.createGrasp('l_grasp', 'pr2/l_gripper', 'box/handle', jointNames['pr2'])

cg.createGrasp('r_grasp', 'pr2/r_gripper', 'box/handle2', jointNames['pr2'])

cg.createPreGrasp('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp('r_pregrasp', 'pr2/r_gripper', 'box/handle2')

lockbox = p.lockFreeFlyerJoint('box/base_joint', 'box_lock', parametric=True)

locklhand = ['l_l_finger', 'l_r_finger']
p.createLockedDofConstraint('l_l_finger', 'pr2/l_gripper_l_finger_joint', 0.5,
                            0, 0)
p.createLockedDofConstraint('l_r_finger', 'pr2/l_gripper_r_finger_joint', 0.5,
                            0, 0)

lockrhand = ['r_l_finger', 'r_r_finger']
Exemplo n.º 6
0
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint']
q_goal[rank] = -0.5
rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint']
q_goal[rank] = -0.5
rank = robot.rankInConfiguration['box/base_joint_xyz']
q_goal[rank:rank + 3] = [-4.8, -4.6, 0.9]
rank = robot.rankInConfiguration['box/base_joint_SO3']
q_goal[rank:rank + 4] = [0, 0, 0, 1]
# 2}}}

# Create the constraints. {{{2
graph.createGrasp('l_grasp',
                  'pr2/l_gripper',
                  'box/handle',
                  passiveJoints='pr2')
graph.createPreGrasp('l_pregrasp', 'pr2/l_gripper', 'box/handle')

lockbox = ps.lockFreeFlyerJoint('box/base_joint', 'box_lock')
lockpr2 = ps.lockPlanarJoint('pr2/base_joint', 'pr2_lock')
lockboth = lockpr2[:]
lockboth.extend(lockbox)

locklhand = ['l_l_finger', 'l_r_finger']
ps.createLockedJoint('l_l_finger', 'pr2/l_gripper_l_finger_joint', [
    0.5,
])
ps.createLockedJoint('l_r_finger', 'pr2/l_gripper_r_finger_joint', [
    0.5,
])
robot.client.manipulation.problem.createPlacementConstraint (
  'box_placement', 'box/box_surface', 'kitchen_area/pancake_table_table_top')

jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
  if n.startswith ("pr2"):
    jointNames['pr2'].append (n)
  if not n.startswith ("pr2/l_gripper"):
    jointNames['allButPR2LeftArm'].append (n)

ps.addPassiveDofs ('pr2', jointNames ['pr2'])

cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', 'pr2')

cg.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2', 'pr2')

cg.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2')

lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock')

locklhand = ['l_l_finger','l_r_finger'];
ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5])

lockrhand = ['r_l_finger','r_r_finger'];
ps.createLockedJoint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', [0.5])
ps.createLockedJoint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', [0.5])
    if not n.startswith ("romeo/r_"):
      jointNames['romeoWithoutRightArm'].append (n)
  if n.startswith ("kitchen_area"):
      jointNames["kitchen_area"].append (n)
  if n.startswith ("cup"):
      jointNames["cup"].append (n)

ps.addPassiveDofs ('romeo', jointNames ['romeo'])
ps.addPassiveDofs ('romeoWithoutLeftArm', jointNames ['romeoWithoutLeftArm'])
ps.addPassiveDofs ('romeoWithoutRightArm', jointNames ['romeoWithoutRightArm'])
# 4}}}

# Create the grasps {{{4
# cg.createGrasp    ('r_fridge_grasp'   , 'pr2/r_gripper', Kitchen.handle, 'pr2WithoutRightArm')
# cg.createPreGrasp ('r_fridge_pregrasp', 'pr2/r_gripper', Kitchen.handle, 'pr2WithoutRightArm')
cg.createGrasp    ('r_fridge_grasp'   , 'romeo/r_gripper', Kitchen.handle)
cg.createPreGrasp ('r_fridge_pregrasp', 'romeo/r_gripper', Kitchen.handle)

# cg.createGrasp    ('l_cup_grasp'   , 'pr2/l_gripper', Cup.handle, 'pr2WithoutLeftArm')
# cg.createPreGrasp ('l_cup_pregrasp', 'pr2/l_gripper', Cup.handle, 'pr2WithoutLeftArm')
cg.createGrasp    ('l_cup_grasp'   , 'romeo/l_gripper', Cup.handle, 'romeo')
cg.createPreGrasp ('l_cup_pregrasp', 'romeo/l_gripper', Cup.handle, 'romeo')
# 4}}}

# Locks joints that are not used for this problem {{{4
lockKitchen = list ()
for n in jointNames["kitchen_area"]:
    if not n == Kitchen.joint:
        ps.createLockedJoint (n, n, [0,])
        lockKitchen.append (n)
Exemplo n.º 9
0
def toVector (s):
  return list(map (float, [x for x in s.split (" ") if x != ""]))

N=10000
qs = [None] * N
for i in range(N):
    qs[i] = robot.shootRandomConfig()

implicitGraspConstraints = dict()
explicitGraspConstraints = dict()
for handles in handlesPerObjects:
    for h in handles:
        for g in grippers:
            n = createGraspConstraint (g,h)
            ne = g + " grasps " + h
            cg.createGrasp (ne, g, h)
            implicitGraspConstraints[(g,h)] = n
            explicitGraspConstraints[(g,h)] = ne + "/hold"

# List of grasps for each node. From any node to the next one, one grasp is
# added or removed
grasps = set ()
# list of set of grasps which are benchmarked
benchGrasps = list()

# grasp high
grasps.add (('romeo/r_hand', placard.handles['high']))
benchGrasps.append (grasps.copy())

# grasp low
grasps.add (('romeo/l_hand', placard.handles['low']))
Exemplo n.º 10
0
        if not n.startswith("romeo/r_"):
            jointNames['romeoWithoutRightArm'].append(n)
    if n.startswith("kitchen_area"):
        jointNames["kitchen_area"].append(n)
    if n.startswith("cup"):
        jointNames["cup"].append(n)

ps.addPassiveDofs('romeo', jointNames['romeo'])
ps.addPassiveDofs('romeoWithoutLeftArm', jointNames['romeoWithoutLeftArm'])
ps.addPassiveDofs('romeoWithoutRightArm', jointNames['romeoWithoutRightArm'])
# 4}}}

# Create the grasps {{{4
# cg.createGrasp    ('r_fridge_grasp'   , 'pr2/r_gripper', Kitchen.handle, 'pr2WithoutRightArm')
# cg.createPreGrasp ('r_fridge_pregrasp', 'pr2/r_gripper', Kitchen.handle, 'pr2WithoutRightArm')
cg.createGrasp('r_fridge_grasp', 'romeo/r_gripper', Kitchen.handle)
cg.createPreGrasp('r_fridge_pregrasp', 'romeo/r_gripper', Kitchen.handle)

# cg.createGrasp    ('l_cup_grasp'   , 'pr2/l_gripper', Cup.handle, 'pr2WithoutLeftArm')
# cg.createPreGrasp ('l_cup_pregrasp', 'pr2/l_gripper', Cup.handle, 'pr2WithoutLeftArm')
cg.createGrasp('l_cup_grasp', 'romeo/l_gripper', Cup.handle, 'romeo')
cg.createPreGrasp('l_cup_pregrasp', 'romeo/l_gripper', Cup.handle, 'romeo')
# 4}}}

# Locks joints that are not used for this problem {{{4
lockKitchen = list()
for n in jointNames["kitchen_area"]:
    if not n == Kitchen.joint:
        ps.createLockedJoint(n, n, [
            0,
        ])
cg = ConstraintGraph (robot, 'graph')

robot.client.manipulation.problem.createPlacementConstraint (
  'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top')

jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
  if n.startswith ("pr2"):
    jointNames['pr2'].append (n)
  if not n.startswith ("pr2/l_gripper"):
    jointNames['allButPR2LeftArm'].append (n)

cg.createGrasp ('l_grasp', 'pr2/l_gripper', 'box/handle', jointNames['pr2'])

cg.createGrasp ('r_grasp', 'pr2/r_gripper', 'box/handle2', jointNames['pr2'])

cg.createPreGrasp ('l_pregrasp', 'pr2/l_gripper', 'box/handle')
cg.createPreGrasp ('r_pregrasp', 'pr2/r_gripper', 'box/handle2')

lockbox = p.lockFreeFlyerJoint ('box/base_joint', 'box_lock', parametric = True)

locklhand = ['l_l_finger','l_r_finger'];
p.createLockedDofConstraint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', 0.5, 0, 0)
p.createLockedDofConstraint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', 0.5, 0, 0)

lockrhand = ['r_l_finger','r_r_finger'];
p.createLockedDofConstraint ('r_l_finger', 'pr2/r_gripper_l_finger_joint', 0.5, 0, 0)
p.createLockedDofConstraint ('r_r_finger', 'pr2/r_gripper_r_finger_joint', 0.5, 0, 0)
Exemplo n.º 12
0
# Generate constraints {{{3
graph = ConstraintGraph (robot, 'graph')


jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['hrp2'] = list ()
jointNames['allButHRP2LeftArm'] = list ()
for n in jointNames['all']:
  if n.startswith ("hrp2"):
    jointNames['hrp2'].append (n)
  if not n.startswith ("hrp2/LARM"):
    jointNames['allButHRP2LeftArm'].append (n)

ps.addPassiveDofs ('hrp2', jointNames ['hrp2'])
graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', 'hrp2')
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')

lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock')

locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
ps.createLockedJoint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
ps.createLockedJoint \
    ('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
ps.createLockedJoint \
    ('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJoint \
    ('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJoint \
    ('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJoint \
Exemplo n.º 13
0
        if n.startswith("baxter/left_"):
            jointNames['baxterLeftSide'].append(n)
        if n.startswith("baxter/right_"):
            jointNames['baxterRightSide'].append(n)
    elif n.startswith("box1"):
        jointNames["box1"].append(n)
    elif n.startswith("box2"):
        jointNames["box2"].append(n)

ps.addPassiveDofs('box1', jointNames['box1'])
ps.addPassiveDofs('box2', jointNames['box2'])
ps.addPassiveDofs('baxter', jointNames['baxter'])
# 4}}}

# Create the grasps {{{4
cg.createGrasp('grasp1', 'baxter/r_gripper', 'box1/handle2')
cg.createPreGrasp('pregrasp1', 'baxter/r_gripper', 'box1/handle2')

cg.createGrasp('grasp2', 'baxter/r_gripper', 'box2/handle2')
cg.createPreGrasp('pregrasp2', 'baxter/r_gripper', 'box2/handle2')
# 4}}}

# Create placement constraints {{{4
robot.client.manipulation.problem.createPlacementConstraint(
    'box1_on_table', [
        'box1/box_surface',
    ], [
        'table/pancake_table_table_top',
    ])
robot.client.manipulation.problem.createPrePlacementConstraint(
    'box1_above_table', [
Exemplo n.º 14
0
lockBase = ps.lockPlanarJoint ('tom/base_joint', 'base_lock', [0,0,1,0])

# Build constraint graph
graph = ConstraintGraph (robot, 'graph')
graph.createNode (['hold_box_l1', 'hold_box_l2', 'hold_box_r1', 'hold_box_r2',
                   'free'])

jointNames = dict ()
jointNames ['tom'] = list ()
for j in robot.jointNames:
    if j.startswith ('tom'):
        jointNames ['tom'].append (j)

ps.addPassiveDofs ('tom', jointNames ['tom'])

graph.createGrasp ('l2_grasp', 'tom/l_finger_tip', 'box/handle2',
                   passiveJoints = 'tom')
graph.createGrasp ('l1_grasp', 'tom/l_finger_tip', 'box/handle',
                   passiveJoints = 'tom')
graph.createGrasp ('r2_grasp', 'tom/r_finger_tip', 'box/handle2',
                   passiveJoints = 'tom')
graph.createGrasp ('r1_grasp', 'tom/r_finger_tip', 'box/handle',
                   passiveJoints = 'tom')
graph.createPreGrasp ('l2_pregrasp', 'tom/l_finger_tip', 'box/handle2')
graph.createPreGrasp ('l1_pregrasp', 'tom/l_finger_tip', 'box/handle')
graph.createPreGrasp ('r2_pregrasp', 'tom/r_finger_tip', 'box/handle2')
graph.createPreGrasp ('r1_pregrasp', 'tom/r_finger_tip', 'box/handle')

graph.createWaypointEdge ('free', 'hold_box_l1', 'grasp_box_l1',
                          nb=1, weight=10)
graph.createWaypointEdge ('free', 'hold_box_l2', 'grasp_box_l2',
                          nb=1, weight=10)
Exemplo n.º 15
0
# Generate constraints {{{3
graph = ConstraintGraph (robot, 'graph')


jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['hrp2'] = list ()
jointNames['allButHRP2LeftArm'] = list ()
for n in jointNames['all']:
  if n.startswith ("hrp2"):
    jointNames['hrp2'].append (n)
  if not n.startswith ("hrp2/LARM"):
    jointNames['allButHRP2LeftArm'].append (n)

graph.createGrasp ('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', jointNames ['hrp2'])
graph.createPreGrasp ('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')

lockscrewgun = ps.lockFreeFlyerJoint ('screw_gun/base_joint', 'screwgun_lock')

locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
ps.createLockedJointConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
ps.createLockedJointConstraint \
    ('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
ps.createLockedJointConstraint \
    ('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJointConstraint \
    ('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJointConstraint \
    ('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJointConstraint \
Exemplo n.º 16
0
# Generate constraints {{{3
graph = ConstraintGraph(robot, 'graph')

jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['hrp2'] = list()
jointNames['allButHRP2LeftArm'] = list()
for n in jointNames['all']:
    if n.startswith("hrp2"):
        jointNames['hrp2'].append(n)
    if not n.startswith("hrp2/LARM"):
        jointNames['allButHRP2LeftArm'].append(n)

ps.addPassiveDofs('hrp2', jointNames['hrp2'])
graph.createGrasp('l_grasp', 'hrp2/leftHand', 'screw_gun/handle2', 'hrp2')
graph.createPreGrasp('l_pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')

lockscrewgun = ps.lockFreeFlyerJoint('screw_gun/base_joint', 'screwgun_lock')

locklhand = ['larm_6', 'lhand_0', 'lhand_1', 'lhand_2', 'lhand_3', 'lhand_4']
ps.createLockedJoint('larm_6', 'hrp2/LARM_JOINT6', [
    q_init[ilh],
])
ps.createLockedJoint \
    ('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
ps.createLockedJoint \
    ('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJoint \
    ('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJoint \