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):
