40 #include <tf2_eigen/tf2_eigen.hpp>
45 static const rclcpp::Logger
LOGGER =
46 rclcpp::get_logger(
"moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window");
54 RCLCPP_INFO(LOGGER,
"Start trajectory blending using transition window.");
56 double sampling_time = 0.;
57 if (!validateRequest(req, sampling_time, res.
error_code))
59 RCLCPP_ERROR(LOGGER,
"Trajectory blend request is not valid.");
66 std::size_t first_intersection_index;
67 std::size_t second_intersection_index;
68 if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index))
70 RCLCPP_ERROR(LOGGER,
"Blend radius to large.");
71 res.
error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
77 std::size_t blend_align_index;
78 determineTrajectoryAlignment(req, first_intersection_index, second_intersection_index, blend_align_index);
82 blendTrajectoryCartesian(req, first_intersection_index, second_intersection_index, blend_align_index, sampling_time,
83 blend_trajectory_cartesian);
86 std::map<std::string, double> initial_joint_position, initial_joint_velocity;
87 for (
const std::string& joint_name :
90 initial_joint_position[joint_name] =
91 req.
first_trajectory->getWayPoint(first_intersection_index - 1).getVariablePosition(joint_name);
92 initial_joint_velocity[joint_name] =
93 req.
first_trajectory->getWayPoint(first_intersection_index - 1).getVariableVelocity(joint_name);
95 trajectory_msgs::msg::JointTrajectory blend_joint_trajectory;
96 moveit_msgs::msg::MoveItErrorCodes error_code;
100 blend_joint_trajectory, error_code,
true))
103 RCLCPP_INFO(LOGGER,
"Failed to generate joint trajectory for blending trajectory.");
119 for (
size_t i = 0; i < first_intersection_index; ++i)
128 for (
size_t i = second_intersection_index + 1; i < req.
second_trajectory->getWayPointCount(); ++i)
137 res.
error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
141 bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validateRequest(
143 moveit_msgs::msg::MoveItErrorCodes& error_code)
const
145 RCLCPP_DEBUG(LOGGER,
"Validate the trajectory blend request.");
150 RCLCPP_ERROR_STREAM(LOGGER,
"Unknown planning group: " << req.
group_name);
151 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME;
159 RCLCPP_ERROR_STREAM(LOGGER,
"Unknown link name: " << req.
link_name);
160 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME;
166 RCLCPP_ERROR(LOGGER,
"Blending radius must be positive");
167 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
177 LOGGER,
"During blending the last point of the preceding and the first point of the succeeding trajectory");
178 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
186 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
198 RCLCPP_ERROR(LOGGER,
"Intersection point of the blending trajectories has non-zero velocities/accelerations.");
199 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
206 void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTrajectoryCartesian(
208 const std::size_t second_interse_index,
const std::size_t blend_align_index,
double sampling_time,
216 Eigen::Isometry3d blend_sample_pose1 =
220 Eigen::Isometry3d blend_sample_pose2 =
224 double blend_sample_num = second_interse_index + blend_align_index - first_interse_index + 1;
229 Eigen::Isometry3d blend_sample_pose;
230 for (std::size_t i = 0; i < blend_sample_num; ++i)
239 if ((first_interse_index + i) > blend_align_index)
241 blend_sample_pose2 = req.
second_trajectory->getWayPoint(first_interse_index + i - blend_align_index)
245 double s = (i + 1) / blend_sample_num;
246 double alpha = 6 * std::pow(s, 5) - 15 * std::pow(s, 4) + 10 * std::pow(s, 3);
249 blend_sample_pose.translation() = blend_sample_pose1.translation() +
250 alpha * (blend_sample_pose2.translation() - blend_sample_pose1.translation());
253 Eigen::Quaterniond start_quat(blend_sample_pose1.rotation());
254 Eigen::Quaterniond end_quat(blend_sample_pose2.rotation());
255 blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix();
258 waypoint.
pose = tf2::toMsg(blend_sample_pose);
259 waypoint.
time_from_start = rclcpp::Duration::from_seconds((i + 1.0) * sampling_time);
260 trajectory.
points.push_back(waypoint);
264 bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIntersectionPoints(
266 std::size_t& second_interse_index)
const
268 RCLCPP_INFO(LOGGER,
"Search for start and end point of blending trajectory.");
276 true, first_interse_index))
278 RCLCPP_ERROR_STREAM(LOGGER,
"Intersection point of first trajectory not found.");
281 RCLCPP_INFO_STREAM(LOGGER,
"Intersection point of first trajectory found, index: " << first_interse_index);
284 false, second_interse_index))
286 RCLCPP_ERROR_STREAM(LOGGER,
"Intersection point of second trajectory not found.");
290 RCLCPP_INFO_STREAM(LOGGER,
"Intersection point of second trajectory found, index: " << second_interse_index);
294 void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::determineTrajectoryAlignment(
296 std::size_t second_interse_index, std::size_t& blend_align_index)
const
298 size_t way_point_count_1 = req.
first_trajectory->getWayPointCount() - first_interse_index;
299 size_t way_point_count_2 = second_interse_index + 1;
301 if (way_point_count_1 > way_point_count_2)
303 blend_align_index = req.
first_trajectory->getWayPointCount() - second_interse_index - 1;
307 blend_align_index = first_interse_index;
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
bool blend(const planning_scene::PlanningSceneConstPtr &planning_scene, const pilz_industrial_motion_planner::TrajectoryBlendRequest &req, pilz_industrial_motion_planner::TrajectoryBlendResponse &res) override
Blend two trajectories using transition window. The trajectories have to be equally and uniformly dis...
const pilz_industrial_motion_planner::LimitsContainer limits_
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 linearSearchIntersectionPoint(const std::string &link_name, const Eigen::Vector3d ¢er_position, const double &r, const robot_trajectory::RobotTrajectoryPtr &traj, bool inverseOrder, std::size_t &index)
Performs a linear search for the intersection point of the trajectory with the blending radius.
bool isRobotStateStationary(const moveit::core::RobotState &state, const std::string &group, double EPSILON)
check if the robot state have zero velocity/acceleration
bool determineAndCheckSamplingTime(const robot_trajectory::RobotTrajectoryPtr &first_trajectory, const robot_trajectory::RobotTrajectoryPtr &second_trajectory, double EPSILON, double &sampling_time)
Determines the sampling time and checks that both trajectroies use the same sampling time.
bool isRobotStateEqual(const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const std::string &joint_group_name, double epsilon)
Check if the two robot states have the same joint position/velocity/acceleration.
This namespace includes the central class for representing planning contexts.
const rclcpp::Logger LOGGER
geometry_msgs::msg::Pose pose
rclcpp::Duration time_from_start
std::vector< CartesianTrajectoryPoint > points
robot_trajectory::RobotTrajectoryPtr second_trajectory
robot_trajectory::RobotTrajectoryPtr first_trajectory
moveit_msgs::msg::MoveItErrorCodes error_code
robot_trajectory::RobotTrajectoryPtr first_trajectory
robot_trajectory::RobotTrajectoryPtr blend_trajectory
robot_trajectory::RobotTrajectoryPtr second_trajectory