moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
utils.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#include <algorithm>
38
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>
47
48#include <rclcpp/node.hpp>
49#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
50#include <tf2_eigen/tf2_eigen.hpp>
51
52using namespace moveit::core;
53
55{
56namespace
57{
58rclcpp::Logger getLogger()
59{
60 return moveit::getLogger("moveit.core.kinematic_constraints");
61}
62} // namespace
63
64moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints& first,
65 const moveit_msgs::msg::Constraints& second)
66{
67 moveit_msgs::msg::Constraints r;
68
69 // add all joint constraints that are in first but not in second
70 // and merge joint constraints that are for the same joint
71 for (const moveit_msgs::msg::JointConstraint& jc_first : first.joint_constraints)
72 {
73 bool add = true;
74 for (const moveit_msgs::msg::JointConstraint& jc_second : second.joint_constraints)
75 {
76 if (jc_second.joint_name == jc_first.joint_name)
77 {
78 add = false;
79 // now we merge
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);
85 if (low > high)
86 {
87 RCLCPP_ERROR(getLogger(),
88 "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
89 a.joint_name.c_str());
90 }
91 else
92 {
93 m.joint_name = a.joint_name;
94 m.position =
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);
100 }
101 break;
102 }
103 }
104 if (add)
105 r.joint_constraints.push_back(jc_first);
106 }
107
108 // add all joint constraints that are in second but not in first
109 for (const moveit_msgs::msg::JointConstraint& jc_second : second.joint_constraints)
110 {
111 bool add = true;
112 for (const moveit_msgs::msg::JointConstraint& jc_first : first.joint_constraints)
113 {
114 if (jc_second.joint_name == jc_first.joint_name)
115 {
116 add = false;
117 break;
118 }
119 }
120 if (add)
121 r.joint_constraints.push_back(jc_second);
122 }
123
124 // merge rest of constraints
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);
128
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);
132
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);
136
137 return r;
138}
139
140std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints& constr)
141{
142 return constr.position_constraints.size() + constr.orientation_constraints.size() +
143 constr.visibility_constraints.size() + constr.joint_constraints.size();
144}
145
146moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState& state,
147 const moveit::core::JointModelGroup* jmg, double tolerance)
148{
149 return constructGoalConstraints(state, jmg, tolerance, tolerance);
150}
151
152moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState& state,
153 const moveit::core::JointModelGroup* jmg, double tolerance_below,
154 double tolerance_above)
155{
156 moveit_msgs::msg::Constraints goal;
157 std::vector<double> vals;
158 state.copyJointGroupPositions(jmg, vals);
159 goal.joint_constraints.resize(vals.size());
160 for (std::size_t i = 0; i < vals.size(); ++i)
161 {
162 goal.joint_constraints[i].joint_name = jmg->getVariableNames()[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;
167 }
168
169 return goal;
170}
171
172bool updateJointConstraints(moveit_msgs::msg::Constraints& constraints, const moveit::core::RobotState& state,
174{
175 const std::vector<std::string>& jmg_active_joints = jmg->getActiveJointModelNames();
176
177 // For each constraint, update it if the joint is found within jmg
178 for (auto& constraint : constraints.joint_constraints)
179 {
180 const auto itr = find(jmg_active_joints.begin(), jmg_active_joints.end(), constraint.joint_name);
181 if (itr != jmg_active_joints.end())
182 {
183 constraint.position = state.getVariablePosition(constraint.joint_name);
184 }
185 // The joint was not found within jmg
186 else
187 {
188 return false;
189 }
190 }
191 return true;
192}
193
194moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
195 const geometry_msgs::msg::PoseStamped& pose,
196 double tolerance_pos, double tolerance_angle)
197{
198 moveit_msgs::msg::Constraints goal;
199
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;
211
212 pcm.header = pose.header;
213 pcm.constraint_region.primitive_poses.resize(1);
214 // orientation of constraint region does not affect anything, since it is a sphere
215 pcm.constraint_region.primitive_poses[0].position = pose.pose.position;
216 pcm.weight = 1.0;
217
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;
226 ocm.weight = 1.0;
227
228 return goal;
229}
230
231moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
232 const geometry_msgs::msg::PoseStamped& pose,
233 const std::vector<double>& tolerance_pos,
234 const std::vector<double>& tolerance_angle)
235{
236 moveit_msgs::msg::Constraints goal = constructGoalConstraints(link_name, pose);
237 if (tolerance_pos.size() == 3)
238 {
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];
245 }
246 if (tolerance_angle.size() == 3)
247 {
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];
252 }
253 return goal;
254}
255
256bool updatePoseConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name,
257 const geometry_msgs::msg::PoseStamped& pose)
258{
259 // Convert message types so the existing functions can be used
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;
265
266 geometry_msgs::msg::QuaternionStamped quat_stamped;
267 quat_stamped.header = pose.header;
268 quat_stamped.quaternion = pose.pose.orientation;
269
270 return updatePositionConstraint(constraints, link_name, point) &&
271 updateOrientationConstraint(constraints, link_name, quat_stamped);
272}
273
274moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
275 const geometry_msgs::msg::QuaternionStamped& quat,
276 double tolerance)
277{
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;
287 ocm.weight = 1.0;
288 return goal;
289}
290
291bool updateOrientationConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name,
292 const geometry_msgs::msg::QuaternionStamped& quat)
293{
294 for (auto& constraint : constraints.orientation_constraints)
295 {
296 if (constraint.link_name == link_name)
297 {
298 if (quat.header.frame_id.empty())
299 {
300 RCLCPP_ERROR(getLogger(), "Cannot update orientation constraint, frame_id in the header is empty");
301 return false;
302 }
303 constraint.header = quat.header;
304 constraint.orientation = quat.quaternion;
305 return true;
306 }
307 }
308 return false;
309}
310
311moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
312 const geometry_msgs::msg::PointStamped& goal_point,
313 double tolerance)
314{
315 geometry_msgs::msg::Point p;
316 p.x = 0;
317 p.y = 0;
318 p.z = 0;
319 return constructGoalConstraints(link_name, p, goal_point, tolerance);
320}
321
322bool updatePositionConstraint(moveit_msgs::msg::Constraints& constraints, const std::string& link_name,
323 const geometry_msgs::msg::PointStamped& goal_point)
324{
325 for (auto& constraint : constraints.position_constraints)
326 {
327 if (constraint.link_name == link_name)
328 {
329 if (goal_point.header.frame_id.empty())
330 {
331 RCLCPP_ERROR(getLogger(), "Cannot update position constraint, frame_id in the header is empty");
332 return false;
333 }
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;
338 return true;
339 }
340 }
341 return false;
342}
343
344moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
345 const geometry_msgs::msg::Point& reference_point,
346 const geometry_msgs::msg::PointStamped& goal_point,
347 double tolerance)
348{
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;
361
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;
365
366 // orientation of constraint region does not affect anything, since it is a sphere
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;
371 pcm.weight = 1.0;
372
373 return goal;
374}
375
376// Initialize a PoseStamped message from node parameters specified at pose_param.
377static bool constructPoseStamped(const rclcpp::Node::SharedPtr& node, const std::string& pose_param,
378 geometry_msgs::msg::PoseStamped& pose)
379{
380 if (!node->get_parameter(pose_param + ".frame_id", pose.header.frame_id))
381 return false;
382
383 std::vector<double> orientation;
384 if (!node->get_parameter(pose_param + ".orientation", orientation) || orientation.size() != 3)
385 return false;
386
387 tf2::Quaternion q;
388 q.setRPY(orientation[0], orientation[1], orientation[2]);
389 pose.pose.orientation = toMsg(q);
390
391 std::vector<double> position;
392 if (!node->get_parameter(pose_param + ".position", position) || position.size() != 3)
393 return false;
394
395 pose.pose.position.x = position[0];
396 pose.pose.position.y = position[1];
397 pose.pose.position.z = position[2];
398
399 return true;
400}
401
402// Initialize a JointConstraint message from node parameters specified at constraint_param.
403static bool constructConstraint(const rclcpp::Node::SharedPtr& node, const std::string& constraint_param,
404 moveit_msgs::msg::JointConstraint& constraint)
405{
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);
409
410 double tolerance;
411 if (node->get_parameter(constraint_param + ".tolerance", tolerance))
412 {
413 constraint.tolerance_below = tolerance;
414 constraint.tolerance_above = tolerance;
415 }
416 else if (node->has_parameter(constraint_param + ".tolerances"))
417 {
418 std::vector<double> tolerances;
419 node->get_parameter(constraint_param + ".tolerances", tolerances);
420 if (tolerances.size() != 2)
421 return false;
422
423 constraint.tolerance_below = tolerances[0];
424 constraint.tolerance_above = tolerances[1];
425 }
426 else if (node->has_parameter(constraint_param + ".bounds"))
427 {
428 std::vector<double> bounds;
429 node->get_parameter(constraint_param + ".bounds", bounds);
430 if (bounds.size() != 2)
431 return false;
432
433 const double lower_bound = bounds[0];
434 const double upper_bound = bounds[1];
435
436 constraint.position = (lower_bound + upper_bound) / 2;
437 constraint.tolerance_below = constraint.position - lower_bound;
438 constraint.tolerance_above = upper_bound - constraint.position;
439 }
440
441 return true;
442}
443
444// Initialize a PositionConstraint message from node parameters specified at constraint_param.
445static bool constructConstraint(const rclcpp::Node::SharedPtr& node, const std::string& constraint_param,
446 moveit_msgs::msg::PositionConstraint& constraint)
447{
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);
451
452 std::vector<double> target_offset;
453 if (node->get_parameter(constraint_param + ".target_offset", target_offset))
454 {
455 if (target_offset.size() != 3)
456 return false;
457
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];
461 }
462 if (!node->list_parameters({ constraint_param + ".region" }, 1).names.empty()) // TODO(henningkayser): specify depth
463 {
464 geometry_msgs::msg::Pose region_pose;
465 region_pose.orientation.w = 1.0;
466
467 shape_msgs::msg::SolidPrimitive region_primitive;
468 region_primitive.type = shape_msgs::msg::SolidPrimitive::BOX;
469 region_primitive.dimensions.resize(3);
470
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)
475 return false;
476
477 center = (dimension_limits[0] + dimension_limits[1]) / 2;
478 dimension = dimension_limits[1] - dimension_limits[2];
479 return true;
480 };
481
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]))
488 return false;
489
490 constraint.constraint_region.primitive_poses.push_back(region_pose);
491 constraint.constraint_region.primitives.emplace_back(region_primitive);
492 }
493
494 return true;
495}
496
497// Initialize an OrientationConstraint message from node parameters specified at constraint_param.
498static bool constructConstraint(const rclcpp::Node::SharedPtr& node, const std::string& constraint_param,
499 moveit_msgs::msg::OrientationConstraint& constraint)
500{
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);
504
505 std::vector<double> orientation;
506 if (node->get_parameter(constraint_param + ".orientation", orientation))
507 {
508 if (orientation.size() != 3)
509 return false;
510
511 tf2::Quaternion q;
512 q.setRPY(orientation[0], orientation[1], orientation[2]);
513 constraint.orientation = toMsg(q);
514 }
515
516 std::vector<double> tolerances;
517 if (node->get_parameter(constraint_param + ".tolerances", tolerances))
518 {
519 if (tolerances.size() != 3)
520 return false;
521
522 constraint.absolute_x_axis_tolerance = tolerances[0];
523 constraint.absolute_y_axis_tolerance = tolerances[1];
524 constraint.absolute_z_axis_tolerance = tolerances[2];
525 }
526
527 return true;
528}
529
530// Initialize a VisibilityConstraint message from node parameters specified at constraint_param.
531static bool constructConstraint(const rclcpp::Node::SharedPtr& node, const std::string& constraint_param,
532 moveit_msgs::msg::VisibilityConstraint& constraint)
533{
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);
539
540 // TODO(henningkayser): specify depth
541 if (!node->list_parameters({ constraint_param + ".target_pose" }, 1).names.empty())
542 {
543 if (!constructPoseStamped(node, constraint_param + ".target_pose", constraint.target_pose))
544 return false;
545 }
546 // TODO(henningkayser): specify depth
547 if (!node->list_parameters({ constraint_param + ".sensor_pose" }, 1).names.empty())
548 {
549 if (!constructPoseStamped(node, constraint_param + ".sensor_pose", constraint.sensor_pose))
550 return false;
551 }
552
553 constraint.sensor_view_direction = moveit_msgs::msg::VisibilityConstraint::SENSOR_X;
554
555 return true;
556}
557
558// Initialize a Constraints message containing constraints specified by node parameters under constraint_ids.
559static bool collectConstraints(const rclcpp::Node::SharedPtr& node, const std::vector<std::string>& constraint_ids,
560 moveit_msgs::msg::Constraints& constraints)
561{
562 for (const auto& constraint_id : constraint_ids)
563 {
564 const auto constraint_param = "constraints." + constraint_id;
565 if (!node->has_parameter(constraint_param + ".type"))
566 {
567 RCLCPP_ERROR(getLogger(), "constraint parameter \"%s\" does not specify its type", constraint_param.c_str());
568 return false;
569 }
570 std::string constraint_type;
571 node->get_parameter(constraint_param + ".type", constraint_type);
572 if (constraint_type == "joint")
573 {
574 constraints.joint_constraints.emplace_back();
575 if (!constructConstraint(node, constraint_param, constraints.joint_constraints.back()))
576 return false;
577 }
578 else if (constraint_type == "position")
579 {
580 constraints.position_constraints.emplace_back();
581 if (!constructConstraint(node, constraint_param, constraints.position_constraints.back()))
582 return false;
583 }
584 else if (constraint_type == "orientation")
585 {
586 constraints.orientation_constraints.emplace_back();
587 if (!constructConstraint(node, constraint_param, constraints.orientation_constraints.back()))
588 return false;
589 }
590 else if (constraint_type == "visibility")
591 {
592 constraints.visibility_constraints.emplace_back();
593 if (!constructConstraint(node, constraint_param, constraints.visibility_constraints.back()))
594 return false;
595 }
596 else
597 {
598 RCLCPP_ERROR_STREAM(getLogger(), "Unable to process unknown constraint type: " << constraint_type);
599 return false;
600 }
601 }
602
603 return true;
604}
605
606bool constructConstraints(const rclcpp::Node::SharedPtr& node, const std::string& constraints_param,
607 moveit_msgs::msg::Constraints& constraints)
608{
609 if (!node->get_parameter(constraints_param + ".name", constraints.name))
610 return false;
611
612 std::vector<std::string> constraint_ids;
613 if (!node->get_parameter(constraints_param + ".constraint_ids", constraint_ids))
614 return false;
615
616 for (auto& constraint_id : constraint_ids)
617 constraint_id.insert(0, constraints_param + std::string("."));
618
619 return collectConstraints(node, constraint_ids, constraints);
620}
621
622bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs::msg::Constraints& constraints)
623{
624 for (auto& c : constraints.position_constraints)
625 {
626 bool frame_found;
627 const moveit::core::LinkModel* robot_link;
628 const Eigen::Isometry3d& transform = state.getFrameInfo(c.link_name, robot_link, frame_found);
629 if (!frame_found)
630 return false;
631
632 // If the frame of the constraint is not part of the robot link model (but an attached body or subframe),
633 // the constraint needs to be expressed in the frame of a robot link.
634 if (c.link_name != robot_link->getName())
635 {
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;
639
640 c.link_name = robot_link->getName();
641 tf2::toMsg(offset_robot_link, c.target_point_offset);
642 }
643 }
644
645 for (auto& c : constraints.orientation_constraints)
646 {
647 bool frame_found;
648 const moveit::core::LinkModel* robot_link;
649 // getFrameInfo() returns a valid isometry by contract
650 const Eigen::Isometry3d& transform = state.getFrameInfo(c.link_name, robot_link, frame_found);
651 if (!frame_found)
652 return false;
653
654 // If the frame of the constraint is not part of the robot link model (but an attached body or subframe),
655 // the constraint needs to be expressed in the frame of a robot link.
656 if (c.link_name != robot_link->getName())
657 {
658 c.link_name = robot_link->getName();
659 Eigen::Quaterniond link_name_to_robot_link(transform.linear().transpose() *
660 state.getGlobalLinkTransform(robot_link).linear());
661 Eigen::Quaterniond quat_target;
662 tf2::fromMsg(c.orientation, quat_target);
663 c.orientation = tf2::toMsg(quat_target * link_name_to_robot_link);
664 }
665 }
666 return true;
667}
668} // namespace kinematic_constraints
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.
Definition link_model.h:72
const std::string & getName() const
The name of this link.
Definition link_model.h:86
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
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...
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.
Definition utils.cpp:291
std::size_t countIndividualConstraints(const moveit_msgs::msg::Constraints &constr)
Definition utils.cpp:140
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.
Definition utils.cpp:322
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....
Definition utils.cpp:152
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.
Definition utils.cpp:172
bool resolveConstraintFrames(const moveit::core::RobotState &state, moveit_msgs::msg::Constraints &constraints)
Resolves frames used in constraints to links in the robot model.
Definition utils.cpp:622
bool constructConstraints(const rclcpp::Node::SharedPtr &node, const std::string &constraints_param, moveit_msgs::msg::Constraints &constraints)
extract constraint message from node parameters.
Definition utils.cpp:606
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.
Definition utils.cpp:256
moveit_msgs::msg::Constraints mergeConstraints(const moveit_msgs::msg::Constraints &first, const moveit_msgs::msg::Constraints &second)
Merge two sets of constraints into one.
Definition utils.cpp:64
Core components of MoveIt.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition logger.cpp:79