38 #include <rclcpp/duration.hpp>
39 #include <rclcpp/logger.hpp>
40 #include <rclcpp/logging.hpp>
44 #include <tf2_eigen/tf2_eigen.hpp>
45 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
49 static const rclcpp::Logger LOGGER =
50 rclcpp::get_logger(
"moveit.pilz_industrial_motion_planner.trajectory_generator_ptp");
57 throw TrajectoryGeneratorInvalidLimitsException(
"joint limit not set");
63 const auto* jmg = robot_model->getJointModelGroup(group_name);
65 throw TrajectoryGeneratorInvalidLimitsException(
"invalid group: " + group_name);
67 const auto& active_joints = jmg->getActiveJointModelNames();
70 if (!active_joints.empty())
76 throw TrajectoryGeneratorInvalidLimitsException(
"velocity limit not set for group " + group_name);
80 throw TrajectoryGeneratorInvalidLimitsException(
"acceleration limit not set for group " + group_name);
84 throw TrajectoryGeneratorInvalidLimitsException(
"deceleration limit not set for group " + group_name);
88 RCLCPP_INFO(LOGGER,
"Initialized Point-to-Point Trajectory Generator.");
91 void TrajectoryGeneratorPTP::planPTP(
const std::map<std::string, double>& start_pos,
92 const std::map<std::string, double>& goal_pos,
93 trajectory_msgs::msg::JointTrajectory& joint_trajectory,
94 const double& velocity_scaling_factor,
const double& acceleration_scaling_factor,
95 const double& sampling_time)
98 for (
const auto& item : goal_pos)
100 joint_trajectory.joint_names.push_back(item.first);
104 bool goal_reached =
true;
105 for (
auto const& goal : goal_pos)
107 if (fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT)
109 goal_reached =
false;
115 RCLCPP_INFO_STREAM(LOGGER,
"Goal already reached, set one goal point explicitly.");
116 if (joint_trajectory.points.empty())
118 trajectory_msgs::msg::JointTrajectoryPoint point;
119 point.time_from_start = rclcpp::Duration::from_seconds(sampling_time);
120 for (
const std::string& joint_name : joint_trajectory.joint_names)
122 point.positions.push_back(start_pos.at(joint_name));
123 point.velocities.push_back(0);
124 point.accelerations.push_back(0);
126 joint_trajectory.points.push_back(point);
132 std::string leading_axis = joint_trajectory.joint_names.front();
133 double max_duration = -1.0;
135 std::map<std::string, VelocityProfileATrap> velocity_profile;
136 for (
const auto& joint_name : joint_trajectory.joint_names)
139 velocity_profile.insert(std::make_pair(
140 joint_name, VelocityProfileATrap(velocity_scaling_factor * most_strict_limit_.
max_velocity,
144 velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name));
145 if (velocity_profile.at(joint_name).Duration() > max_duration)
147 max_duration = velocity_profile.at(joint_name).Duration();
148 leading_axis = joint_name;
156 double acc_time = velocity_profile.at(leading_axis).firstPhaseDuration();
157 double const_time = velocity_profile.at(leading_axis).secondPhaseDuration();
158 double dec_time = velocity_profile.at(leading_axis).thirdPhaseDuration();
160 for (
const auto& joint_name : joint_trajectory.joint_names)
162 if (joint_name != leading_axis)
168 if (!velocity_profile.at(joint_name)
169 .setProfileAllDurations(start_pos.at(joint_name), goal_pos.at(joint_name), acc_time, const_time,
173 std::stringstream error_str;
174 error_str <<
"TrajectoryGeneratorPTP::planPTP(): Can not synchronize "
175 "velocity profile of axis "
176 << joint_name <<
" with leading axis " << leading_axis;
177 throw PtpVelocityProfileSyncFailed(error_str.str());
184 std::vector<double> time_samples;
185 for (
double t_sample = 0.0; t_sample < max_duration; t_sample += sampling_time)
187 time_samples.push_back(t_sample);
190 time_samples.push_back(max_duration);
193 for (
double time_stamp : time_samples)
195 trajectory_msgs::msg::JointTrajectoryPoint point;
196 point.time_from_start = rclcpp::Duration::from_seconds(time_stamp);
197 for (std::string& joint_name : joint_trajectory.joint_names)
199 point.positions.push_back(velocity_profile.at(joint_name).Pos(time_stamp));
200 point.velocities.push_back(velocity_profile.at(joint_name).Vel(time_stamp));
201 point.accelerations.push_back(velocity_profile.at(joint_name).Acc(time_stamp));
203 joint_trajectory.points.push_back(point);
207 std::fill(joint_trajectory.points.back().velocities.begin(), joint_trajectory.points.back().velocities.end(), 0.0);
208 std::fill(joint_trajectory.points.back().accelerations.begin(), joint_trajectory.points.back().accelerations.end(),
212 void TrajectoryGeneratorPTP::extractMotionPlanInfo(
const planning_scene::PlanningSceneConstPtr&
scene,
214 MotionPlanInfo& info)
const
216 info.group_name = req.group_name;
219 info.start_joint_position.clear();
220 for (std::size_t i = 0; i < req.start_state.joint_state.name.size(); ++i)
222 info.start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i];
226 info.goal_joint_position.clear();
227 if (!req.goal_constraints.at(0).joint_constraints.empty())
229 for (
const auto& joint_constraint : req.goal_constraints.at(0).joint_constraints)
231 info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position;
238 if (!
computePoseIK(
scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
239 goal_pose,
robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
241 throw PtpNoIkSolutionForGoalPose(
"No IK solution for goal pose");
246 void TrajectoryGeneratorPTP::plan(
const planning_scene::PlanningSceneConstPtr& ,
248 const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
251 planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory,
252 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.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
bool hasJointLimits() const
Return if this LimitsContainer has defined joint limits.
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_
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.