# 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]
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['pr2/l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_elbow_flex_joint']
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot('pr2-box', 'pr2') # was anchor joint
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadObjectModel(Box, 'box')
vf.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7])
robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig()
q_init[0:4] = [-3.2, -4, 1, 0] # FIX ME ! (see up )
rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint']
q_init[rank] = 0.5
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
rank = robot.rankInConfiguration['pr2/torso_lift_joint']
q_init[rank] = 0.2
q_goal = q_init[::]
rank = robot.rankInConfiguration['box/root_joint']
c = sqrt(2) / 2
