39#include <tf2_eigen/tf2_eigen.hpp>
40#include <tf2_kdl/tf2_kdl.hpp>
41#include <boost/range/combine.hpp>
43#include <kdl/velocityprofile_trap.hpp>
59sensor_msgs::msg::JointState TrajectoryGenerator::filterGroupValues(
const sensor_msgs::msg::JointState& robot_state,
60 const std::string& group)
const
62 const std::vector<std::string>& group_joints{
robot_model_->getJointModelGroup(group)->getActiveJointModelNames() };
63 sensor_msgs::msg::JointState group_state;
64 group_state.name.reserve(group_joints.size());
65 group_state.position.reserve(group_joints.size());
66 group_state.velocity.reserve(group_joints.size());
68 for (
size_t i = 0; i < robot_state.name.size(); ++i)
70 if (std::find(group_joints.begin(), group_joints.end(), robot_state.name.at(i)) != group_joints.end())
72 group_state.name.push_back(robot_state.name.at(i));
73 group_state.position.push_back(robot_state.position.at(i));
74 if (i < robot_state.velocity.size())
76 group_state.velocity.push_back(robot_state.velocity.at(i));
89void TrajectoryGenerator::checkVelocityScaling(
double scaling_factor)
91 if (!isScalingFactorValid(scaling_factor))
93 std::ostringstream os;
95 <<
"actual value is: " << scaling_factor;
96 throw VelocityScalingIncorrect(os.str());
100void TrajectoryGenerator::checkAccelerationScaling(
double scaling_factor)
102 if (!isScalingFactorValid(scaling_factor))
104 std::ostringstream os;
106 <<
"actual value is: " << scaling_factor;
107 throw AccelerationScalingIncorrect(os.str());
111void TrajectoryGenerator::checkForValidGroupName(
const std::string& group_name)
const
115 std::ostringstream os;
116 os <<
"Unknown planning group: " << group_name;
117 throw UnknownPlanningGroup(os.str());
121void TrajectoryGenerator::checkStartState(
const moveit_msgs::msg::RobotState& start_state,
122 const std::string& group)
const
124 if (start_state.joint_state.name.empty())
126 throw NoJointNamesInStartState(
"No joint names for state state given");
129 if (start_state.joint_state.name.size() != start_state.joint_state.position.size())
131 throw SizeMismatchInStartState(
"Joint state name and position do not match in start state");
134 sensor_msgs::msg::JointState group_start_state{ filterGroupValues(start_state.joint_state, group) };
138 std::string error_msg;
139 for (
auto joint : boost::combine(group_start_state.
name, group_start_state.position))
141 if (!limits.verifyPositionLimit(joint.get<0>(), joint.get<1>()))
143 error_msg.append(error_msg.empty() ?
"start state joints outside their position limits: " :
", ");
144 error_msg.append(joint.get<0>());
147 if (!error_msg.empty())
149 throw JointsOfStartStateOutOfRange(error_msg);
153 if (!std::all_of(group_start_state.velocity.begin(), group_start_state.velocity.end(),
154 [](
double v) { return std::fabs(v) < VELOCITY_TOLERANCE; }))
156 throw NonZeroVelocityInStartState(
"Trajectory Generator does not allow non-zero start velocity");
160void TrajectoryGenerator::checkJointGoalConstraint(
const moveit_msgs::msg::Constraints& constraint,
161 const std::vector<std::string>& expected_joint_names,
162 const std::string& group_name)
const
164 for (
const auto& joint_constraint : constraint.joint_constraints)
166 const std::string& curr_joint_name{ joint_constraint.joint_name };
167 if (std::find(expected_joint_names.cbegin(), expected_joint_names.cend(), curr_joint_name) ==
168 expected_joint_names.cend())
170 std::ostringstream os;
171 os <<
"Cannot find joint \"" << curr_joint_name <<
"\" from start state in goal constraint";
172 throw StartStateGoalStateMismatch(os.str());
175 if (!
robot_model_->getJointModelGroup(group_name)->hasJointModel(curr_joint_name))
177 std::ostringstream os;
178 os <<
"Joint \"" << curr_joint_name <<
"\" does not belong to group \"" << group_name <<
'\"';
179 throw JointConstraintDoesNotBelongToGroup(os.str());
184 std::ostringstream os;
185 os <<
"Joint \"" << curr_joint_name <<
"\" violates joint limits in goal constraints";
186 throw JointsOfGoalOutOfRange(os.str());
191void TrajectoryGenerator::checkCartesianGoalConstraint(
const moveit_msgs::msg::Constraints& constraint,
192 const std::string& group_name)
const
194 assert(constraint.position_constraints.size() == 1);
195 assert(constraint.orientation_constraints.size() == 1);
196 const moveit_msgs::msg::PositionConstraint& pos_constraint{ constraint.position_constraints.front() };
197 const moveit_msgs::msg::OrientationConstraint& ori_constraint{ constraint.orientation_constraints.front() };
199 if (pos_constraint.link_name.empty())
201 throw PositionConstraintNameMissing(
"Link name of position constraint missing");
204 if (ori_constraint.link_name.empty())
206 throw OrientationConstraintNameMissing(
"Link name of orientation constraint missing");
209 if (pos_constraint.link_name != ori_constraint.link_name)
211 std::ostringstream os;
212 os <<
"Position and orientation constraint name do not match"
213 <<
"(Position constraint name: \"" << pos_constraint.link_name <<
"\" | Orientation constraint name: \""
214 << ori_constraint.link_name <<
"\")";
215 throw PositionOrientationConstraintNameMismatch(os.str());
218 if (!
robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name))
220 std::ostringstream os;
221 os <<
"No IK solver available for link: \"" << pos_constraint.link_name <<
'\"';
222 throw NoIKSolverAvailable(os.str());
225 if (pos_constraint.constraint_region.primitive_poses.empty())
227 throw NoPrimitivePoseGiven(
"Primitive pose in position constraints of goal missing");
231void TrajectoryGenerator::checkGoalConstraints(
232 const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints,
233 const std::vector<std::string>& expected_joint_names,
const std::string& group_name)
const
235 if (goal_constraints.size() != 1)
237 std::ostringstream os;
238 os <<
"Exactly one goal constraint required, but " << goal_constraints.size() <<
" goal constraints given";
239 throw NotExactlyOneGoalConstraintGiven(os.str());
242 const moveit_msgs::msg::Constraints& goal_con{ goal_constraints.front() };
243 if (!isOnlyOneGoalTypeGiven(goal_con))
245 throw OnlyOneGoalTypeAllowed(
"Only cartesian XOR joint goal allowed");
248 if (isJointGoalGiven(goal_con))
250 checkJointGoalConstraint(goal_con, expected_joint_names, group_name);
254 checkCartesianGoalConstraint(goal_con, group_name);
260 checkVelocityScaling(req.max_velocity_scaling_factor);
261 checkAccelerationScaling(req.max_acceleration_scaling_factor);
262 checkForValidGroupName(req.group_name);
263 checkStartState(req.start_state, req.group_name);
264 checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name);
267void TrajectoryGenerator::setSuccessResponse(
const moveit::core::RobotState& start_state,
const std::string& group_name,
268 const trajectory_msgs::msg::JointTrajectory& joint_trajectory,
269 const rclcpp::Time& planning_start,
272 auto rt = std::make_shared<robot_trajectory::RobotTrajectory>(
robot_model_, group_name);
273 rt->setRobotTrajectoryMsg(start_state, joint_trajectory);
276 res.
error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
280void TrajectoryGenerator::setFailureResponse(
const rclcpp::Time& planning_start,
290std::unique_ptr<KDL::VelocityProfile>
292 double max_acceleration_scaling_factor,
293 const std::unique_ptr<KDL::Path>& path)
const
295 std::unique_ptr<KDL::VelocityProfile> vp_trans = std::make_unique<KDL::VelocityProfile_Trap>(
299 if (path->PathLength() > std::numeric_limits<double>::epsilon())
301 vp_trans->SetProfile(0, path->PathLength());
305 vp_trans->SetProfile(0, std::numeric_limits<double>::epsilon());
314 RCLCPP_INFO_STREAM(getLogger(),
"Generating " << req.planner_id <<
" trajectory...");
315 rclcpp::Time planning_begin =
clock_->now();
320 validateRequest(req);
324 RCLCPP_ERROR_STREAM(getLogger(), ex.what());
326 setFailureResponse(planning_begin, res);
332 cmdSpecificRequestValidation(req);
336 RCLCPP_ERROR_STREAM(getLogger(), ex.what());
338 setFailureResponse(planning_begin, res);
345 extractMotionPlanInfo(scene, req, plan_info);
349 RCLCPP_ERROR_STREAM(getLogger(), ex.what());
351 setFailureResponse(planning_begin, res);
355 trajectory_msgs::msg::JointTrajectory joint_trajectory;
358 plan(scene, req, plan_info, sampling_time, joint_trajectory);
362 RCLCPP_ERROR_STREAM(getLogger(), ex.what());
364 setFailureResponse(planning_begin, res);
370 setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res);
Representation of a robot's state. This includes position, velocity, acceleration and effort.
bool verifyPositionLimit(const std::string &joint_name, double joint_position) const
verify position limit of single joint
const cartesian_limits::Params & getCartesianLimits() const
Return the cartesian limits.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
Exception storing an moveit_msgs::msg::MoveItErrorCodes value.
virtual const moveit_msgs::msg::MoveItErrorCodes::_val_type & getErrorCode() const =0
This class is used to extract needed information from motion plan request.
const moveit::core::RobotModelConstPtr robot_model_
static constexpr double MIN_SCALING_FACTOR
const pilz_industrial_motion_planner::LimitsContainer planner_limits_
const std::unique_ptr< rclcpp::Clock > clock_
void generate(const planning_scene::PlanningSceneConstPtr &scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, double sampling_time=0.1)
generate robot trajectory with given sampling time
static constexpr double MAX_SCALING_FACTOR
std::unique_ptr< KDL::VelocityProfile > cartesianTrapVelocityProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor, const std::unique_ptr< KDL::Path > &path) const
build cartesian velocity profile for the path
rclcpp::Logger getLogger()
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
Response to a planning query.
moveit::core::MoveItErrorCode error_code
robot_trajectory::RobotTrajectoryPtr trajectory