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"])