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>
50 static const rclcpp::Logger LOGGER = rclcpp::get_logger(
"moveit.pilz_industrial_motion_planner.trajectory_generator");
52 sensor_msgs::msg::JointState TrajectoryGenerator::filterGroupValues(
const sensor_msgs::msg::JointState& robot_state,
53 const std::string&
group)
const
55 const std::vector<std::string>& group_joints{
robot_model_->getJointModelGroup(
group)->getActiveJointModelNames() };
56 sensor_msgs::msg::JointState group_state;
57 group_state.name.reserve(group_joints.size());
58 group_state.position.reserve(group_joints.size());
59 group_state.velocity.reserve(group_joints.size());
61 for (
size_t i = 0; i < robot_state.name.size(); ++i)
63 if (std::find(group_joints.begin(), group_joints.end(), robot_state.name.at(i)) != group_joints.end())
65 group_state.name.push_back(robot_state.name.at(i));
66 group_state.position.push_back(robot_state.position.at(i));
67 if (i < robot_state.velocity.size())
69 group_state.velocity.push_back(robot_state.velocity.at(i));
82 void TrajectoryGenerator::checkVelocityScaling(
const double& scaling_factor)
84 if (!isScalingFactorValid(scaling_factor))
86 std::ostringstream os;
88 <<
"actual value is: " << scaling_factor;
89 throw VelocityScalingIncorrect(os.str());
93 void TrajectoryGenerator::checkAccelerationScaling(
const double& scaling_factor)
95 if (!isScalingFactorValid(scaling_factor))
97 std::ostringstream os;
99 <<
"actual value is: " << scaling_factor;
100 throw AccelerationScalingIncorrect(os.str());
104 void TrajectoryGenerator::checkForValidGroupName(
const std::string& group_name)
const
108 std::ostringstream os;
109 os <<
"Unknown planning group: " << group_name;
110 throw UnknownPlanningGroup(os.str());
114 void TrajectoryGenerator::checkStartState(
const moveit_msgs::msg::RobotState& start_state,
115 const std::string&
group)
const
117 if (start_state.joint_state.name.empty())
119 throw NoJointNamesInStartState(
"No joint names for state state given");
122 if (start_state.joint_state.name.size() != start_state.joint_state.position.size())
124 throw SizeMismatchInStartState(
"Joint state name and position do not match in start state");
127 sensor_msgs::msg::JointState group_start_state{ filterGroupValues(start_state.joint_state,
group) };
131 std::string error_msg;
132 for (
auto joint : boost::combine(group_start_state.name, group_start_state.position))
134 if (!limits.verifyPositionLimit(joint.get<0>(), joint.get<1>()))
136 error_msg.append(error_msg.empty() ?
"start state joints outside their position limits: " :
", ");
137 error_msg.append(joint.get<0>());
140 if (!error_msg.empty())
142 throw JointsOfStartStateOutOfRange(error_msg);
146 if (!std::all_of(group_start_state.velocity.begin(), group_start_state.velocity.end(),
147 [
this](
double v) { return std::fabs(v) < this->VELOCITY_TOLERANCE; }))
149 throw NonZeroVelocityInStartState(
"Trajectory Generator does not allow non-zero start velocity");
153 void TrajectoryGenerator::checkJointGoalConstraint(
const moveit_msgs::msg::Constraints& constraint,
154 const std::vector<std::string>& expected_joint_names,
155 const std::string& group_name)
const
157 for (
auto const& joint_constraint : constraint.joint_constraints)
159 const std::string& curr_joint_name{ joint_constraint.joint_name };
160 if (std::find(expected_joint_names.cbegin(), expected_joint_names.cend(), curr_joint_name) ==
161 expected_joint_names.cend())
163 std::ostringstream os;
164 os <<
"Cannot find joint \"" << curr_joint_name <<
"\" from start state in goal constraint";
165 throw StartStateGoalStateMismatch(os.str());
168 if (!
robot_model_->getJointModelGroup(group_name)->hasJointModel(curr_joint_name))
170 std::ostringstream os;
171 os <<
"Joint \"" << curr_joint_name <<
"\" does not belong to group \"" << group_name <<
"\"";
172 throw JointConstraintDoesNotBelongToGroup(os.str());
177 std::ostringstream os;
178 os <<
"Joint \"" << curr_joint_name <<
"\" violates joint limits in goal constraints";
179 throw JointsOfGoalOutOfRange(os.str());
184 void TrajectoryGenerator::checkCartesianGoalConstraint(
const moveit_msgs::msg::Constraints& constraint,
185 const std::string& group_name)
const
187 assert(constraint.position_constraints.size() == 1);
188 assert(constraint.orientation_constraints.size() == 1);
189 const moveit_msgs::msg::PositionConstraint& pos_constraint{ constraint.position_constraints.front() };
190 const moveit_msgs::msg::OrientationConstraint& ori_constraint{ constraint.orientation_constraints.front() };
192 if (pos_constraint.link_name.empty())
194 throw PositionConstraintNameMissing(
"Link name of position constraint missing");
197 if (ori_constraint.link_name.empty())
199 throw OrientationConstraintNameMissing(
"Link name of orientation constraint missing");
202 if (pos_constraint.link_name != ori_constraint.link_name)
204 std::ostringstream os;
205 os <<
"Position and orientation constraint name do not match"
206 <<
"(Position constraint name: \"" << pos_constraint.link_name <<
"\" | Orientation constraint name: \""
207 << ori_constraint.link_name <<
"\")";
208 throw PositionOrientationConstraintNameMismatch(os.str());
211 if (!
robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name))
213 std::ostringstream os;
214 os <<
"No IK solver available for link: \"" << pos_constraint.link_name <<
"\"";
215 throw NoIKSolverAvailable(os.str());
218 if (pos_constraint.constraint_region.primitive_poses.empty())
220 throw NoPrimitivePoseGiven(
"Primitive pose in position constraints of goal missing");
224 void TrajectoryGenerator::checkGoalConstraints(
225 const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints,
226 const std::vector<std::string>& expected_joint_names,
const std::string& group_name)
const
228 if (goal_constraints.size() != 1)
230 std::ostringstream os;
231 os <<
"Exactly one goal constraint required, but " << goal_constraints.size() <<
" goal constraints given";
232 throw NotExactlyOneGoalConstraintGiven(os.str());
235 const moveit_msgs::msg::Constraints& goal_con{ goal_constraints.front() };
236 if (!isOnlyOneGoalTypeGiven(goal_con))
238 throw OnlyOneGoalTypeAllowed(
"Only cartesian XOR joint goal allowed");
241 if (isJointGoalGiven(goal_con))
243 checkJointGoalConstraint(goal_con, expected_joint_names, group_name);
247 checkCartesianGoalConstraint(goal_con, group_name);
253 checkVelocityScaling(req.max_velocity_scaling_factor);
254 checkAccelerationScaling(req.max_acceleration_scaling_factor);
255 checkForValidGroupName(req.group_name);
256 checkStartState(req.start_state, req.group_name);
257 checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name);
260 void TrajectoryGenerator::setSuccessResponse(
const moveit::core::RobotState& start_state,
const std::string& group_name,
261 const trajectory_msgs::msg::JointTrajectory& joint_trajectory,
262 const rclcpp::Time& planning_start,
265 auto rt = std::make_shared<robot_trajectory::RobotTrajectory>(
robot_model_, group_name);
266 rt->setRobotTrajectoryMsg(start_state, joint_trajectory);
269 res.
error_code_.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
273 void TrajectoryGenerator::setFailureResponse(
const rclcpp::Time& planning_start,
283 std::unique_ptr<KDL::VelocityProfile>
285 const double& max_acceleration_scaling_factor,
286 const std::unique_ptr<KDL::Path>& path)
const
288 std::unique_ptr<KDL::VelocityProfile> vp_trans = std::make_unique<KDL::VelocityProfile_Trap>(
292 if (path->PathLength() > std::numeric_limits<double>::epsilon())
294 vp_trans->SetProfile(0, path->PathLength());
298 vp_trans->SetProfile(0, std::numeric_limits<double>::epsilon());
307 RCLCPP_INFO_STREAM(LOGGER,
"Generating " << req.planner_id <<
" trajectory...");
308 rclcpp::Time planning_begin =
clock_->now();
312 validateRequest(req);
316 RCLCPP_ERROR_STREAM(LOGGER, ex.what());
318 setFailureResponse(planning_begin, res);
324 cmdSpecificRequestValidation(req);
328 RCLCPP_ERROR_STREAM(LOGGER, ex.what());
330 setFailureResponse(planning_begin, res);
337 extractMotionPlanInfo(
scene, req, plan_info);
341 RCLCPP_ERROR_STREAM(LOGGER, ex.what());
343 setFailureResponse(planning_begin, res);
347 trajectory_msgs::msg::JointTrajectory joint_trajectory;
350 plan(
scene, req, plan_info, sampling_time, joint_trajectory);
354 RCLCPP_ERROR_STREAM(LOGGER, ex.what());
356 setFailureResponse(planning_begin, res);
362 setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res);
Representation of a robot's state. This includes position, velocity, acceleration and effort.
double getMaxTranslationalAcceleration() const
Return the maximal translational acceleration [m/s^2], 0 if nothing was set.
double getMaxTranslationalVelocity() const
Return the maximal translational velocity [m/s], 0 if nothing was set.
bool verifyPositionLimit(const std::string &joint_name, const double &joint_position) const
verify position limit of single joint
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
const CartesianLimit & getCartesianLimits() const
Return the cartesian limits.
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_
bool 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
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
static constexpr double MIN_SCALING_FACTOR
const pilz_industrial_motion_planner::LimitsContainer planner_limits_
const std::unique_ptr< rclcpp::Clock > clock_
static constexpr double MAX_SCALING_FACTOR
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.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
robot_trajectory::RobotTrajectoryPtr trajectory_
moveit_msgs::msg::MoveItErrorCodes error_code_