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
# It may be needed to shoot from time to time but so far it works without doing it.
# qrand = robot.shootRandomConfig()
# shoot_cfg += 1
ys.reverse()
hpp_poses = []
for q in q_proj_list:
# 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"])
ps.setLockedJointConstraints(
"proj", left_gripper_lock + right_gripper_lock + other_lock)
res, qproj2, err = ps.applyConstraints(q_init)
