40#include <tf2_eigen/tf2_eigen.hpp>
48 return moveit::getLogger(
"moveit.planners.pilz.trajectory_blender_transition_window");
57 RCLCPP_INFO(getLogger(),
"Start trajectory blending using transition window.");
59 double sampling_time = 0.;
60 if (!validateRequest(req, sampling_time, res.
error_code))
62 RCLCPP_ERROR(getLogger(),
"Trajectory blend request is not valid.");
69 std::size_t first_intersection_index;
70 std::size_t second_intersection_index;
71 if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index))
73 RCLCPP_ERROR(getLogger(),
"Blend radius too large.");
74 res.
error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
80 std::size_t blend_align_index;
81 determineTrajectoryAlignment(req, first_intersection_index, second_intersection_index, blend_align_index);
85 blendTrajectoryCartesian(req, first_intersection_index, second_intersection_index, blend_align_index, sampling_time,
86 blend_trajectory_cartesian);
89 std::map<std::string, double> initial_joint_position, initial_joint_velocity;
90 for (
const std::string& joint_name :
93 initial_joint_position[joint_name] =
94 req.
first_trajectory->getWayPoint(first_intersection_index - 1).getVariablePosition(joint_name);
95 initial_joint_velocity[joint_name] =
96 req.
first_trajectory->getWayPoint(first_intersection_index - 1).getVariableVelocity(joint_name);
98 trajectory_msgs::msg::JointTrajectory blend_joint_trajectory;
99 moveit_msgs::msg::MoveItErrorCodes error_code;
103 blend_joint_trajectory, error_code,
true))
106 RCLCPP_INFO(getLogger(),
"Failed to generate joint trajectory for blending trajectory.");
122 for (
size_t i = 0; i < first_intersection_index; ++i)
132 for (
size_t i = second_intersection_index + 1; i < req.
second_trajectory->getWayPointCount(); ++i)
141 res.
error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
145bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validateRequest(
147 moveit_msgs::msg::MoveItErrorCodes& error_code)
const
149 RCLCPP_DEBUG(getLogger(),
"Validate the trajectory blend request.");
154 RCLCPP_ERROR_STREAM(getLogger(),
"Unknown planning group: " << req.
group_name);
155 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME;
163 RCLCPP_ERROR_STREAM(getLogger(),
"Unknown link name: " << req.
link_name);
164 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME;
170 RCLCPP_ERROR(
getLogger(),
"Blending radius must be positive");
171 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
180 RCLCPP_ERROR_STREAM(
getLogger(),
"During blending the last point of the preceding and the first point of the "
181 "succeeding trajectory");
182 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
190 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
202 RCLCPP_ERROR(
getLogger(),
"Intersection point of the blending trajectories has non-zero velocities/accelerations.");
203 error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
210void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTrajectoryCartesian(
212 const std::size_t second_interse_index,
const std::size_t blend_align_index,
double sampling_time,
220 Eigen::Isometry3d blend_sample_pose1 =
224 Eigen::Isometry3d blend_sample_pose2 =
228 double blend_sample_num = second_interse_index + blend_align_index - first_interse_index + 1;
233 Eigen::Isometry3d blend_sample_pose;
234 for (std::size_t i = 0; i < blend_sample_num; ++i)
243 if ((first_interse_index + i) > blend_align_index)
245 blend_sample_pose2 = req.
second_trajectory->getWayPoint(first_interse_index + i - blend_align_index)
249 double s = (i + 1) / blend_sample_num;
250 double alpha = 6 * std::pow(s, 5) - 15 * std::pow(s, 4) + 10 * std::pow(s, 3);
253 blend_sample_pose.translation() = blend_sample_pose1.translation() +
254 alpha * (blend_sample_pose2.translation() - blend_sample_pose1.translation());
257 Eigen::Quaterniond start_quat(blend_sample_pose1.rotation());
258 Eigen::Quaterniond end_quat(blend_sample_pose2.rotation());
259 blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix();
262 waypoint.
pose = tf2::toMsg(blend_sample_pose);
263 waypoint.
time_from_start = rclcpp::Duration::from_seconds((i + 1.0) * sampling_time);
264 trajectory.
points.push_back(waypoint);
268bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIntersectionPoints(
270 std::size_t& second_interse_index)
const
272 RCLCPP_INFO(
getLogger(),
"Search for start and end point of blending trajectory.");
280 true, first_interse_index))
282 RCLCPP_ERROR_STREAM(
getLogger(),
"Intersection point of first trajectory not found.");
285 RCLCPP_INFO_STREAM(
getLogger(),
"Intersection point of first trajectory found, index: " << first_interse_index);
288 false, second_interse_index))
290 RCLCPP_ERROR_STREAM(
getLogger(),
"Intersection point of second trajectory not found.");
294 RCLCPP_INFO_STREAM(
getLogger(),
"Intersection point of second trajectory found, index: " << second_interse_index);
298void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::determineTrajectoryAlignment(
300 std::size_t second_interse_index, std::size_t& blend_align_index)
const
302 size_t way_point_count_1 = req.
first_trajectory->getWayPointCount() - first_interse_index;
303 size_t way_point_count_2 = second_interse_index + 1;
305 if (way_point_count_1 > way_point_count_2)
307 blend_align_index = req.
first_trajectory->getWayPointCount() - second_interse_index - 1;
311 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_
rclcpp::Logger getLogger()
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
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 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, 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 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.
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