half_sitting = robot.getCurrentConfig (); qinit=[0,0,0,0,0,0,0, 0.5,0,1,0,0,0] qgoal=[0,0,0,0,0,0,0,-0.5,0,1,0,0,0] p = ProblemSolver (robot) p.createGrasp ('grasp', 'ur5/ee', 'box/handle') p.createGrasp ('grasp-passive', 'ur5/ee', 'box/handle') if withWaypoint: p.createPreGrasp ('pregrasp', 'ur5/ee', 'box/handle') p.createLockedDofConstraint ('box_lock_x' , 'box/base_joint_x' , 0, 0, 0) p.createLockedDofConstraint ('box_lock_y' , 'box/base_joint_y' , 0, 0, 0) p.createLockedDofConstraint ('box_lock_z' , 'box/base_joint_z' , 0, 0, 0) p.createLockedDofConstraint ('box_lock_rx', 'box/base_joint_SO3', 0, 1, 0) p.createLockedDofConstraint ('box_lock_ry', 'box/base_joint_SO3', 0, 2, 1) p.createLockedDofConstraint ('box_lock_rz', 'box/base_joint_SO3', 0, 3, 2) p.isLockedDofParametric ('box_lock_x' ,True) p.isLockedDofParametric ('box_lock_y' ,True) p.isLockedDofParametric ('box_lock_z' ,True) p.isLockedDofParametric ('box_lock_rx',True) p.isLockedDofParametric ('box_lock_ry',True) p.isLockedDofParametric ('box_lock_rz',True) lockbox = ['box_lock_x', 'box_lock_y', 'box_lock_z', 'box_lock_rx', 'box_lock_ry', 'box_lock_rz'] jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['ur5'] = list () for n in jointNames['all']: if n.startswith ("ur5"): jointNames['ur5'].append (n) robot.client.basic.problem.setPassiveDofs ('grasp-passive', jointNames['ur5'])
half_sitting = robot.getCurrentConfig() qinit = [0, 0, 0, 0, 0, 0, 0, 0.5, 0, 1, 0, 0, 0] qgoal = [0, 0, 0, 0, 0, 0, 0, -0.5, 0, 1, 0, 0, 0] p = ProblemSolver(robot) p.createGrasp('grasp', 'ur5/ee', 'box/handle') p.createGrasp('grasp-passive', 'ur5/ee', 'box/handle') if withWaypoint: p.createPreGrasp('pregrasp', 'ur5/ee', 'box/handle') p.createLockedDofConstraint('box_lock_x', 'box/base_joint_x', 0, 0, 0) p.createLockedDofConstraint('box_lock_y', 'box/base_joint_y', 0, 0, 0) p.createLockedDofConstraint('box_lock_z', 'box/base_joint_z', 0, 0, 0) p.createLockedDofConstraint('box_lock_rx', 'box/base_joint_SO3', 0, 1, 0) p.createLockedDofConstraint('box_lock_ry', 'box/base_joint_SO3', 0, 2, 1) p.createLockedDofConstraint('box_lock_rz', 'box/base_joint_SO3', 0, 3, 2) p.isLockedDofParametric('box_lock_x', True) p.isLockedDofParametric('box_lock_y', True) p.isLockedDofParametric('box_lock_z', True) p.isLockedDofParametric('box_lock_rx', True) p.isLockedDofParametric('box_lock_ry', True) p.isLockedDofParametric('box_lock_rz', True) lockbox = [ 'box_lock_x', 'box_lock_y', 'box_lock_z', 'box_lock_rx', 'box_lock_ry', 'box_lock_rz' ] jointNames = dict() jointNames['all'] = robot.getJointNames() jointNames['ur5'] = list() for n in jointNames['all']: if n.startswith("ur5"):
0) p.createLockedDofConstraint('screwgun_lock_z', 'screw_gun/base_joint_z', 0, 0, 0) p.createLockedDofConstraint('screwgun_lock_rx', 'screw_gun/base_joint_SO3', 0, 1, 0) p.createLockedDofConstraint('screwgun_lock_ry', 'screw_gun/base_joint_SO3', 0, 2, 1) p.createLockedDofConstraint('screwgun_lock_rz', 'screw_gun/base_joint_SO3', 0, 3, 2) p.createLockedDofConstraint('larm_6', 'hrp2/LARM_JOINT6', q1[17], 0, 0) p.createLockedDofConstraint('lhand_0', 'hrp2/LHAND_JOINT0', q1[18], 0, 0) p.createLockedDofConstraint('lhand_1', 'hrp2/LHAND_JOINT1', q1[19], 0, 0) p.createLockedDofConstraint('lhand_2', 'hrp2/LHAND_JOINT2', q1[20], 0, 0) p.createLockedDofConstraint('lhand_3', 'hrp2/LHAND_JOINT3', q1[21], 0, 0) p.createLockedDofConstraint('lhand_4', 'hrp2/LHAND_JOINT4', q1[22], 0, 0) p.isLockedDofParametric('screwgun_lock_x', True) p.isLockedDofParametric('screwgun_lock_y', True) p.isLockedDofParametric('screwgun_lock_z', True) p.isLockedDofParametric('screwgun_lock_rx', True) p.isLockedDofParametric('screwgun_lock_ry', True) p.isLockedDofParametric('screwgun_lock_rz', True) lockscrewgun = [ 'screwgun_lock_x', 'screwgun_lock_y', 'screwgun_lock_z', 'screwgun_lock_rx', 'screwgun_lock_ry', 'screwgun_lock_rz' ] locklhand = ['larm_6', 'lhand_0', 'lhand_1', 'lhand_2', 'lhand_3', 'lhand_4'] jointNames = dict() jointNames['all'] = robot.getJointNames() jointNames['hrp2'] = list() jointNames['allButHRP2LeftArm'] = list()
p.createPreGrasp ('left-hand-pregrasp-passive', 'hrp2/leftHand', 'screw_gun/handle2') p.createGrasp ('left-hand-grasp', 'hrp2/leftHand', 'screw_gun/handle2') p.createGrasp ('left-hand-grasp-passive', 'hrp2/leftHand', 'screw_gun/handle2') p.createLockedDofConstraint ('screwgun_lock_x' , 'screw_gun/base_joint_x' , 0, 0, 0) p.createLockedDofConstraint ('screwgun_lock_y' , 'screw_gun/base_joint_y' , 0, 0, 0) p.createLockedDofConstraint ('screwgun_lock_z' , 'screw_gun/base_joint_z' , 0, 0, 0) p.createLockedDofConstraint ('screwgun_lock_rx', 'screw_gun/base_joint_SO3', 0, 1, 0) p.createLockedDofConstraint ('screwgun_lock_ry', 'screw_gun/base_joint_SO3', 0, 2, 1) p.createLockedDofConstraint ('screwgun_lock_rz', 'screw_gun/base_joint_SO3', 0, 3, 2) p.createLockedDofConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , q1[17], 0, 0) p.createLockedDofConstraint ('lhand_0', 'hrp2/LHAND_JOINT0', q1[18], 0, 0) p.createLockedDofConstraint ('lhand_1', 'hrp2/LHAND_JOINT1', q1[19], 0, 0) p.createLockedDofConstraint ('lhand_2', 'hrp2/LHAND_JOINT2', q1[20], 0, 0) p.createLockedDofConstraint ('lhand_3', 'hrp2/LHAND_JOINT3', q1[21], 0, 0) p.createLockedDofConstraint ('lhand_4', 'hrp2/LHAND_JOINT4', q1[22], 0, 0) p.isLockedDofParametric ('screwgun_lock_x' ,True) p.isLockedDofParametric ('screwgun_lock_y' ,True) p.isLockedDofParametric ('screwgun_lock_z' ,True) p.isLockedDofParametric ('screwgun_lock_rx',True) p.isLockedDofParametric ('screwgun_lock_ry',True) p.isLockedDofParametric ('screwgun_lock_rz',True) lockscrewgun = ['screwgun_lock_x', 'screwgun_lock_y', 'screwgun_lock_z', 'screwgun_lock_rx', 'screwgun_lock_ry', 'screwgun_lock_rz'] locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4'] jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['hrp2'] = list () jointNames['allButHRP2LeftArm'] = list () for n in jointNames['all']: if n.startswith ("hrp2"): jointNames['hrp2'].append (n)