Ejemplos de ProblemSolver.lockFreeFlyerJoint en Python

Ejemplo n.º 1

Archivo: tutorial_manipulation.py Proyecto: mylene-campana/hpp_tutorial

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'])

we = dict ()
we["ungrasp"] = graph.createWaypointEdge ('box', 'free', "ungrasp", nb=1, weight=1)

Ejemplo n.º 2

Archivo: baxter_table_multiple_boxes.py Proyecto: pFernbach/hpp_benchmark

lockHead = ['head_pan',]
ps.createLockedJoint ('head_pan', 'baxter/head_pan',
    [q_init[robot.rankInConfiguration['baxter/head_pan']]])

for n in jointNames["baxterRightSide"]:
    ps.createLockedJoint (n, n, [0,])

for n in jointNames["baxterLeftSide"]:
    ps.createLockedJoint (n, n, [0,])

lockAll = lockFingers + lockHead + jointNames["baxterLeftSide"]
# 4}}}

# Create constraints corresponding to each object {{{4
lockBox1 = ps.lockFreeFlyerJoint ("box1/" + Box.joint, 'box1_lock')
lockBox2 = ps.lockFreeFlyerJoint ("box2/" + Box.joint, 'box2_lock')
# 4}}}

# Create gaze constraints {{{4
ps.createPositionConstraint ("gaze1", "baxter/display_joint", "box1/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
ps.createPositionConstraint ("gaze2", "baxter/display_joint", "box2/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
# 4}}}

# 3}}}

# Create the graph. {{{3

# cg.createNode (['r2', 'r1', '2on1', 'free'])
cg.createNode (['r2', 'r1', 'free'])

Ejemplo n.º 3

Archivo: script.py Proyecto: humanoid-path-planner/hpp_benchmark

lockHead = ['head_pan',]
ps.createLockedJoint ('head_pan', 'baxter/head_pan',
    [q_init[robot.rankInConfiguration['baxter/head_pan']]])

for n in jointNames["baxterRightSide"]:
    ps.createLockedJoint (n, n, [0,])

for n in jointNames["baxterLeftSide"]:
    ps.createLockedJoint (n, n, [0,])

lockHeadFingersAndLeftSide = lockFingers + lockHead + jointNames["baxterLeftSide"]
# 4}}}

# Create constraints corresponding to each object {{{4
lockBox1 = ps.lockFreeFlyerJoint ("box1/" + Box.joint, 'box1_lock')
lockBox2 = ps.lockFreeFlyerJoint ("box2/" + Box.joint, 'box2_lock')
# 4}}}

# Create gaze constraints {{{4
ps.createPositionConstraint ("gaze1", "baxter/display_joint", "box1/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
ps.createPositionConstraint ("gaze2", "baxter/display_joint", "box2/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0])
# 4}}}

# 3}}}

# Create the graph. {{{3

cg.createNode (['grasp2', 'grasp1', 'free'])

cg.setConstraints (node='free', numConstraints=['box1_on_table','box2_on_table'])

Ejemplo n.º 4

Archivo: test_graph_two_hands.py Proyecto: nim65s/test-hpp

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])

lockhands = lockrhand + locklhand

lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser']
ps.createLockedJoint('head_pan', 'pr2/head_pan_joint',
                     [q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])
ps.createLockedJoint(

Ejemplo n.º 5

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)

lockhands = ['l_l_finger', 'l_r_finger', 'r_l_finger', 'r_r_finger']

Ejemplo n.º 6

Archivo: script.py Proyecto: nim65s/hpp_benchmark

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')

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

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

lockbox = ps.lockFreeFlyerJoint('box/root_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])
lockPlanar = ps.lockPlanarJoint("pr2/root_joint", "lockplanar",
                                [-3.2, -4, 1, 0])

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])

lockhands = lockrhand + locklhand

lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser']
ps.createLockedJoint('head_pan', 'pr2/head_pan_joint',

Ejemplo n.º 7

Archivo: test_graph_two_hands.py Proyecto: humanoid-path-planner/test-hpp

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])

lockhands = lockrhand + locklhand

lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'];
ps.createLockedJoint ('head_pan', 'pr2/head_pan_joint',
    [q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])
ps.createLockedJoint ('head_tilt', 'pr2/head_tilt_joint',

Ejemplo n.º 8

q_goal = q_init[:]
rank = robot.rankInConfiguration['ur5/shoulder_lift_joint']
q_goal[rank] = -PI
rank = robot.rankInConfiguration['ur5/elbow_joint']
q_goal[rank] = PI / 2
# 3}}}

robot.client.basic.problem.resetRoadmap()
robot.client.basic.problem.selectPathOptimizer('None')
robot.client.basic.problem.setErrorThreshold(1e-3)
robot.client.basic.problem.setMaxIterations(40)

# Create constraints. {{{3
lockbox = ps.lockFreeFlyerJoint('box/base_joint',
                                'box_lock',
                                values=(1, 1, 1, 1, 0, 0, 0))

ps.createLockedJoint('shoulder_pan', 'ur5/shoulder_pan_joint', [0])
ps.createLockedJoint('wrist_1', 'ur5/wrist_1_joint', [0])
ps.createLockedJoint('wrist_2', 'ur5/wrist_2_joint', [0])
ps.createLockedJoint('wrist_3', 'ur5/wrist_3_joint', [0])
lockur5 = ['shoulder_pan', 'wrist_1', 'wrist_2', 'wrist_3']

lockjoints = list()
lockjoints.extend(lockbox)
lockjoints.extend(lockur5)

robot.setCurrentConfig(q_init)
tf = robot.getJointPosition('ur5/wrist_1_joint')
#robot.client.basic.problem.createPositionConstraint \

Ejemplo n.º 9

Archivo: ur5.py Proyecto: anna-seppala/test-hpp

q_init [rank:rank+4] = [1,0,0,0]

q_goal = q_init [:]
rank = robot.rankInConfiguration ['ur5/shoulder_lift_joint']
q_goal [rank] = - PI
rank = robot.rankInConfiguration ['ur5/elbow_joint']
q_goal [rank] = PI / 2
# 3}}}

robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)

# Create constraints. {{{3
lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock', values=(1, 1, 1, 1,0,0,0))

ps.createLockedJoint ('shoulder_pan', 'ur5/shoulder_pan_joint', [0])
ps.createLockedJoint ('wrist_1', 'ur5/wrist_1_joint', [0])
ps.createLockedJoint ('wrist_2', 'ur5/wrist_2_joint', [0])
ps.createLockedJoint ('wrist_3', 'ur5/wrist_3_joint', [0])
lockur5 = ['shoulder_pan','wrist_1', 'wrist_2', 'wrist_3'];

lockjoints = list ()
lockjoints.extend (lockbox)
lockjoints.extend (lockur5)

robot.setCurrentConfig (q_init)
tf = robot.getJointPosition ('ur5/wrist_1_joint')
#robot.client.basic.problem.createPositionConstraint \
#('pos', '', 'ur5/wrist_1_joint', (0, tf[1], tf[2]), (0,0,0), (False, True, True))

Ejemplo n.º 10

Archivo: test_graph_two_hands.py Proyecto: florent-lamiraux/test-hpp

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)

lockhands = ['l_l_finger','l_r_finger','r_l_finger','r_r_finger'];

# 3}}}

# Create the graph. {{{3

Ejemplo n.º 11

Archivo: romeo_kitchen.py Proyecto: humanoid-path-planner/test-hpp

# ps.createLockedJoint ('head_tilt', 'pr2/head_tilt_joint',
    # [q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])
# ps.createLockedJoint ('torso', 'pr2/torso_lift_joint',
    # [q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])
# ps.createLockedJoint ('laser', 'pr2/laser_tilt_mount_joint',
    # [q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])

# lockAll = lockhands + lockHeadAndTorso + lockKitchen
lockAll = lockhands + lockKitchen
# 4}}}

# Create constraints corresponding to each object {{{4
lockFridge = ["fridge_lock", ]
ps.createLockedJoint ("fridge_lock", Kitchen.joint, [0,])

lockCup = ps.lockFreeFlyerJoint (Cup.joint, 'cup_lock')

robot.leftAnkle  = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom ('romeo', ['romeo/base_joint_xyz'])
ps.createStaticStabilityConstraints ("balance-romeo", q_init, 'romeo')
# 4}}}

# 3}}}

# Create the graph. {{{3

cg.createNode (['both', 'cup', 'fridge', 'free'])

# Cup and fridge {{{4
cg.setConstraints (node = 'both', grasps = [ 'l_cup_grasp', 'r_fridge_grasp', ])

Ejemplo n.º 12

Archivo: script.py Proyecto: humanoid-path-planner/hpp_benchmark

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')

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

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

lockbox = ps.lockFreeFlyerJoint ('box/root_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])
lockPlanar = ps.lockPlanarJoint ("pr2/root_joint", "lockplanar", [-3.2,-4,1,0])

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])

lockhands = lockrhand + locklhand

lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'];
ps.createLockedJoint ('head_pan', 'pr2/head_pan_joint',
    [q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])

Ejemplo n.º 13

Archivo: romeo_kitchen.py Proyecto: nim65s/test-hpp

# ps.createLockedJoint ('laser', 'pr2/laser_tilt_mount_joint',
# [q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])

# lockAll = lockhands + lockHeadAndTorso + lockKitchen
lockAll = lockhands + lockKitchen
# 4}}}

# Create constraints corresponding to each object {{{4
lockFridge = [
    "fridge_lock",
]
ps.createLockedJoint("fridge_lock", Kitchen.joint, [
    0,
])

lockCup = ps.lockFreeFlyerJoint(Cup.joint, 'cup_lock')

robot.leftAnkle = robot.displayName + '/' + robot.leftAnkle
robot.rightAnkle = robot.displayName + '/' + robot.rightAnkle
ps.addPartialCom('romeo', ['romeo/base_joint_xyz'])
ps.createStaticStabilityConstraints("balance-romeo", q_init, 'romeo')
# 4}}}

# 3}}}

# Create the graph. {{{3

cg.createNode(['both', 'cup', 'fridge', 'free'])

# Cup and fridge {{{4
cg.setConstraints(node='both', grasps=[

Ejemplo n.º 14

Archivo: tutorial_manipulation.py Proyecto: anna-seppala/hpp_tutorial

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'])

we = dict ()
we["ungrasp"] = graph.createWaypointEdge ('box', 'free', "ungrasp", nb=1, weight=1)

Ejemplo n.º 15

Archivo: test_simple_graph.py Proyecto: anna-seppala/test-hpp

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 \
    ('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])

ps.addPartialCom ('hrp2', ['hrp2/base_joint_xyz'])

Ejemplo n.º 16

Archivo: test_simple_graph.py Proyecto: florent-lamiraux/test-hpp

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 \
    ('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])

ps.createStaticStabilityConstraints ("balance", q_init)