moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
trajectory_generator_ptp.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
38
39#include <rclcpp/duration.hpp>
40#include <rclcpp/logger.hpp>
41#include <rclcpp/logging.hpp>
42#include <iostream>
43#include <sstream>
44
45#include <tf2_eigen/tf2_eigen.hpp>
46#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
47
49{
50namespace
51{
52rclcpp::Logger getLogger()
53{
54 return moveit::getLogger("moveit.planners.pilz.trajectory_generator.ptp");
55}
56} // namespace
57TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model,
58 const LimitsContainer& planner_limits, const std::string& group_name)
59 : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
60{
62 {
63 throw TrajectoryGeneratorInvalidLimitsException("joint limit not set");
64 }
65
67
68 // collect most strict joint limits for each group in robot model
69 const auto* jmg = robot_model->getJointModelGroup(group_name);
70 if (!jmg)
71 throw TrajectoryGeneratorInvalidLimitsException("invalid group: " + group_name);
72
73 const auto& active_joints = jmg->getActiveJointModelNames();
74
75 // no active joints
76 if (!active_joints.empty())
77 {
78 most_strict_limit_ = joint_limits_.getCommonLimit(active_joints);
79
80 if (!most_strict_limit_.has_velocity_limits)
81 {
82 throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + group_name);
83 }
84 if (!most_strict_limit_.has_acceleration_limits)
85 {
86 throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + group_name);
87 }
88 if (!most_strict_limit_.has_deceleration_limits)
89 {
90 throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + group_name);
91 }
92 }
93
94 RCLCPP_INFO(getLogger(), "Initialized Point-to-Point Trajectory Generator.");
95}
96
97void TrajectoryGeneratorPTP::planPTP(const std::map<std::string, double>& start_pos,
98 const std::map<std::string, double>& goal_pos,
99 trajectory_msgs::msg::JointTrajectory& joint_trajectory,
100 double velocity_scaling_factor, double acceleration_scaling_factor,
101 double sampling_time)
102{
103 // initialize joint names
104 for (const auto& item : goal_pos)
105 {
106 joint_trajectory.joint_names.push_back(item.first);
107 }
108
109 // check if goal already reached
110 bool goal_reached = true;
111 for (const auto& goal : goal_pos)
112 {
113 if (fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT)
114 {
115 goal_reached = false;
116 break;
117 }
118 }
119 if (goal_reached)
120 {
121 RCLCPP_INFO_STREAM(getLogger(), "Goal already reached, set one goal point explicitly.");
122 if (joint_trajectory.points.empty())
123 {
124 trajectory_msgs::msg::JointTrajectoryPoint point;
125 point.time_from_start = rclcpp::Duration::from_seconds(sampling_time);
126 for (const std::string& joint_name : joint_trajectory.joint_names)
127 {
128 point.positions.push_back(start_pos.at(joint_name));
129 point.velocities.push_back(0);
130 point.accelerations.push_back(0);
131 }
132 joint_trajectory.points.push_back(point);
133 }
134 return;
135 }
136
137 // compute the fastest trajectory and choose the slowest joint as leading axis
138 std::string leading_axis = joint_trajectory.joint_names.front();
139 double max_duration = -1.0;
140
141 std::map<std::string, VelocityProfileATrap> velocity_profile;
142 for (const auto& joint_name : joint_trajectory.joint_names)
143 {
144 // create vecocity profile if necessary
145 velocity_profile.insert(std::make_pair(
146 joint_name, VelocityProfileATrap(velocity_scaling_factor * most_strict_limit_.max_velocity,
147 acceleration_scaling_factor * most_strict_limit_.max_acceleration,
148 acceleration_scaling_factor * most_strict_limit_.max_deceleration)));
149
150 velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name));
151 if (velocity_profile.at(joint_name).Duration() > max_duration)
152 {
153 max_duration = velocity_profile.at(joint_name).Duration();
154 leading_axis = joint_name;
155 }
156 }
157
158 // Full Synchronization
159 // This should only work if all axes have same max_vel, max_acc, max_dec
160 // values
161 // reset the velocity profile for other joints
162 double acc_time = velocity_profile.at(leading_axis).firstPhaseDuration();
163 double const_time = velocity_profile.at(leading_axis).secondPhaseDuration();
164 double dec_time = velocity_profile.at(leading_axis).thirdPhaseDuration();
165
166 for (const auto& joint_name : joint_trajectory.joint_names)
167 {
168 if (joint_name != leading_axis)
169 {
170 // make full synchronization
171 // causes the program to terminate if acc_time<=0 or dec_time<=0 (should
172 // be prevented by goal_reached block above)
173 // by using the most strict limit, the following should always return true
174 if (!velocity_profile.at(joint_name)
175 .setProfileAllDurations(start_pos.at(joint_name), goal_pos.at(joint_name), acc_time, const_time,
176 dec_time))
177 // LCOV_EXCL_START
178 {
179 std::stringstream error_str;
180 error_str << "TrajectoryGeneratorPTP::planPTP(): Can not synchronize "
181 "velocity profile of axis "
182 << joint_name << " with leading axis " << leading_axis;
183 throw PtpVelocityProfileSyncFailed(error_str.str());
184 }
185 // LCOV_EXCL_STOP
186 }
187 }
188
189 // first generate the time samples
190 std::vector<double> time_samples;
191 for (double t_sample = 0.0; t_sample < max_duration; t_sample += sampling_time)
192 {
193 time_samples.push_back(t_sample);
194 }
195 // add last time
196 time_samples.push_back(max_duration);
197
198 // construct joint trajectory point
199 for (double time_stamp : time_samples)
200 {
201 trajectory_msgs::msg::JointTrajectoryPoint point;
202 point.time_from_start = rclcpp::Duration::from_seconds(time_stamp);
203 for (std::string& joint_name : joint_trajectory.joint_names)
204 {
205 point.positions.push_back(velocity_profile.at(joint_name).Pos(time_stamp));
206 point.velocities.push_back(velocity_profile.at(joint_name).Vel(time_stamp));
207 point.accelerations.push_back(velocity_profile.at(joint_name).Acc(time_stamp));
208 }
209 joint_trajectory.points.push_back(point);
210 }
211
212 // Set last point velocity and acceleration to zero
213 std::fill(joint_trajectory.points.back().velocities.begin(), joint_trajectory.points.back().velocities.end(), 0.0);
214 std::fill(joint_trajectory.points.back().accelerations.begin(), joint_trajectory.points.back().accelerations.end(),
215 0.0);
216}
217
218void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
220 MotionPlanInfo& info) const
221{
222 info.group_name = req.group_name;
223
224 // extract start state information
225 info.start_joint_position.clear();
226 for (std::size_t i = 0; i < req.start_state.joint_state.name.size(); ++i)
227 {
228 info.start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i];
229 }
230
231 // extract goal
232 info.goal_joint_position.clear();
233 if (!req.goal_constraints.at(0).joint_constraints.empty())
234 {
235 for (const auto& joint_constraint : req.goal_constraints.at(0).joint_constraints)
236 {
237 info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position;
238 }
239 }
240 // solve the ik
241 else
242 {
243 Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front());
244 if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
245 goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
246 {
247 throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose");
248 }
249 }
250}
251
252void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /*scene*/,
253 const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
254 double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
255{
256 // plan the ptp trajectory
257 planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory,
258 req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time);
259}
260
261} // namespace pilz_industrial_motion_planner
JointLimit getCommonLimit() const
Returns joint limit fusion of all(position, velocity, acceleration, deceleration) limits for all join...
This class combines CartesianLimit and JointLimits into on single class.
bool hasJointLimits() const
Return if this LimitsContainer has defined joint limits.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr &robot_model, const pilz_industrial_motion_planner::LimitsContainer &planner_limits, const std::string &group_name)
Constructor of PTP Trajectory Generator.
const moveit::core::RobotModelConstPtr robot_model_
const pilz_industrial_motion_planner::LimitsContainer planner_limits_
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition logger.cpp:79
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
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
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.