class Environment(object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot('pr2-box', 'pr2', rootJointType="anchor")
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
robot.setJointPosition('pr2/base_joint', (-3.2, -4, 0, 1, 0, 0, 0))
vf.loadObjectModel(Box, 'box')
vf.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("box/base_joint_xyz", [-3, -2, -5, -3, 0.7, 1])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig()
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
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot ('pr2-box', 'pr2', rootJointType = "anchor")
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
robot.setJointPosition ('pr2/base_joint', (-3.2, -4, 0, 1, 0, 0, 0))
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("box/base_joint_xyz", [-3,-2,-5,-3,0.7,1])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
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
