moveit2
The MoveIt Motion Planning Framework for ROS 2.
trajectory_generator_lin.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 
39 #include <cassert>
40 #include <sstream>
41 #include <time.h>
43 #include <kdl/path_line.hpp>
44 #include <kdl/trajectory_segment.hpp>
45 #include <kdl/utilities/error.h>
46 #include <tf2/convert.h>
47 #include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
48 #include <rclcpp/logger.hpp>
49 #include <rclcpp/logging.hpp>
50 #include <tf2_eigen/tf2_eigen.hpp>
51 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
52 
54 {
55 static const rclcpp::Logger LOGGER =
56  rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_lin");
57 TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(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  RCLCPP_ERROR(LOGGER, "Cartesian limits not set for LIN trajectory generator.");
64  throw TrajectoryGeneratorInvalidLimitsException("Cartesian limits are not fully set for LIN trajectory generator.");
65  }
66 }
67 
68 void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
71 {
72  RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request.");
73 
74  info.group_name = req.group_name;
75  std::string frame_id{ robot_model_->getModelFrame() };
76 
77  // goal given in joint space
78  if (!req.goal_constraints.front().joint_constraints.empty())
79  {
80  info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name));
81 
82  if (req.goal_constraints.front().joint_constraints.size() !=
83  robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size())
84  {
85  std::ostringstream os;
86  os << "Number of joints in goal does not match number of joints of group "
87  "(Number joints goal: "
88  << req.goal_constraints.front().joint_constraints.size() << " | Number of joints of group: "
89  << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size() << ")";
90  throw JointNumberMismatch(os.str());
91  }
92 
93  for (const auto& joint_item : req.goal_constraints.front().joint_constraints)
94  {
95  info.goal_joint_position[joint_item.joint_name] = joint_item.position;
96  }
97 
98  // Ignored return value because at this point the function should always
99  // return 'true'.
101  }
102  // goal given in Cartesian space
103  else
104  {
105  info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
106  if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
107  req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
108  {
109  RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of "
110  "goal. Use model frame as default");
111  frame_id = robot_model_->getModelFrame();
112  }
113  else
114  {
115  frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
116  }
117  info.goal_pose = getConstraintPose(req.goal_constraints.front());
118  }
119 
120  assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
121  for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames())
122  {
123  auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) };
124  if (it == req.start_state.joint_state.name.cend())
125  {
126  std::ostringstream os;
127  os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name
128  << "\" in start state of request";
129  throw LinJointMissingInStartState(os.str());
130  }
131  size_t index = it - req.start_state.joint_state.name.cbegin();
132  info.start_joint_position[joint_name] = req.start_state.joint_state.position[index];
133  }
134 
135  // Ignored return value because at this point the function should always
136  // return 'true'.
138 
139  // check goal pose ik before Cartesian motion plan starts
140  std::map<std::string, double> ik_solution;
142  ik_solution))
143  {
144  std::ostringstream os;
145  os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
146  throw LinInverseForGoalIncalculable(os.str());
147  }
148 }
149 
150 void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene,
151  const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
152  const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
153 {
154  // create Cartesian path for lin
155  std::unique_ptr<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose));
156 
157  // create velocity profile
158  std::unique_ptr<KDL::VelocityProfile> vp(
159  cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path));
160 
161  // combine path and velocity profile into Cartesian trajectory
162  // with the third parameter set to false, KDL::Trajectory_Segment does not
163  // take
164  // the ownship of Path and Velocity Profile
165  KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(), false);
166 
167  moveit_msgs::msg::MoveItErrorCodes error_code;
168  // sample the Cartesian trajectory and compute joint trajectory using inverse
169  // kinematics
170  if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
171  plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
172  error_code))
173  {
174  std::ostringstream os;
175  os << "Failed to generate valid joint trajectory from the Cartesian path";
176  throw LinTrajectoryConversionFailure(os.str(), error_code.val);
177  }
178 }
179 
180 std::unique_ptr<KDL::Path> TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affine3d& start_pose,
181  const Eigen::Affine3d& goal_pose) const
182 {
183  RCLCPP_DEBUG(LOGGER, "Set Cartesian path for LIN command.");
184 
185  KDL::Frame kdl_start_pose, kdl_goal_pose;
186  tf2::transformEigenToKDL(start_pose, kdl_start_pose);
187  tf2::transformEigenToKDL(goal_pose, kdl_goal_pose);
190  KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis();
191 
192  return std::unique_ptr<KDL::Path>(
193  std::make_unique<KDL::Path_Line>(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius, true));
194 }
195 
196 } // namespace pilz_industrial_motion_planner
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.
TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr &robot_model, const pilz_industrial_motion_planner::LimitsContainer &planner_limits, const std::string &group_name)
Constructor of LIN Trajectory Generator.
This class is used to extract needed information from motion plan request.
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_
frame_id
Definition: pick.py:63
scene
Definition: pick.py:52
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
bool computeLinkFK(const planning_scene::PlanningSceneConstPtr &scene, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
compute the pose of a link at give robot state
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.