contactsPerObject = [[] for i in range(nSphere)] ## Contact surfaces shapesPerObject = [[] for o in objects] ## Constraints # for i in range(nSphere): # Change mask of sphere handle o = objects[i] h = r.getHandle(o + '/handle') h.setMask([True, True, True, False, True, True]) # placement constraint placementName = "place_sphere{0}".format(i) ps.createTransformationConstraint(placementName, "", "sphere{0}/root_joint".format(i), [0, 0, 0.02, 0, 0, 0, 1], [False, False, True, True, True, False]) # placement complement constraint ps.createTransformationConstraint(placementName + '/complement', "", "sphere{0}/root_joint".format(i), [0, 0, 0.02, 0, 0, 0, 1], [True, True, False, False, False, True]) ps.setConstantRightHandSide(placementName + '/complement', False) # combination of placement and complement ps.createLockedJoint( placementName + '/hold', "sphere{0}/root_joint".format(i), [0, 0, 0.02, 0, 0, 0, 1], [Equality, Equality, EqualToZero, EqualToZero, EqualToZero, Equality]) ps.registerConstraints(placementName, placementName + '/complement', placementName + '/hold')
## Handles # handlesPerObjects = [[ 'sphere{0}/handle'.format(i), 'sphere{0}/magnet'.format(i) ] for i in range(nSphere)] handlesPerObjects += [['cylinder{0}/handle'.format(i)] for i in range(nCylinder)] ## Contact surfaces shapesPerObject = [[] for o in objects] ## Constraints # for i in range(nSphere): placementName = "place_sphere{0}".format(i) ps.createTransformationConstraint(placementName, "", "sphere{0}/root_joint".format(i), [0, 0, 0.025, 0, 0, 0, 1], [False, False, True, True, True, False]) for i in range(nCylinder): placementName = "place_cylinder{0}".format(i) ps.createTransformationConstraint(placementName, "", "cylinder{0}/root_joint".format(i), [0, 0, 0.025, 0, 0, 0, 1], [False, False, True, True, True, False]) for i in range(nSphere): placementName = "preplace_sphere{0}".format(i) ps.createTransformationConstraint(placementName, "", "sphere{0}/root_joint".format(i), [0, 0, 0.075, 0, 0, 0, 1], [False, False, True, True, True, False]) for i in range(nCylinder):
numConstraints=[] + foot_placement, ) graph.initialize() q_init = half_sitting[:] ps.setParameter("SimpleTimeParameterization/safety", 0.5) ps.setParameter("SimpleTimeParameterization/order", 2) ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0) # Create a relative pose constraint between the root_joint and the gripper we want to move ps.createTransformationConstraint( "hand_pose_1", "talos/root_joint", "talos/gripper_right_joint", [0.3, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], [True, True, True, False, False, False], ) ps.resetConstraints() ps.setNumericalConstraints("proj", foot_placement + ["hand_pose_1"]) ps.setLockedJointConstraints( "proj", left_gripper_lock + right_gripper_lock + other_lock) res, qproj1, err = ps.applyConstraints(q_init) ps.createTransformationConstraint( "hand_pose_2", "talos/root_joint", "talos/gripper_right_joint",
for pt in chessboard_pts ]): continue q = Quaternion().fromRPY( -pi, 0, -pi) * q # Switch tn the real camera frame chessboard_pose = ( chessboard_position[0, 0], chessboard_position[0, 1], chessboard_position[0, 2], ) + q.toTuple() ps.createTransformationConstraint( "gaze", "talos/rgbd_rgb_optical_joint", "mire/root_joint", chessboard_pose, [True] * 6, ) ps.resetConstraints() ps.setNumericalConstraints( "proj", foot_placement + [ "com_talos_mire", "talos/left_gripper grasps mire/left", "gaze" ], ) ps.setLockedJointConstraints( "proj", left_gripper_lock + right_gripper_lock + other_lock)
## Handles # handlesPerObjects = [['sphere{0}/handle'.format (i), 'sphere{0}/magnet'.format (i)] for i in range (nSphere)] handlesPerObjects += [['cylinder{0}/handle'.format (i)] for i in range (nCylinder)] ## Contact surfaces shapesPerObject = [[] for o in objects] ## Constraints # for i in range (nSphere): placementName = "place_sphere{0}".format (i) ps.createTransformationConstraint (placementName, "", "sphere{0}/root_joint".format (i), [0, 0, 0.025, 0, 0, 0, 1], [False, False, True, True, True, False]) for i in range (nCylinder): placementName = "place_cylinder{0}".format (i) ps.createTransformationConstraint (placementName, "", "cylinder{0}/root_joint".format (i), [0, 0, 0.025, 0, 0, 0, 1], [False, False, True, True, True, False]) for i in range (nSphere): placementName = "preplace_sphere{0}".format (i) ps.createTransformationConstraint (placementName, "", "sphere{0}/root_joint".format (i), [0, 0, 0.075, 0, 0, 0, 1], [False, False, True, True, True, False]) for i in range (nCylinder):