moveit2
The MoveIt Motion Planning Framework for ROS 2.
trajectory_generator_circ.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 <cassert>
39 #include <sstream>
40 
41 #include <kdl/rotational_interpolation_sa.hpp>
42 #include <kdl/trajectory_segment.hpp>
43 #include <kdl/utilities/error.h>
44 #include <kdl/utilities/utility.h>
46 #include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
47 #include <rclcpp/logger.hpp>
48 #include <rclcpp/logging.hpp>
49 #include <tf2_eigen/tf2_eigen.hpp>
50 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
51 
53 {
54 static const rclcpp::Logger LOGGER =
55  rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_circ");
56 TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model,
57  const LimitsContainer& planner_limits,
58  const std::string& /*group_name*/)
59  : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
60 {
62  {
63  throw TrajectoryGeneratorInvalidLimitsException(
64  "Cartesian limits are not fully set for CIRC trajectory generator.");
65  }
66 }
67 
68 void TrajectoryGeneratorCIRC::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const
69 {
70  if (!(req.path_constraints.name == "interim" || req.path_constraints.name == "center"))
71  {
72  std::ostringstream os;
73  os << "No path constraint named \"interim\" or \"center\" found (found "
74  "unknown constraint: "
75  << "\"req.path_constraints.name\""
76  << " instead)";
77  throw UnknownPathConstraintName(os.str());
78  }
79 
80  if (req.path_constraints.position_constraints.size() != 1)
81  {
82  throw NoPositionConstraints("CIRC trajectory generator needs valid a position constraint");
83  }
84 
85  if (req.path_constraints.position_constraints.front().constraint_region.primitive_poses.size() != 1)
86  {
87  throw NoPrimitivePose("CIRC trajectory generator needs valid a primitive pose");
88  }
89 }
90 
91 void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
93  TrajectoryGenerator::MotionPlanInfo& info) const
94 {
95  RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request.");
96 
97  info.group_name = req.group_name;
98  moveit::core::RobotState robot_state = scene->getCurrentState();
99 
100  // goal given in joint space
101  if (!req.goal_constraints.front().joint_constraints.empty())
102  {
103  // TODO: link name from goal constraint and path constraint
104  info.link_name = req.path_constraints.position_constraints.front().link_name;
105  if (!robot_model_->hasLinkModel(info.link_name))
106  {
107  throw UnknownLinkNameOfAuxiliaryPoint("Unknown link name of CIRC auxiliary point");
108  }
109 
110  if (req.goal_constraints.front().joint_constraints.size() !=
111  robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size())
112  {
113  std::ostringstream os;
114  os << "Number of joint constraint = " << req.goal_constraints.front().joint_constraints.size()
115  << " not equal to active joints of group = "
116  << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size();
117  throw NumberOfConstraintsMismatch(os.str());
118  }
119 
120  for (const auto& joint_item : req.goal_constraints.front().joint_constraints)
121  {
122  info.goal_joint_position[joint_item.joint_name] = joint_item.position;
123  }
124 
125  computeLinkFK(robot_state, info.link_name, info.goal_joint_position, info.goal_pose);
126  }
127  // goal given in Cartesian space
128  else
129  {
130  std::string frame_id;
131  info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
132  if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
133  req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
134  {
135  RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of "
136  "goal. Use model frame as default");
137  frame_id = robot_model_->getModelFrame();
138  }
139  else
140  {
141  frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
142  }
143 
144  // goal pose with optional offset wrt. the planning frame
145  info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
146  frame_id = robot_model_->getModelFrame();
147 
148  // check goal pose ik before Cartesian motion plan starts
149  std::map<std::string, double> ik_solution;
150  if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
151  ik_solution))
152  {
153  // LCOV_EXCL_START
154  std::ostringstream os;
155  os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
156  throw CircInverseForGoalIncalculable(os.str());
157  // LCOV_EXCL_STOP // not able to trigger here since lots of checks before
158  // are in place
159  }
160  }
161 
162  computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
163 
164  // center point with wrt. the planning frame
165  std::string center_point_frame_id;
166 
167  info.circ_path_point.first = req.path_constraints.name;
168  if (req.path_constraints.position_constraints.front().header.frame_id.empty())
169  {
170  RCLCPP_WARN(LOGGER, "Frame id is not set in position constraints of "
171  "path. Use model frame as default");
172  center_point_frame_id = robot_model_->getModelFrame();
173  }
174  else
175  {
176  center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id;
177  }
178 
179  Eigen::Isometry3d center_point_pose;
180  tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(),
181  center_point_pose);
182 
183  center_point_pose = scene->getFrameTransform(center_point_frame_id) * center_point_pose;
184 
185  if (!req.goal_constraints.front().position_constraints.empty())
186  {
187  const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front();
188  geometry_msgs::msg::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.translation()));
189  info.circ_path_point.second = getConstraintPose(center_point, goal.orientation_constraints.front().orientation,
190  goal.position_constraints.front().target_point_offset)
191  .translation();
192  }
193  else
194  {
195  info.circ_path_point.second = center_point_pose.translation();
196  }
197 }
198 
199 void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene,
200  const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
201  const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
202 {
203  std::unique_ptr<KDL::Path> cart_path(setPathCIRC(plan_info));
204  std::unique_ptr<KDL::VelocityProfile> vel_profile(
205  cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, cart_path));
206 
207  // combine path and velocity profile into Cartesian trajectory
208  // with the third parameter set to false, KDL::Trajectory_Segment does not
209  // take
210  // the ownship of Path and Velocity Profile
211  KDL::Trajectory_Segment cart_trajectory(cart_path.get(), vel_profile.get(), false);
212 
213  moveit_msgs::msg::MoveItErrorCodes error_code;
214  // sample the Cartesian trajectory and compute joint trajectory using inverse
215  // kinematics
216  if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
217  plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
218  error_code))
219  {
220  throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path",
221  error_code.val);
222  }
223 }
224 
225 std::unique_ptr<KDL::Path> TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlanInfo& info) const
226 {
227  RCLCPP_DEBUG(LOGGER, "Set Cartesian path for CIRC command.");
228 
229  KDL::Frame start_pose, goal_pose;
230  tf2::transformEigenToKDL(info.start_pose, start_pose);
231  tf2::transformEigenToKDL(info.goal_pose, goal_pose);
232 
233  const auto& eigen_path_point = info.circ_path_point.second;
234  const KDL::Vector path_point{ eigen_path_point.x(), eigen_path_point.y(), eigen_path_point.z() };
235 
236  // pass the ratio of translational by rotational velocity as equivalent radius
237  // to get a trajectory with rotational speed, if no (or very little)
238  // translational distance
239  // The KDL::Path implementation chooses the motion with the longer duration
240  // (translation vs. rotation)
241  // and uses eqradius as scaling factor between the distances.
244 
245  try
246  {
247  if (info.circ_path_point.first == "center")
248  {
249  return PathCircleGenerator::circleFromCenter(start_pose, goal_pose, path_point, eqradius);
250  }
251  else // if (info.circ_path_point.first == "interim")
252  {
253  return PathCircleGenerator::circleFromInterim(start_pose, goal_pose, path_point, eqradius);
254  }
255  }
256  catch (KDL::Error_MotionPlanning_Circle_No_Plane& e)
257  {
258  std::ostringstream os;
259  os << "Failed to create path object for circle." << e.Description();
260  throw CircleNoPlane(os.str());
261  }
262  catch (KDL::Error_MotionPlanning_Circle_ToSmall& e)
263  {
264  std::ostringstream os;
265  os << "Failed to create path object for circle." << e.Description();
266  throw CircleToSmall(os.str());
267  }
269  {
270  std::ostringstream os;
271  os << "Failed to create path object for circle." << e.Description();
272  throw CenterPointDifferentRadius(os.str());
273  }
274 
275  return nullptr;
276 }
277 
278 } // namespace pilz_industrial_motion_planner
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
double getMaxRotationalVelocity() const
Return the maximal rotational velocity [rad/s], 0 if nothing was set.
double getMaxTranslationalVelocity() const
Return the maximal translational velocity [m/s], 0 if nothing was set.
This class combines CartesianLimit and JointLimits into on single class.
bool hasFullCartesianLimits() const
Return if this LimitsContainer has defined cartesian limits.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
const CartesianLimit & getCartesianLimits() const
Return the cartesian limits.
static std::unique_ptr< KDL::Path > circleFromCenter(const KDL::Frame &start_pose, const KDL::Frame &goal_pose, const KDL::Vector &center_point, double eqradius)
set the path circle from start, goal and center point
static std::unique_ptr< KDL::Path > circleFromInterim(const KDL::Frame &start_pose, const KDL::Frame &goal_pose, const KDL::Vector &interim_point, double eqradius)
set circle from start, goal and interim point
TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr &robot_model, const pilz_industrial_motion_planner::LimitsContainer &planner_limits, const std::string &group_name)
Constructor of CIRC Trajectory Generator.
const moveit::core::RobotModelConstPtr robot_model_
std::unique_ptr< KDL::VelocityProfile > cartesianTrapVelocityProfile(const double &max_velocity_scaling_factor, const double &max_acceleration_scaling_factor, const std::unique_ptr< KDL::Path > &path) const
build cartesian velocity profile for the path
const pilz_industrial_motion_planner::LimitsContainer planner_limits_
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
frame_id
Definition: pick.py:63
scene
Definition: pick.py:52
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 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, const 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
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.