39#include <rclcpp/duration.hpp>
40#include <rclcpp/logger.hpp>
41#include <rclcpp/logging.hpp>
45#include <tf2_eigen/tf2_eigen.hpp>
46#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
63 throw TrajectoryGeneratorInvalidLimitsException(
"joint limit not set");
69 const auto* jmg = robot_model->getJointModelGroup(group_name);
71 throw TrajectoryGeneratorInvalidLimitsException(
"invalid group: " + group_name);
73 const auto& active_joints = jmg->getActiveJointModelNames();
76 if (!active_joints.empty())
82 throw TrajectoryGeneratorInvalidLimitsException(
"velocity limit not set for group " + group_name);
86 throw TrajectoryGeneratorInvalidLimitsException(
"acceleration limit not set for group " + group_name);
90 throw TrajectoryGeneratorInvalidLimitsException(
"deceleration limit not set for group " + group_name);
94 RCLCPP_INFO(getLogger(),
"Initialized Point-to-Point Trajectory Generator.");
97void TrajectoryGeneratorPTP::planPTP(
const std::map<std::string, double>& start_pos,
98 const std::map<std::string, double>& goal_pos,
99 trajectory_msgs::msg::JointTrajectory& joint_trajectory,
100 double velocity_scaling_factor,
double acceleration_scaling_factor,
101 double sampling_time)
104 for (
const auto& item : goal_pos)
106 joint_trajectory.joint_names.push_back(item.first);
110 bool goal_reached =
true;
111 for (
const auto& goal : goal_pos)
113 if (fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT)
115 goal_reached =
false;
121 RCLCPP_INFO_STREAM(
getLogger(),
"Goal already reached, set one goal point explicitly.");
122 if (joint_trajectory.points.empty())
124 trajectory_msgs::msg::JointTrajectoryPoint point;
125 point.time_from_start = rclcpp::Duration::from_seconds(sampling_time);
126 for (
const std::string& joint_name : joint_trajectory.joint_names)
128 point.positions.push_back(start_pos.at(joint_name));
129 point.velocities.push_back(0);
130 point.accelerations.push_back(0);
132 joint_trajectory.points.push_back(point);
138 std::string leading_axis = joint_trajectory.joint_names.front();
139 double max_duration = -1.0;
141 std::map<std::string, VelocityProfileATrap> velocity_profile;
142 for (
const auto& joint_name : joint_trajectory.joint_names)
145 velocity_profile.insert(std::make_pair(
146 joint_name, VelocityProfileATrap(velocity_scaling_factor * most_strict_limit_.
max_velocity,
150 velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name));
151 if (velocity_profile.at(joint_name).Duration() > max_duration)
153 max_duration = velocity_profile.at(joint_name).Duration();
154 leading_axis = joint_name;
162 double acc_time = velocity_profile.at(leading_axis).firstPhaseDuration();
163 double const_time = velocity_profile.at(leading_axis).secondPhaseDuration();
164 double dec_time = velocity_profile.at(leading_axis).thirdPhaseDuration();
166 for (
const auto& joint_name : joint_trajectory.joint_names)
168 if (joint_name != leading_axis)
174 if (!velocity_profile.at(joint_name)
175 .setProfileAllDurations(start_pos.at(joint_name), goal_pos.at(joint_name), acc_time, const_time,
179 std::stringstream error_str;
180 error_str <<
"TrajectoryGeneratorPTP::planPTP(): Can not synchronize "
181 "velocity profile of axis "
182 << joint_name <<
" with leading axis " << leading_axis;
183 throw PtpVelocityProfileSyncFailed(error_str.str());
190 std::vector<double> time_samples;
191 for (
double t_sample = 0.0; t_sample < max_duration; t_sample += sampling_time)
193 time_samples.push_back(t_sample);
196 time_samples.push_back(max_duration);
199 for (
double time_stamp : time_samples)
201 trajectory_msgs::msg::JointTrajectoryPoint point;
202 point.time_from_start = rclcpp::Duration::from_seconds(time_stamp);
203 for (std::string& joint_name : joint_trajectory.joint_names)
205 point.positions.push_back(velocity_profile.at(joint_name).Pos(time_stamp));
206 point.velocities.push_back(velocity_profile.at(joint_name).Vel(time_stamp));
207 point.accelerations.push_back(velocity_profile.at(joint_name).Acc(time_stamp));
209 joint_trajectory.points.push_back(point);
213 std::fill(joint_trajectory.points.back().velocities.begin(), joint_trajectory.points.back().velocities.end(), 0.0);
214 std::fill(joint_trajectory.points.back().accelerations.begin(), joint_trajectory.points.back().accelerations.end(),
218void TrajectoryGeneratorPTP::extractMotionPlanInfo(
const planning_scene::PlanningSceneConstPtr& scene,
220 MotionPlanInfo& info)
const
222 info.group_name = req.group_name;
225 info.start_joint_position.clear();
226 for (std::size_t i = 0; i < req.start_state.joint_state.name.size(); ++i)
228 info.start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i];
232 info.goal_joint_position.clear();
233 if (!req.goal_constraints.at(0).joint_constraints.empty())
235 for (
const auto& joint_constraint : req.goal_constraints.at(0).joint_constraints)
237 info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position;
244 if (!
computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
245 goal_pose,
robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
247 throw PtpNoIkSolutionForGoalPose(
"No IK solution for goal pose");
252void TrajectoryGeneratorPTP::plan(
const planning_scene::PlanningSceneConstPtr& ,
254 double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
257 planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory,
258 req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time);
JointLimit getCommonLimit() const
Returns joint limit fusion of all(position, velocity, acceleration, deceleration) limits for all join...
This class combines CartesianLimit and JointLimits into on single class.
bool hasJointLimits() const
Return if this LimitsContainer has defined joint limits.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr &robot_model, const pilz_industrial_motion_planner::LimitsContainer &planner_limits, const std::string &group_name)
Constructor of PTP Trajectory Generator.
Base class of trajectory generators.
const moveit::core::RobotModelConstPtr robot_model_
const pilz_industrial_motion_planner::LimitsContainer planner_limits_
rclcpp::Logger getLogger()
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
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
bool has_acceleration_limits
bool has_deceleration_limits
double max_deceleration
maximum deceleration MUST(!) be negative
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.