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