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'])
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 (
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])
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']
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)
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']))
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)
# 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 \
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', [
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)
# 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 \
# 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 \