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