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)
