moveit2
The MoveIt Motion Planning Framework for ROS 2.
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
trajectory_functions.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2018 Pilz GmbH & Co. KG
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 Pilz GmbH & Co. KG 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
36
38// TODO: Remove conditional include when released to all active distros.
39#if __has_include(<tf2/LinearMath/Quaternion.hpp>)
40#include <tf2/LinearMath/Quaternion.hpp>
41#else
42#include <tf2/LinearMath/Quaternion.h>
43#endif
44#include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
45#include <tf2_eigen/tf2_eigen.hpp>
46#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
48
49namespace
50{
51rclcpp::Logger getLogger()
52{
53 return moveit::getLogger("pilz_trajectory_functions");
54}
55} // namespace
56
57bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::PlanningSceneConstPtr& scene,
58 const std::string& group_name, const std::string& link_name,
59 const Eigen::Isometry3d& pose, const std::string& frame_id,
60 const std::map<std::string, double>& seed,
61 std::map<std::string, double>& solution, bool check_self_collision,
62 const double timeout)
63{
64 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
65 if (!robot_model->hasJointModelGroup(group_name))
66 {
67 RCLCPP_ERROR_STREAM(getLogger(), "Robot model has no planning group named as " << group_name);
68 return false;
69 }
70
71 if (frame_id != robot_model->getModelFrame())
72 {
73 RCLCPP_ERROR_STREAM(getLogger(), "Given frame (" << frame_id << ") is unequal to model frame("
74 << robot_model->getModelFrame() << ')');
75 return false;
76 }
77
78 moveit::core::RobotState rstate{ scene->getCurrentState() };
79 rstate.setVariablePositions(seed);
80
81 moveit::core::GroupStateValidityCallbackFn ik_constraint_function;
82 if (check_self_collision)
83 {
84 ik_constraint_function = [scene](moveit::core::RobotState* robot_state,
85 const moveit::core::JointModelGroup* joint_group,
86 const double* joint_group_variable_values) {
87 return pilz_industrial_motion_planner::isStateColliding(scene, robot_state, joint_group,
88 joint_group_variable_values);
89 };
90 }
91
92 // call ik
93 const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group_name);
94 if (rstate.setFromIK(jmg, pose, link_name, timeout, ik_constraint_function))
95 {
96 // copy the solution
97 for (const auto& joint_name : jmg->getActiveJointModelNames())
98 {
99 solution[joint_name] = rstate.getVariablePosition(joint_name);
100 }
101 return true;
102 }
103 else
104 {
105 RCLCPP_ERROR(getLogger(), "Unable to find IK solution.");
106 // TODO(henning): Re-enable logging error
107 // RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics for pose \n" << pose.translation() << " has no solution.");
108 return false;
109 }
110}
111
112bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::PlanningSceneConstPtr& scene,
113 const std::string& group_name, const std::string& link_name,
114 const geometry_msgs::msg::Pose& pose, const std::string& frame_id,
115 const std::map<std::string, double>& seed,
116 std::map<std::string, double>& solution, bool check_self_collision,
117 const double timeout)
118{
119 Eigen::Isometry3d pose_eigen;
120 tf2::convert<geometry_msgs::msg::Pose, Eigen::Isometry3d>(pose, pose_eigen);
121 return computePoseIK(scene, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision,
122 timeout);
123}
124
125bool pilz_industrial_motion_planner::computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name,
126 const std::map<std::string, double>& joint_state,
127 Eigen::Isometry3d& pose)
128{
129 // check the reference frame of the target pose
130 if (!robot_state.knowsFrameTransform(link_name))
131 {
132 RCLCPP_ERROR_STREAM(getLogger(), "The target link " << link_name << " is not known by robot.");
133 return false;
134 }
135
136 robot_state.setVariablePositions(joint_state);
137
138 // update the frame
139 robot_state.update();
140 pose = robot_state.getFrameTransform(link_name);
141
142 return true;
143}
144
146 const std::map<std::string, double>& position_last, const std::map<std::string, double>& velocity_last,
147 const std::map<std::string, double>& position_current, double duration_last, double duration_current,
149{
150 const double epsilon = 10e-6;
151 if (duration_current <= epsilon)
152 {
153 RCLCPP_ERROR(getLogger(), "Sample duration too small, cannot compute the velocity");
154 return false;
155 }
156
157 double velocity_current, acceleration_current;
158
159 for (const auto& pos : position_current)
160 {
161 velocity_current = (pos.second - position_last.at(pos.first)) / duration_current;
162
163 if (!joint_limits.verifyVelocityLimit(pos.first, velocity_current))
164 {
165 RCLCPP_ERROR_STREAM(getLogger(), "Joint velocity limit of "
166 << pos.first << " violated. Set the velocity scaling factor lower!"
167 << " Actual joint velocity is " << velocity_current
168 << ", while the limit is " << joint_limits.getLimit(pos.first).max_velocity
169 << ". ");
170 return false;
171 }
172
173 acceleration_current = (velocity_current - velocity_last.at(pos.first)) / (duration_last + duration_current) * 2;
174 // acceleration case
175 if (fabs(velocity_last.at(pos.first)) <= fabs(velocity_current))
176 {
177 if (!joint_limits.verifyAccelerationLimit(pos.first, acceleration_current))
178 {
179 RCLCPP_ERROR_STREAM(getLogger(), "Joint acceleration limit of "
180 << pos.first << " violated. Set the acceleration scaling factor lower!"
181 << " Actual joint acceleration is " << acceleration_current
182 << ", while the limit is "
183 << joint_limits.getLimit(pos.first).max_acceleration << ". ");
184 return false;
185 }
186 }
187 // deceleration case
188 else
189 {
190 if (!joint_limits.verifyDecelerationLimit(pos.first, acceleration_current))
191 {
192 RCLCPP_ERROR_STREAM(getLogger(), "Joint deceleration limit of "
193 << pos.first << " violated. Set the acceleration scaling factor lower!"
194 << " Actual joint deceleration is " << acceleration_current
195 << ", while the limit is "
196 << joint_limits.getLimit(pos.first).max_deceleration << ". ");
197 return false;
198 }
199 }
200 }
201
202 return true;
203}
204
206 const planning_scene::PlanningSceneConstPtr& scene,
207 const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory,
208 const std::string& group_name, const std::string& link_name,
209 const std::map<std::string, double>& initial_joint_position, double sampling_time,
210 trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code,
211 bool check_self_collision)
212{
213 RCLCPP_DEBUG(getLogger(), "Generate joint trajectory from a Cartesian trajectory.");
214
215 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
216 rclcpp::Clock clock;
217 rclcpp::Time generation_begin = clock.now();
218
219 // generate the time samples
220 const double epsilon = 10e-06; // avoid adding the last time sample twice
221 std::vector<double> time_samples;
222 for (double t_sample = 0.0; t_sample < trajectory.Duration() - epsilon; t_sample += sampling_time)
223 {
224 time_samples.push_back(t_sample);
225 }
226 time_samples.push_back(trajectory.Duration());
227
228 // sample the trajectory and solve the inverse kinematics
229 Eigen::Isometry3d pose_sample;
230 std::map<std::string, double> ik_solution_last, ik_solution, joint_velocity_last;
231 ik_solution_last = initial_joint_position;
232 for (const auto& item : ik_solution_last)
233 {
234 joint_velocity_last[item.first] = 0.0;
235 }
236
237 for (std::vector<double>::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end();
238 ++time_iter)
239 {
240 tf2::transformKDLToEigen(trajectory.Pos(*time_iter), pose_sample);
241
242 if (!computePoseIK(scene, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last,
243 ik_solution, check_self_collision))
244 {
245 RCLCPP_ERROR(getLogger(), "Failed to compute inverse kinematics solution for sampled Cartesian pose.");
246 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
247 joint_trajectory.points.clear();
248 return false;
249 }
250
251 // check the joint limits
252 double duration_current_sample = sampling_time;
253 // last interval can be shorter than the sampling time
254 if (time_iter == (time_samples.end() - 1) && time_samples.size() > 1)
255 {
256 duration_current_sample = *time_iter - *(time_iter - 1);
257 }
258 if (time_samples.size() == 1)
259 {
260 duration_current_sample = *time_iter;
261 }
262
263 // skip the first sample with zero time from start for limits checking
264 if (time_iter != time_samples.begin() &&
265 !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time,
266 duration_current_sample, joint_limits))
267 {
268 RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution at "
269 << *time_iter
270 << "s violates the joint velocity/acceleration/deceleration limits.");
271 error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
272 joint_trajectory.points.clear();
273 return false;
274 }
275
276 // fill the point with joint values
277 trajectory_msgs::msg::JointTrajectoryPoint point;
278
279 // set joint names
280 joint_trajectory.joint_names.clear();
281 for (const auto& start_joint : initial_joint_position)
282 {
283 joint_trajectory.joint_names.push_back(start_joint.first);
284 }
285
286 point.time_from_start = rclcpp::Duration::from_seconds(*time_iter);
287 for (const auto& joint_name : joint_trajectory.joint_names)
288 {
289 point.positions.push_back(ik_solution.at(joint_name));
290
291 if (time_iter != time_samples.begin() && time_iter != time_samples.end() - 1)
292 {
293 double joint_velocity =
294 (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current_sample;
295 point.velocities.push_back(joint_velocity);
296 point.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) /
297 (duration_current_sample + sampling_time) * 2);
298 joint_velocity_last[joint_name] = joint_velocity;
299 }
300 else
301 {
302 point.velocities.push_back(0.);
303 point.accelerations.push_back(0.);
304 joint_velocity_last[joint_name] = 0.;
305 }
306 }
307
308 // update joint trajectory
309 joint_trajectory.points.push_back(point);
310 ik_solution_last = ik_solution;
311 }
312
313 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
314 double duration_ms = (clock.now() - generation_begin).seconds() * 1000;
315 RCLCPP_DEBUG_STREAM(getLogger(), "Generate trajectory (N-Points: "
316 << joint_trajectory.points.size() << ") took " << duration_ms << " ms | "
317 << duration_ms / joint_trajectory.points.size() << " ms per Point");
318
319 return true;
320}
321
323 const planning_scene::PlanningSceneConstPtr& scene,
325 const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, const std::string& group_name,
326 const std::string& link_name, const std::map<std::string, double>& initial_joint_position,
327 const std::map<std::string, double>& initial_joint_velocity,
328 trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code,
329 bool check_self_collision)
330{
331 RCLCPP_DEBUG(getLogger(), "Generate joint trajectory from a Cartesian trajectory.");
332
333 const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
334 rclcpp::Clock clock;
335 rclcpp::Time generation_begin = clock.now();
336
337 std::map<std::string, double> ik_solution_last = initial_joint_position;
338 std::map<std::string, double> joint_velocity_last = initial_joint_velocity;
339 double duration_last = 0;
340 double duration_current = 0;
341 joint_trajectory.joint_names.clear();
342 for (const auto& joint_position : ik_solution_last)
343 {
344 joint_trajectory.joint_names.push_back(joint_position.first);
345 }
346 std::map<std::string, double> ik_solution;
347 for (size_t i = 0; i < trajectory.points.size(); ++i)
348 {
349 // compute inverse kinematics
350 if (!computePoseIK(scene, group_name, link_name, trajectory.points.at(i).pose, robot_model->getModelFrame(),
351 ik_solution_last, ik_solution, check_self_collision))
352 {
353 RCLCPP_ERROR(getLogger(), "Failed to compute inverse kinematics solution for sampled "
354 "Cartesian pose.");
355 error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
356 joint_trajectory.points.clear();
357 return false;
358 }
359
360 // verify the joint limits
361 if (i == 0)
362 {
363 duration_current = trajectory.points.front().time_from_start.seconds();
364 duration_last = duration_current;
365 }
366 else
367 {
368 duration_current =
369 trajectory.points.at(i).time_from_start.seconds() - trajectory.points.at(i - 1).time_from_start.seconds();
370 }
371
372 if (!verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, duration_last, duration_current,
374 {
375 // LCOV_EXCL_START since the same code was captured in a test in the other
376 // overload generateJointTrajectory(...,
377 // KDL::Trajectory, ...)
378 // TODO: refactor to avoid code duplication.
379 RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution of the "
380 << i
381 << "th sample violates the joint "
382 "velocity/acceleration/deceleration limits.");
383 error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
384 joint_trajectory.points.clear();
385 return false;
386 // LCOV_EXCL_STOP
387 }
388
389 // compute the waypoint
390 trajectory_msgs::msg::JointTrajectoryPoint waypoint_joint;
391 waypoint_joint.time_from_start = trajectory.points.at(i).time_from_start;
392 for (const auto& joint_name : joint_trajectory.joint_names)
393 {
394 waypoint_joint.positions.push_back(ik_solution.at(joint_name));
395 double joint_velocity = (ik_solution.at(joint_name) - ik_solution_last.at(joint_name)) / duration_current;
396 waypoint_joint.velocities.push_back(joint_velocity);
397 waypoint_joint.accelerations.push_back((joint_velocity - joint_velocity_last.at(joint_name)) /
398 (duration_current + duration_last) * 2);
399 // update the joint velocity
400 joint_velocity_last[joint_name] = joint_velocity;
401 }
402
403 // update joint trajectory
404 joint_trajectory.points.push_back(waypoint_joint);
405 ik_solution_last = ik_solution;
406 duration_last = duration_current;
407 }
408
409 error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
410
411 double duration_ms = (clock.now() - generation_begin).seconds() * 1000;
412 RCLCPP_DEBUG_STREAM(getLogger(), "Generate trajectory (N-Points: "
413 << joint_trajectory.points.size() << ") took " << duration_ms << " ms | "
414 << duration_ms / joint_trajectory.points.size() << " ms per Point");
415
416 return true;
417}
418
420 const robot_trajectory::RobotTrajectoryPtr& first_trajectory,
421 const robot_trajectory::RobotTrajectoryPtr& second_trajectory, double epsilon, double& sampling_time)
422{
423 // The last sample is ignored because it is allowed to violate the sampling
424 // time.
425 std::size_t n1 = first_trajectory->getWayPointCount() - 1;
426 std::size_t n2 = second_trajectory->getWayPointCount() - 1;
427 if ((n1 < 2) && (n2 < 2))
428 {
429 RCLCPP_ERROR_STREAM(getLogger(), "Both trajectories do not have enough points to determine sampling time.");
430 return false;
431 }
432
433 if (n1 >= 2)
434 {
435 sampling_time = first_trajectory->getWayPointDurationFromPrevious(1);
436 }
437 else
438 {
439 sampling_time = second_trajectory->getWayPointDurationFromPrevious(1);
440 }
441
442 for (std::size_t i = 1; i < std::max(n1, n2); ++i)
443 {
444 if (i < n1)
445 {
446 if (fabs(sampling_time - first_trajectory->getWayPointDurationFromPrevious(i)) > epsilon)
447 {
448 RCLCPP_ERROR_STREAM(getLogger(), "First trajectory violates sampline time "
449 << sampling_time << " between points " << (i - 1) << "and " << i
450 << " (indices).");
451 return false;
452 }
453 }
454
455 if (i < n2)
456 {
457 if (fabs(sampling_time - second_trajectory->getWayPointDurationFromPrevious(i)) > epsilon)
458 {
459 RCLCPP_ERROR_STREAM(getLogger(), "Second trajectory violates sampline time "
460 << sampling_time << " between points " << (i - 1) << "and " << i
461 << " (indices).");
462 return false;
463 }
464 }
465 }
466
467 return true;
468}
469
471 const moveit::core::RobotState& state2,
472 const std::string& joint_group_name, double epsilon)
473{
474 Eigen::VectorXd joint_position_1, joint_position_2;
475
476 state1.copyJointGroupPositions(joint_group_name, joint_position_1);
477 state2.copyJointGroupPositions(joint_group_name, joint_position_2);
478
479 if ((joint_position_1 - joint_position_2).norm() > epsilon)
480 {
481 RCLCPP_DEBUG_STREAM(getLogger(), "Joint positions of the two states are different. state1: "
482 << joint_position_1 << " state2: " << joint_position_2);
483 return false;
484 }
485
486 Eigen::VectorXd joint_velocity_1, joint_velocity_2;
487
488 state1.copyJointGroupVelocities(joint_group_name, joint_velocity_1);
489 state2.copyJointGroupVelocities(joint_group_name, joint_velocity_2);
490
491 if ((joint_velocity_1 - joint_velocity_2).norm() > epsilon)
492 {
493 RCLCPP_DEBUG_STREAM(getLogger(), "Joint velocities of the two states are different. state1: "
494 << joint_velocity_1 << " state2: " << joint_velocity_2);
495 return false;
496 }
497
498 Eigen::VectorXd joint_acc_1, joint_acc_2;
499
500 state1.copyJointGroupAccelerations(joint_group_name, joint_acc_1);
501 state2.copyJointGroupAccelerations(joint_group_name, joint_acc_2);
502
503 if ((joint_acc_1 - joint_acc_2).norm() > epsilon)
504 {
505 RCLCPP_DEBUG_STREAM(getLogger(), "Joint accelerations of the two states are different. state1: "
506 << joint_acc_1 << " state2: " << joint_acc_2);
507 return false;
508 }
509
510 return true;
511}
512
514 const std::string& group, double EPSILON)
515{
516 Eigen::VectorXd joint_variable;
517 state.copyJointGroupVelocities(group, joint_variable);
518 if (joint_variable.norm() > EPSILON)
519 {
520 RCLCPP_DEBUG(getLogger(), "Joint velocities are not zero.");
521 return false;
522 }
523 state.copyJointGroupAccelerations(group, joint_variable);
524 if (joint_variable.norm() > EPSILON)
525 {
526 RCLCPP_DEBUG(getLogger(), "Joint accelerations are not zero.");
527 return false;
528 }
529 return true;
530}
531
533 const Eigen::Vector3d& center_position, double r,
534 const robot_trajectory::RobotTrajectoryPtr& traj,
535 bool inverseOrder, std::size_t& index)
536{
537 RCLCPP_DEBUG(getLogger(), "Start linear search for intersection point.");
538
539 const size_t waypoint_num = traj->getWayPointCount();
540
541 if (inverseOrder)
542 {
543 for (size_t i = waypoint_num - 1; i > 0; --i)
544 {
545 if (intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(),
546 traj->getWayPointPtr(i - 1)->getFrameTransform(link_name).translation(), r))
547 {
548 index = i;
549 return true;
550 }
551 }
552 }
553 else
554 {
555 for (size_t i = 0; i < waypoint_num - 1; ++i)
556 {
557 if (intersectionFound(center_position, traj->getWayPointPtr(i)->getFrameTransform(link_name).translation(),
558 traj->getWayPointPtr(i + 1)->getFrameTransform(link_name).translation(), r))
559 {
560 index = i;
561 return true;
562 }
563 }
564 }
565
566 return false;
567}
568
569bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_center,
570 const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next,
571 double r)
572{
573 return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r);
574}
575
576bool pilz_industrial_motion_planner::isStateColliding(const planning_scene::PlanningSceneConstPtr& scene,
578 const moveit::core::JointModelGroup* const group,
579 const double* const ik_solution)
580{
581 rstate->setJointGroupPositions(group, ik_solution);
582 rstate->update();
584 collision_req.group_name = group->getName();
585 collision_req.verbose = true;
587 scene->checkSelfCollision(collision_req, collision_res, *rstate);
588 return !collision_res.collision;
589}
590
591void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat)
592{
593 tf2::Quaternion q;
594 tf2::convert<geometry_msgs::msg::Quaternion, tf2::Quaternion>(quat, q);
595 quat = tf2::toMsg(q.normalized());
596}
597
598Eigen::Isometry3d getConstraintPose(const geometry_msgs::msg::Point& position,
599 const geometry_msgs::msg::Quaternion& orientation,
600 const geometry_msgs::msg::Vector3& offset)
601{
602 Eigen::Quaterniond quat;
603 tf2::fromMsg(orientation, quat);
604 quat.normalize();
605 Eigen::Vector3d v;
606 tf2::fromMsg(position, v);
607
608 Eigen::Isometry3d pose = Eigen::Translation3d(v) * quat;
609
610 tf2::fromMsg(offset, v);
611 pose.translation() -= quat * v;
612 return pose;
613}
614
615Eigen::Isometry3d getConstraintPose(const moveit_msgs::msg::Constraints& goal)
616{
617 return getConstraintPose(goal.position_constraints.front().constraint_region.primitive_poses.front().position,
618 goal.orientation_constraints.front().orientation,
619 goal.position_constraints.front().target_point_offset);
620}
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::string & getName() const
Get the name of the joint group.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
void copyJointGroupVelocities(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the velocity values of the variables that make up the group into another loca...
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
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...
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void update(bool force=false)
Update all transforms.
void copyJointGroupAccelerations(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the acceleration values of the variables that make up the group into another ...
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame (root of model) to frame frame_id is known.
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
std::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_...
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition logger.cpp:79
bool computeLinkFK(moveit::core::RobotState &robot_state, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
compute the pose of a link at a given robot state
bool linearSearchIntersectionPoint(const std::string &link_name, const Eigen::Vector3d &center_position, const double r, const robot_trajectory::RobotTrajectoryPtr &traj, bool inverseOrder, std::size_t &index)
Performs a linear search for the intersection point of the trajectory with the blending radius.
bool isStateColliding(const planning_scene::PlanningSceneConstPtr &scene, moveit::core::RobotState *state, const moveit::core::JointModelGroup *const group, const double *const ik_solution)
Checks if current robot state is in self collision.
bool isRobotStateStationary(const moveit::core::RobotState &state, const std::string &group, double EPSILON)
check if the robot state have zero velocity/acceleration
bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr &first_trajectory, const robot_trajectory::RobotTrajectoryPtr &second_trajectory, double EPSILON, double &sampling_time)
Determines the sampling time and checks that both trajectroies use the same sampling time.
bool intersectionFound(const Eigen::Vector3d &p_center, const Eigen::Vector3d &p_current, const Eigen::Vector3d &p_next, double r)
bool verifySampleJointLimits(const std::map< std::string, double > &position_last, const std::map< std::string, double > &velocity_last, const std::map< std::string, double > &position_current, double duration_last, double duration_current, const JointLimitsContainer &joint_limits)
verify the velocity/acceleration limits of current sample (based on backward difference computation) ...
bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr &scene, const JointLimitsContainer &joint_limits, const KDL::Trajectory &trajectory, const std::string &group_name, const std::string &link_name, const std::map< std::string, double > &initial_joint_position, double sampling_time, trajectory_msgs::msg::JointTrajectory &joint_trajectory, moveit_msgs::msg::MoveItErrorCodes &error_code, bool check_self_collision=false)
Generate joint trajectory from a KDL Cartesian trajectory.
bool computePoseIK(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const std::string &link_name, const Eigen::Isometry3d &pose, const std::string &frame_id, const std::map< std::string, double > &seed, std::map< std::string, double > &solution, bool check_self_collision=true, const double timeout=0.0)
compute the inverse kinematics of a given pose, also check robot self collision
bool isRobotStateEqual(const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const std::string &joint_group_name, double epsilon)
Check if the two robot states have the same joint position/velocity/acceleration.
Representation of a collision checking request.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)....
bool verbose
Flag indicating whether information about detected collisions should be reported.
Representation of a collision checking result.
bool collision
True if collision was found, false otherwise.
void normalizeQuaternion(geometry_msgs::msg::Quaternion &quat)
Eigen::Isometry3d getConstraintPose(const geometry_msgs::msg::Point &position, const geometry_msgs::msg::Quaternion &orientation, const geometry_msgs::msg::Vector3 &offset)
Adapt goal pose, defined by position+orientation, to consider offset.