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'])
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',
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
# 2}}}
# Create lists of joint names - useful to define passive joints. {{{2
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'])
# 2}}}
# Generate initial and goal configuration. {{{2
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_r_finger_joint']
q_init [rank] = 0.5
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
q_init [rank] = 0.2
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_init [rank:rank+3] = [-2.5, -4, 0.8]
jointNames['baxterLeftSide'] = list ()
jointNames['box1'] = list ()
jointNames['box2'] = list ()
for n in jointNames['all']:
if n.startswith ("baxter"):
jointNames['baxter'].append (n)
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')
jointNames['baxterLeftSide'] = list ()
jointNames['box1'] = list ()
jointNames['box2'] = list ()
for n in jointNames['all']:
if n.startswith ("baxter"):
jointNames['baxter'].append (n)
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',])
jointNames['romeoWithoutRightArm'] = list ()
jointNames['kitchen_area'] = list ()
jointNames['cup'] = list ()
for n in jointNames['all']:
if n.startswith ("romeo"):
jointNames['romeo'].append (n)
if not n.startswith ("romeo/l_"):
jointNames['romeoWithoutLeftArm'].append (n)
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}}}
jointNames['romeoWithoutRightArm'] = list()
jointNames['kitchen_area'] = list()
jointNames['cup'] = list()
for n in jointNames['all']:
if n.startswith("romeo"):
jointNames['romeo'].append(n)
if not n.startswith("romeo/l_"):
jointNames['romeoWithoutLeftArm'].append(n)
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}}}
# 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],])
lockbox = ps.lockFreeFlyerJoint ('box/base_joint', 'box_lock')
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',
# 3}}}
# 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],])
