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)
res, qproj, err = ps.applyConstraints(qrand)
if res:
valid, msg = robot.isConfigValid(qproj)
if valid:
print("Found pose", shoot_pose, "\t", shoot_cfg)
expected_poses.append(chessboard_pose)
q_proj_list.append(qproj)
qrand = qproj[:]
break
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",
[0.6, 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_2"])
