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