47 std::optional<std::vector<double>> cartesian_position,
48 std::optional<double> cartesian_position_tolerance,
49 std::optional<std::vector<double>> orientation,
50 std::optional<double> orientation_tolerance)
53 if (!cartesian_position && !orientation)
55 throw std::invalid_argument(
"No link cartesian or orientation constraints specified");
58 moveit_msgs::msg::Constraints constraints_cpp;
61 if (cartesian_position && orientation)
64 geometry_msgs::msg::PointStamped point;
65 point.header.frame_id = source_frame;
66 point.point.x = cartesian_position.value()[0];
67 point.point.y = cartesian_position.value()[1];
68 point.point.z = cartesian_position.value()[2];
70 moveit_msgs::msg::Constraints position_constraints =
74 geometry_msgs::msg::QuaternionStamped quaternion;
75 quaternion.header.frame_id = source_frame;
76 quaternion.quaternion.x = orientation.value()[0];
77 quaternion.quaternion.y = orientation.value()[1];
78 quaternion.quaternion.z = orientation.value()[2];
79 quaternion.quaternion.w = orientation.value()[3];
81 moveit_msgs::msg::Constraints orientation_constraints =
88 else if (cartesian_position)
91 geometry_msgs::msg::PointStamped point;
92 point.header.frame_id = source_frame;
93 point.point.x = cartesian_position.value()[0];
94 point.point.y = cartesian_position.value()[1];
95 point.point.z = cartesian_position.value()[2];
98 auto logger = rclcpp::get_logger(
"moveit_py");
100 RCLCPP_DEBUG(rclcpp::get_logger(
"moveit_py"),
"Point: %f, %f, %f", point.point.x, point.point.y, point.point.z);
109 geometry_msgs::msg::QuaternionStamped quaternion;
110 quaternion.header.frame_id = source_frame;
111 quaternion.quaternion.x = orientation.value()[0];
112 quaternion.quaternion.y = orientation.value()[1];
113 quaternion.quaternion.z = orientation.value()[2];
114 quaternion.quaternion.w = orientation.value()[3];
119 return constraints_cpp;