39#include <geometric_shapes/solid_primitive_dims.h>
42#include <rclcpp/logger.hpp>
43#include <rclcpp/logging.hpp>
44#include <rclcpp/node.hpp>
45#include <rclcpp/parameter_value.hpp>
48#include <rclcpp/node.hpp>
49#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
50#include <tf2_eigen/tf2_eigen.hpp>
64moveit_msgs::msg::Constraints
mergeConstraints(
const moveit_msgs::msg::Constraints& first,
65 const moveit_msgs::msg::Constraints& second)
67 moveit_msgs::msg::Constraints r;
71 for (
const moveit_msgs::msg::JointConstraint& jc_first : first.joint_constraints)
74 for (
const moveit_msgs::msg::JointConstraint& jc_second : second.joint_constraints)
76 if (jc_second.joint_name == jc_first.joint_name)
80 moveit_msgs::msg::JointConstraint m;
81 const moveit_msgs::msg::JointConstraint& a = jc_first;
82 const moveit_msgs::msg::JointConstraint& b = jc_second;
83 double low = std::max(a.position - a.tolerance_below, b.position - b.tolerance_below);
84 double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above);
87 RCLCPP_ERROR(getLogger(),
88 "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
89 a.joint_name.c_str());
93 m.joint_name = a.joint_name;
95 std::max(low, std::min((a.position * a.weight + b.position * b.weight) / (a.weight + b.weight), high));
96 m.weight = (a.weight + b.weight) / 2.0;
97 m.tolerance_above = std::max(0.0, high - m.position);
98 m.tolerance_below = std::max(0.0, m.position - low);
99 r.joint_constraints.push_back(m);
105 r.joint_constraints.push_back(jc_first);
109 for (
const moveit_msgs::msg::JointConstraint& jc_second : second.joint_constraints)
112 for (
const moveit_msgs::msg::JointConstraint& jc_first : first.joint_constraints)
114 if (jc_second.joint_name == jc_first.joint_name)
121 r.joint_constraints.push_back(jc_second);
125 r.position_constraints = first.position_constraints;
126 for (
const moveit_msgs::msg::PositionConstraint& position_constraint : second.position_constraints)
127 r.position_constraints.push_back(position_constraint);
129 r.orientation_constraints = first.orientation_constraints;
130 for (
const moveit_msgs::msg::OrientationConstraint& orientation_constraint : second.orientation_constraints)
131 r.orientation_constraints.push_back(orientation_constraint);
133 r.visibility_constraints = first.visibility_constraints;
134 for (
const moveit_msgs::msg::VisibilityConstraint& visibility_constraint : second.visibility_constraints)
135 r.visibility_constraints.push_back(visibility_constraint);
142 return constr.position_constraints.size() + constr.orientation_constraints.size() +
143 constr.visibility_constraints.size() + constr.joint_constraints.size();
154 double tolerance_above)
156 moveit_msgs::msg::Constraints goal;
157 std::vector<double> vals;
159 goal.joint_constraints.resize(vals.size());
160 for (std::size_t i = 0; i < vals.size(); ++i)
163 goal.joint_constraints[i].position = vals[i];
164 goal.joint_constraints[i].tolerance_above = tolerance_above;
165 goal.joint_constraints[i].tolerance_below = tolerance_below;
166 goal.joint_constraints[i].weight = 1.0;
178 for (
auto& constraint : constraints.joint_constraints)
180 const auto itr = find(jmg_active_joints.begin(), jmg_active_joints.end(), constraint.joint_name);
181 if (itr != jmg_active_joints.end())
195 const geometry_msgs::msg::PoseStamped& pose,
196 double tolerance_pos,
double tolerance_angle)
198 moveit_msgs::msg::Constraints goal;
200 goal.position_constraints.resize(1);
201 moveit_msgs::msg::PositionConstraint& pcm = goal.position_constraints[0];
202 pcm.link_name = link_name;
203 pcm.target_point_offset.x = 0;
204 pcm.target_point_offset.y = 0;
205 pcm.target_point_offset.z = 0;
206 pcm.constraint_region.primitives.resize(1);
207 shape_msgs::msg::SolidPrimitive& bv = pcm.constraint_region.primitives[0];
208 bv.type = shape_msgs::msg::SolidPrimitive::SPHERE;
209 bv.dimensions.resize(geometric_shapes::solidPrimitiveDimCount<shape_msgs::msg::SolidPrimitive::SPHERE>());
210 bv.dimensions[shape_msgs::msg::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos;
212 pcm.header = pose.header;
213 pcm.constraint_region.primitive_poses.resize(1);
215 pcm.constraint_region.primitive_poses[0].position = pose.pose.position;
218 goal.orientation_constraints.resize(1);
219 moveit_msgs::msg::OrientationConstraint& ocm = goal.orientation_constraints[0];
220 ocm.link_name = link_name;
221 ocm.header = pose.header;
222 ocm.orientation = pose.pose.orientation;
223 ocm.absolute_x_axis_tolerance = tolerance_angle;
224 ocm.absolute_y_axis_tolerance = tolerance_angle;
225 ocm.absolute_z_axis_tolerance = tolerance_angle;
232 const geometry_msgs::msg::PoseStamped& pose,
233 const std::vector<double>& tolerance_pos,
234 const std::vector<double>& tolerance_angle)
237 if (tolerance_pos.size() == 3)
239 shape_msgs::msg::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0];
240 bv.type = shape_msgs::msg::SolidPrimitive::BOX;
241 bv.dimensions.resize(geometric_shapes::solidPrimitiveDimCount<shape_msgs::msg::SolidPrimitive::BOX>());
242 bv.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = tolerance_pos[0];
243 bv.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = tolerance_pos[1];
244 bv.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = tolerance_pos[2];
246 if (tolerance_angle.size() == 3)
248 moveit_msgs::msg::OrientationConstraint& ocm = goal.orientation_constraints[0];
249 ocm.absolute_x_axis_tolerance = tolerance_angle[0];
250 ocm.absolute_y_axis_tolerance = tolerance_angle[1];
251 ocm.absolute_z_axis_tolerance = tolerance_angle[2];
257 const geometry_msgs::msg::PoseStamped& pose)
260 geometry_msgs::msg::PointStamped point;
261 point.header = pose.header;
262 point.point.x = pose.pose.position.x;
263 point.point.y = pose.pose.position.y;
264 point.point.z = pose.pose.position.z;
266 geometry_msgs::msg::QuaternionStamped quat_stamped;
267 quat_stamped.header = pose.header;
268 quat_stamped.quaternion = pose.pose.orientation;
275 const geometry_msgs::msg::QuaternionStamped& quat,
278 moveit_msgs::msg::Constraints goal;
279 goal.orientation_constraints.resize(1);
280 moveit_msgs::msg::OrientationConstraint& ocm = goal.orientation_constraints[0];
281 ocm.link_name = link_name;
282 ocm.header = quat.header;
283 ocm.orientation = quat.quaternion;
284 ocm.absolute_x_axis_tolerance = tolerance;
285 ocm.absolute_y_axis_tolerance = tolerance;
286 ocm.absolute_z_axis_tolerance = tolerance;
292 const geometry_msgs::msg::QuaternionStamped& quat)
294 for (
auto& constraint : constraints.orientation_constraints)
296 if (constraint.link_name == link_name)
298 if (quat.header.frame_id.empty())
300 RCLCPP_ERROR(getLogger(),
"Cannot update orientation constraint, frame_id in the header is empty");
303 constraint.header = quat.header;
304 constraint.orientation = quat.quaternion;
312 const geometry_msgs::msg::PointStamped& goal_point,
315 geometry_msgs::msg::Point p;
323 const geometry_msgs::msg::PointStamped& goal_point)
325 for (
auto& constraint : constraints.position_constraints)
327 if (constraint.link_name == link_name)
329 if (goal_point.header.frame_id.empty())
331 RCLCPP_ERROR(getLogger(),
"Cannot update position constraint, frame_id in the header is empty");
334 constraint.header = goal_point.header;
335 constraint.constraint_region.primitive_poses.at(0).position.x = goal_point.point.x;
336 constraint.constraint_region.primitive_poses.at(0).position.y = goal_point.point.y;
337 constraint.constraint_region.primitive_poses.at(0).position.z = goal_point.point.z;
345 const geometry_msgs::msg::Point& reference_point,
346 const geometry_msgs::msg::PointStamped& goal_point,
349 moveit_msgs::msg::Constraints goal;
350 goal.position_constraints.resize(1);
351 moveit_msgs::msg::PositionConstraint& pcm = goal.position_constraints[0];
352 pcm.link_name = link_name;
353 pcm.target_point_offset.x = reference_point.x;
354 pcm.target_point_offset.y = reference_point.y;
355 pcm.target_point_offset.z = reference_point.z;
356 pcm.constraint_region.primitives.resize(1);
357 pcm.constraint_region.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
358 pcm.constraint_region.primitives[0].dimensions.resize(
359 geometric_shapes::solidPrimitiveDimCount<shape_msgs::msg::SolidPrimitive::SPHERE>());
360 pcm.constraint_region.primitives[0].dimensions[shape_msgs::msg::SolidPrimitive::SPHERE_RADIUS] = tolerance;
362 pcm.header = goal_point.header;
363 pcm.constraint_region.primitive_poses.resize(1);
364 pcm.constraint_region.primitive_poses[0].position = goal_point.point;
367 pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
368 pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
369 pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
370 pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
377static bool constructPoseStamped(
const rclcpp::Node::SharedPtr& node,
const std::string& pose_param,
378 geometry_msgs::msg::PoseStamped& pose)
380 if (!node->get_parameter(pose_param +
".frame_id", pose.header.frame_id))
383 std::vector<double> orientation;
384 if (!node->get_parameter(pose_param +
".orientation", orientation) || orientation.size() != 3)
388 q.setRPY(orientation[0], orientation[1], orientation[2]);
389 pose.pose.orientation = toMsg(q);
391 std::vector<double> position;
392 if (!node->get_parameter(pose_param +
".position", position) || position.size() != 3)
395 pose.pose.position.x = position[0];
396 pose.pose.position.y = position[1];
397 pose.pose.position.z = position[2];
403static bool constructConstraint(
const rclcpp::Node::SharedPtr& node,
const std::string& constraint_param,
404 moveit_msgs::msg::JointConstraint& constraint)
406 node->get_parameter(constraint_param +
".weight", constraint.weight);
407 node->get_parameter(constraint_param +
".joint_name", constraint.joint_name);
408 node->get_parameter(constraint_param +
".position", constraint.position);
411 if (node->get_parameter(constraint_param +
".tolerance", tolerance))
413 constraint.tolerance_below = tolerance;
414 constraint.tolerance_above = tolerance;
416 else if (node->has_parameter(constraint_param +
".tolerances"))
418 std::vector<double> tolerances;
419 node->get_parameter(constraint_param +
".tolerances", tolerances);
420 if (tolerances.size() != 2)
423 constraint.tolerance_below = tolerances[0];
424 constraint.tolerance_above = tolerances[1];
426 else if (node->has_parameter(constraint_param +
".bounds"))
428 std::vector<double> bounds;
429 node->get_parameter(constraint_param +
".bounds", bounds);
430 if (bounds.size() != 2)
433 const double lower_bound = bounds[0];
434 const double upper_bound = bounds[1];
436 constraint.position = (lower_bound + upper_bound) / 2;
437 constraint.tolerance_below = constraint.position - lower_bound;
438 constraint.tolerance_above = upper_bound - constraint.position;
445static bool constructConstraint(
const rclcpp::Node::SharedPtr& node,
const std::string& constraint_param,
446 moveit_msgs::msg::PositionConstraint& constraint)
448 node->get_parameter(constraint_param +
".frame_id", constraint.header.frame_id);
449 node->get_parameter(constraint_param +
".weight", constraint.weight);
450 node->get_parameter(constraint_param +
".link_name", constraint.link_name);
452 std::vector<double> target_offset;
453 if (node->get_parameter(constraint_param +
".target_offset", target_offset))
455 if (target_offset.size() != 3)
458 constraint.target_point_offset.x = target_offset[0];
459 constraint.target_point_offset.y = target_offset[1];
460 constraint.target_point_offset.z = target_offset[2];
462 if (!node->list_parameters({ constraint_param +
".region" }, 1).names.empty())
464 geometry_msgs::msg::Pose region_pose;
465 region_pose.orientation.w = 1.0;
467 shape_msgs::msg::SolidPrimitive region_primitive;
468 region_primitive.type = shape_msgs::msg::SolidPrimitive::BOX;
469 region_primitive.dimensions.resize(3);
471 const auto parse_dimension = [&](
const std::string& dimension_param,
double& center,
double& dimension) ->
bool {
472 std::vector<double> dimension_limits;
473 if (!node->get_parameter(constraint_param +
".region." + dimension_param, dimension_limits) ||
474 dimension_limits.size() != 2)
477 center = (dimension_limits[0] + dimension_limits[1]) / 2;
478 dimension = dimension_limits[1] - dimension_limits[2];
482 if (!parse_dimension(
"x", region_pose.position.x,
483 region_primitive.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X]) ||
484 !parse_dimension(
"y", region_pose.position.y,
485 region_primitive.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y]) ||
486 !parse_dimension(
"z", region_pose.position.z,
487 region_primitive.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z]))
490 constraint.constraint_region.primitive_poses.push_back(region_pose);
491 constraint.constraint_region.primitives.emplace_back(region_primitive);
498static bool constructConstraint(
const rclcpp::Node::SharedPtr& node,
const std::string& constraint_param,
499 moveit_msgs::msg::OrientationConstraint& constraint)
501 node->get_parameter(constraint_param +
".frame_id", constraint.header.frame_id);
502 node->get_parameter(constraint_param +
".weight", constraint.weight);
503 node->get_parameter(constraint_param +
".link_name", constraint.link_name);
505 std::vector<double> orientation;
506 if (node->get_parameter(constraint_param +
".orientation", orientation))
508 if (orientation.size() != 3)
512 q.setRPY(orientation[0], orientation[1], orientation[2]);
513 constraint.orientation = toMsg(q);
516 std::vector<double> tolerances;
517 if (node->get_parameter(constraint_param +
".tolerances", tolerances))
519 if (tolerances.size() != 3)
522 constraint.absolute_x_axis_tolerance = tolerances[0];
523 constraint.absolute_y_axis_tolerance = tolerances[1];
524 constraint.absolute_z_axis_tolerance = tolerances[2];
531static bool constructConstraint(
const rclcpp::Node::SharedPtr& node,
const std::string& constraint_param,
532 moveit_msgs::msg::VisibilityConstraint& constraint)
534 node->get_parameter(constraint_param +
".weight", constraint.weight);
535 node->get_parameter(constraint_param +
".target_radius", constraint.target_radius);
536 node->get_parameter(constraint_param +
".cone_sides", constraint.cone_sides);
537 node->get_parameter(constraint_param +
".max_view_angle", constraint.max_view_angle);
538 node->get_parameter(constraint_param +
".max_range_angle", constraint.max_range_angle);
541 if (!node->list_parameters({ constraint_param +
".target_pose" }, 1).names.empty())
543 if (!constructPoseStamped(node, constraint_param +
".target_pose", constraint.target_pose))
547 if (!node->list_parameters({ constraint_param +
".sensor_pose" }, 1).names.empty())
549 if (!constructPoseStamped(node, constraint_param +
".sensor_pose", constraint.sensor_pose))
553 constraint.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_X;
559static bool collectConstraints(
const rclcpp::Node::SharedPtr& node,
const std::vector<std::string>& constraint_ids,
560 moveit_msgs::msg::Constraints& constraints)
562 for (
const auto& constraint_id : constraint_ids)
564 const auto constraint_param =
"constraints." + constraint_id;
565 if (!node->has_parameter(constraint_param +
".type"))
567 RCLCPP_ERROR(
getLogger(),
"constraint parameter \"%s\" does not specify its type", constraint_param.c_str());
570 std::string constraint_type;
571 node->get_parameter(constraint_param +
".type", constraint_type);
572 if (constraint_type ==
"joint")
574 constraints.joint_constraints.emplace_back();
575 if (!constructConstraint(node, constraint_param, constraints.joint_constraints.back()))
578 else if (constraint_type ==
"position")
580 constraints.position_constraints.emplace_back();
581 if (!constructConstraint(node, constraint_param, constraints.position_constraints.back()))
584 else if (constraint_type ==
"orientation")
586 constraints.orientation_constraints.emplace_back();
587 if (!constructConstraint(node, constraint_param, constraints.orientation_constraints.back()))
590 else if (constraint_type ==
"visibility")
592 constraints.visibility_constraints.emplace_back();
593 if (!constructConstraint(node, constraint_param, constraints.visibility_constraints.back()))
598 RCLCPP_ERROR_STREAM(
getLogger(),
"Unable to process unknown constraint type: " << constraint_type);
607 moveit_msgs::msg::Constraints& constraints)
609 if (!node->get_parameter(constraints_param +
".name", constraints.name))
612 std::vector<std::string> constraint_ids;
613 if (!node->get_parameter(constraints_param +
".constraint_ids", constraint_ids))
616 for (
auto& constraint_id : constraint_ids)
617 constraint_id.insert(0, constraints_param + std::string(
"."));
619 return collectConstraints(node, constraint_ids, constraints);
624 for (
auto& c : constraints.position_constraints)
628 const Eigen::Isometry3d& transform = state.
getFrameInfo(c.link_name, robot_link, frame_found);
634 if (c.link_name != robot_link->
getName())
636 Eigen::Isometry3d robot_link_to_link_name = state.
getGlobalLinkTransform(robot_link).inverse() * transform;
637 Eigen::Vector3d offset_link_name(c.target_point_offset.x, c.target_point_offset.y, c.target_point_offset.z);
638 Eigen::Vector3d offset_robot_link = robot_link_to_link_name * offset_link_name;
640 c.link_name = robot_link->
getName();
641 tf2::toMsg(offset_robot_link, c.target_point_offset);
645 for (
auto& c : constraints.orientation_constraints)
650 const Eigen::Isometry3d& transform = state.
getFrameInfo(c.link_name, robot_link, frame_found);
656 if (c.link_name != robot_link->
getName())
658 c.link_name = robot_link->
getName();
659 Eigen::Quaterniond link_name_to_robot_link(transform.linear().transpose() *
661 Eigen::Quaterniond quat_target;
662 tf2::fromMsg(c.orientation, quat_target);
663 c.orientation = tf2::toMsg(quat_target * link_name_to_robot_link);
const std::vector< std::string > & getActiveJointModelNames() const
Get the names of the active joints in this group. These are the names of the joints returned by getJo...
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
A link from the robot. Contains the constant transform applied to the link and its geometry.
const std::string & getName() const
The name of this link.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
const Eigen::Isometry3d & getFrameInfo(const std::string &frame_id, const LinkModel *&robot_link, bool &frame_found) const
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
rclcpp::Logger getLogger()
Representation and evaluation of kinematic constraints.
bool updateOrientationConstraint(moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::QuaternionStamped &quat)
Update an orientation constraint for one link with a new quaternion.
std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints &constr)
bool updatePositionConstraint(moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::PointStamped &goal_point)
Update a position constraint for one link with a new position.
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 updateJointConstraints(moveit_msgs::msg::Constraints &constraints, const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg)
Update joint constraints with a new JointModelGroup state.
bool resolveConstraintFrames(const moveit::core::RobotState &state, moveit_msgs::msg::Constraints &constraints)
Resolves frames used in constraints to links in the robot model.
bool constructConstraints(const rclcpp::Node::SharedPtr &node, const std::string &constraints_param, moveit_msgs::msg::Constraints &constraints)
extract constraint message from node parameters.
bool updatePoseConstraint(moveit_msgs::msg::Constraints &constraints, const std::string &link_name, const geometry_msgs::msg::PoseStamped &pose)
Update a pose constraint for one link with a new pose.
moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints &first, const moveit_msgs::msg::Constraints &second)
Merge two sets of constraints into one.
Core components of MoveIt.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.