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', [q_init[robot.rankInConfiguration['pr2/head_pan_joint']]]) ps.createLockedJoint( 'head_tilt', 'pr2/head_tilt_joint', [q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]]) ps.createLockedJoint( 'torso', 'pr2/torso_lift_joint',
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) we[ "grasp"] = graph.createWaypointEdge ('free', 'box', "grasp", nb=1, weight=10)
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,]) # 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) we[ "grasp"] = graph.createWaypointEdge ('free', 'box', "grasp", nb=1, weight=10)
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', [q_init[robot.rankInConfiguration['pr2/head_pan_joint']]]) 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_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, ]) # 2}}} # Create the constraint graph. {{{2 # Create nodes and edges. {{{3
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/root_joint'] q_goal [rank:rank+7] = [-4.8, -4.6, 0.9, 1, 0, 0, 0] #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/root_joint', 'box_lock') lockpr2 = ps.lockPlanarJoint ('pr2/root_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,]) # 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) we[ "grasp"] = graph.createWaypointEdge ('free', 'box', "grasp", nb=1, weight=10)
q_goal[rank] = -0.5 rank = robot.rankInConfiguration['box/root_joint'] q_goal[rank:rank + 7] = [-4.8, -4.6, 0.9, 1, 0, 0, 0] #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/root_joint', 'box_lock') lockpr2 = ps.lockPlanarJoint('pr2/root_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, ]) # 2}}} # Create the constraint graph. {{{2 # Create nodes and edges. {{{3
ps.createLockedJoint (lockedJointName, j, [v,]) closeLeftHand = [] for j, v in robot.openHand (None, .24, "left").iteritems () : lockedJointName = "close/"+j closeLeftHand.append (lockedJointName) ps.createLockedJoint (lockedJointName, j, [v,]) closeRightHand = [] for j, v in robot.openHand (None, .24, "right").iteritems () : lockedJointName = "close/"+j closeRightHand.append (lockedJointName) ps.createLockedJoint (lockedJointName, j, [v,]) 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',