From c86e3b2a1a30148aa2ed12a579610708deff7f39 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Fri, 24 Oct 2025 22:12:11 +0300 Subject: [PATCH 01/27] Format max_velocity and max_acceleration for YAML output as float --- .../src/srdf_config.cpp | 32 +++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp index 69002a3ec7..066dc9e59f 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp @@ -300,7 +300,21 @@ bool SRDFConfig::GeneratedJointLimits::writeYaml(YAML::Emitter& emitter) // Output property emitter << YAML::Key << "max_velocity"; - emitter << YAML::Value << static_cast(std::min(fabs(b.max_velocity_), fabs(b.min_velocity_))); + + // To Solve Putting integer Values like 100 as 100.0 in YAML + double val = std::min(fabs(b.max_velocity_), fabs(b.min_velocity_)); + + // Check if val is a full integer (within floating-point tolerance) + bool is_integer = std::fabs(val - std::round(val)) < 1e-9; + + // Format value as string + std::ostringstream oss; + if (is_integer) + oss << std::fixed << std::setprecision(1) << val; // e.g. 100.0 + else + oss << val; // e.g. 99.75 + + emitter << YAML::Value << oss.str(); // Output property emitter << YAML::Key << "has_acceleration_limits"; @@ -315,7 +329,21 @@ bool SRDFConfig::GeneratedJointLimits::writeYaml(YAML::Emitter& emitter) // Output property emitter << YAML::Key << "max_acceleration"; - emitter << YAML::Value << static_cast(std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_))); + + // To Solve Putting integer Values like 100 as 100.0 in YAML + val = std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_)); + + // Check if val is a full integer (within floating-point tolerance) + is_integer = std::fabs(val - std::round(val)) < 1e-9; + + // Format value as string + oss.str(""); // Clear the stringstream + if (is_integer) + oss << std::fixed << std::setprecision(1) << val; // e.g. 100.0 + else + oss << val; // e.g. 99.75 + + emitter << YAML::Value << oss.str(); emitter << YAML::EndMap; } From 38aac46e7c295ee21dc97083324921bed08511df Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Wed, 5 Nov 2025 15:02:20 +0300 Subject: [PATCH 02/27] make use of max_cartesian_speed in MotionSequenceRequest --- .../trajectory_generator.hpp | 1 + .../src/trajectory_generator.cpp | 2 +- .../src/trajectory_generator_lin.cpp | 14 ++++++++++++-- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp index c17bcc8be3..c838e625dd 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp @@ -270,6 +270,7 @@ class TrajectoryGenerator protected: const moveit::core::RobotModelConstPtr robot_model_; const pilz_industrial_motion_planner::LimitsContainer planner_limits_; + double max_cartesian_speed_; static constexpr double MIN_SCALING_FACTOR{ 0.0001 }; static constexpr double MAX_SCALING_FACTOR{ 1. }; static constexpr double VELOCITY_TOLERANCE{ 1e-8 }; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 6f17fbc3a2..1e6480b2f3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -283,7 +283,7 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(double max_velocity_scaling_fa const std::unique_ptr& path) const { std::unique_ptr vp_trans = std::make_unique( - max_velocity_scaling_factor * planner_limits_.getCartesianLimits().max_trans_vel, + max_velocity_scaling_factor * max_cartesian_speed_, max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().max_trans_acc); if (path->PathLength() > std::numeric_limits::epsilon()) // avoid division by zero diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 77ed726c0e..75f4f0437d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -149,6 +149,17 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s // create Cartesian path for lin std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); + // set pilz cartesian limits for each item + if (req.max_cartesian_speed > 0.0 && req.cartesian_speed_limited_link != "") + { + max_cartesian_speed_ = req.max_cartesian_speed; + RCLCPP_INFO(getLogger(), "I get into hell with: %f", max_cartesian_speed_); + } + else + { + max_cartesian_speed_ = planner_limits_.getCartesianLimits().max_trans_vel; + RCLCPP_INFO(getLogger(), "I get into hell with: %f", max_cartesian_speed_); + } // create velocity profile std::unique_ptr vp( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); @@ -180,8 +191,7 @@ std::unique_ptr TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affin KDL::Frame kdl_start_pose, kdl_goal_pose; tf2::transformEigenToKDL(start_pose, kdl_start_pose); tf2::transformEigenToKDL(goal_pose, kdl_goal_pose); - double eqradius = - planner_limits_.getCartesianLimits().max_trans_vel / planner_limits_.getCartesianLimits().max_rot_vel; + double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel; KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); return std::unique_ptr( From 7d70a27ae9f3033450068558f445d7c9378e5e8c Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Wed, 5 Nov 2025 15:10:15 +0300 Subject: [PATCH 03/27] use feature in circ command --- .../src/trajectory_generator_circ.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 388542daf3..4a8dee318c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -203,6 +203,17 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { std::unique_ptr cart_path(setPathCIRC(plan_info)); + // set pilz cartesian limits for each item + if (req.max_cartesian_speed > 0.0 && req.cartesian_speed_limited_link != "") + { + max_cartesian_speed_ = req.max_cartesian_speed; + RCLCPP_INFO(getLogger(), "I get into hell with: %f", max_cartesian_speed_); + } + else + { + max_cartesian_speed_ = planner_limits_.getCartesianLimits().max_trans_vel; + RCLCPP_INFO(getLogger(), "I get into hell with: %f", max_cartesian_speed_); + } std::unique_ptr vel_profile( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, cart_path)); @@ -241,8 +252,7 @@ std::unique_ptr TrajectoryGeneratorCIRC::setPathCIRC(const MotionPlan // The KDL::Path implementation chooses the motion with the longer duration // (translation vs. rotation) // and uses eqradius as scaling factor between the distances. - double eqradius = - planner_limits_.getCartesianLimits().max_trans_vel / planner_limits_.getCartesianLimits().max_rot_vel; + double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel; try { From 0bb71f20b637bc356a30d7db5d8d695d7d9e6da3 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Wed, 5 Nov 2025 21:40:15 +0300 Subject: [PATCH 04/27] organize into function --- .../trajectory_generator.hpp | 5 +++++ .../src/trajectory_generator.cpp | 14 ++++++++++++++ .../src/trajectory_generator_circ.cpp | 12 ++---------- .../src/trajectory_generator_lin.cpp | 11 +---------- 4 files changed, 22 insertions(+), 20 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp index c838e625dd..39c4c9c537 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp @@ -139,6 +139,11 @@ class TrajectoryGenerator std::unique_ptr cartesianTrapVelocityProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor, const std::unique_ptr& path) const; + /** + * @brief Set the max cartesian speed from motion request + */ + + void setMaxCartesianSpeed(const moveit_msgs::msg::MotionPlanRequest& req); private: virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 1e6480b2f3..317ab0a018 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -380,4 +380,18 @@ TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(const planning_scene::Planni } } +void TrajectoryGenerator::setMaxCartesianSpeed(const moveit_msgs::msg::MotionPlanRequest& req) +{ + if (req.max_cartesian_speed > 0.0 && req.cartesian_speed_limited_link != "") + { + max_cartesian_speed_ = req.max_cartesian_speed; + RCLCPP_INFO(getLogger(), "received max_cartesian_speed: %f", max_cartesian_speed_); + } + else + { + max_cartesian_speed_ = planner_limits_.getCartesianLimits().max_trans_vel; + RCLCPP_INFO(getLogger(), "using default max_cartesian_speed: %f", max_cartesian_speed_); + } +} + } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 4a8dee318c..2cf78219c1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -204,16 +204,8 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& { std::unique_ptr cart_path(setPathCIRC(plan_info)); // set pilz cartesian limits for each item - if (req.max_cartesian_speed > 0.0 && req.cartesian_speed_limited_link != "") - { - max_cartesian_speed_ = req.max_cartesian_speed; - RCLCPP_INFO(getLogger(), "I get into hell with: %f", max_cartesian_speed_); - } - else - { - max_cartesian_speed_ = planner_limits_.getCartesianLimits().max_trans_vel; - RCLCPP_INFO(getLogger(), "I get into hell with: %f", max_cartesian_speed_); - } + setMaxCartesianSpeed(req); + // create velocity profile std::unique_ptr vel_profile( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, cart_path)); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 75f4f0437d..ade93bd965 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -150,16 +150,7 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); // set pilz cartesian limits for each item - if (req.max_cartesian_speed > 0.0 && req.cartesian_speed_limited_link != "") - { - max_cartesian_speed_ = req.max_cartesian_speed; - RCLCPP_INFO(getLogger(), "I get into hell with: %f", max_cartesian_speed_); - } - else - { - max_cartesian_speed_ = planner_limits_.getCartesianLimits().max_trans_vel; - RCLCPP_INFO(getLogger(), "I get into hell with: %f", max_cartesian_speed_); - } + setMaxCartesianSpeed(req); // create velocity profile std::unique_ptr vp( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); From ce9099d415939bc1a424873557f65cc45810a06b Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Fri, 7 Nov 2025 23:45:55 +0300 Subject: [PATCH 05/27] add free path planner using composite kdl path --- .../CMakeLists.txt | 10 + .../planning_context_free.h | 49 ++++ .../planning_context_free.hpp | 67 ++++++ .../planning_context_loader_free.h | 49 ++++ .../planning_context_loader_free.hpp | 68 ++++++ .../trajectory_generator.hpp | 1 + .../trajectory_generator_free.hpp | 86 +++++++ .../src/planning_context_loader_free.cpp | 84 +++++++ .../src/trajectory_generator_free.cpp | 212 ++++++++++++++++++ 9 files changed, 626 insertions(+) create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index e8420aec32..e6d344ba92 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -127,6 +127,15 @@ ament_target_dependencies(planning_context_loader_circ target_link_libraries(planning_context_loader_circ planning_context_loader_base joint_limits_common trajectory_generation_common) +add_library( + planning_context_loader_free SHARED + src/planning_context_loader_free.cpp src/trajectory_generator_free.cpp + src/velocity_profile_atrap.cpp) +ament_target_dependencies(planning_context_loader_free + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(planning_context_loader_free planning_context_loader_base + joint_limits_common trajectory_generation_common) + add_library(sequence_capability SHARED src/move_group_sequence_action.cpp src/move_group_sequence_service.cpp) ament_target_dependencies(sequence_capability ${THIS_PACKAGE_INCLUDE_DEPENDS}) @@ -153,6 +162,7 @@ install( planning_context_loader_ptp planning_context_loader_lin planning_context_loader_circ + planning_context_loader_free command_list_manager sequence_capability trajectory_generation_common diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h new file mode 100644 index 0000000000..b5ef7e00ee --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h @@ -0,0 +1,49 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp new file mode 100644 index 0000000000..42566ea9d4 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +MOVEIT_CLASS_FORWARD(PlanningContext); + +/** + * @brief PlanningContext for obtaining LIN trajectories + */ +class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContextBase +{ +public: + PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + { + } +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h new file mode 100644 index 0000000000..04338d1e9e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h @@ -0,0 +1,49 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp new file mode 100644 index 0000000000..4c971ec417 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Plugin that can generate instances of PlanningContextLIN. + * Generates instances of PlanningContextLIN. + */ +class PlanningContextLoaderFree : public PlanningContextLoader +{ +public: + PlanningContextLoaderFree(); + ~PlanningContextLoaderFree() override; + + /** + * @brief return a instance of + * pilz_industrial_motion_planner::PlanningContextLIN + * @param planning_context returned context + * @param name + * @param group + * @return true on success, false otherwise + */ + bool loadContext(planning_interface::PlanningContextPtr& planning_context, const std::string& name, + const std::string& group) const override; +}; + +typedef std::shared_ptr PlanningContextLoaderFreePtr; +typedef std::shared_ptr PlanningContextLoaderFreeConstPtr; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp index 39c4c9c537..71db4f3ea1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.hpp @@ -122,6 +122,7 @@ class TrajectoryGenerator std::string link_name; Eigen::Isometry3d start_pose; Eigen::Isometry3d goal_pose; + std::vector waypoints; std::map start_joint_position; std::map goal_joint_position; std::pair circ_path_point; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp new file mode 100644 index 0000000000..ebec769627 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp @@ -0,0 +1,86 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include + +namespace pilz_industrial_motion_planner +{ +// TODO date type of units + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE); + +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); + +/** + * @brief This class implements a linear trajectory generator in Cartesian + * space. + * The Cartesian trajetory are based on trapezoid velocity profile. + */ +class TrajectoryGeneratorFree : public TrajectoryGenerator +{ +public: + /** + * @brief Constructor of Free Trajectory Generator + * @throw TrajectoryGeneratorInvalidLimitsException + * @param model: robot model + * @param planner_limits: limits in joint and Cartesian spaces + */ + TrajectoryGeneratorFree(const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); + +private: + void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; + + void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, + const MotionPlanInfo& plan_info, double sampling_time, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; + + /** + * @brief construct a KDL::Path object for a Cartesian straight line + * @return a unique pointer of the path object. null_ptr in case of an error. + */ + std::unique_ptr setPathFree(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& goal_pose) const; +}; + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp new file mode 100644 index 0000000000..a66a7ed97e --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include +#include + +#include + +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.planners.pilz.planning_context_loader.free"); +} +} // namespace + +pilz_industrial_motion_planner::PlanningContextLoaderFree::PlanningContextLoaderFree() +{ + alg_ = "Free"; +} + +pilz_industrial_motion_planner::PlanningContextLoaderFree::~PlanningContextLoaderFree() +{ +} + +bool pilz_industrial_motion_planner::PlanningContextLoaderFree::loadContext( + planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const +{ + if (limits_set_ && model_set_) + { + planning_context = std::make_shared(name, group, model_, limits_); + return true; + } + else + { + if (!limits_set_) + { + RCLCPP_ERROR_STREAM(getLogger(), + "Limits are not defined. Cannot load planning context. Call setLimits loadContext"); + } + if (!model_set_) + { + RCLCPP_ERROR_STREAM(getLogger(), "Robot model was not set"); + } + return false; + } +} + +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderLIN, + pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp new file mode 100644 index 0000000000..4da823f375 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -0,0 +1,212 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +// TODO: Remove conditional include when released to all active distros. +#if __has_include() +#include +#else +#include +#endif +#include +#include +#include +#include +#include +#include + +namespace pilz_industrial_motion_planner +{ +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.planners.pilz.trajectory_generator.lin"); +} +} // namespace +TrajectoryGeneratorFree::TrajectoryGeneratorFree(const moveit::core::RobotModelConstPtr& robot_model, + const LimitsContainer& planner_limits, + const std::string& /*group_name*/) + : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) +{ + planner_limits_.printCartesianLimits(); +} + +void TrajectoryGeneratorFree::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, + TrajectoryGenerator::MotionPlanInfo& info) const +{ + RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request."); + + info.group_name = req.group_name; + moveit::core::RobotState robot_state = scene->getCurrentState(); + + // goal given in joint space + if (!req.goal_constraints.front().joint_constraints.empty()) + { + info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name)); + + if (req.goal_constraints.front().joint_constraints.size() != + robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) + { + std::ostringstream os; + os << "Number of joints in goal does not match number of joints of group " + "(Number joints goal: " + << req.goal_constraints.front().joint_constraints.size() << " | Number of joints of group: " + << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size() << ')'; + throw JointNumberMismatch(os.str()); + } + + for (const auto& joint_item : req.goal_constraints.front().joint_constraints) + { + info.goal_joint_position[joint_item.joint_name] = joint_item.position; + } + + computeLinkFK(robot_state, info.link_name, info.goal_joint_position, info.goal_pose); + } + // goal given in Cartesian space + else + { + std::string frame_id; + + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) + { + RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + + // goal pose with optional offset wrt. the planning frame + for (const auto& pc : req.path_constraints.position_constraints) + { + Eigen::Isometry3d waypoint; + tf2::fromMsg(pc.constraint_region.primitive_poses.front(), waypoint); + waypoint = scene->getFrameTransform(frame_id) * waypoint; + info.waypoints.push_back(waypoint); + } + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw LinInverseForGoalIncalculable(os.str()); + } + } + + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); +} + +void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) +{ + // create Cartesian path for free + std::unique_ptr path(setPathFree(plan_info.start_pose, plan_info.waypoints)); + + // set pilz cartesian limits for each item + setMaxCartesianSpeed(req); + // create velocity profile + std::unique_ptr vp( + cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); + + // combine path and velocity profile into Cartesian trajectory + // with the third parameter set to false, KDL::Trajectory_Segment does not + // take + // the ownship of Path and Velocity Profile + KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(), false); + + moveit_msgs::msg::MoveItErrorCodes error_code; + // sample the Cartesian trajectory and compute joint trajectory using inverse + // kinematics + if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, + plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory, + error_code)) + { + std::ostringstream os; + os << "Failed to generate valid joint trajectory from the Cartesian path"; + throw LinTrajectoryConversionFailure(os.str(), error_code.val); + } +} + +std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Affine3d& start_pose, + const std::vector& waypoints) const +{ + RCLCPP_DEBUG(getLogger(), "Set Cartesian path for LIN command."); + + KDL::Frame kdl_start_pose; + tf2::transformEigenToKDL(start_pose, kdl_start_pose); + std::vector kdl_waypoints; + for (const auto& waypoint : waypoints) + { + KDL::Frame kdl_waypoint; + tf2::transformEigenToKDL(waypoint, kdl_waypoint); + kdl_waypoints.push_back(kdl_waypoint); + } + double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel; + KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); + KDL::Path_Composite* composite_path = new KDL::Path_Composite(); + KDL::Frame previous_pose = kdl_start_pose; + + for (const auto& kdl_waypoint : kdl_waypoints) + { + composite_path->Add(new KDL::Path_Line(previous_pose, kdl_waypoint, rot_interpo, eqradius, true)); + previous_pose = kdl_waypoint; + } + return std::unique_ptr(composite_path); +} + +} // namespace pilz_industrial_motion_planner From a0bc8e4745186c76c2e5635549dd9da4f996dc8b Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Sat, 8 Nov 2025 08:10:00 +0300 Subject: [PATCH 06/27] fix include files and add it to plugin --- .../planning_context_free.hpp | 10 +++++----- .../planning_context_loader_free.hpp | 4 ++-- .../trajectory_generator_free.hpp | 3 ++- .../plugins/planning_context_plugin_description.xml | 7 +++++++ .../src/planning_context_loader_free.cpp | 8 ++++---- .../src/trajectory_generator_free.cpp | 4 ++-- 6 files changed, 22 insertions(+), 14 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp index 42566ea9d4..eabcbe8904 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp @@ -52,14 +52,14 @@ namespace pilz_industrial_motion_planner MOVEIT_CLASS_FORWARD(PlanningContext); /** - * @brief PlanningContext for obtaining LIN trajectories + * @brief PlanningContext for obtaining Free trajectories */ -class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContextBase +class PlanningContextFree : public pilz_industrial_motion_planner::PlanningContextBase { public: - PlanningContextLIN(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, - const pilz_industrial_motion_planner::LimitsContainer& limits) - : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + PlanningContextFree(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) { } }; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp index 4c971ec417..25eea0fcbe 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp @@ -41,8 +41,8 @@ namespace pilz_industrial_motion_planner { /** - * @brief Plugin that can generate instances of PlanningContextLIN. - * Generates instances of PlanningContextLIN. + * @brief Plugin that can generate instances of PlanningContextFree. + * Generates instances of PlanningContextFree. */ class PlanningContextLoaderFree : public PlanningContextLoader { diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp index ebec769627..2cc421e5d1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp @@ -80,7 +80,8 @@ class TrajectoryGeneratorFree : public TrajectoryGenerator * @brief construct a KDL::Path object for a Cartesian straight line * @return a unique pointer of the path object. null_ptr in case of an error. */ - std::unique_ptr setPathFree(const Eigen::Affine3d& start_pose, const Eigen::Affine3d& goal_pose) const; + std::unique_ptr setPathFree(const Eigen::Affine3d& start_pose, + const std::vector& waypoints) const; }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml index 4c95fcb49e..6c8522a8dd 100644 --- a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml @@ -20,4 +20,11 @@ Loader for CIRC Context + + + Loader for Free Context + + diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp index a66a7ed97e..8c47fcbb34 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp @@ -32,10 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include -#include +#include #include #include @@ -50,7 +50,7 @@ rclcpp::Logger getLogger() pilz_industrial_motion_planner::PlanningContextLoaderFree::PlanningContextLoaderFree() { - alg_ = "Free"; + alg_ = "FREE"; } pilz_industrial_motion_planner::PlanningContextLoaderFree::~PlanningContextLoaderFree() @@ -80,5 +80,5 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderFree::loadContext( } } -PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderLIN, +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderFree, pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 4da823f375..7421d4be29 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include @@ -183,7 +183,7 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& } std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Affine3d& start_pose, - const std::vector& waypoints) const + const std::vector& waypoints) const { RCLCPP_DEBUG(getLogger(), "Set Cartesian path for LIN command."); From 4e62e818de37aeeeeee3acd7b2635c1574239196 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Sat, 8 Nov 2025 10:56:50 +0300 Subject: [PATCH 07/27] convert to roundedComposite due to derivatives discontinuouties normal composite fail to Pose IK --- .../src/trajectory_generator_free.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 7421d4be29..5cb244c62c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include // TODO: Remove conditional include when released to all active distros. @@ -130,8 +130,10 @@ void TrajectoryGeneratorFree::extractMotionPlanInfo(const planning_scene::Planni Eigen::Isometry3d waypoint; tf2::fromMsg(pc.constraint_region.primitive_poses.front(), waypoint); waypoint = scene->getFrameTransform(frame_id) * waypoint; + RCLCPP_INFO_STREAM(getLogger(), "Added waypoint at position: " << waypoint.translation().transpose()); info.waypoints.push_back(waypoint); } + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); frame_id = robot_model_->getModelFrame(); // check goal pose ik before Cartesian motion plan starts @@ -196,15 +198,17 @@ std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Aff tf2::transformEigenToKDL(waypoint, kdl_waypoint); kdl_waypoints.push_back(kdl_waypoint); } + + RCLCPP_INFO_STREAM(getLogger(), "Transformed waypoints number: " << kdl_waypoints.size()); double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel; KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); - KDL::Path_Composite* composite_path = new KDL::Path_Composite(); - KDL::Frame previous_pose = kdl_start_pose; - + KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(0.0005, eqradius, rot_interpo); + // make sure the start pose is the same as the first pose in waypoints with tolerance + if ((kdl_start_pose.p - kdl_waypoints.front().p).Norm() > 1e-3) + composite_path->Add(kdl_start_pose); for (const auto& kdl_waypoint : kdl_waypoints) { - composite_path->Add(new KDL::Path_Line(previous_pose, kdl_waypoint, rot_interpo, eqradius, true)); - previous_pose = kdl_waypoint; + composite_path->Add(kdl_waypoint); } return std::unique_ptr(composite_path); } From e9f63ae36c0ef6f1f830afef1041053449f35600 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Sat, 8 Nov 2025 13:15:49 +0300 Subject: [PATCH 08/27] fix bug andadd check of near start point to curve tip --- .../src/trajectory_generator_free.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 5cb244c62c..0a799faf2b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -156,11 +156,10 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { - // create Cartesian path for free - std::unique_ptr path(setPathFree(plan_info.start_pose, plan_info.waypoints)); - // set pilz cartesian limits for each item setMaxCartesianSpeed(req); + // create Cartesian path for free + std::unique_ptr path(setPathFree(plan_info.start_pose, plan_info.waypoints)); // create velocity profile std::unique_ptr vp( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); @@ -198,14 +197,23 @@ std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Aff tf2::transformEigenToKDL(waypoint, kdl_waypoint); kdl_waypoints.push_back(kdl_waypoint); } + double dist = (kdl_start_pose.p - kdl_waypoints.front().p).Norm(); + RCLCPP_INFO(getLogger(), "distance: %f", dist); RCLCPP_INFO_STREAM(getLogger(), "Transformed waypoints number: " << kdl_waypoints.size()); double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel; KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); - KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(0.0005, eqradius, rot_interpo); + KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(0.002, eqradius, rot_interpo); + RCLCPP_INFO_STREAM(getLogger(), "RoundedComposite with radius: " << 0.002); // make sure the start pose is the same as the first pose in waypoints with tolerance - if ((kdl_start_pose.p - kdl_waypoints.front().p).Norm() > 1e-3) + if (dist > 4e-3) + { composite_path->Add(kdl_start_pose); + } + else + RCLCPP_WARN_STREAM(getLogger(), "Close start point. start: " << kdl_start_pose.p + << "fisrt_waypoint: " << kdl_waypoints.front().p); + for (const auto& kdl_waypoint : kdl_waypoints) { composite_path->Add(kdl_waypoint); From 4008bdc313edc8853968aa8e4644044a5e6781a2 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Sat, 8 Nov 2025 13:18:59 +0300 Subject: [PATCH 09/27] fix bug in set max cart speed before set of path --- .../src/trajectory_generator_circ.cpp | 2 +- .../src/trajectory_generator_lin.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 2cf78219c1..744374132d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -202,9 +202,9 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { - std::unique_ptr cart_path(setPathCIRC(plan_info)); // set pilz cartesian limits for each item setMaxCartesianSpeed(req); + std::unique_ptr cart_path(setPathCIRC(plan_info)); // create velocity profile std::unique_ptr vel_profile( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, cart_path)); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index ade93bd965..c0c542fa76 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -146,11 +146,10 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { - // create Cartesian path for lin - std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); - // set pilz cartesian limits for each item setMaxCartesianSpeed(req); + // create Cartesian path for lin + std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); // create velocity profile std::unique_ptr vp( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); From 99f146bf352a0cbac517b4a51f6c97bce8e76dc4 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Sat, 8 Nov 2025 20:08:56 +0300 Subject: [PATCH 10/27] add check request validation --- .../trajectory_generator_free.hpp | 3 +++ .../src/trajectory_generator_free.cpp | 14 ++++++++++++-- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp index 2cc421e5d1..bb929bfaef 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp @@ -49,6 +49,7 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs:: CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoWaypointsSpecified, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); /** * @brief This class implements a linear trajectory generator in Cartesian @@ -69,6 +70,8 @@ class TrajectoryGeneratorFree : public TrajectoryGenerator const std::string& group_name); private: + void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; + void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 0a799faf2b..1c8d0870f0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -186,7 +186,7 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Affine3d& start_pose, const std::vector& waypoints) const { - RCLCPP_DEBUG(getLogger(), "Set Cartesian path for LIN command."); + RCLCPP_DEBUG(getLogger(), "Set Cartesian path for FREE command."); KDL::Frame kdl_start_pose; tf2::transformEigenToKDL(start_pose, kdl_start_pose); @@ -206,7 +206,7 @@ std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Aff KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(0.002, eqradius, rot_interpo); RCLCPP_INFO_STREAM(getLogger(), "RoundedComposite with radius: " << 0.002); // make sure the start pose is the same as the first pose in waypoints with tolerance - if (dist > 4e-3) + if (dist > 1.0e-3) { composite_path->Add(kdl_start_pose); } @@ -221,4 +221,14 @@ std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Aff return std::unique_ptr(composite_path); } +void TrajectoryGeneratorFree::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const +{ + if (req.path_constraints.position_constraints.size() < 2) + { + std::ostringstream os; + os << "waypoints specified in path constraints is less than 2 for FREE motion."; + throw NoWaypointsSpecified(os.str()); + } +} + } // namespace pilz_industrial_motion_planner From 24d27ce53dd5b033ee88c94b2d70c0c9633c6014 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Sun, 9 Nov 2025 14:43:40 +0300 Subject: [PATCH 11/27] add function to compute the max rounding circle radius --- .../trajectory_generator_free.hpp | 8 ++- .../src/trajectory_generator_free.cpp | 62 +++++++++++++++++-- 2 files changed, 65 insertions(+), 5 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp index bb929bfaef..061dbbf5fd 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp @@ -71,6 +71,11 @@ class TrajectoryGeneratorFree : public TrajectoryGenerator private: void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; + /** + * @brief set the max possible blend radius + * for rounded composite path with smoothness scaler + */ + double computeBlendRadius(const std::vector& waypoints_, double smoothness_) const; void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; @@ -84,7 +89,8 @@ class TrajectoryGeneratorFree : public TrajectoryGenerator * @return a unique pointer of the path object. null_ptr in case of an error. */ std::unique_ptr setPathFree(const Eigen::Affine3d& start_pose, - const std::vector& waypoints) const; + const std::vector& waypoints, + double smoothness_level) const; }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 1c8d0870f0..4ffe941722 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -159,7 +159,7 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& // set pilz cartesian limits for each item setMaxCartesianSpeed(req); // create Cartesian path for free - std::unique_ptr path(setPathFree(plan_info.start_pose, plan_info.waypoints)); + std::unique_ptr path(setPathFree(plan_info.start_pose, plan_info.waypoints, req.smoothness_level)); // create velocity profile std::unique_ptr vp( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); @@ -184,12 +184,14 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& } std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Affine3d& start_pose, - const std::vector& waypoints) const + const std::vector& waypoints, + double smoothness_level) const { RCLCPP_DEBUG(getLogger(), "Set Cartesian path for FREE command."); KDL::Frame kdl_start_pose; tf2::transformEigenToKDL(start_pose, kdl_start_pose); + // transform waypoints to KDL frames std::vector kdl_waypoints; for (const auto& waypoint : waypoints) { @@ -197,14 +199,17 @@ std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Aff tf2::transformEigenToKDL(waypoint, kdl_waypoint); kdl_waypoints.push_back(kdl_waypoint); } + // distance between start pose and first waypoint double dist = (kdl_start_pose.p - kdl_waypoints.front().p).Norm(); RCLCPP_INFO(getLogger(), "distance: %f", dist); RCLCPP_INFO_STREAM(getLogger(), "Transformed waypoints number: " << kdl_waypoints.size()); double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel; KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); - KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(0.002, eqradius, rot_interpo); - RCLCPP_INFO_STREAM(getLogger(), "RoundedComposite with radius: " << 0.002); + // get the largest possible blend radius based on waypoints and smoothness level + double blend_radius = computeBlendRadius(kdl_waypoints, smoothness_level); + RCLCPP_INFO(getLogger(), "Computed blend radius: %f", blend_radius); + KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(blend_radius, eqradius, rot_interpo); // make sure the start pose is the same as the first pose in waypoints with tolerance if (dist > 1.0e-3) { @@ -231,4 +236,53 @@ void TrajectoryGeneratorFree::cmdSpecificRequestValidation(const planning_interf } } +double TrajectoryGeneratorFree::computeBlendRadius(const std::vector& waypoints_, double smoothness) const +{ + double max_allowed_radius = std::numeric_limits::infinity(); + + auto pose_distance = [](const KDL::Frame& p1, const KDL::Frame& p2) { return (p1.p - p2.p).Norm(); }; + + // to calculate the angle between two segments + auto segment_angle = [](const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3) { + KDL::Vector v1 = p2.p - p1.p; + KDL::Vector v2 = p2.p - p3.p; + + double norm_product = v1.Norm() * v2.Norm(); + if (norm_product < 0.25e-6) + return 0.0; // avoid division by zero + + double cos_theta = KDL::dot(v1, v2) / norm_product; + cos_theta = std::clamp(cos_theta, -1.0, 1.0); + + return std::acos(cos_theta); + }; + + for (size_t i = 1; i + 1 < waypoints_.size(); ++i) + { + double dist1 = pose_distance(waypoints_[i], waypoints_[i - 1]); + double dist2 = pose_distance(waypoints_[i + 1], waypoints_[i]); + + if (dist1 < 1e-4 || dist2 < 1e-4) + { + RCLCPP_WARN(getLogger(), "Waypoint %zu is too close to neighbors (%.6f, %.6f).", i, dist1, dist2); + continue; + } + + // The maximum feasible radius for this junction + double local_max_radius = std::tan(segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]) / 2.0) * + std::min(dist1 / 2.0, dist2 / 2.0); + + // Keep track of the tightest constraint + // due to roundedcomposite don't support changing radius + if (local_max_radius < max_allowed_radius) + max_allowed_radius = local_max_radius; + } + + // Apply the smoothness scaling factor + double max_radius = max_allowed_radius * std::clamp(smoothness, 0.01, 0.99); + + RCLCPP_DEBUG(getLogger(), "Validated smoothness scaling %.2f → blend radius %.6f", smoothness, max_radius); + + return max_radius; +} } // namespace pilz_industrial_motion_planner From 84a8e942eedb9d8a77d0b3ee47cafc823d832668 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Sun, 9 Nov 2025 16:47:39 +0300 Subject: [PATCH 12/27] add close points check to avoid Not_Feasable error --- .../src/trajectory_generator_free.cpp | 37 ++++++++++++++----- 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 4ffe941722..8fba016d46 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -210,19 +210,36 @@ std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Aff double blend_radius = computeBlendRadius(kdl_waypoints, smoothness_level); RCLCPP_INFO(getLogger(), "Computed blend radius: %f", blend_radius); KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(blend_radius, eqradius, rot_interpo); - // make sure the start pose is the same as the first pose in waypoints with tolerance - if (dist > 1.0e-3) - { - composite_path->Add(kdl_start_pose); - } - else - RCLCPP_WARN_STREAM(getLogger(), "Close start point. start: " << kdl_start_pose.p - << "fisrt_waypoint: " << kdl_waypoints.front().p); - for (const auto& kdl_waypoint : kdl_waypoints) + composite_path->Add(kdl_start_pose); + // index for the last add point + size_t last_added_point_indx = -1; // -1 for the start_pose + + // to get the last added point in the rounded composite path + auto last_point = [&]() { + return last_added_point_indx != -1 ? kdl_waypoints[last_added_point_indx].p : kdl_start_pose.p; + }; + // add points and skip the points which are too close to each other + for (size_t i = 0; i < kdl_waypoints.size(); ++i) { - composite_path->Add(kdl_waypoint); + dist = (last_point() - kdl_waypoints[i].p).Norm(); + RCLCPP_INFO(getLogger(), "the last point (x,y,z): (%f, %f, %f)", last_point().x(), last_point().y(), + last_point().z()); + if (dist > 1e-4) + { + composite_path->Add(kdl_waypoints[i]); + ++last_added_point_indx; + } + else + RCLCPP_WARN_STREAM(getLogger(), "Skip close waypoint " << i << " at position: " << kdl_waypoints[i].p); } + // ensure the last waypoint is added + // dist = (last_point() - kdl_waypoints.back().p).Norm(); + // if (dist > 1.0e-6) + // { + // composite_path->Add(kdl_waypoints.back()); + // } + composite_path->Finish(); return std::unique_ptr(composite_path); } From 749d7e45b1969185239a287d0148d1aedec4b3b9 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 10 Nov 2025 12:03:16 +0300 Subject: [PATCH 13/27] add helper class for generate free paths --- .../CMakeLists.txt | 2 +- .../path_free_generator.h | 49 ++++++++ .../path_free_generator.hpp | 93 ++++++++++++++ .../planning_context_loader_free.hpp | 2 +- .../trajectory_generator_free.h | 49 ++++++++ .../trajectory_generator_free.hpp | 5 - .../src/path_free_generator.cpp | 116 ++++++++++++++++++ .../src/trajectory_generator_free.cpp | 93 +------------- 8 files changed, 313 insertions(+), 96 deletions(-) create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp create mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index e6d344ba92..e11934c8ef 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -130,7 +130,7 @@ target_link_libraries(planning_context_loader_circ planning_context_loader_base add_library( planning_context_loader_free SHARED src/planning_context_loader_free.cpp src/trajectory_generator_free.cpp - src/velocity_profile_atrap.cpp) + src/velocity_profile_atrap.cpp src/path_free_generator.cpp) ament_target_dependencies(planning_context_loader_free ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(planning_context_loader_free planning_context_loader_base diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h new file mode 100644 index 0000000000..363b6d037c --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h @@ -0,0 +1,49 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp new file mode 100644 index 0000000000..ae923873ca --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace pilz_industrial_motion_planner +{ +/** + * @brief Generator class for KDL::Path_RoundedComposite from different free path + * representations + */ +class PathFreeGenerator +{ +public: + /** + * @brief set the path free from waypoints + * + */ + static std::unique_ptr freeFromWaypoints(const KDL::Frame& start_pose, + const std::vector& waypoints, + KDL::RotationalInterpolation* rot_interpo, double smoothness, + double eqradius); + + /** + * @brief compute the maximum rounding radius from KDL::Path_RoundedComosite + */ + + static double computeBlendRadius(const std::vector& waypoints_, double smoothness); + +private: + PathFreeGenerator(){}; // no instantiation of this helper class! + + static constexpr double MAX_SEGMENT_LENGTH{ 0.2e-3 }; + static constexpr double MIN_SMOOTHNESS{ 0.01 }; + static constexpr double MAX_SMOOTHNESS{ 0.99 }; +}; + +} // namespace pilz_industrial_motion_planner + +// class ErrorMotionPlanningCenterPointDifferentRadius : public KDL::Error_MotionPlanning +// { +// public: +// const char* Description() const override +// { +// return "Distances between start-center and goal-center are different." +// " A free cannot be created."; +// } +// int GetType() const override +// { +// return ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS; +// } // LCOV_EXCL_LINE + +// private: +// static constexpr int ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS{ 3006 }; +// }; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp index 25eea0fcbe..893f0bce6c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp @@ -52,7 +52,7 @@ class PlanningContextLoaderFree : public PlanningContextLoader /** * @brief return a instance of - * pilz_industrial_motion_planner::PlanningContextLIN + * pilz_industrial_motion_planner::PlanningContextFree * @param planning_context returned context * @param name * @param group diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h new file mode 100644 index 0000000000..15f133cd01 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h @@ -0,0 +1,49 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp index 061dbbf5fd..327bfb92c0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp @@ -71,11 +71,6 @@ class TrajectoryGeneratorFree : public TrajectoryGenerator private: void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; - /** - * @brief set the max possible blend radius - * for rounded composite path with smoothness scaler - */ - double computeBlendRadius(const std::vector& waypoints_, double smoothness_) const; void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp new file mode 100644 index 0000000000..8daf3e3c78 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp @@ -0,0 +1,116 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Pilz GmbH & Co. KG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Pilz GmbH & Co. KG nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +namespace pilz_industrial_motion_planner +{ +std::unique_ptr PathFreeGenerator::freeFromWaypoints(const KDL::Frame& start_pose, + const std::vector& waypoints, + KDL::RotationalInterpolation* rot_interpo, + double smoothness, double eqradius) +{ + double blend_radius = computeBlendRadius(waypoints, smoothness); + KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(blend_radius, eqradius, rot_interpo); + + composite_path->Add(start_pose); + // index for the last add point + size_t last_added_point_indx = -1; // -1 for the start_pose + + // to get the last added point in the rounded composite path + auto last_point = [&]() { return last_added_point_indx != -1 ? waypoints[last_added_point_indx].p : start_pose.p; }; + // distance between start pose and first waypoint + double dist = (start_pose.p - waypoints.front().p).Norm(); + + // add points and skip the points which are too close to each other + for (size_t i = 0; i < waypoints.size(); ++i) + { + dist = (last_point() - waypoints[i].p).Norm(); + if (dist > MAX_SEGMENT_LENGTH) + { + composite_path->Add(waypoints[i]); + ++last_added_point_indx; + } + } + composite_path->Finish(); + return std::unique_ptr(composite_path); +} +double PathFreeGenerator::computeBlendRadius(const std::vector& waypoints_, double smoothness) +{ + double max_allowed_radius = std::numeric_limits::infinity(); + + auto pose_distance = [](const KDL::Frame& p1, const KDL::Frame& p2) { return (p1.p - p2.p).Norm(); }; + + // to calculate the angle between two segments + auto segment_angle = [](const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3) { + KDL::Vector v1 = p2.p - p1.p; + KDL::Vector v2 = p2.p - p3.p; + + double norm_product = v1.Norm() * v2.Norm(); + if (norm_product < MAX_SEGMENT_LENGTH * MAX_SEGMENT_LENGTH) + return 0.0; // avoid division by zero + + double cos_theta = KDL::dot(v1, v2) / norm_product; + cos_theta = std::clamp(cos_theta, -1.0, 1.0); + + return std::acos(cos_theta); + }; + + for (size_t i = 1; i + 1 < waypoints_.size(); ++i) + { + double dist1 = pose_distance(waypoints_[i], waypoints_[i - 1]); + double dist2 = pose_distance(waypoints_[i + 1], waypoints_[i]); + + if (dist1 < MAX_SEGMENT_LENGTH || dist2 < MAX_SEGMENT_LENGTH) + { + continue; + } + + // The maximum feasible radius for this junction + double local_max_radius = std::tan(segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]) / 2.0) * + std::min(dist1 / 2.0, dist2 / 2.0); + + // Keep track of the tightest constraint + // due to roundedcomposite don't support changing radius + if (local_max_radius < max_allowed_radius) + max_allowed_radius = local_max_radius; + } + + // Apply the smoothness scaling factor + double max_radius = max_allowed_radius * std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS); + + return max_radius; +} + +} // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 8fba016d46..602b9deeaf 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -33,6 +33,7 @@ *********************************************************************/ #include +#include #include @@ -130,7 +131,6 @@ void TrajectoryGeneratorFree::extractMotionPlanInfo(const planning_scene::Planni Eigen::Isometry3d waypoint; tf2::fromMsg(pc.constraint_region.primitive_poses.front(), waypoint); waypoint = scene->getFrameTransform(frame_id) * waypoint; - RCLCPP_INFO_STREAM(getLogger(), "Added waypoint at position: " << waypoint.translation().transpose()); info.waypoints.push_back(waypoint); } info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); @@ -158,7 +158,7 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& { // set pilz cartesian limits for each item setMaxCartesianSpeed(req); - // create Cartesian path for free + // create Cartesian FREE path std::unique_ptr path(setPathFree(plan_info.start_pose, plan_info.waypoints, req.smoothness_level)); // create velocity profile std::unique_ptr vp( @@ -199,48 +199,13 @@ std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Aff tf2::transformEigenToKDL(waypoint, kdl_waypoint); kdl_waypoints.push_back(kdl_waypoint); } - // distance between start pose and first waypoint - double dist = (kdl_start_pose.p - kdl_waypoints.front().p).Norm(); - RCLCPP_INFO(getLogger(), "distance: %f", dist); RCLCPP_INFO_STREAM(getLogger(), "Transformed waypoints number: " << kdl_waypoints.size()); + double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel; KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); - // get the largest possible blend radius based on waypoints and smoothness level - double blend_radius = computeBlendRadius(kdl_waypoints, smoothness_level); - RCLCPP_INFO(getLogger(), "Computed blend radius: %f", blend_radius); - KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(blend_radius, eqradius, rot_interpo); - - composite_path->Add(kdl_start_pose); - // index for the last add point - size_t last_added_point_indx = -1; // -1 for the start_pose - // to get the last added point in the rounded composite path - auto last_point = [&]() { - return last_added_point_indx != -1 ? kdl_waypoints[last_added_point_indx].p : kdl_start_pose.p; - }; - // add points and skip the points which are too close to each other - for (size_t i = 0; i < kdl_waypoints.size(); ++i) - { - dist = (last_point() - kdl_waypoints[i].p).Norm(); - RCLCPP_INFO(getLogger(), "the last point (x,y,z): (%f, %f, %f)", last_point().x(), last_point().y(), - last_point().z()); - if (dist > 1e-4) - { - composite_path->Add(kdl_waypoints[i]); - ++last_added_point_indx; - } - else - RCLCPP_WARN_STREAM(getLogger(), "Skip close waypoint " << i << " at position: " << kdl_waypoints[i].p); - } - // ensure the last waypoint is added - // dist = (last_point() - kdl_waypoints.back().p).Norm(); - // if (dist > 1.0e-6) - // { - // composite_path->Add(kdl_waypoints.back()); - // } - composite_path->Finish(); - return std::unique_ptr(composite_path); + return PathFreeGenerator::freeFromWaypoints(kdl_start_pose, kdl_waypoints, rot_interpo, smoothness_level, eqradius); } void TrajectoryGeneratorFree::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const @@ -252,54 +217,4 @@ void TrajectoryGeneratorFree::cmdSpecificRequestValidation(const planning_interf throw NoWaypointsSpecified(os.str()); } } - -double TrajectoryGeneratorFree::computeBlendRadius(const std::vector& waypoints_, double smoothness) const -{ - double max_allowed_radius = std::numeric_limits::infinity(); - - auto pose_distance = [](const KDL::Frame& p1, const KDL::Frame& p2) { return (p1.p - p2.p).Norm(); }; - - // to calculate the angle between two segments - auto segment_angle = [](const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3) { - KDL::Vector v1 = p2.p - p1.p; - KDL::Vector v2 = p2.p - p3.p; - - double norm_product = v1.Norm() * v2.Norm(); - if (norm_product < 0.25e-6) - return 0.0; // avoid division by zero - - double cos_theta = KDL::dot(v1, v2) / norm_product; - cos_theta = std::clamp(cos_theta, -1.0, 1.0); - - return std::acos(cos_theta); - }; - - for (size_t i = 1; i + 1 < waypoints_.size(); ++i) - { - double dist1 = pose_distance(waypoints_[i], waypoints_[i - 1]); - double dist2 = pose_distance(waypoints_[i + 1], waypoints_[i]); - - if (dist1 < 1e-4 || dist2 < 1e-4) - { - RCLCPP_WARN(getLogger(), "Waypoint %zu is too close to neighbors (%.6f, %.6f).", i, dist1, dist2); - continue; - } - - // The maximum feasible radius for this junction - double local_max_radius = std::tan(segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]) / 2.0) * - std::min(dist1 / 2.0, dist2 / 2.0); - - // Keep track of the tightest constraint - // due to roundedcomposite don't support changing radius - if (local_max_radius < max_allowed_radius) - max_allowed_radius = local_max_radius; - } - - // Apply the smoothness scaling factor - double max_radius = max_allowed_radius * std::clamp(smoothness, 0.01, 0.99); - - RCLCPP_DEBUG(getLogger(), "Validated smoothness scaling %.2f → blend radius %.6f", smoothness, max_radius); - - return max_radius; -} } // namespace pilz_industrial_motion_planner From 440a77d89b338b3a589082aaee6ec15656961fb3 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 10 Nov 2025 14:31:09 +0300 Subject: [PATCH 14/27] clean code and add copyright! --- .../path_free_generator.h | 1 + .../path_free_generator.hpp | 21 +---- .../planning_context_free.h | 1 + .../planning_context_free.hpp | 1 + .../planning_context_loader_free.h | 1 + .../planning_context_loader_free.hpp | 1 + .../trajectory_generator_free.h | 1 + .../trajectory_generator_free.hpp | 8 +- .../src/path_free_generator.cpp | 9 +- .../src/planning_context_loader_free.cpp | 1 + .../src/trajectory_generator_free.cpp | 90 +++++++------------ 11 files changed, 55 insertions(+), 80 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h index 363b6d037c..1a03211cc0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h @@ -14,6 +14,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp index ae923873ca..d4c60814d1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -61,6 +62,9 @@ class PathFreeGenerator /** * @brief compute the maximum rounding radius from KDL::Path_RoundedComosite + * @param waypoints_: waypoints defining the path + * @param smoothness: smoothness level [0..1] scaling the maximum blend radius + * @return maximum blend radius */ static double computeBlendRadius(const std::vector& waypoints_, double smoothness); @@ -74,20 +78,3 @@ class PathFreeGenerator }; } // namespace pilz_industrial_motion_planner - -// class ErrorMotionPlanningCenterPointDifferentRadius : public KDL::Error_MotionPlanning -// { -// public: -// const char* Description() const override -// { -// return "Distances between start-center and goal-center are different." -// " A free cannot be created."; -// } -// int GetType() const override -// { -// return ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS; -// } // LCOV_EXCL_LINE - -// private: -// static constexpr int ERROR_CODE_CENTER_POINT_DIFFERENT_RADIUS{ 3006 }; -// }; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h index b5ef7e00ee..beb3646f9f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h @@ -14,6 +14,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp index eabcbe8904..310fe0d5a7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h index 04338d1e9e..d0466d1cef 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h @@ -14,6 +14,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp index 893f0bce6c..be1cf94713 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h index 15f133cd01..06dcc5e375 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h @@ -14,6 +14,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp index 327bfb92c0..eb71857e11 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -52,7 +53,7 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::m CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoWaypointsSpecified, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); /** - * @brief This class implements a linear trajectory generator in Cartesian + * @brief This class implements a free trajectory generator in Cartesian * space. * The Cartesian trajetory are based on trapezoid velocity profile. */ @@ -80,7 +81,10 @@ class TrajectoryGeneratorFree : public TrajectoryGenerator trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** - * @brief construct a KDL::Path object for a Cartesian straight line + * @brief construct a KDL::Path object for a Cartesian free path + * @param start_pose: start pose of the path + * @param waypoints: waypoints defining the path + * @param smoothness_level: smoothness level for blending the waypoints * @return a unique pointer of the path object. null_ptr in case of an error. */ std::unique_ptr setPathFree(const Eigen::Affine3d& start_pose, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp index 8daf3e3c78..6965574033 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -48,6 +49,8 @@ std::unique_ptr PathFreeGenerator::freeFromWaypoints(const KDL::Frame // index for the last add point size_t last_added_point_indx = -1; // -1 for the start_pose + // the following is to remove very close waypoints + // to avoid issues in KDL::Path_RoundedComposite like throwing Not_Fesible exceptions // to get the last added point in the rounded composite path auto last_point = [&]() { return last_added_point_indx != -1 ? waypoints[last_added_point_indx].p : start_pose.p; }; // distance between start pose and first waypoint @@ -72,7 +75,7 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp auto pose_distance = [](const KDL::Frame& p1, const KDL::Frame& p2) { return (p1.p - p2.p).Norm(); }; - // to calculate the angle between two segments + // to calculate the angle between the two segments of p2 auto segment_angle = [](const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3) { KDL::Vector v1 = p2.p - p1.p; KDL::Vector v2 = p2.p - p3.p; @@ -101,8 +104,8 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp double local_max_radius = std::tan(segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]) / 2.0) * std::min(dist1 / 2.0, dist2 / 2.0); - // Keep track of the tightest constraint - // due to roundedcomposite don't support changing radius + // track the tightest constraint + // due to KDL::Path_RoundedComposite don't support changing radius if (local_max_radius < max_allowed_radius) max_allowed_radius = local_max_radius; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp index 8c47fcbb34..c9ef0576ae 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 602b9deeaf..8ad76ea56b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018 Pilz GmbH & Co. KG + * Copyright (c) 2025 Aiman Haidar * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -41,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -64,7 +64,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit.planners.pilz.trajectory_generator.lin"); + return moveit::getLogger("moveit.planners.pilz.trajectory_generator.free"); } } // namespace TrajectoryGeneratorFree::TrajectoryGeneratorFree(const moveit::core::RobotModelConstPtr& robot_model, @@ -84,67 +84,41 @@ void TrajectoryGeneratorFree::extractMotionPlanInfo(const planning_scene::Planni info.group_name = req.group_name; moveit::core::RobotState robot_state = scene->getCurrentState(); - // goal given in joint space - if (!req.goal_constraints.front().joint_constraints.empty()) + std::string frame_id; + + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) { - info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name)); - - if (req.goal_constraints.front().joint_constraints.size() != - robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) - { - std::ostringstream os; - os << "Number of joints in goal does not match number of joints of group " - "(Number joints goal: " - << req.goal_constraints.front().joint_constraints.size() << " | Number of joints of group: " - << robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size() << ')'; - throw JointNumberMismatch(os.str()); - } - - for (const auto& joint_item : req.goal_constraints.front().joint_constraints) - { - info.goal_joint_position[joint_item.joint_name] = joint_item.position; - } - - computeLinkFK(robot_state, info.link_name, info.goal_joint_position, info.goal_pose); + RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); } - // goal given in Cartesian space else { - std::string frame_id; - - info.link_name = req.goal_constraints.front().position_constraints.front().link_name; - if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || - req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) - { - RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of " - "goal. Use model frame as default"); - frame_id = robot_model_->getModelFrame(); - } - else - { - frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; - } - - // goal pose with optional offset wrt. the planning frame - for (const auto& pc : req.path_constraints.position_constraints) - { - Eigen::Isometry3d waypoint; - tf2::fromMsg(pc.constraint_region.primitive_poses.front(), waypoint); - waypoint = scene->getFrameTransform(frame_id) * waypoint; - info.waypoints.push_back(waypoint); - } - info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); - frame_id = robot_model_->getModelFrame(); + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } - // check goal pose ik before Cartesian motion plan starts - std::map ik_solution; - if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, - ik_solution)) - { - std::ostringstream os; - os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; - throw LinInverseForGoalIncalculable(os.str()); - } + // Add the path waypoints + for (const auto& pc : req.path_constraints.position_constraints) + { + Eigen::Isometry3d waypoint; + tf2::fromMsg(pc.constraint_region.primitive_poses.front(), waypoint); + waypoint = scene->getFrameTransform(frame_id) * waypoint; + info.waypoints.push_back(waypoint); + } + // goal constraint is just the final pose + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw LinInverseForGoalIncalculable(os.str()); } // Ignored return value because at this point the function should always From 7e08974ed198a3c0c97b8d9f7974dcf0bc7d50d1 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 10 Nov 2025 21:02:21 +0300 Subject: [PATCH 15/27] handle colinear points errors --- .../path_free_generator.hpp | 19 +++++++++++++++++++ .../trajectory_generator_free.hpp | 2 ++ .../src/path_free_generator.cpp | 14 +++++++++++++- .../src/trajectory_generator_free.cpp | 13 ++++++++++++- 4 files changed, 46 insertions(+), 2 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp index d4c60814d1..c9ee9eca01 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp @@ -68,6 +68,7 @@ class PathFreeGenerator */ static double computeBlendRadius(const std::vector& waypoints_, double smoothness); + static void checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3); private: PathFreeGenerator(){}; // no instantiation of this helper class! @@ -75,6 +76,24 @@ class PathFreeGenerator static constexpr double MAX_SEGMENT_LENGTH{ 0.2e-3 }; static constexpr double MIN_SMOOTHNESS{ 0.01 }; static constexpr double MAX_SMOOTHNESS{ 0.99 }; + static constexpr double MAX_COLINEAR_NORM{ 1e-9 }; +}; + +class ErrorMotionPlanningColinearConsicutiveWaypoints : public KDL::Error_MotionPlanning +{ +public: + const char* Description() const override + { + return "Three collinear consecutive waypoints." + " A Free Path cannot be created."; + } + int GetType() const override + { + return ERROR_CODE_COLINEAR_CONSECUTIVE_WAYPOINTS; + } // LCOV_EXCL_LINE + +private: + static constexpr int ERROR_CODE_COLINEAR_CONSECUTIVE_WAYPOINTS{ 3104 }; }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp index eb71857e11..21d650b79b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp @@ -51,6 +51,8 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs:: CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NoWaypointsSpecified, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); +CREATE_MOVEIT_ERROR_CODE_EXCEPTION(ConsicutiveColinearWaypoints, + moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); /** * @brief This class implements a free trajectory generator in Cartesian diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp index 6965574033..ef7b0c4673 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp @@ -94,7 +94,7 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp { double dist1 = pose_distance(waypoints_[i], waypoints_[i - 1]); double dist2 = pose_distance(waypoints_[i + 1], waypoints_[i]); - + checkConsecutiveColinearWaypoints(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]); if (dist1 < MAX_SEGMENT_LENGTH || dist2 < MAX_SEGMENT_LENGTH) { continue; @@ -115,5 +115,17 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp return max_radius; } +void PathFreeGenerator::checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, + const KDL::Frame& p3) +{ + KDL::Vector v1 = p2.p - p1.p; + KDL::Vector v2 = p3.p - p2.p; + + KDL::Vector cross_product = v1 * v2; + if (cross_product.Norm() < MAX_COLINEAR_NORM) + { + throw ErrorMotionPlanningColinearConsicutiveWaypoints(); + } +} } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 8ad76ea56b..ce89191512 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -133,7 +133,18 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& // set pilz cartesian limits for each item setMaxCartesianSpeed(req); // create Cartesian FREE path - std::unique_ptr path(setPathFree(plan_info.start_pose, plan_info.waypoints, req.smoothness_level)); + std::unique_ptr path; + try + { + path = setPathFree(plan_info.start_pose, plan_info.waypoints, req.smoothness_level); + } + catch (const KDL::Error_MotionPlanning& e) + { + RCLCPP_ERROR(getLogger(), "Motion planning error: %s", e.Description()); + std::ostringstream os; + os << "waypoints specified in path constraints have three consicutive colinear points"; + throw ConsicutiveColinearWaypoints(os.str()); + } // create velocity profile std::unique_ptr vp( cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); From 5072fbd6adebdd9f13bac2542195f7a938329a15 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Sat, 22 Nov 2025 09:41:33 +0300 Subject: [PATCH 16/27] correct error massages and fix bug --- .../src/path_free_generator.cpp | 5 +++-- .../src/trajectory_generator_free.cpp | 8 +++++++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp index ef7b0c4673..6b535d32fa 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp @@ -101,8 +101,9 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp } // The maximum feasible radius for this junction - double local_max_radius = std::tan(segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]) / 2.0) * - std::min(dist1 / 2.0, dist2 / 2.0); + double local_max_radius = + std::tan((M_PI - segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1])) / 2.0) * + std::min(dist1 / 2.0, dist2 / 2.0); // track the tightest constraint // due to KDL::Path_RoundedComposite don't support changing radius diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index ce89191512..22865a4b79 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -141,8 +141,14 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& catch (const KDL::Error_MotionPlanning& e) { RCLCPP_ERROR(getLogger(), "Motion planning error: %s", e.Description()); + int code = e.GetType(); std::ostringstream os; - os << "waypoints specified in path constraints have three consicutive colinear points"; + if (code == 3102 || code == 3103) + os << "zero distance between two points"; + else if (code == 3104) + os << "waypoints specified in path constraints have three consicutive colinear points"; + else if (code == 3105 || code == 3106) + os << "rounding circle of a point is bigger than the distance with one of the neighbor points"; throw ConsicutiveColinearWaypoints(os.str()); } // create velocity profile From 9fc413a5030c329695ce950df08dc99d68319ec9 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 24 Nov 2025 10:34:33 +0300 Subject: [PATCH 17/27] restrict the rounding radius to not exceed KDL epsilon --- .../src/path_free_generator.cpp | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp index 6b535d32fa..ecb7e40115 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp @@ -101,20 +101,26 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp } // The maximum feasible radius for this junction - double local_max_radius = - std::tan((M_PI - segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1])) / 2.0) * - std::min(dist1 / 2.0, dist2 / 2.0); + double theta = segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]); + double local_max_radius = std::tan((M_PI - theta) / 2.0) * std::min(dist1 / 2.0, dist2 / 2.0); // track the tightest constraint // due to KDL::Path_RoundedComposite don't support changing radius if (local_max_radius < max_allowed_radius) max_allowed_radius = local_max_radius; + // to ensure Path_RoundedComposite not throw + // Error_MotionPlanning_Circle_ToSmall or + // Error_MotionPlanning_Circle_No_Plane + if (max_allowed_radius * std::sin(M_PI - std::acos(theta)) * std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS) < + KDL::epsilon) + { + max_allowed_radius = KDL::epsilon; + break; + } } + max_allowed_radius *= std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS); - // Apply the smoothness scaling factor - double max_radius = max_allowed_radius * std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS); - - return max_radius; + return max_allowed_radius; } void PathFreeGenerator::checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3) From 616d427be9b57c756b0cf9bc4fd5cca5a9fcdf6a Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 24 Nov 2025 11:04:19 +0300 Subject: [PATCH 18/27] fix bug --- .../src/path_free_generator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp index ecb7e40115..af19fca87d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp @@ -102,7 +102,7 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp // The maximum feasible radius for this junction double theta = segment_angle(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]); - double local_max_radius = std::tan((M_PI - theta) / 2.0) * std::min(dist1 / 2.0, dist2 / 2.0); + double local_max_radius = std::abs(std::tan(theta / 2.0)) * std::min(dist1 / 2.0, dist2 / 2.0); // track the tightest constraint // due to KDL::Path_RoundedComposite don't support changing radius @@ -111,7 +111,7 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp // to ensure Path_RoundedComposite not throw // Error_MotionPlanning_Circle_ToSmall or // Error_MotionPlanning_Circle_No_Plane - if (max_allowed_radius * std::sin(M_PI - std::acos(theta)) * std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS) < + if (max_allowed_radius * std::sin(M_PI - theta) * std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS) < KDL::epsilon) { max_allowed_radius = KDL::epsilon; From ab5d6548f18d93e89a857514800d2bdb219324d5 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 24 Nov 2025 13:02:36 +0300 Subject: [PATCH 19/27] fix:compute rounding(blend) radius after filtering near points --- .../path_free_generator.hpp | 6 +-- .../src/path_free_generator.cpp | 45 +++++++++++-------- .../src/trajectory_generator_free.cpp | 2 + 3 files changed, 31 insertions(+), 22 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp index c9ee9eca01..6a2d16b110 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp @@ -66,17 +66,17 @@ class PathFreeGenerator * @param smoothness: smoothness level [0..1] scaling the maximum blend radius * @return maximum blend radius */ - + static std::vector filterWaypoints(const KDL::Frame& start_pose, const std::vector& waypoints); static double computeBlendRadius(const std::vector& waypoints_, double smoothness); static void checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3); private: PathFreeGenerator(){}; // no instantiation of this helper class! - static constexpr double MAX_SEGMENT_LENGTH{ 0.2e-3 }; + static constexpr double MIN_SEGMENT_LENGTH{ 0.2e-3 }; static constexpr double MIN_SMOOTHNESS{ 0.01 }; static constexpr double MAX_SMOOTHNESS{ 0.99 }; - static constexpr double MAX_COLINEAR_NORM{ 1e-9 }; + static constexpr double MIN_COLINEAR_NORM{ 1e-9 }; }; class ErrorMotionPlanningColinearConsicutiveWaypoints : public KDL::Error_MotionPlanning diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp index af19fca87d..d968049dba 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp @@ -42,11 +42,27 @@ std::unique_ptr PathFreeGenerator::freeFromWaypoints(const KDL::Frame KDL::RotationalInterpolation* rot_interpo, double smoothness, double eqradius) { - double blend_radius = computeBlendRadius(waypoints, smoothness); + std::vector filtered_waypoints = filterWaypoints(start_pose, waypoints); + double blend_radius = computeBlendRadius(filtered_waypoints, smoothness); + KDL::Path_RoundedComposite* composite_path = new KDL::Path_RoundedComposite(blend_radius, eqradius, rot_interpo); - composite_path->Add(start_pose); + for (const auto& waypoint : filtered_waypoints) + { + composite_path->Add(waypoint); + } + + composite_path->Finish(); + return std::unique_ptr(composite_path); +} + +std::vector PathFreeGenerator::filterWaypoints(const KDL::Frame& start_pose, + const std::vector& waypoints) +{ + std::vector filtered_waypoints = {}; + // index for the last add point + filtered_waypoints.push_back(start_pose); size_t last_added_point_indx = -1; // -1 for the start_pose // the following is to remove very close waypoints @@ -54,20 +70,19 @@ std::unique_ptr PathFreeGenerator::freeFromWaypoints(const KDL::Frame // to get the last added point in the rounded composite path auto last_point = [&]() { return last_added_point_indx != -1 ? waypoints[last_added_point_indx].p : start_pose.p; }; // distance between start pose and first waypoint - double dist = (start_pose.p - waypoints.front().p).Norm(); + double dist; // add points and skip the points which are too close to each other for (size_t i = 0; i < waypoints.size(); ++i) { dist = (last_point() - waypoints[i].p).Norm(); - if (dist > MAX_SEGMENT_LENGTH) + if (dist > MIN_SEGMENT_LENGTH) { - composite_path->Add(waypoints[i]); + filtered_waypoints.push_back(waypoints[i]); ++last_added_point_indx; } } - composite_path->Finish(); - return std::unique_ptr(composite_path); + return filtered_waypoints; } double PathFreeGenerator::computeBlendRadius(const std::vector& waypoints_, double smoothness) { @@ -81,7 +96,7 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp KDL::Vector v2 = p2.p - p3.p; double norm_product = v1.Norm() * v2.Norm(); - if (norm_product < MAX_SEGMENT_LENGTH * MAX_SEGMENT_LENGTH) + if (norm_product < MIN_SEGMENT_LENGTH * MIN_SEGMENT_LENGTH) return 0.0; // avoid division by zero double cos_theta = KDL::dot(v1, v2) / norm_product; @@ -95,7 +110,7 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp double dist1 = pose_distance(waypoints_[i], waypoints_[i - 1]); double dist2 = pose_distance(waypoints_[i + 1], waypoints_[i]); checkConsecutiveColinearWaypoints(waypoints_[i - 1], waypoints_[i], waypoints_[i + 1]); - if (dist1 < MAX_SEGMENT_LENGTH || dist2 < MAX_SEGMENT_LENGTH) + if (dist1 < MIN_SEGMENT_LENGTH || dist2 < MIN_SEGMENT_LENGTH) { continue; } @@ -108,16 +123,8 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp // due to KDL::Path_RoundedComposite don't support changing radius if (local_max_radius < max_allowed_radius) max_allowed_radius = local_max_radius; - // to ensure Path_RoundedComposite not throw - // Error_MotionPlanning_Circle_ToSmall or - // Error_MotionPlanning_Circle_No_Plane - if (max_allowed_radius * std::sin(M_PI - theta) * std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS) < - KDL::epsilon) - { - max_allowed_radius = KDL::epsilon; - break; - } } + max_allowed_radius *= std::clamp(smoothness, MIN_SMOOTHNESS, MAX_SMOOTHNESS); return max_allowed_radius; @@ -129,7 +136,7 @@ void PathFreeGenerator::checkConsecutiveColinearWaypoints(const KDL::Frame& p1, KDL::Vector v2 = p3.p - p2.p; KDL::Vector cross_product = v1 * v2; - if (cross_product.Norm() < MAX_COLINEAR_NORM) + if (cross_product.Norm() < MIN_COLINEAR_NORM) { throw ErrorMotionPlanningColinearConsicutiveWaypoints(); } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index 22865a4b79..e63c655008 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -149,6 +149,8 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& os << "waypoints specified in path constraints have three consicutive colinear points"; else if (code == 3105 || code == 3106) os << "rounding circle of a point is bigger than the distance with one of the neighbor points"; + else if (code == 3001 || code == 3002) + os << "the rounding radius is lower than KDL::epsilon. use bigger smoothness or resample your waypoints"; throw ConsicutiveColinearWaypoints(os.str()); } // create velocity profile From 5f7a8baeb03371dbafcf6c4df214e92dc1bf85b6 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 1 Dec 2025 17:01:01 +0300 Subject: [PATCH 20/27] change name to polyline --- .../CMakeLists.txt | 16 ++++--- .../path_free_generator.h | 2 +- .../path_free_generator.hpp | 18 ++++---- .../planning_context_free.h | 2 +- .../planning_context_free.hpp | 13 +++--- .../planning_context_loader_free.h | 2 +- .../planning_context_loader_free.hpp | 16 +++---- .../trajectory_generator_free.h | 2 +- .../trajectory_generator_free.hpp | 20 ++++----- .../planning_context_plugin_description.xml | 8 ++-- .../src/path_free_generator.cpp | 20 ++++----- .../src/planning_context_loader_free.cpp | 18 ++++---- .../src/trajectory_generator_free.cpp | 44 ++++++++++--------- .../acceptance_tests/acceptance_test_lin.md | 2 +- 14 files changed, 94 insertions(+), 89 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index e11934c8ef..18610f07ae 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -128,13 +128,15 @@ target_link_libraries(planning_context_loader_circ planning_context_loader_base joint_limits_common trajectory_generation_common) add_library( - planning_context_loader_free SHARED - src/planning_context_loader_free.cpp src/trajectory_generator_free.cpp - src/velocity_profile_atrap.cpp src/path_free_generator.cpp) -ament_target_dependencies(planning_context_loader_free + planning_context_loader_polyline SHARED + src/planning_context_loader_polyline.cpp + src/trajectory_generator_polyline.cpp src/velocity_profile_atrap.cpp + src/path_polyline_generator.cpp) +ament_target_dependencies(planning_context_loader_polyline ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(planning_context_loader_free planning_context_loader_base - joint_limits_common trajectory_generation_common) +target_link_libraries( + planning_context_loader_polyline planning_context_loader_base + joint_limits_common trajectory_generation_common) add_library(sequence_capability SHARED src/move_group_sequence_action.cpp src/move_group_sequence_service.cpp) @@ -162,7 +164,7 @@ install( planning_context_loader_ptp planning_context_loader_lin planning_context_loader_circ - planning_context_loader_free + planning_context_loader_polyline command_list_manager sequence_capability trajectory_generation_common diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h index 1a03211cc0..e3796499d2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h @@ -47,4 +47,4 @@ #pragma once #pragma message(".h header is obsolete. Please use the .hpp header instead.") -#include +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp index 6a2d16b110..ce94879891 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp @@ -45,20 +45,20 @@ namespace pilz_industrial_motion_planner { /** - * @brief Generator class for KDL::Path_RoundedComposite from different free path + * @brief Generator class for KDL::Path_RoundedComposite from different polyline path * representations */ -class PathFreeGenerator +class PathPolylineGenerator { public: /** - * @brief set the path free from waypoints + * @brief set the path polyline from waypoints * */ - static std::unique_ptr freeFromWaypoints(const KDL::Frame& start_pose, - const std::vector& waypoints, - KDL::RotationalInterpolation* rot_interpo, double smoothness, - double eqradius); + static std::unique_ptr polylineFromWaypoints(const KDL::Frame& start_pose, + const std::vector& waypoints, + KDL::RotationalInterpolation* rot_interpo, double smoothness, + double eqradius); /** * @brief compute the maximum rounding radius from KDL::Path_RoundedComosite @@ -71,7 +71,7 @@ class PathFreeGenerator static void checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, const KDL::Frame& p3); private: - PathFreeGenerator(){}; // no instantiation of this helper class! + PathPolylineGenerator(){}; // no instantiation of this helper class! static constexpr double MIN_SEGMENT_LENGTH{ 0.2e-3 }; static constexpr double MIN_SMOOTHNESS{ 0.01 }; @@ -85,7 +85,7 @@ class ErrorMotionPlanningColinearConsicutiveWaypoints : public KDL::Error_Motion const char* Description() const override { return "Three collinear consecutive waypoints." - " A Free Path cannot be created."; + " A Polyline Path cannot be created."; } int GetType() const override { diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h index beb3646f9f..937eb49860 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h @@ -47,4 +47,4 @@ #pragma once #pragma message(".h header is obsolete. Please use the .hpp header instead.") -#include +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp index 310fe0d5a7..b9c668c0e6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp @@ -46,21 +46,22 @@ #include #include -#include +#include namespace pilz_industrial_motion_planner { MOVEIT_CLASS_FORWARD(PlanningContext); /** - * @brief PlanningContext for obtaining Free trajectories + * @brief PlanningContext for obtaining Polyline trajectories */ -class PlanningContextFree : public pilz_industrial_motion_planner::PlanningContextBase +class PlanningContextPolyline : public pilz_industrial_motion_planner::PlanningContextBase { public: - PlanningContextFree(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, - const pilz_industrial_motion_planner::LimitsContainer& limits) - : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) + PlanningContextPolyline(const std::string& name, const std::string& group, + const moveit::core::RobotModelConstPtr& model, + const pilz_industrial_motion_planner::LimitsContainer& limits) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits) { } }; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h index d0466d1cef..29997dc3cf 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h @@ -47,4 +47,4 @@ #pragma once #pragma message(".h header is obsolete. Please use the .hpp header instead.") -#include +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp index be1cf94713..13b7c38ef9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp @@ -42,18 +42,18 @@ namespace pilz_industrial_motion_planner { /** - * @brief Plugin that can generate instances of PlanningContextFree. - * Generates instances of PlanningContextFree. + * @brief Plugin that can generate instances of PlanningContextPolyline. + * Generates instances of PlanningContextPolyline. */ -class PlanningContextLoaderFree : public PlanningContextLoader +class PlanningContextLoaderPolyline : public PlanningContextLoader { public: - PlanningContextLoaderFree(); - ~PlanningContextLoaderFree() override; + PlanningContextLoaderPolyline(); + ~PlanningContextLoaderPolyline() override; /** * @brief return a instance of - * pilz_industrial_motion_planner::PlanningContextFree + * pilz_industrial_motion_planner::PlanningContextPolyline * @param planning_context returned context * @param name * @param group @@ -63,7 +63,7 @@ class PlanningContextLoaderFree : public PlanningContextLoader const std::string& group) const override; }; -typedef std::shared_ptr PlanningContextLoaderFreePtr; -typedef std::shared_ptr PlanningContextLoaderFreeConstPtr; +typedef std::shared_ptr PlanningContextLoaderPolylinePtr; +typedef std::shared_ptr PlanningContextLoaderPolylineConstPtr; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h index 06dcc5e375..53d3ca38ee 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h @@ -47,4 +47,4 @@ #pragma once #pragma message(".h header is obsolete. Please use the .hpp header instead.") -#include +#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp index 21d650b79b..fce646b8fe 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp @@ -55,22 +55,22 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(ConsicutiveColinearWaypoints, moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN); /** - * @brief This class implements a free trajectory generator in Cartesian + * @brief This class implements a polyline trajectory generator in Cartesian * space. * The Cartesian trajetory are based on trapezoid velocity profile. */ -class TrajectoryGeneratorFree : public TrajectoryGenerator +class TrajectoryGeneratorPolyline : public TrajectoryGenerator { public: /** - * @brief Constructor of Free Trajectory Generator + * @brief Constructor of Polyline Trajectory Generator * @throw TrajectoryGeneratorInvalidLimitsException * @param model: robot model * @param planner_limits: limits in joint and Cartesian spaces */ - TrajectoryGeneratorFree(const moveit::core::RobotModelConstPtr& robot_model, - const pilz_industrial_motion_planner::LimitsContainer& planner_limits, - const std::string& group_name); + TrajectoryGeneratorPolyline(const moveit::core::RobotModelConstPtr& robot_model, + const pilz_industrial_motion_planner::LimitsContainer& planner_limits, + const std::string& group_name); private: void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override; @@ -83,15 +83,15 @@ class TrajectoryGeneratorFree : public TrajectoryGenerator trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** - * @brief construct a KDL::Path object for a Cartesian free path + * @brief construct a KDL::Path object for a Cartesian polyline path * @param start_pose: start pose of the path * @param waypoints: waypoints defining the path * @param smoothness_level: smoothness level for blending the waypoints * @return a unique pointer of the path object. null_ptr in case of an error. */ - std::unique_ptr setPathFree(const Eigen::Affine3d& start_pose, - const std::vector& waypoints, - double smoothness_level) const; + std::unique_ptr setPathPolyline(const Eigen::Affine3d& start_pose, + const std::vector& waypoints, + double smoothness_level) const; }; } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml index 6c8522a8dd..0a07d45f01 100644 --- a/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml +++ b/moveit_planners/pilz_industrial_motion_planner/plugins/planning_context_plugin_description.xml @@ -20,11 +20,11 @@ Loader for CIRC Context - - + - Loader for Free Context + Loader for Polyline Context diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp index d968049dba..df694ae718 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp @@ -33,14 +33,14 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include namespace pilz_industrial_motion_planner { -std::unique_ptr PathFreeGenerator::freeFromWaypoints(const KDL::Frame& start_pose, - const std::vector& waypoints, - KDL::RotationalInterpolation* rot_interpo, - double smoothness, double eqradius) +std::unique_ptr PathPolylineGenerator::polylineFromWaypoints(const KDL::Frame& start_pose, + const std::vector& waypoints, + KDL::RotationalInterpolation* rot_interpo, + double smoothness, double eqradius) { std::vector filtered_waypoints = filterWaypoints(start_pose, waypoints); double blend_radius = computeBlendRadius(filtered_waypoints, smoothness); @@ -56,8 +56,8 @@ std::unique_ptr PathFreeGenerator::freeFromWaypoints(const KDL::Frame return std::unique_ptr(composite_path); } -std::vector PathFreeGenerator::filterWaypoints(const KDL::Frame& start_pose, - const std::vector& waypoints) +std::vector PathPolylineGenerator::filterWaypoints(const KDL::Frame& start_pose, + const std::vector& waypoints) { std::vector filtered_waypoints = {}; @@ -84,7 +84,7 @@ std::vector PathFreeGenerator::filterWaypoints(const KDL::Frame& sta } return filtered_waypoints; } -double PathFreeGenerator::computeBlendRadius(const std::vector& waypoints_, double smoothness) +double PathPolylineGenerator::computeBlendRadius(const std::vector& waypoints_, double smoothness) { double max_allowed_radius = std::numeric_limits::infinity(); @@ -129,8 +129,8 @@ double PathFreeGenerator::computeBlendRadius(const std::vector& wayp return max_allowed_radius; } -void PathFreeGenerator::checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, - const KDL::Frame& p3) +void PathPolylineGenerator::checkConsecutiveColinearWaypoints(const KDL::Frame& p1, const KDL::Frame& p2, + const KDL::Frame& p3) { KDL::Vector v1 = p2.p - p1.p; KDL::Vector v2 = p3.p - p2.p; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp index c9ef0576ae..67ef057275 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp @@ -33,10 +33,10 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include +#include #include #include -#include +#include #include #include @@ -45,25 +45,25 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit.planners.pilz.planning_context_loader.free"); + return moveit::getLogger("moveit.planners.pilz.planning_context_loader.polyline"); } } // namespace -pilz_industrial_motion_planner::PlanningContextLoaderFree::PlanningContextLoaderFree() +pilz_industrial_motion_planner::PlanningContextLoaderPolyline::PlanningContextLoaderPolyline() { - alg_ = "FREE"; + alg_ = "POLYLINE"; } -pilz_industrial_motion_planner::PlanningContextLoaderFree::~PlanningContextLoaderFree() +pilz_industrial_motion_planner::PlanningContextLoaderPolyline::~PlanningContextLoaderPolyline() { } -bool pilz_industrial_motion_planner::PlanningContextLoaderFree::loadContext( +bool pilz_industrial_motion_planner::PlanningContextLoaderPolyline::loadContext( planning_interface::PlanningContextPtr& planning_context, const std::string& name, const std::string& group) const { if (limits_set_ && model_set_) { - planning_context = std::make_shared(name, group, model_, limits_); + planning_context = std::make_shared(name, group, model_, limits_); return true; } else @@ -81,5 +81,5 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderFree::loadContext( } } -PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderFree, +PLUGINLIB_EXPORT_CLASS(pilz_industrial_motion_planner::PlanningContextLoaderPolyline, pilz_industrial_motion_planner::PlanningContextLoader) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp index e63c655008..1fd5e99e62 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp @@ -33,8 +33,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include +#include #include @@ -64,20 +64,20 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit.planners.pilz.trajectory_generator.free"); + return moveit::getLogger("moveit.planners.pilz.trajectory_generator.polyline"); } } // namespace -TrajectoryGeneratorFree::TrajectoryGeneratorFree(const moveit::core::RobotModelConstPtr& robot_model, - const LimitsContainer& planner_limits, - const std::string& /*group_name*/) +TrajectoryGeneratorPolyline::TrajectoryGeneratorPolyline(const moveit::core::RobotModelConstPtr& robot_model, + const LimitsContainer& planner_limits, + const std::string& /*group_name*/) : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits) { planner_limits_.printCartesianLimits(); } -void TrajectoryGeneratorFree::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, - const planning_interface::MotionPlanRequest& req, - TrajectoryGenerator::MotionPlanInfo& info) const +void TrajectoryGeneratorPolyline::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, + TrajectoryGenerator::MotionPlanInfo& info) const { RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request."); @@ -126,17 +126,18 @@ void TrajectoryGeneratorFree::extractMotionPlanInfo(const planning_scene::Planni computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); } -void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& scene, - const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) +void TrajectoryGeneratorPolyline::plan(const planning_scene::PlanningSceneConstPtr& scene, + const planning_interface::MotionPlanRequest& req, + const MotionPlanInfo& plan_info, double sampling_time, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // set pilz cartesian limits for each item setMaxCartesianSpeed(req); - // create Cartesian FREE path + // create Cartesian POLYLINE path std::unique_ptr path; try { - path = setPathFree(plan_info.start_pose, plan_info.waypoints, req.smoothness_level); + path = setPathPolyline(plan_info.start_pose, plan_info.waypoints, req.smoothness_level); } catch (const KDL::Error_MotionPlanning& e) { @@ -176,11 +177,11 @@ void TrajectoryGeneratorFree::plan(const planning_scene::PlanningSceneConstPtr& } } -std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Affine3d& start_pose, - const std::vector& waypoints, - double smoothness_level) const +std::unique_ptr TrajectoryGeneratorPolyline::setPathPolyline(const Eigen::Affine3d& start_pose, + const std::vector& waypoints, + double smoothness_level) const { - RCLCPP_DEBUG(getLogger(), "Set Cartesian path for FREE command."); + RCLCPP_DEBUG(getLogger(), "Set Cartesian path for POLYLINE command."); KDL::Frame kdl_start_pose; tf2::transformEigenToKDL(start_pose, kdl_start_pose); @@ -198,15 +199,16 @@ std::unique_ptr TrajectoryGeneratorFree::setPathFree(const Eigen::Aff double eqradius = max_cartesian_speed_ / planner_limits_.getCartesianLimits().max_rot_vel; KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); - return PathFreeGenerator::freeFromWaypoints(kdl_start_pose, kdl_waypoints, rot_interpo, smoothness_level, eqradius); + return PathPolylineGenerator::polylineFromWaypoints(kdl_start_pose, kdl_waypoints, rot_interpo, smoothness_level, + eqradius); } -void TrajectoryGeneratorFree::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const +void TrajectoryGeneratorPolyline::cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const { if (req.path_constraints.position_constraints.size() < 2) { std::ostringstream os; - os << "waypoints specified in path constraints is less than 2 for FREE motion."; + os << "waypoints specified in path constraints is less than 2 for POLYLINE motion."; throw NoWaypointsSpecified(os.str()); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md index 867c7f6587..41dffd6de2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md +++ b/moveit_planners/pilz_industrial_motion_planner/test/acceptance_tests/acceptance_test_lin.md @@ -44,7 +44,7 @@ to be moved out of singularities. 2. Run `roslaunch moveit_resources_prbt_moveit_config demo.launch pipeline:=pilz_industrial_motion_planner` 3. In the motion planing widget (lower left part of moveit) choose PTP in the dropdown below "Trapezoidal Command Planner" (see image) ![moveit_1](img/acceptance_test_lin_img1.png) - 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity free position. Click on "plan and execute". + 4. Switch to the tab "Planning" in the moveit planning plugin. Move the ball handle to select a singularity polyline position. Click on "plan and execute". ![moveit_2](img/acceptance_test_lin_img2.png) 5. The motion planning widget (lower left part of moveit) choose LIN in the dropdown below "Trapezoidal Command Planner" (see image) ![moveit_1](img/acceptance_test_lin_img3.png) From d5ce36cc980821d34fda81c3ff26d45d11b11a37 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 1 Dec 2025 17:05:31 +0300 Subject: [PATCH 21/27] change files' names --- ...g_context_loader_free.h => planning_context_loader_polyline.h} | 0 ...ntext_loader_free.hpp => planning_context_loader_polyline.hpp} | 0 .../{planning_context_free.h => planning_context_polyline.h} | 0 .../{planning_context_free.hpp => planning_context_polyline.hpp} | 0 ...rajectory_generator_free.h => trajectory_generator_polyline.h} | 0 ...ctory_generator_free.hpp => trajectory_generator_polyline.hpp} | 0 .../src/{path_free_generator.cpp => path_polyline_generator.cpp} | 0 ...ntext_loader_free.cpp => planning_context_loader_polyline.cpp} | 0 ...ctory_generator_free.cpp => trajectory_generator_polyline.cpp} | 0 9 files changed, 0 insertions(+), 0 deletions(-) rename moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/{planning_context_loader_free.h => planning_context_loader_polyline.h} (100%) rename moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/{planning_context_loader_free.hpp => planning_context_loader_polyline.hpp} (100%) rename moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/{planning_context_free.h => planning_context_polyline.h} (100%) rename moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/{planning_context_free.hpp => planning_context_polyline.hpp} (100%) rename moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/{trajectory_generator_free.h => trajectory_generator_polyline.h} (100%) rename moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/{trajectory_generator_free.hpp => trajectory_generator_polyline.hpp} (100%) rename moveit_planners/pilz_industrial_motion_planner/src/{path_free_generator.cpp => path_polyline_generator.cpp} (100%) rename moveit_planners/pilz_industrial_motion_planner/src/{planning_context_loader_free.cpp => planning_context_loader_polyline.cpp} (100%) rename moveit_planners/pilz_industrial_motion_planner/src/{trajectory_generator_free.cpp => trajectory_generator_polyline.cpp} (100%) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.h similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.h rename to moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.h diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.hpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_free.hpp rename to moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.hpp diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.h similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.h rename to moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.h diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.hpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_free.hpp rename to moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.hpp diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.h similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.h rename to moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.h diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.hpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_free.hpp rename to moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.hpp diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/src/path_free_generator.cpp rename to moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_polyline.cpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_free.cpp rename to moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_polyline.cpp diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_free.cpp rename to moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp From b1df42490abdd67e3c6d9e221069eded67d37212 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 1 Dec 2025 19:48:46 +0300 Subject: [PATCH 22/27] rename --- .../{path_free_generator.h => path_polyline_generator.h} | 0 .../{path_free_generator.hpp => path_polyline_generator.hpp} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/{path_free_generator.h => path_polyline_generator.h} (100%) rename moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/{path_free_generator.hpp => path_polyline_generator.hpp} (100%) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.h similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.h rename to moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.h diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.hpp similarity index 100% rename from moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_free_generator.hpp rename to moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.hpp From 61c8742f7a1427991423e449d1cf7f4f36b8a72d Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Mon, 1 Dec 2025 20:44:07 +0300 Subject: [PATCH 23/27] fix jazzy-ci issue --- .../src/path_polyline_generator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp index df694ae718..5a8df63d56 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp @@ -63,7 +63,7 @@ std::vector PathPolylineGenerator::filterWaypoints(const KDL::Frame& // index for the last add point filtered_waypoints.push_back(start_pose); - size_t last_added_point_indx = -1; // -1 for the start_pose + int last_added_point_indx = -1; // -1 for the start_pose // the following is to remove very close waypoints // to avoid issues in KDL::Path_RoundedComposite like throwing Not_Fesible exceptions From 012059180e33598b366609878e487eed9b2f263d Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Tue, 2 Dec 2025 10:27:15 +0300 Subject: [PATCH 24/27] add test for the cartesian max speed edit algorithms loader to include the 4th POLYLINE COMMAND --- ...nittest_pilz_industrial_motion_planner.cpp | 3 +- .../unittest_trajectory_generator_circ.cpp | 53 +++++++++++++++++++ .../src/unittest_trajectory_generator_lin.cpp | 53 +++++++++++++++++++ 3 files changed, 108 insertions(+), 1 deletion(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp index 33f9e2dc31..f8c0886a78 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_pilz_industrial_motion_planner.cpp @@ -128,7 +128,7 @@ TEST_F(CommandPlannerTest, ObtainLoadedPlanningAlgorithms) // Check for the algorithms std::vector algs; planner_instance_->getPlanningAlgorithms(algs); - ASSERT_EQ(3u, algs.size()) << "Found more or less planning algorithms as expected! Found:" + ASSERT_EQ(4u, algs.size()) << "Found more or less planning algorithms as expected! Found:" << ::testing::PrintToString(algs); // Collect the algorithms, check for uniqueness @@ -141,6 +141,7 @@ TEST_F(CommandPlannerTest, ObtainLoadedPlanningAlgorithms) ASSERT_TRUE(algs_set.find("LIN") != algs_set.end()); ASSERT_TRUE(algs_set.find("PTP") != algs_set.end()); ASSERT_TRUE(algs_set.find("CIRC") != algs_set.end()); + ASSERT_TRUE(algs_set.find("POLYLINE") != algs_set.end()); } /** diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index 33fdb0af0e..31a8645c1a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp @@ -734,6 +734,59 @@ TEST_F(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) checkCircResult(req, res); } +/** + * @brief Set a proper max cartesian speed and check that trajectory is generated + */ +TEST_F(TrajectoryGeneratorCIRCTest, cartesianSpeedLimitProper) +{ + // set max cartesian speed to a positive value + moveit_msgs::msg::MotionPlanRequest circ_req{ tdp_->getCircJointInterimCart("circ3_interim").toRequest() }; + circ_req.max_cartesian_speed = 0.5; + circ_req.cartesian_speed_limited_link = "test_link"; + + // generate circ trajectory + planning_interface::MotionPlanResponse res; + circ_->generate(planning_scene_, circ_req, res); + EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + // checkCircResult(circ_req, res); +} + +/** + * @brief Set a cartesian speed limit to less than or equal zero or send request + * without setting any speed limit + * + * Expected Results: + * 1. Use the default max_trans_vel from limits container + */ +TEST_F(TrajectoryGeneratorCIRCTest, cartesianSpeedLimitLessEqualZero) +{ + // construct motion plan request + moveit_msgs::msg::MotionPlanRequest circ_req{ tdp_->getCircJointInterimCart("circ3_interim").toRequest() }; + // Case 1: don't set any max cartesian speed (set to zero) + // generate circ trajectory + planning_interface::MotionPlanResponse res1; + circ_->generate(this->planning_scene_, circ_req, res1); + EXPECT_TRUE(res1.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + // checkCircResult(circ_req, res1); + + // Case 2: set max cartesian speed to negative value + circ_req.max_cartesian_speed = -1.0; + + // generate circ trajectory + planning_interface::MotionPlanResponse res2; + circ_->generate(this->planning_scene_, circ_req, res2); + EXPECT_TRUE(res2.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + // checkCircResult(circ_req, res2); + // Case 3: set max cartesian speed to zero + circ_req.max_cartesian_speed = 0.0; + + // generate circ trajectory + planning_interface::MotionPlanResponse res3; + circ_->generate(this->planning_scene_, circ_req, res3); + EXPECT_TRUE(res3.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + // checkCircResult(circ_req, res3); +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index 6f049ed9a4..2117c21b09 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -431,6 +431,59 @@ TEST_F(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints) EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); } +/** + * @brief Set a proper max cartesian speed and check that trajectory is generated + */ +TEST_F(TrajectoryGeneratorLINTest, cartesianSpeedLimitProper) +{ + // set max cartesian speed to a positive value + moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + lin_cart_req.max_cartesian_speed = 0.5; + lin_cart_req.cartesian_speed_limited_link = "test_link"; + + // generate lin trajectory + planning_interface::MotionPlanResponse res; + lin_->generate(planning_scene_, lin_cart_req, res); + EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(checkLinResponse(lin_cart_req, res)); +} + +/** + * @brief Set a cartesian speed limit to less than or equal zero or send request + * without setting any speed limit + * + * Expected Results: + * 1. Use the default max_trans_vel from limits container + */ +TEST_F(TrajectoryGeneratorLINTest, cartesianSpeedLimitLessEqualZero) +{ + // construct motion plan request + moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; + // Case 1: don't set any max cartesian speed (set to zero) + // generate lin trajectory + planning_interface::MotionPlanResponse res1; + lin_->generate(this->planning_scene_, lin_cart_req, res1); + EXPECT_TRUE(res1.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(checkLinResponse(lin_cart_req, res1)); + + // Case 2: set max cartesian speed to negative value + lin_cart_req.max_cartesian_speed = -1.0; + + // generate lin trajectory + planning_interface::MotionPlanResponse res2; + lin_->generate(this->planning_scene_, lin_cart_req, res2); + EXPECT_TRUE(res2.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(checkLinResponse(lin_cart_req, res2)); + // Case 3: set max cartesian speed to zero + lin_cart_req.max_cartesian_speed = 0.0; + + // generate lin trajectory + planning_interface::MotionPlanResponse res3; + lin_->generate(this->planning_scene_, lin_cart_req, res3); + EXPECT_TRUE(res3.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + EXPECT_TRUE(checkLinResponse(lin_cart_req, res3)); +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); From ef3294f0ae05064ebeaa982a331b817d0df67886 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Tue, 2 Dec 2025 10:37:48 +0300 Subject: [PATCH 25/27] Revert "Format max_velocity and max_acceleration for YAML output as float" This reverts commit c86e3b2a1a30148aa2ed12a579610708deff7f39. --- .../src/srdf_config.cpp | 32 ++----------------- 1 file changed, 2 insertions(+), 30 deletions(-) diff --git a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp index 066dc9e59f..69002a3ec7 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/srdf_config.cpp @@ -300,21 +300,7 @@ bool SRDFConfig::GeneratedJointLimits::writeYaml(YAML::Emitter& emitter) // Output property emitter << YAML::Key << "max_velocity"; - - // To Solve Putting integer Values like 100 as 100.0 in YAML - double val = std::min(fabs(b.max_velocity_), fabs(b.min_velocity_)); - - // Check if val is a full integer (within floating-point tolerance) - bool is_integer = std::fabs(val - std::round(val)) < 1e-9; - - // Format value as string - std::ostringstream oss; - if (is_integer) - oss << std::fixed << std::setprecision(1) << val; // e.g. 100.0 - else - oss << val; // e.g. 99.75 - - emitter << YAML::Value << oss.str(); + emitter << YAML::Value << static_cast(std::min(fabs(b.max_velocity_), fabs(b.min_velocity_))); // Output property emitter << YAML::Key << "has_acceleration_limits"; @@ -329,21 +315,7 @@ bool SRDFConfig::GeneratedJointLimits::writeYaml(YAML::Emitter& emitter) // Output property emitter << YAML::Key << "max_acceleration"; - - // To Solve Putting integer Values like 100 as 100.0 in YAML - val = std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_)); - - // Check if val is a full integer (within floating-point tolerance) - is_integer = std::fabs(val - std::round(val)) < 1e-9; - - // Format value as string - oss.str(""); // Clear the stringstream - if (is_integer) - oss << std::fixed << std::setprecision(1) << val; // e.g. 100.0 - else - oss << val; // e.g. 99.75 - - emitter << YAML::Value << oss.str(); + emitter << YAML::Value << static_cast(std::min(fabs(b.max_acceleration_), fabs(b.min_acceleration_))); emitter << YAML::EndMap; } From d60d8dd9619d1326809d1439af46ca436c7ee6c2 Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Wed, 3 Dec 2025 19:40:17 +0300 Subject: [PATCH 26/27] fix clang-tidy failures for rolling --- .../src/path_polyline_generator.cpp | 6 +++--- .../src/trajectory_generator.cpp | 2 +- .../src/trajectory_generator_polyline.cpp | 8 ++++++++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp index 5a8df63d56..5ed0caaddd 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/path_polyline_generator.cpp @@ -73,12 +73,12 @@ std::vector PathPolylineGenerator::filterWaypoints(const KDL::Frame& double dist; // add points and skip the points which are too close to each other - for (size_t i = 0; i < waypoints.size(); ++i) + for (const auto& waypoint : waypoints) { - dist = (last_point() - waypoints[i].p).Norm(); + dist = (last_point() - waypoint.p).Norm(); if (dist > MIN_SEGMENT_LENGTH) { - filtered_waypoints.push_back(waypoints[i]); + filtered_waypoints.push_back(waypoint); ++last_added_point_indx; } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 317ab0a018..4c5c40d747 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -382,7 +382,7 @@ TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(const planning_scene::Planni void TrajectoryGenerator::setMaxCartesianSpeed(const moveit_msgs::msg::MotionPlanRequest& req) { - if (req.max_cartesian_speed > 0.0 && req.cartesian_speed_limited_link != "") + if (req.max_cartesian_speed > 0.0 && !req.cartesian_speed_limited_link.empty()) { max_cartesian_speed_ = req.max_cartesian_speed; RCLCPP_INFO(getLogger(), "received max_cartesian_speed: %f", max_cartesian_speed_); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp index 1fd5e99e62..9a14a6020a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_polyline.cpp @@ -145,13 +145,21 @@ void TrajectoryGeneratorPolyline::plan(const planning_scene::PlanningSceneConstP int code = e.GetType(); std::ostringstream os; if (code == 3102 || code == 3103) + { os << "zero distance between two points"; + } else if (code == 3104) + { os << "waypoints specified in path constraints have three consicutive colinear points"; + } else if (code == 3105 || code == 3106) + { os << "rounding circle of a point is bigger than the distance with one of the neighbor points"; + } else if (code == 3001 || code == 3002) + { os << "the rounding radius is lower than KDL::epsilon. use bigger smoothness or resample your waypoints"; + } throw ConsicutiveColinearWaypoints(os.str()); } // create velocity profile From 7ff2294f02a2485407071184ccb9c8e359d845ec Mon Sep 17 00:00:00 2001 From: AimanHaidar Date: Thu, 4 Dec 2025 07:13:05 +0300 Subject: [PATCH 27/27] delete .h files --- .../path_polyline_generator.h | 50 ------------------- .../planning_context_loader_polyline.h | 50 ------------------- .../planning_context_polyline.h | 50 ------------------- .../trajectory_generator_polyline.h | 50 ------------------- 4 files changed, 200 deletions(-) delete mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.h delete mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.h delete mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.h delete mode 100644 moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.h diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.h deleted file mode 100644 index e3796499d2..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/path_polyline_generator.h +++ /dev/null @@ -1,50 +0,0 @@ - -/********************************************************************* - * All MoveIt 2 headers have been updated to use the .hpp extension. - * - * .h headers are now autogenerated via create_deprecated_headers.py, - * and will import the corresponding .hpp with a deprecation warning. - * - * imports via .h files may be removed in future releases, so please - * modify your imports to use the corresponding .hpp imports. - * - * See https://github.com/moveit/moveit2/pull/3113 for extra details. - *********************************************************************/ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018 Pilz GmbH & Co. KG - * Copyright (c) 2025 Aiman Haidar - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Pilz GmbH & Co. KG nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once -#pragma message(".h header is obsolete. Please use the .hpp header instead.") -#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.h deleted file mode 100644 index 29997dc3cf..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader_polyline.h +++ /dev/null @@ -1,50 +0,0 @@ - -/********************************************************************* - * All MoveIt 2 headers have been updated to use the .hpp extension. - * - * .h headers are now autogenerated via create_deprecated_headers.py, - * and will import the corresponding .hpp with a deprecation warning. - * - * imports via .h files may be removed in future releases, so please - * modify your imports to use the corresponding .hpp imports. - * - * See https://github.com/moveit/moveit2/pull/3113 for extra details. - *********************************************************************/ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018 Pilz GmbH & Co. KG - * Copyright (c) 2025 Aiman Haidar - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Pilz GmbH & Co. KG nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once -#pragma message(".h header is obsolete. Please use the .hpp header instead.") -#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.h deleted file mode 100644 index 937eb49860..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_polyline.h +++ /dev/null @@ -1,50 +0,0 @@ - -/********************************************************************* - * All MoveIt 2 headers have been updated to use the .hpp extension. - * - * .h headers are now autogenerated via create_deprecated_headers.py, - * and will import the corresponding .hpp with a deprecation warning. - * - * imports via .h files may be removed in future releases, so please - * modify your imports to use the corresponding .hpp imports. - * - * See https://github.com/moveit/moveit2/pull/3113 for extra details. - *********************************************************************/ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018 Pilz GmbH & Co. KG - * Copyright (c) 2025 Aiman Haidar - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Pilz GmbH & Co. KG nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once -#pragma message(".h header is obsolete. Please use the .hpp header instead.") -#include diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.h deleted file mode 100644 index 53d3ca38ee..0000000000 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_polyline.h +++ /dev/null @@ -1,50 +0,0 @@ - -/********************************************************************* - * All MoveIt 2 headers have been updated to use the .hpp extension. - * - * .h headers are now autogenerated via create_deprecated_headers.py, - * and will import the corresponding .hpp with a deprecation warning. - * - * imports via .h files may be removed in future releases, so please - * modify your imports to use the corresponding .hpp imports. - * - * See https://github.com/moveit/moveit2/pull/3113 for extra details. - *********************************************************************/ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018 Pilz GmbH & Co. KG - * Copyright (c) 2025 Aiman Haidar - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Pilz GmbH & Co. KG nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#pragma once -#pragma message(".h header is obsolete. Please use the .hpp header instead.") -#include