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