# 2}}} # 1}}} # Initialization. {{{1 # Set parameters. {{{2 robot.client.basic.problem.resetRoadmap () robot.client.basic.problem.selectPathOptimizer ('None') robot.client.basic.problem.setErrorThreshold (1e-3) robot.client.basic.problem.setMaxIterations (40) # 2}}} # 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
# 3}}} robot.client.basic.problem.resetRoadmap() robot.client.basic.problem.setErrorThreshold(1e-3) robot.client.basic.problem.setMaxIterProjection(40) ps.selectPathProjector('Progressive', 0.2) # Create constraints. {{{3 robot.client.manipulation.problem.createPlacementConstraint \ ('box_placement', ['box/box_surface',], ['kitchen_area/pancake_table_table_top',]) 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])