)
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:
robot.setCurrentConfig(q)
oMc = Transform(robot.getJointPosition("talos/rgbd_rgb_optical_joint"))
