From 8a9702b97109d6b9e95d2ef0eac6f6d75b075fda Mon Sep 17 00:00:00 2001 From: Sangtaek Lee Date: Mon, 2 Mar 2026 15:59:22 +0000 Subject: [PATCH 1/5] rescale trajectory when vel/acc violated --- .../trajectory_functions.hpp | 2 +- .../src/trajectory_functions.cpp | 57 +++++++++++++++---- 2 files changed, 46 insertions(+), 13 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp index b76ff8a705..a8e34878b6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp @@ -107,7 +107,7 @@ bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& lin * @param joint_limits: joint limits * @return */ -bool verifySampleJointLimits(const std::map& position_last, +double verifySampleJointLimits(const std::map& position_last, const std::map& velocity_last, const std::map& position_current, double duration_last, double duration_current, const JointLimitsContainer& joint_limits); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 01a7f11b59..272d5d5414 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -142,7 +142,7 @@ bool pilz_industrial_motion_planner::computeLinkFK(moveit::core::RobotState& rob return true; } -bool pilz_industrial_motion_planner::verifySampleJointLimits( +double pilz_industrial_motion_planner::verifySampleJointLimits( const std::map& position_last, const std::map& velocity_last, const std::map& position_current, double duration_last, double duration_current, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits) @@ -151,11 +151,13 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( if (duration_current <= epsilon) { RCLCPP_ERROR(getLogger(), "Sample duration too small, cannot compute the velocity"); - return false; + return -1.0; } double velocity_current, acceleration_current; + double max_scaling_factor = 0.0; + for (const auto& pos : position_current) { velocity_current = (pos.second - position_last.at(pos.first)) / duration_current; @@ -167,8 +169,8 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( << " Actual joint velocity is " << velocity_current << ", while the limit is " << joint_limits.getLimit(pos.first).max_velocity << ". "); - return false; } + max_scaling_factor = std::max(max_scaling_factor, std::fabs(velocity_current / joint_limits.getLimit(pos.first).max_velocity)); acceleration_current = (velocity_current - velocity_last.at(pos.first)) / (duration_last + duration_current) * 2; // acceleration case @@ -181,7 +183,6 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( << " Actual joint acceleration is " << acceleration_current << ", while the limit is " << joint_limits.getLimit(pos.first).max_acceleration << ". "); - return false; } } // deceleration case @@ -194,12 +195,13 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( << " Actual joint deceleration is " << acceleration_current << ", while the limit is " << joint_limits.getLimit(pos.first).max_deceleration << ". "); - return false; } } + max_scaling_factor = std::max(max_scaling_factor, std::fabs(acceleration_current / joint_limits.getLimit(pos.first).max_acceleration)); + max_scaling_factor = std::max(max_scaling_factor, std::fabs(acceleration_current / joint_limits.getLimit(pos.first).max_deceleration)); } - return true; + return max_scaling_factor; } bool pilz_industrial_motion_planner::generateJointTrajectory( @@ -234,6 +236,8 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( joint_velocity_last[item.first] = 0.0; } + double max_scaling_factor = 1.0; + for (std::vector::const_iterator time_iter = time_samples.begin(); time_iter != time_samples.end(); ++time_iter) { @@ -261,18 +265,21 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( } // skip the first sample with zero time from start for limits checking - if (time_iter != time_samples.begin() && - !verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, sampling_time, - duration_current_sample, joint_limits)) + auto max_scaling_factor_sample = verifySampleJointLimits(ik_solution_last, joint_velocity_last, ik_solution, + sampling_time, duration_current_sample, joint_limits); + if (time_iter != time_samples.begin() && max_scaling_factor_sample < 0.0) { - RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution at " - << *time_iter - << "s violates the joint velocity/acceleration/deceleration limits."); + RCLCPP_ERROR_STREAM(getLogger(), "Can't verify the joint limits for the sample at " << *time_iter); error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; joint_trajectory.points.clear(); return false; } + RCLCPP_INFO_STREAM(getLogger(), "The sample at " << *time_iter << " has a scaling factor of " + << max_scaling_factor_sample << "."); + + max_scaling_factor = std::max(max_scaling_factor, max_scaling_factor_sample); + // fill the point with joint values trajectory_msgs::msg::JointTrajectoryPoint point; @@ -310,6 +317,30 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( ik_solution_last = ik_solution; } + if (max_scaling_factor > 1.5) + { + RCLCPP_ERROR_STREAM(getLogger(), "Velocity/acceleration limits are severely violated. The trajectory is not safe to execute."); + error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; + joint_trajectory.points.clear(); + return false; + } + else if (max_scaling_factor > 1.0) + { + RCLCPP_WARN_STREAM(getLogger(), "Velocity/acceleration limits are violated. Re-scaling the trajectory with a factor of " << max_scaling_factor); + for (auto& point : joint_trajectory.points) + { + point.time_from_start = rclcpp::Duration::from_seconds((point.time_from_start.sec + (point.time_from_start.nanosec * 1e-9)) * max_scaling_factor); + for (auto& velocity : point.velocities) + { + velocity /= max_scaling_factor; + } + for (auto& acceleration : point.accelerations) + { + acceleration /= max_scaling_factor; + } + } + } + error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; double duration_ms = (clock.now() - generation_begin).seconds() * 1000; RCLCPP_DEBUG_STREAM(getLogger(), "Generate trajectory (N-Points: " @@ -376,6 +407,8 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( // overload generateJointTrajectory(..., // KDL::Trajectory, ...) // TODO: refactor to avoid code duplication. + // TODO(): Re-scale velocity and acceleration instead of failing. + // It seems this overload is not used at all, so it is not critical to fix this in the current state. RCLCPP_ERROR_STREAM(getLogger(), "Inverse kinematics solution of the " << i << "th sample violates the joint " From a68b456b8e3d031d397552a2184500f4bd00e9f2 Mon Sep 17 00:00:00 2001 From: Sangtaek Lee Date: Mon, 2 Mar 2026 16:43:16 +0000 Subject: [PATCH 2/5] fix duration calculation --- .../pilz_industrial_motion_planner/src/trajectory_functions.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 272d5d5414..ac19fc02eb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -329,7 +329,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( RCLCPP_WARN_STREAM(getLogger(), "Velocity/acceleration limits are violated. Re-scaling the trajectory with a factor of " << max_scaling_factor); for (auto& point : joint_trajectory.points) { - point.time_from_start = rclcpp::Duration::from_seconds((point.time_from_start.sec + (point.time_from_start.nanosec * 1e-9)) * max_scaling_factor); + point.time_from_start = rclcpp::Duration::from_seconds(rclcpp::Duration(point.time_from_start).seconds() * max_scaling_factor); for (auto& velocity : point.velocities) { velocity /= max_scaling_factor; From f18885acceeb313f3f625b94a931b28854271dd1 Mon Sep 17 00:00:00 2001 From: Sangtaek Lee Date: Mon, 2 Mar 2026 17:09:12 +0000 Subject: [PATCH 3/5] fix acc scaling --- .../pilz_industrial_motion_planner/src/trajectory_functions.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index ac19fc02eb..c0e301508a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -336,7 +336,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( } for (auto& acceleration : point.accelerations) { - acceleration /= max_scaling_factor; + acceleration /= (max_scaling_factor * max_scaling_factor); } } } From b4ca0243d91283402880debf249506655636d89c Mon Sep 17 00:00:00 2001 From: Sangtaek Lee Date: Mon, 2 Mar 2026 17:12:11 +0000 Subject: [PATCH 4/5] fix acc scaling --- .../src/trajectory_functions.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index c0e301508a..3c2077fd0e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -197,8 +197,8 @@ double pilz_industrial_motion_planner::verifySampleJointLimits( << joint_limits.getLimit(pos.first).max_deceleration << ". "); } } - max_scaling_factor = std::max(max_scaling_factor, std::fabs(acceleration_current / joint_limits.getLimit(pos.first).max_acceleration)); - max_scaling_factor = std::max(max_scaling_factor, std::fabs(acceleration_current / joint_limits.getLimit(pos.first).max_deceleration)); + max_scaling_factor = std::max(max_scaling_factor, std::sqrt(std::fabs(acceleration_current / joint_limits.getLimit(pos.first).max_acceleration))); + max_scaling_factor = std::max(max_scaling_factor, std::sqrt(std::fabs(acceleration_current / joint_limits.getLimit(pos.first).max_deceleration))); } return max_scaling_factor; From c5ba0da694b2aa4fe593b3074454aca1e3d2f9ae Mon Sep 17 00:00:00 2001 From: Sangtaek Lee Date: Fri, 6 Mar 2026 08:33:00 +0000 Subject: [PATCH 5/5] change log level --- .../src/trajectory_functions.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 3c2077fd0e..0cf1b538b3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -164,7 +164,7 @@ double pilz_industrial_motion_planner::verifySampleJointLimits( if (!joint_limits.verifyVelocityLimit(pos.first, velocity_current)) { - RCLCPP_ERROR_STREAM(getLogger(), "Joint velocity limit of " + RCLCPP_DEBUG_STREAM(getLogger(), "Joint velocity limit of " << pos.first << " violated. Set the velocity scaling factor lower!" << " Actual joint velocity is " << velocity_current << ", while the limit is " << joint_limits.getLimit(pos.first).max_velocity @@ -178,7 +178,7 @@ double pilz_industrial_motion_planner::verifySampleJointLimits( { if (!joint_limits.verifyAccelerationLimit(pos.first, acceleration_current)) { - RCLCPP_ERROR_STREAM(getLogger(), "Joint acceleration limit of " + RCLCPP_DEBUG_STREAM(getLogger(), "Joint acceleration limit of " << pos.first << " violated. Set the acceleration scaling factor lower!" << " Actual joint acceleration is " << acceleration_current << ", while the limit is " @@ -190,7 +190,7 @@ double pilz_industrial_motion_planner::verifySampleJointLimits( { if (!joint_limits.verifyDecelerationLimit(pos.first, acceleration_current)) { - RCLCPP_ERROR_STREAM(getLogger(), "Joint deceleration limit of " + RCLCPP_DEBUG_STREAM(getLogger(), "Joint deceleration limit of " << pos.first << " violated. Set the acceleration scaling factor lower!" << " Actual joint deceleration is " << acceleration_current << ", while the limit is "