38 #include <rclcpp/rclcpp.hpp>
44 namespace bind_kinematic_constraints
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;
127 moveit_msgs::msg::Constraints joint_constraints =
130 return joint_constraints;
134 const std::string& ns)
137 moveit_msgs::msg::Constraints constraints_cpp;
140 return constraints_cpp;
148 py::arg(
"source_frame"), py::arg(
"cartesian_position") =
nullptr,
149 py::arg(
"cartesian_position_tolerance") =
nullptr, py::arg(
"orientation") =
nullptr,
150 py::arg(
"orientation_tolerance") =
nullptr,
"Construct a link constraint message");
152 py::arg(
"joint_model_group"), py::arg(
"tolerance") = 0.01,
153 "Construct a joint constraint message");
155 py::arg(
"ns"),
"Construct a constraint message from a node");
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Representation and evaluation of kinematic constraints.
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
bool constructConstraints(const rclcpp::Node::SharedPtr &node, const std::string &constraints_param, moveit_msgs::msg::Constraints &constraints)
extract constraint message from node parameters.
moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints &first, const moveit_msgs::msg::Constraints &second)
Merge two sets of constraints into one.
void initKinematicConstraints(py::module &m)
moveit_msgs::msg::Constraints constructJointConstraint(moveit::core::RobotState &robot_state, moveit::core::JointModelGroup *joint_model_group, double tolerance)
moveit_msgs::msg::Constraints constructConstraintsFromNode(const std::shared_ptr< rclcpp::Node > &node_name, const std::string &ns)
moveit_msgs::msg::Constraints constructLinkConstraint(const std::string &link_name, const std::string &source_frame, std::optional< std::vector< double >> cartesian_position, std::optional< double > cartesian_position_tolerance, std::optional< std::vector< double >> orientation, std::optional< double > orientation_tolerance)