From fcff6769165935866a7270ea5085ed986c92cb58 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 5 Feb 2025 08:43:14 +0100 Subject: [PATCH 1/9] feat: adds first draft of interpolation_parameters --- .../CMakeLists.txt | 9 ++++- .../pilz_industrial_motion_planner.hpp | 4 +++ .../planning_context_base.hpp | 9 +++-- .../planning_context_circ.hpp | 6 ++-- .../planning_context_lin.hpp | 6 ++-- .../planning_context_loader.hpp | 13 +++++++- .../planning_context_ptp.hpp | 6 ++-- .../trajectory_generator.hpp | 7 ++-- .../trajectory_generator_circ.hpp | 2 +- .../trajectory_generator_lin.hpp | 2 +- .../trajectory_generator_ptp.hpp | 2 +- .../src/interpolation_parameters.yaml | 33 +++++++++++++++++++ .../src/pilz_industrial_motion_planner.cpp | 4 +++ .../src/planning_context_loader.cpp | 7 ++++ .../src/planning_context_loader_circ.cpp | 3 +- .../src/planning_context_loader_lin.cpp | 3 +- .../src/planning_context_loader_ptp.cpp | 3 +- .../src/trajectory_generator.cpp | 5 +-- .../src/trajectory_generator_circ.cpp | 13 ++++++-- .../src/trajectory_generator_lin.cpp | 13 ++++++-- .../src/trajectory_generator_ptp.cpp | 5 +-- .../test/test_utils.cpp | 8 +++-- .../src/unittest_planning_context.cpp | 7 ++-- ...t_trajectory_blender_transition_window.cpp | 8 +++-- .../src/unittest_trajectory_generator_lin.cpp | 4 ++- 25 files changed, 147 insertions(+), 35 deletions(-) create mode 100644 moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index e8420aec32..de7ce290bc 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -63,8 +63,14 @@ generate_parameter_library( src/cartesian_limits_parameters.yaml # path to input yaml file ) +generate_parameter_library( + interpolation_parameters # cmake target name for the parameter library + src/interpolation_parameters.yaml # path to input yaml file +) + add_library(planning_context_loader_base SHARED src/planning_context_loader.cpp) -target_link_libraries(planning_context_loader_base cartesian_limits_parameters) +target_link_libraries(planning_context_loader_base cartesian_limits_parameters + interpolation_parameters) ament_target_dependencies(planning_context_loader_base ${THIS_PACKAGE_INCLUDE_DEPENDS}) @@ -148,6 +154,7 @@ pluginlib_export_plugin_description_file( install( TARGETS pilz_industrial_motion_planner cartesian_limits_parameters + interpolation_parameters joint_limits_common planning_context_loader_base planning_context_loader_ptp diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp index bacb5f559e..abb64bf8e5 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/pilz_industrial_motion_planner.hpp @@ -46,6 +46,7 @@ #include #include +#include namespace pilz_industrial_motion_planner { @@ -136,6 +137,9 @@ class CommandPlanner : public planning_interface::PlannerManager /// cartesian limit std::shared_ptr param_listener_; cartesian_limits::Params params_; + + // interpolation params + std::shared_ptr interpolation_param_listener_; }; MOVEIT_CLASS_FORWARD(CommandPlanner); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp index 0abba27e76..5f961c29a1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp @@ -58,11 +58,13 @@ class PlanningContextBase : public planning_interface::PlanningContext public: PlanningContextBase(const std::string& name, const std::string& group, const moveit::core::RobotModelConstPtr& model, - const pilz_industrial_motion_planner::LimitsContainer& limits) + const pilz_industrial_motion_planner::LimitsContainer& limits, + const std::shared_ptr& interpolation_param_listener) : planning_interface::PlanningContext(name, group) , terminated_(false) , model_(model) , limits_(limits) + , interpolation_param_listener_(interpolation_param_listener) , generator_(model, limits_, group) { } @@ -112,6 +114,9 @@ class PlanningContextBase : public planning_interface::PlanningContext /// Joint limits to be used during planning pilz_industrial_motion_planner::LimitsContainer limits_; + /// Listener for interpolation parameters + std::shared_ptr interpolation_param_listener_; + protected: GeneratorT generator_; }; @@ -126,7 +131,7 @@ void pilz_industrial_motion_planner::PlanningContextBase::solve(plan res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return; } - generator_.generate(getPlanningScene(), request_, res); + generator_.generate(getPlanningScene(), request_, res, interpolation_param_listener_->get_params()); } template diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp index e0b975c6f0..4b608784be 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_circ.hpp @@ -58,8 +58,10 @@ class PlanningContextCIRC : public pilz_industrial_motion_planner::PlanningConte { public: PlanningContextCIRC(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) + const pilz_industrial_motion_planner::LimitsContainer& limits, + const std::shared_ptr& interpolation_param_listener) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits, + interpolation_param_listener) { } }; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp index 865febf5c6..dcab786ed2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_lin.hpp @@ -58,8 +58,10 @@ class PlanningContextLIN : public pilz_industrial_motion_planner::PlanningContex { 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) + const pilz_industrial_motion_planner::LimitsContainer& limits, + const std::shared_ptr& interpolation_param_listener) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits, + interpolation_param_listener) { } }; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp index 5d93a34d14..3c266384c3 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_loader.hpp @@ -42,6 +42,7 @@ #include #include +#include namespace pilz_industrial_motion_planner { @@ -75,6 +76,13 @@ class PlanningContextLoader */ virtual bool setLimits(const pilz_industrial_motion_planner::LimitsContainer& limits); + /** + * @brief Set the listener for interpolation parameters + * @param param_listener + * @return true on success, false otherwise + */ + virtual bool setInterpolationParamListener(const std::shared_ptr& param_listener); + /** * @brief Return the planning context * @param planning_context @@ -112,6 +120,9 @@ class PlanningContextLoader /// The robot model moveit::core::RobotModelConstPtr model_; + + /// Listener for interpolation parameters + std::shared_ptr interpolation_param_listener_; }; typedef std::shared_ptr PlanningContextLoaderPtr; @@ -123,7 +134,7 @@ bool PlanningContextLoader::loadContext(planning_interface::PlanningContextPtr& { if (limits_set_ && model_set_) { - planning_context = std::make_shared(name, group, model_, limits_); + planning_context = std::make_shared(name, group, model_, limits_, interpolation_param_listener_); return true; } else diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp index 994020aad7..7260974c9c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_ptp.hpp @@ -58,8 +58,10 @@ class PlanningContextPTP : public pilz_industrial_motion_planner::PlanningContex { public: PlanningContextPTP(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) + const pilz_industrial_motion_planner::LimitsContainer& limits, + const std::shared_ptr& interpolation_param_listener) + : pilz_industrial_motion_planner::PlanningContextBase(name, group, model, limits, + interpolation_param_listener) { } }; 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..255845d12f 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 @@ -48,6 +48,7 @@ #include #include #include +#include namespace pilz_industrial_motion_planner { @@ -107,7 +108,8 @@ class TrajectoryGenerator * @param sampling_time: sampling time of the generate trajectory */ void generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, double sampling_time = 0.1); + planning_interface::MotionPlanResponse& res, + const interpolation::Params& interpolation_params = interpolation::Params()); protected: /** @@ -157,7 +159,8 @@ class TrajectoryGenerator virtual 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) = 0; + const interpolation::Params& interpolation_params, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; private: /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp index c73c89a5fa..513b02e68a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.hpp @@ -89,7 +89,7 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator 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, + const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp index 3c301b61e3..fc6aea2494 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.hpp @@ -73,7 +73,7 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator 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, + const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp index f4514c123f..ccb6a9922e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.hpp @@ -83,7 +83,7 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator double acceleration_scaling_factor, double sampling_time); void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, double sampling_time, + const MotionPlanInfo& plan_info, const interpolation::Params& interpolation_params, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; private: diff --git a/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml new file mode 100644 index 0000000000..76ca746076 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml @@ -0,0 +1,33 @@ +interpolation: + max_sample_time: { + type: double, + default_value: 0.1, + description: "Max time interval between two consecutive samples", + validation: { + gt: [ 0.0 ] + } + } + min_number_of_samples: { + type: int, + default_value: 0, + description: "Minimum number of samples for the generated trajectory", + validation: { + gt_eq: [ 0.0 ] + } + } + max_translation_interpolation_distance: { + type: double, + default_value: 10000000.0, + description: "Max translation distance between two consecutive samples", + validation: { + gt: [ 0.0 ] + } + } + max_rotation_interpolation_distance: { + type: double, + default_value: 10000000.0, + description: "Max rotation distance between two consecutive samples", + validation: { + gt: [ 0.0 ] + } + } \ No newline at end of file diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index c3c3e83034..05defd0717 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -80,6 +80,9 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c std::make_shared(node, PARAM_NAMESPACE_LIMITS + ".cartesian_limits"); params_ = param_listener_->get_params(); + interpolation_param_listener_ = + std::make_shared(node, PARAM_NAMESPACE_LIMITS + ".interpolation"); + // Load the planning context loader planner_context_loader_ = std::make_unique>( "pilz_industrial_motion_planner", "pilz_industrial_motion_planner::PlanningContextLoader"); @@ -106,6 +109,7 @@ bool CommandPlanner::initialize(const moveit::core::RobotModelConstPtr& model, c loader_pointer->setLimits(limits); loader_pointer->setModel(model_); + loader_pointer->setInterpolationParamListener(interpolation_param_listener_); registerContextLoader(loader_pointer); } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp index 0d575fe496..ab175155d9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader.cpp @@ -57,6 +57,13 @@ bool pilz_industrial_motion_planner::PlanningContextLoader::setLimits( return true; } +bool pilz_industrial_motion_planner::PlanningContextLoader::setInterpolationParamListener( + const std::shared_ptr& param_listener) +{ + interpolation_param_listener_ = param_listener; + return true; +} + std::string pilz_industrial_motion_planner::PlanningContextLoader::getAlgorithm() const { return alg_; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp index 1ab4ab0471..b9a38002ec 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp @@ -65,7 +65,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderCIRC::loadContext( { if (limits_set_ && model_set_) { - planning_context = std::make_shared(name, group, model_, limits_); + planning_context = + std::make_shared(name, group, model_, limits_, interpolation_param_listener_); return true; } else diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp index a3c261f9be..e5e6982f9f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp @@ -62,7 +62,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderLIN::loadContext( { if (limits_set_ && model_set_) { - planning_context = std::make_shared(name, group, model_, limits_); + planning_context = + std::make_shared(name, group, model_, limits_, interpolation_param_listener_); return true; } else diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp index 905ee08237..07a378f2a8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp @@ -61,7 +61,8 @@ bool pilz_industrial_motion_planner::PlanningContextLoaderPTP::loadContext( { if (limits_set_ && model_set_) { - planning_context = std::make_shared(name, group, model_, limits_); + planning_context = + std::make_shared(name, group, model_, limits_, interpolation_param_listener_); return true; } else 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..a659a6237c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -299,7 +299,8 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(double max_velocity_scaling_fa void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - planning_interface::MotionPlanResponse& res, double sampling_time) + planning_interface::MotionPlanResponse& res, + const interpolation::Params& interpolation_params) { RCLCPP_INFO_STREAM(getLogger(), "Generating " << req.planner_id << " trajectory..."); rclcpp::Time planning_begin = clock_->now(); @@ -345,7 +346,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& trajectory_msgs::msg::JointTrajectory joint_trajectory; try { - plan(plan_info.start_scene, req, plan_info, sampling_time, joint_trajectory); + plan(plan_info.start_scene, req, plan_info, interpolation_params, joint_trajectory); } catch (const MoveItErrorCodeException& ex) { 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..3984c91704 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 @@ -200,7 +200,8 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + const interpolation::Params& interpolation_params, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) { std::unique_ptr cart_path(setPathCIRC(plan_info)); std::unique_ptr vel_profile( @@ -215,9 +216,15 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& moveit_msgs::msg::MoveItErrorCodes error_code; // sample the Cartesian trajectory and compute joint trajectory using inverse // kinematics + auto cartesian_limits = planner_limits_.getCartesianLimits(); + auto st = std::min({ interpolation_params.max_sample_time, + interpolation_params.max_translation_interpolation_distance / + (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor), + interpolation_params.max_rotation_interpolation_distance / + (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) }); + RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", st); 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)) + plan_info.link_name, plan_info.start_joint_position, st, joint_trajectory, error_code)) { throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path", error_code.val); 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..42a124ef29 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 @@ -144,7 +144,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + const interpolation::Params& interpolation_params, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // create Cartesian path for lin std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); @@ -162,9 +163,15 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s moveit_msgs::msg::MoveItErrorCodes error_code; // sample the Cartesian trajectory and compute joint trajectory using inverse // kinematics + auto cartesian_limits = planner_limits_.getCartesianLimits(); + auto st = std::min({ interpolation_params.max_sample_time, + interpolation_params.max_translation_interpolation_distance / + (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor), + interpolation_params.max_rotation_interpolation_distance / + (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) }); + RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", st); 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)) + plan_info.link_name, plan_info.start_joint_position, st, joint_trajectory, error_code)) { std::ostringstream os; os << "Failed to generate valid joint trajectory from the Cartesian path"; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index 6113b603eb..2284bd157a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -265,11 +265,12 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /*scene*/, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + const interpolation::Params& interpolation_params, + trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // plan the ptp trajectory planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, - req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time); + req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, interpolation_params.max_sample_time); } } // namespace pilz_industrial_motion_planner diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 24acb2b0a2..ef21140538 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -1076,7 +1076,9 @@ bool testutils::generateTrajFromBlendTestData( goal_state_1, goal_state_1.getRobotModel()->getJointModelGroup(group_name))); // trajectory generation - tg->generate(scene, req_1, res_1, sampling_time_1); + interpolation::Params interpolation_params; + interpolation_params.max_sample_time = sampling_time_1; + tg->generate(scene, req_1, res_1, interpolation_params); if (res_1.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { std::cout << "Failed to generate first trajectory." << '\n'; @@ -1096,8 +1098,10 @@ bool testutils::generateTrajFromBlendTestData( goal_state_2.setJointGroupPositions(group_name, data.end_position); req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints( goal_state_2, goal_state_2.getRobotModel()->getJointModelGroup(group_name))); + + interpolation_params.max_sample_time = sampling_time_2; // trajectory generation - tg->generate(scene, req_2, res_2, sampling_time_2); + tg->generate(scene, req_2, res_2, interpolation_params); if (res_2.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { std::cout << "Failed to generate second trajectory." << '\n'; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index 48dfe23752..b3bb22f7b0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -118,8 +118,11 @@ class PlanningContextTest : public ::testing::Test limits.setJointLimits(joint_limits); limits.setCartesianLimits(cartesian_limit); - planning_context_ = std::unique_ptr( - new typename T::Type_("TestPlanningContext", planning_group_, robot_model_, limits)); + auto interpolation_param_listener = + std::make_shared(node_, "robot_description_planning.interpolation"); + + planning_context_ = std::unique_ptr(new typename T::Type_( + "TestPlanningContext", planning_group_, robot_model_, limits, interpolation_param_listener)); // Define and set the current scene auto scene = std::make_shared(robot_model_); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 63f65f9903..8fbd8461c8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -145,9 +145,11 @@ class TrajectoryBlenderTransitionWindowTest : public testing::Test { moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory->getLastWayPoint(), req.start_state); } + interpolation::Params interpolation_params; + interpolation_params.max_sample_time = sampling_time_; // generate trajectory planning_interface::MotionPlanResponse resp; - lin_generator_->generate(planning_scene_, req, resp, sampling_time_); + lin_generator_->generate(planning_scene_, req, resp, interpolation_params); if (resp.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { std::runtime_error("Failed to generate trajectory."); @@ -325,8 +327,10 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) sampling_time_ *= 2; } // generate trajectory + interpolation::Params interpolation_params; + interpolation_params.max_sample_time = sampling_time_; planning_interface::MotionPlanResponse resp; - lin_generator_->generate(planning_scene_, req, resp, sampling_time_); + lin_generator_->generate(planning_scene_, req, resp, interpolation_params); if (resp.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { std::runtime_error("Failed to generate trajectory."); 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..0b8c7b41a4 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 @@ -292,7 +292,9 @@ TEST_F(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) /// + plan LIN trajectory + /// +++++++++++++++++++++++ planning_interface::MotionPlanResponse res; - lin_->generate(planning_scene_, lin_joint_req, res, 0.01); + interpolation::Params interpolation_params; + interpolation_params.max_sample_time = 0.01; + lin_->generate(planning_scene_, lin_joint_req, res, interpolation_params); EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory, target_link_hcd_)); From 3511d7c82fafcb6370df55485a63b8bc7a841e55 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Thu, 6 Feb 2025 14:48:21 +0100 Subject: [PATCH 2/9] fix: `interpolation.min_number_of_samples` correct type to `int` --- .../src/interpolation_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml index 76ca746076..18e7bdd45c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml +++ b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml @@ -12,7 +12,7 @@ interpolation: default_value: 0, description: "Minimum number of samples for the generated trajectory", validation: { - gt_eq: [ 0.0 ] + gt_eq: [ 0 ] } } max_translation_interpolation_distance: { From eb933e514d667ef9dbf9164a927ec84e172c9790 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Thu, 6 Feb 2025 15:51:02 +0100 Subject: [PATCH 3/9] fix: removes last trajectory segment with different sampling time. this ensures that the new sample time will be lowered, thus not violating precomputed distance interpolation constraints at the same time avoids the issue on the last point generating small displacement interpolation --- .../src/trajectory_functions.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 01a7f11b59..4cd0ab23d8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -217,11 +217,12 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( rclcpp::Time generation_begin = clock.now(); // generate the time samples - const double epsilon = 10e-06; // avoid adding the last time sample twice std::vector time_samples; - for (double t_sample = 0.0; t_sample < trajectory.Duration() - epsilon; t_sample += sampling_time) + int num_samples = std::floor(trajectory.Duration() / sampling_time); + sampling_time = trajectory.Duration() / num_samples; + for (int i = 0; i < num_samples; ++i) { - time_samples.push_back(t_sample); + time_samples.push_back(i * sampling_time); } time_samples.push_back(trajectory.Duration()); From 8271d5685695eddbcbf4e07f42a0e22435882045 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Sat, 8 Feb 2025 10:42:31 +0100 Subject: [PATCH 4/9] feat: remove requirements on const and equal sampling_time on blended trajectories from `pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow` --- .../trajectory_blender_transition_window.hpp | 7 +- .../src/interpolation_parameters.yaml | 2 +- .../trajectory_blender_transition_window.cpp | 141 +++++++++++--- .../test/test_utils.cpp | 58 +++--- ...t_trajectory_blender_transition_window.cpp | 175 +++++++++--------- 5 files changed, 236 insertions(+), 147 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.hpp index 79672d8b00..bbfdad21b1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.hpp @@ -101,12 +101,10 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender /** * @brief validate trajectory blend request * @param req - * @param sampling_time: get the same sampling time of the two input - * trajectories * @param error_code * @return */ - bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, + bool validateRequest(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, moveit_msgs::msg::MoveItErrorCodes& error_code) const; /** * @brief searchBlendPoint @@ -164,13 +162,12 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender * @param first_interse_index * @param second_interse_index * @param blend_begin_index - * @param sampling_time * @param trajectory: the resulting blend trajectory inside the blending * sphere */ void blendTrajectoryCartesian(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, const std::size_t first_interse_index, const std::size_t second_interse_index, - const std::size_t blend_align_index, double sampling_time, + const std::size_t blend_align_index, pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const; private: // static members diff --git a/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml index 18e7bdd45c..44afc02d60 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml +++ b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml @@ -30,4 +30,4 @@ interpolation: validation: { gt: [ 0.0 ] } - } \ No newline at end of file + } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 41deb7ccd7..11ebf34d3b 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -56,8 +56,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( { RCLCPP_INFO(getLogger(), "Start trajectory blending using transition window."); - double sampling_time = 0.; - if (!validateRequest(req, sampling_time, res.error_code)) + if (!validateRequest(req, res.error_code)) { RCLCPP_ERROR(getLogger(), "Trajectory blend request is not valid."); return false; @@ -82,7 +81,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( // blend the trajectories in Cartesian space pilz_industrial_motion_planner::CartesianTrajectory blend_trajectory_cartesian; - blendTrajectoryCartesian(req, first_intersection_index, second_intersection_index, blend_align_index, sampling_time, + blendTrajectoryCartesian(req, first_intersection_index, second_intersection_index, blend_align_index, blend_trajectory_cartesian); // generate the blending trajectory in joint space @@ -135,15 +134,35 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( req.second_trajectory->getWayPointDurationFromPrevious(i)); } - // adjust the time from start - res.second_trajectory->setWayPointDurationFromPrevious(0, sampling_time); - + // adjust the time from start of the first second trajectory point to ensure continuity of velocities (this implicitly + // ensures continuity of accelerations) + std::vector time_offsets; + auto second_start = res.second_trajectory->getFirstWayPointPtr(); + auto blend_end = res.blend_trajectory->getLastWayPointPtr(); + + std::vector blend_end_positions, second_start_positions; + blend_end->copyJointGroupPositions(req.first_trajectory->getGroup()->getName(), blend_end_positions); + second_start->copyJointGroupPositions(req.first_trajectory->getGroup()->getName(), second_start_positions); + std::vector second_start_velocities; + second_start->copyJointGroupVelocities(req.first_trajectory->getGroup()->getName(), second_start_velocities); + double time_from_start = 0.0; + for (std::size_t i = 0; i < second_start_velocities.size(); ++i) + { + if (second_start_velocities[i] == 0) + { + continue; + } + time_from_start = (second_start_positions[i] - blend_end_positions[i]) / second_start_velocities[i]; + time_offsets.push_back(time_from_start); + } + time_from_start = *std::max_element(time_offsets.begin(), time_offsets.end()); + res.second_trajectory->setWayPointDurationFromPrevious(0, time_from_start); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return true; } bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validateRequest( - const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time, + const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, moveit_msgs::msg::MoveItErrorCodes& error_code) const { RCLCPP_DEBUG(getLogger(), "Validate the trajectory blend request."); @@ -183,14 +202,6 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate return false; } - // same uniform sampling time - if (!pilz_industrial_motion_planner::determineAndCheckSamplingTime(req.first_trajectory, req.second_trajectory, - EPSILON, sampling_time)) - { - error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; - return false; - } - // end position of the first trajectory and start position of second // trajectory must have zero // velocities/accelerations @@ -207,15 +218,74 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate return true; } +// TODO: improve this method +Eigen::Isometry3d interpolatePose(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& link_name, + double time) +{ + // Find the closest two waypoints surrounding the requested time + std::size_t idx_before = 0, idx_after = 0; + for (std::size_t i = 0; i < trajectory->getWayPointCount() - 1; ++i) + { + double t1 = trajectory->getWayPointDurationFromStart(i); + double t2 = trajectory->getWayPointDurationFromStart(i + 1); + + if (t1 <= time && t2 >= time) + { + idx_before = i; + idx_after = i + 1; + break; + } + } + + // If time is outside known waypoints, return the closest available pose + if (idx_after == 0) + { + return trajectory->getWayPoint(0).getFrameTransform(link_name); + } + if (idx_before == trajectory->getWayPointCount() - 1) + { + return trajectory->getWayPoint(idx_before).getFrameTransform(link_name); + } + + // Get timestamps and transformations + double t1 = trajectory->getWayPointDurationFromStart(idx_before); + double t2 = trajectory->getWayPointDurationFromStart(idx_after); + Eigen::Isometry3d pose1 = trajectory->getWayPoint(idx_before).getFrameTransform(link_name); + Eigen::Isometry3d pose2 = trajectory->getWayPoint(idx_after).getFrameTransform(link_name); + + // Compute interpolation factor + double lambda = (time - t1) / (t2 - t1); + + // Linear interpolation for position + Eigen::Isometry3d interpolated_pose; + interpolated_pose.translation() = pose1.translation() + lambda * (pose2.translation() - pose1.translation()); + + // SLERP interpolation for rotation + Eigen::Quaterniond quat1(pose1.rotation()); + Eigen::Quaterniond quat2(pose2.rotation()); + interpolated_pose.linear() = quat1.slerp(lambda, quat2).toRotationMatrix(); + + return interpolated_pose; +} + void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTrajectoryCartesian( const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, const std::size_t first_interse_index, - const std::size_t second_interse_index, const std::size_t blend_align_index, double sampling_time, + const std::size_t second_interse_index, const std::size_t /* blend_align_index */, pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const { // other fields of the trajectory trajectory.group_name = req.group_name; trajectory.link_name = req.link_name; + // Start time of the blending phase + double t_start = req.first_trajectory->getWayPointDurationFromStart(first_interse_index); + // Duration of the blending phase on the first trajectory + double d1 = req.first_trajectory->getDuration() - t_start; + // Duration of the blending phase on the second trajectory + double d2 = req.second_trajectory->getWayPointDurationFromStart(second_interse_index); + // Time to align the two trajectories + double align_time = (d1 > d2) ? req.first_trajectory->getDuration() - d2 : t_start; + // Pose on first trajectory Eigen::Isometry3d blend_sample_pose1 = req.first_trajectory->getWayPoint(first_interse_index).getFrameTransform(req.link_name); @@ -225,28 +295,36 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra req.second_trajectory->getWayPoint(second_interse_index).getFrameTransform(req.link_name); // blend the trajectory - double blend_sample_num = second_interse_index + blend_align_index - first_interse_index + 1; + double blend_duration = d2 + align_time - t_start; pilz_industrial_motion_planner::CartesianTrajectoryPoint waypoint; blend_sample_pose2 = req.second_trajectory->getFirstWayPoint().getFrameTransform(req.link_name); // Pose on blending trajectory Eigen::Isometry3d blend_sample_pose; - for (std::size_t i = 0; i < blend_sample_num; ++i) + + // Define an arbitrary small sample time to sample the blending trajectory + double sampling_time = 0.01; + + int num_samples = std::floor(blend_duration / sampling_time); + sampling_time = blend_duration / num_samples; + + double blend_time = 0.0; + Eigen::Isometry3d last_blend_sample_pose = blend_sample_pose1; + while (blend_time <= blend_duration) { // if the first trajectory does not reach the last sample, update - if ((first_interse_index + i) < req.first_trajectory->getWayPointCount()) + if ((t_start + blend_time) < req.first_trajectory->getDuration()) { - blend_sample_pose1 = req.first_trajectory->getWayPoint(first_interse_index + i).getFrameTransform(req.link_name); + blend_sample_pose1 = interpolatePose(req.first_trajectory, req.link_name, t_start + blend_time); } // if after the alignment, the second trajectory starts, update - if ((first_interse_index + i) > blend_align_index) + if ((t_start + blend_time) >= align_time) { - blend_sample_pose2 = req.second_trajectory->getWayPoint(first_interse_index + i - blend_align_index) - .getFrameTransform(req.link_name); + blend_sample_pose2 = interpolatePose(req.second_trajectory, req.link_name, (t_start + blend_time) - align_time); } - double s = (i + 1) / blend_sample_num; + double s = (blend_time + sampling_time) / blend_duration; double alpha = 6 * std::pow(s, 5) - 15 * std::pow(s, 4) + 10 * std::pow(s, 3); // blend the translation @@ -258,9 +336,22 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra Eigen::Quaterniond end_quat(blend_sample_pose2.rotation()); blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix(); + blend_time += sampling_time; + // Ensures samples are far enough apart to avoid numerical issues in numerical inverse kinematics + if (((blend_sample_pose.translation() - last_blend_sample_pose.translation()).norm() < EPSILON) && + (blend_sample_pose.rotation().isApprox(last_blend_sample_pose.rotation(), 1e-3))) + { + continue; + } + + // std::cout << "blend_time: " << blend_time << std::endl; + + // Store the last insert pose + last_blend_sample_pose = blend_sample_pose; + // push to the trajectory waypoint.pose = tf2::toMsg(blend_sample_pose); - waypoint.time_from_start = rclcpp::Duration::from_seconds((i + 1.0) * sampling_time); + waypoint.time_from_start = rclcpp::Duration::from_seconds(blend_time); trajectory.points.push_back(waypoint); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index ef21140538..d56dc47cb6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -634,35 +634,35 @@ bool testutils::checkOriginalTrajectoryAfterBlending(const pilz_industrial_motio } // check time from start - if (i < size_second - 1) - { - if (fabs((res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) - - res.second_trajectory->getWayPointDurationFromStart(size_second - i - 2)) - - (req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - - req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2))) > time_tolerance) - { - std::cout << size_second - i - 1 << "th time from start of the second trajectory is not same." - << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) << ", " - << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 2) << ", " - << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) << ", " - << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2) << '\n'; - return false; - } - } - else - { - if (fabs((res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1)) - - (req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - - req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2))) > time_tolerance) - { - std::cout << size_second - i - 1 << "th time from start of the second trajectory is not same." - << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) << ", " - << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - - req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2) - << '\n'; - return false; - } - } + // if (i < size_second - 1) + // { + // if (fabs((res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) - + // res.second_trajectory->getWayPointDurationFromStart(size_second - i - 2)) - + // (req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + // req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2))) > time_tolerance) + // { + // std::cout << size_second - i - 1 << "th time from start of the second trajectory is not same." + // << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) << ", " + // << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 2) << ", " + // << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) << ", " + // << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2) << '\n'; + // return false; + // } + // } + // else + // { + // if (fabs((res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1)) - + // (req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + // req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2))) > time_tolerance) + // { + // std::cout << size_second - i - 1 << "th time from start of the second trajectory is not same." + // << res.second_trajectory->getWayPointDurationFromStart(size_second - i - 1) << ", " + // << req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 1) - + // req.second_trajectory->getWayPointDurationFromStart(size_second_original - i - 2) + // << '\n'; + // return false; + // } + // } } return true; diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp index 8fbd8461c8..22b6eebd34 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_blender_transition_window.cpp @@ -296,93 +296,93 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testZeroRadius) EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); } -/** - * @brief Tests the blending of two trajectories with differenent sampling - * times. - * - * Test Sequence: - * 1. Generate two linear trajectories with different sampling times. - * 2. Try to generate blending trajectory. - * - * Expected Results: - * 1. Two linear trajectories generated. - * 2. Blending trajectory cannot be generated. - */ -TEST_F(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) -{ - Sequence seq{ data_loader_->getSequence("SimpleSequence") }; - - // perform lin trajectory generation and modify sampling time - std::size_t num_cmds{ 2 }; - std::vector responses(num_cmds); - - for (size_t index = 0; index < num_cmds; ++index) - { - planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() }; - // Set start state of request to end state of previous trajectory (except - // for first) - if (index > 0) - { - moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory->getLastWayPoint(), req.start_state); - sampling_time_ *= 2; - } - // generate trajectory - interpolation::Params interpolation_params; - interpolation_params.max_sample_time = sampling_time_; - planning_interface::MotionPlanResponse resp; - lin_generator_->generate(planning_scene_, req, resp, interpolation_params); - if (resp.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - std::runtime_error("Failed to generate trajectory."); - } - responses.at(index) = resp; - } - - pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; - pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; - - blend_req.group_name = planning_group_; - blend_req.link_name = target_link_; - blend_req.first_trajectory = responses[0].trajectory; - blend_req.second_trajectory = responses[1].trajectory; - blend_req.blend_radius = seq.getBlendRadius(0); - EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); -} - -/** - * @brief Tests the blending of two trajectories with one trajectory - * having non-uniform sampling time (apart from the last sample, - * which is ignored). - * - * Test Sequence: - * 1. Generate two linear trajectories and corrupt uniformity of sampling - * time. - * 2. Try to generate blending trajectory. - * - * Expected Results: - * 1. Two linear trajectories generated. - * 2. Blending trajectory cannot be generated. - */ -TEST_F(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) -{ - Sequence seq{ data_loader_->getSequence("SimpleSequence") }; - - std::vector res{ generateLinTrajs(seq, 2) }; - - // Modify first time interval - EXPECT_GT(res[0].trajectory->getWayPointCount(), 2u); - res[0].trajectory->setWayPointDurationFromPrevious(1, 2 * sampling_time_); - - pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; - pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; - - blend_req.group_name = planning_group_; - blend_req.link_name = target_link_; - blend_req.first_trajectory = res.at(0).trajectory; - blend_req.second_trajectory = res.at(1).trajectory; - blend_req.blend_radius = seq.getBlendRadius(0); - EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); -} +// /** +// * @brief Tests the blending of two trajectories with differenent sampling +// * times. +// * +// * Test Sequence: +// * 1. Generate two linear trajectories with different sampling times. +// * 2. Try to generate blending trajectory. +// * +// * Expected Results: +// * 1. Two linear trajectories generated. +// * 2. Blending trajectory cannot be generated. +// */ +// TEST_F(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes) +// { +// Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + +// // perform lin trajectory generation and modify sampling time +// std::size_t num_cmds{ 2 }; +// std::vector responses(num_cmds); + +// for (size_t index = 0; index < num_cmds; ++index) +// { +// planning_interface::MotionPlanRequest req{ seq.getCmd(index).toRequest() }; +// // Set start state of request to end state of previous trajectory (except +// // for first) +// if (index > 0) +// { +// moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory->getLastWayPoint(), req.start_state); +// sampling_time_ *= 2; +// } +// // generate trajectory +// interpolation::Params interpolation_params; +// interpolation_params.max_sample_time = sampling_time_; +// planning_interface::MotionPlanResponse resp; +// lin_generator_->generate(planning_scene_, req, resp, interpolation_params); +// if (resp.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) +// { +// std::runtime_error("Failed to generate trajectory."); +// } +// responses.at(index) = resp; +// } + +// pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; +// pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + +// blend_req.group_name = planning_group_; +// blend_req.link_name = target_link_; +// blend_req.first_trajectory = responses[0].trajectory; +// blend_req.second_trajectory = responses[1].trajectory; +// blend_req.blend_radius = seq.getBlendRadius(0); +// EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); +// } + +// /** +// * @brief Tests the blending of two trajectories with one trajectory +// * having non-uniform sampling time (apart from the last sample, +// * which is ignored). +// * +// * Test Sequence: +// * 1. Generate two linear trajectories and corrupt uniformity of sampling +// * time. +// * 2. Try to generate blending trajectory. +// * +// * Expected Results: +// * 1. Two linear trajectories generated. +// * 2. Blending trajectory cannot be generated. +// */ +// TEST_F(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime) +// { +// Sequence seq{ data_loader_->getSequence("SimpleSequence") }; + +// std::vector res{ generateLinTrajs(seq, 2) }; + +// // Modify first time interval +// EXPECT_GT(res[0].trajectory->getWayPointCount(), 2u); +// res[0].trajectory->setWayPointDurationFromPrevious(1, 2 * sampling_time_); + +// pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req; +// pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res; + +// blend_req.group_name = planning_group_; +// blend_req.link_name = target_link_; +// blend_req.first_trajectory = res.at(0).trajectory; +// blend_req.second_trajectory = res.at(1).trajectory; +// blend_req.blend_radius = seq.getBlendRadius(0); +// EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res)); +// } /** * @brief Tests the blending of two trajectories which do not intersect. @@ -700,6 +700,7 @@ TEST_F(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending) std::runtime_error("Failed to generate trajectory."); } + joint_traj.points.front().time_from_start = rclcpp::Duration::from_seconds(0.0); joint_traj.points.back().velocities.assign(joint_traj.points.back().velocities.size(), 0.0); joint_traj.points.back().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0); From a16f9b0fda35ae2bc12d766e95ab11ff3956c40d Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Sun, 9 Feb 2025 11:39:39 +0100 Subject: [PATCH 5/9] fix: TrajectoryBlenderTransitionWindow - computes shift of the second_trajectory using average instead of max. (this shift may be exactly the sample_time of the second trajectory if uniform) - reduces sampling_time that approximates continuous time to 0.001 increasing the minimum distance between points to 1mm - ensures the addition of the first and the last points Tests: - adapts `checkBlendingJointSpaceContinuity` and `checkBlendingCartSpaceContinuity` acceleration computations to take into account different time samples among the trajectories - fixes occurrences of `joint_velocity_tolerance` and `joint_accleration_tolerance` in `checkBlendingJointSpaceContinuity` - adjust `checkBlendingCartSpaceContinuity` to return False in case of failed checks --- .../trajectory_blender_transition_window.cpp | 39 ++++++---- .../src/trajectory_functions.cpp | 1 + .../test/test_utils.cpp | 73 ++++++++++++------- 3 files changed, 73 insertions(+), 40 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 11ebf34d3b..232896b200 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -136,7 +136,6 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( // adjust the time from start of the first second trajectory point to ensure continuity of velocities (this implicitly // ensures continuity of accelerations) - std::vector time_offsets; auto second_start = res.second_trajectory->getFirstWayPointPtr(); auto blend_end = res.blend_trajectory->getLastWayPointPtr(); @@ -146,17 +145,22 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( std::vector second_start_velocities; second_start->copyJointGroupVelocities(req.first_trajectory->getGroup()->getName(), second_start_velocities); double time_from_start = 0.0; + std::size_t non_zero_velocity_count = 0; for (std::size_t i = 0; i < second_start_velocities.size(); ++i) { - if (second_start_velocities[i] == 0) + if (second_start_velocities[i] != 0) { - continue; + time_from_start += (second_start_positions[i] - blend_end_positions[i]) / second_start_velocities[i]; + ++non_zero_velocity_count; } - time_from_start = (second_start_positions[i] - blend_end_positions[i]) / second_start_velocities[i]; - time_offsets.push_back(time_from_start); } - time_from_start = *std::max_element(time_offsets.begin(), time_offsets.end()); - res.second_trajectory->setWayPointDurationFromPrevious(0, time_from_start); + if (non_zero_velocity_count > 0) + { + RCLCPP_INFO_STREAM(getLogger(), + "Adjusting time from start of second trajectory to ensure velocity continuity. delta_time: " + << time_from_start / non_zero_velocity_count); + res.second_trajectory->setWayPointDurationFromPrevious(0, time_from_start / non_zero_velocity_count); + } res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return true; } @@ -303,17 +307,23 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra Eigen::Isometry3d blend_sample_pose; // Define an arbitrary small sample time to sample the blending trajectory - double sampling_time = 0.01; + double sampling_time = 0.001; int num_samples = std::floor(blend_duration / sampling_time); sampling_time = blend_duration / num_samples; double blend_time = 0.0; Eigen::Isometry3d last_blend_sample_pose = blend_sample_pose1; - while (blend_time <= blend_duration) + + // Add the first point + double time_offset = req.first_trajectory->getWayPointDurationFromPrevious(first_interse_index); + waypoint.pose = tf2::toMsg(blend_sample_pose1); + waypoint.time_from_start = rclcpp::Duration::from_seconds(time_offset); + trajectory.points.push_back(waypoint); + while (blend_time <= blend_duration + EPSILON) { // if the first trajectory does not reach the last sample, update - if ((t_start + blend_time) < req.first_trajectory->getDuration()) + if ((t_start + blend_time) <= req.first_trajectory->getDuration()) { blend_sample_pose1 = interpolatePose(req.first_trajectory, req.link_name, t_start + blend_time); } @@ -338,20 +348,19 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra blend_time += sampling_time; // Ensures samples are far enough apart to avoid numerical issues in numerical inverse kinematics - if (((blend_sample_pose.translation() - last_blend_sample_pose.translation()).norm() < EPSILON) && - (blend_sample_pose.rotation().isApprox(last_blend_sample_pose.rotation(), 1e-3))) + if (((blend_sample_pose.translation() - last_blend_sample_pose.translation()).norm() < 1e-3) && + (blend_sample_pose.rotation().isApprox(last_blend_sample_pose.rotation(), 1e-3)) && + (blend_time < blend_duration)) // Force the addition of the last point { continue; } - // std::cout << "blend_time: " << blend_time << std::endl; - // Store the last insert pose last_blend_sample_pose = blend_sample_pose; // push to the trajectory waypoint.pose = tf2::toMsg(blend_sample_pose); - waypoint.time_from_start = rclcpp::Duration::from_seconds(blend_time); + waypoint.time_from_start = rclcpp::Duration::from_seconds(time_offset + blend_time); trajectory.points.push_back(waypoint); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 4cd0ab23d8..97b96277bd 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -362,6 +362,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( if (i == 0) { duration_current = trajectory.points.front().time_from_start.seconds(); + // This still assumes all the points in first_trajectory have the same duration duration_last = duration_current; } else diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index d56dc47cb6..2dff2e5a9c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -560,7 +560,7 @@ bool testutils::toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, c bool testutils::checkOriginalTrajectoryAfterBlending(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, const pilz_industrial_motion_planner::TrajectoryBlendResponse& res, - const double time_tolerance) + const double /* time_tolerance */) { for (std::size_t i = 0; i < res.first_trajectory->getWayPointCount(); ++i) { @@ -679,6 +679,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p // check the continuity between first trajectory and blend trajectory trajectory_msgs::msg::JointTrajectoryPoint first_end, blend_start; + auto first_pend = first_traj.joint_trajectory.points[first_traj.joint_trajectory.points.size() - 2]; first_end = first_traj.joint_trajectory.points.back(); blend_start = blend_traj.joint_trajectory.points.front(); @@ -709,13 +710,16 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p return false; } - double blend_start_acc = - (blend_start_velo - first_end.velocities.at(i)) / rclcpp::Duration(blend_start.time_from_start).seconds(); - if (fabs(blend_start_acc - blend_start.accelerations.at(i)) > joint_velocity_tolerance) + double blend_start_acc = 2 * (blend_start_velo - first_end.velocities.at(i)) / + (rclcpp::Duration(blend_start.time_from_start).seconds() + + rclcpp::Duration(first_end.time_from_start).seconds() - + rclcpp::Duration(first_pend.time_from_start).seconds()); + if (fabs(blend_start_acc - blend_start.accelerations.at(i)) > joint_accleration_tolerance) { std::cout << "Acceleration computed from positions/velocities are " "different from the value in trajectory." << '\n' + << "position: " << blend_start.positions.at(i) << "; " << first_end.positions.at(i) << "computed: " << blend_start_acc << "; " << "in trajectory: " << blend_start.accelerations.at(i) << '\n'; return false; @@ -724,6 +728,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p // check the continuity between blend trajectory and second trajectory trajectory_msgs::msg::JointTrajectoryPoint blend_end, second_start; + auto blend_pend = blend_traj.joint_trajectory.points[blend_traj.joint_trajectory.points.size() - 2]; blend_end = blend_traj.joint_trajectory.points.back(); second_start = second_traj.joint_trajectory.points.front(); @@ -746,7 +751,7 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p { double second_start_velo = (second_start.positions.at(i) - blend_end.positions.at(i)) / rclcpp::Duration(second_start.time_from_start).seconds(); - if (fabs(second_start_velo - second_start.velocities.at(i)) > joint_accleration_tolerance) + if (fabs(second_start_velo - second_start.velocities.at(i)) > joint_velocity_tolerance) { std::cout << "Velocity computed from positions are different from the " "value in trajectory." @@ -756,8 +761,10 @@ bool testutils::checkBlendingJointSpaceContinuity(const pilz_industrial_motion_p return false; } - double second_start_acc = - (second_start_velo - blend_end.velocities.at(i)) / rclcpp::Duration(second_start.time_from_start).seconds(); + double second_start_acc = 2 * (second_start_velo - blend_end.velocities.at(i)) / + (rclcpp::Duration(second_start.time_from_start).seconds() + + rclcpp::Duration(blend_end.time_from_start).seconds() - + rclcpp::Duration(blend_pend.time_from_start).seconds()); if (fabs(second_start_acc - second_start.accelerations.at(i)) > joint_accleration_tolerance) { std::cout << "Acceleration computed from positions/velocities are " @@ -843,9 +850,13 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl // check the connection points between first trajectory and blend trajectory Eigen::Vector3d v_1, w_1, v_2, w_2, v_3, w_3; - computeCartVelocity(pose_first_end_1, pose_first_end, duration, v_1, w_1); - computeCartVelocity(pose_first_end, pose_blend_start, duration, v_2, w_2); - computeCartVelocity(pose_blend_start, pose_blend_start_1, duration, v_3, w_3); + double duration_first_end = + res.first_trajectory->getWayPointDurationFromPrevious(res.first_trajectory->getWayPointCount() - 1); + double duration_blend_start = res.blend_trajectory->getWayPointDurationFromPrevious(0); + double duration_blend_start_1 = res.blend_trajectory->getWayPointDurationFromPrevious(1); + computeCartVelocity(pose_first_end_1, pose_first_end, duration_first_end, v_1, w_1); + computeCartVelocity(pose_first_end, pose_blend_start, duration_blend_start, v_2, w_2); + computeCartVelocity(pose_blend_start, pose_blend_start_1, duration_blend_start_1, v_3, w_3); // translational velocity if (v_2.norm() > max_trans_velo) @@ -854,6 +865,7 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl "trajectory exceeds the limit." << "Actual velocity (norm): " << v_2.norm() << "; " << "Limits: " << max_trans_velo << '\n'; + return false; } // rotational velocity if (w_2.norm() > max_rot_velo) @@ -862,32 +874,39 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl "trajectory exceeds the limit." << "Actual velocity (norm): " << w_2.norm() << "; " << "Limits: " << max_rot_velo << '\n'; + return false; } // translational acceleration - Eigen::Vector3d a_1 = (v_2 - v_1) / duration; - Eigen::Vector3d a_2 = (v_3 - v_2) / duration; + Eigen::Vector3d a_1 = (v_2 - v_1) / duration_blend_start; + Eigen::Vector3d a_2 = (v_3 - v_2) / duration_blend_start_1; if (a_1.norm() > max_trans_acc || a_2.norm() > max_trans_acc) { std::cout << "Translational acceleration between first trajectory and " "blend trajectory exceeds the limit." - << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_2.norm() << "; " << "Limits: " << max_trans_acc << '\n'; + return false; } // rotational acceleration - a_1 = (w_2 - w_1) / duration; - a_2 = (w_3 - w_2) / duration; + a_1 = (w_2 - w_1) / duration_blend_start; + a_2 = (w_3 - w_2) / duration_blend_start_1; if (a_1.norm() > max_rot_acc || a_2.norm() > max_rot_acc) { std::cout << "Rotational acceleration between first trajectory and blend " "trajectory exceeds the limit." - << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_2.norm() << "; " << "Limits: " << max_rot_acc << '\n'; + return false; } - computeCartVelocity(pose_blend_end_1, pose_blend_end, duration, v_1, w_1); - computeCartVelocity(pose_blend_end, pose_second_start, duration, v_2, w_2); - computeCartVelocity(pose_second_start, pose_second_start_1, duration, v_3, w_3); + double duration_blend_end = + res.blend_trajectory->getWayPointDurationFromPrevious(res.blend_trajectory->getWayPointCount() - 1); + double duration_second_start = res.second_trajectory->getWayPointDurationFromPrevious(0); + double duration_second_start_1 = res.second_trajectory->getWayPointDurationFromPrevious(1); + computeCartVelocity(pose_blend_end_1, pose_blend_end, duration_blend_end, v_1, w_1); + computeCartVelocity(pose_blend_end, pose_second_start, duration_second_start, v_2, w_2); + computeCartVelocity(pose_second_start, pose_second_start_1, duration_second_start_1, v_3, w_3); if (v_2.norm() > max_trans_velo) { @@ -895,6 +914,7 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl "trajectory exceeds the limit." << "Actual velocity (norm): " << v_2.norm() << "; " << "Limits: " << max_trans_velo << '\n'; + return false; } if (w_2.norm() > max_rot_velo) { @@ -902,25 +922,28 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl "trajectory exceeds the limit." << "Actual velocity (norm): " << w_2.norm() << "; " << "Limits: " << max_rot_velo << '\n'; + return false; } - a_1 = (v_2 - v_1) / duration; - a_2 = (v_3 - v_2) / duration; + a_1 = (v_2 - v_1) / duration_second_start; + a_2 = (v_3 - v_2) / duration_second_start_1; if (a_1.norm() > max_trans_acc || a_2.norm() > max_trans_acc) { std::cout << "Translational acceleration between blend trajectory and " "second trajectory exceeds the limit." - << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_2.norm() << "; " << "Limits: " << max_trans_acc << '\n'; + return false; } // check rotational acceleration - a_1 = (w_2 - w_1) / duration; - a_2 = (w_3 - w_2) / duration; + a_1 = (w_2 - w_1) / duration_second_start; + a_2 = (w_3 - w_2) / duration_second_start_1; if (a_1.norm() > max_rot_acc || a_2.norm() > max_rot_acc) { std::cout << "Rotational acceleration between blend trajectory and second " "trajectory exceeds the limit." - << "Actual acceleration (norm): " << a_1.norm() << ", " << a_1.norm() << "; " + << "Actual acceleration (norm): " << a_1.norm() << ", " << a_2.norm() << "; " << "Limits: " << max_rot_acc << '\n'; + return false; } return true; From fefd9e16849a70acddeba75d9ad8fe9cbbbd0009 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Sun, 9 Feb 2025 13:23:14 +0100 Subject: [PATCH 6/9] fix: clang-tidy, removes unnecessary time shift of the second trajectory --- .../trajectory_blender_transition_window.cpp | 27 ------------------- .../src/trajectory_functions.cpp | 1 + 2 files changed, 1 insertion(+), 27 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 232896b200..04af2f7acb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -134,33 +134,6 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( req.second_trajectory->getWayPointDurationFromPrevious(i)); } - // adjust the time from start of the first second trajectory point to ensure continuity of velocities (this implicitly - // ensures continuity of accelerations) - auto second_start = res.second_trajectory->getFirstWayPointPtr(); - auto blend_end = res.blend_trajectory->getLastWayPointPtr(); - - std::vector blend_end_positions, second_start_positions; - blend_end->copyJointGroupPositions(req.first_trajectory->getGroup()->getName(), blend_end_positions); - second_start->copyJointGroupPositions(req.first_trajectory->getGroup()->getName(), second_start_positions); - std::vector second_start_velocities; - second_start->copyJointGroupVelocities(req.first_trajectory->getGroup()->getName(), second_start_velocities); - double time_from_start = 0.0; - std::size_t non_zero_velocity_count = 0; - for (std::size_t i = 0; i < second_start_velocities.size(); ++i) - { - if (second_start_velocities[i] != 0) - { - time_from_start += (second_start_positions[i] - blend_end_positions[i]) / second_start_velocities[i]; - ++non_zero_velocity_count; - } - } - if (non_zero_velocity_count > 0) - { - RCLCPP_INFO_STREAM(getLogger(), - "Adjusting time from start of second trajectory to ensure velocity continuity. delta_time: " - << time_from_start / non_zero_velocity_count); - res.second_trajectory->setWayPointDurationFromPrevious(0, time_from_start / non_zero_velocity_count); - } res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; return true; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 97b96277bd..ca9003c68f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -220,6 +220,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( std::vector time_samples; int num_samples = std::floor(trajectory.Duration() / sampling_time); sampling_time = trajectory.Duration() / num_samples; + time_samples.reserve(num_samples); for (int i = 0; i < num_samples; ++i) { time_samples.push_back(i * sampling_time); From ea65de613b6b50ca92f35a040b46b83d7bab9e3b Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 07:44:27 +0100 Subject: [PATCH 7/9] feat: adds `interpolate` function to trajectory_functions, improves `interpolatePose` efficiency, minor refactor changes --- .../trajectory_functions.hpp | 14 +++++ .../trajectory_blender_transition_window.cpp | 56 +++++++------------ .../src/trajectory_functions.cpp | 12 ++++ .../src/trajectory_generator_circ.cpp | 15 ++--- .../src/trajectory_generator_lin.cpp | 15 ++--- 5 files changed, 62 insertions(+), 50 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp index b76ff8a705..4c97a6f061 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp @@ -112,6 +112,20 @@ bool verifySampleJointLimits(const std::map& position_last, const std::map& position_current, double duration_last, double duration_current, const JointLimitsContainer& joint_limits); +/** + * @brief Interpolates between two poses. + * + * This function computes an interpolated pose between a start pose and an end pose based on the given interpolation factor. + * + * @param start_pose The starting pose as an Eigen::Isometry3d. + * @param end_pose The ending pose as an Eigen::Isometry3d. + * @param interpolation_factor A double value between 0 and 1 that determines the interpolation factor. + * A value of 0 returns the start_pose, and a value of 1 returns the end_pose. + * @param interpolated_pose The resulting interpolated pose as an Eigen::Isometry3d. + */ +void interpolate(const Eigen::Isometry3d start_pose, const Eigen::Isometry3d end_pose, double interpolation_factor, + Eigen::Isometry3d& interpolated_pose); + /** * @brief Generate joint trajectory from a KDL Cartesian trajectory * @param scene: planning scene diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 04af2f7acb..cb5dfa668f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -195,53 +195,41 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate return true; } -// TODO: improve this method Eigen::Isometry3d interpolatePose(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& link_name, - double time) + const double& time, size_t& found_index, const size_t& start_index = 0) { - // Find the closest two waypoints surrounding the requested time - std::size_t idx_before = 0, idx_after = 0; - for (std::size_t i = 0; i < trajectory->getWayPointCount() - 1; ++i) + found_index = 0; + for (std::size_t i = start_index; i < trajectory->getWayPointCount() - 1; ++i) { - double t1 = trajectory->getWayPointDurationFromStart(i); - double t2 = trajectory->getWayPointDurationFromStart(i + 1); - - if (t1 <= time && t2 >= time) + if (trajectory->getWayPointDurationFromStart(i + 1) >= time) { - idx_before = i; - idx_after = i + 1; + found_index = i; break; } } // If time is outside known waypoints, return the closest available pose - if (idx_after == 0) + if (found_index == 0) { return trajectory->getWayPoint(0).getFrameTransform(link_name); } - if (idx_before == trajectory->getWayPointCount() - 1) + if (found_index == trajectory->getWayPointCount() - 1) { - return trajectory->getWayPoint(idx_before).getFrameTransform(link_name); + return trajectory->getWayPoint(found_index).getFrameTransform(link_name); } // Get timestamps and transformations - double t1 = trajectory->getWayPointDurationFromStart(idx_before); - double t2 = trajectory->getWayPointDurationFromStart(idx_after); - Eigen::Isometry3d pose1 = trajectory->getWayPoint(idx_before).getFrameTransform(link_name); - Eigen::Isometry3d pose2 = trajectory->getWayPoint(idx_after).getFrameTransform(link_name); + double t1 = trajectory->getWayPointDurationFromStart(found_index); + double t2 = trajectory->getWayPointDurationFromStart(found_index + 1); + Eigen::Isometry3d pose1 = trajectory->getWayPoint(found_index).getFrameTransform(link_name); + Eigen::Isometry3d pose2 = trajectory->getWayPoint(found_index + 1).getFrameTransform(link_name); // Compute interpolation factor - double lambda = (time - t1) / (t2 - t1); + double interpolation_factor = (time - t1) / (t2 - t1); // Linear interpolation for position Eigen::Isometry3d interpolated_pose; - interpolated_pose.translation() = pose1.translation() + lambda * (pose2.translation() - pose1.translation()); - - // SLERP interpolation for rotation - Eigen::Quaterniond quat1(pose1.rotation()); - Eigen::Quaterniond quat2(pose2.rotation()); - interpolated_pose.linear() = quat1.slerp(lambda, quat2).toRotationMatrix(); - + pilz_industrial_motion_planner::interpolate(pose1, pose2, interpolation_factor, interpolated_pose); return interpolated_pose; } @@ -281,6 +269,7 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra // Define an arbitrary small sample time to sample the blending trajectory double sampling_time = 0.001; + size_t blend_sample_pose1_index, blend_sample_pose2_index = 0; int num_samples = std::floor(blend_duration / sampling_time); sampling_time = blend_duration / num_samples; @@ -298,26 +287,21 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra // if the first trajectory does not reach the last sample, update if ((t_start + blend_time) <= req.first_trajectory->getDuration()) { - blend_sample_pose1 = interpolatePose(req.first_trajectory, req.link_name, t_start + blend_time); + blend_sample_pose1 = interpolatePose(req.first_trajectory, req.link_name, t_start + blend_time, + blend_sample_pose1_index, blend_sample_pose1_index); } // if after the alignment, the second trajectory starts, update if ((t_start + blend_time) >= align_time) { - blend_sample_pose2 = interpolatePose(req.second_trajectory, req.link_name, (t_start + blend_time) - align_time); + blend_sample_pose2 = interpolatePose(req.second_trajectory, req.link_name, (t_start + blend_time) - align_time, + blend_sample_pose2_index, blend_sample_pose2_index); } double s = (blend_time + sampling_time) / blend_duration; double alpha = 6 * std::pow(s, 5) - 15 * std::pow(s, 4) + 10 * std::pow(s, 3); - // blend the translation - blend_sample_pose.translation() = blend_sample_pose1.translation() + - alpha * (blend_sample_pose2.translation() - blend_sample_pose1.translation()); - - // blend the orientation - Eigen::Quaterniond start_quat(blend_sample_pose1.rotation()); - Eigen::Quaterniond end_quat(blend_sample_pose2.rotation()); - blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix(); + interpolate(blend_sample_pose1, blend_sample_pose2, alpha, blend_sample_pose); blend_time += sampling_time; // Ensures samples are far enough apart to avoid numerical issues in numerical inverse kinematics diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index ca9003c68f..ea5e0f2151 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -202,6 +202,18 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( return true; } +void pilz_industrial_motion_planner::interpolate(const Eigen::Isometry3d start_pose, const Eigen::Isometry3d end_pose, + double interpolation_factor, Eigen::Isometry3d& interpolated_pose) +{ + interpolated_pose.translation() = + start_pose.translation() + interpolation_factor * (end_pose.translation() - start_pose.translation()); + + // SLERP interpolation for rotation + Eigen::Quaterniond quat1(start_pose.rotation()); + Eigen::Quaterniond quat2(end_pose.rotation()); + interpolated_pose.linear() = quat1.slerp(interpolation_factor, quat2).toRotationMatrix(); +} + bool pilz_industrial_motion_planner::generateJointTrajectory( const planning_scene::PlanningSceneConstPtr& scene, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, 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 3984c91704..9ce64af29d 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 @@ -217,14 +217,15 @@ void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& // sample the Cartesian trajectory and compute joint trajectory using inverse // kinematics auto cartesian_limits = planner_limits_.getCartesianLimits(); - auto st = std::min({ interpolation_params.max_sample_time, - interpolation_params.max_translation_interpolation_distance / - (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor), - interpolation_params.max_rotation_interpolation_distance / - (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) }); - RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", st); + auto sampling_time = std::min({ interpolation_params.max_sample_time, + interpolation_params.max_translation_interpolation_distance / + (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor), + interpolation_params.max_rotation_interpolation_distance / + (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) }); + RCLCPP_DEBUG(getLogger(), "Sampling time for CIR command: %f", sampling_time); if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, - plan_info.link_name, plan_info.start_joint_position, st, joint_trajectory, error_code)) + plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory, + error_code)) { throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path", error_code.val); 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 42a124ef29..bbd1e715ef 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 @@ -164,14 +164,15 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s // sample the Cartesian trajectory and compute joint trajectory using inverse // kinematics auto cartesian_limits = planner_limits_.getCartesianLimits(); - auto st = std::min({ interpolation_params.max_sample_time, - interpolation_params.max_translation_interpolation_distance / - (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor), - interpolation_params.max_rotation_interpolation_distance / - (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) }); - RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", st); + auto sampling_time = std::min({ interpolation_params.max_sample_time, + interpolation_params.max_translation_interpolation_distance / + (cartesian_limits.max_trans_vel * req.max_velocity_scaling_factor), + interpolation_params.max_rotation_interpolation_distance / + (cartesian_limits.max_rot_vel * req.max_velocity_scaling_factor) }); + RCLCPP_DEBUG(getLogger(), "Sampling time for LIN command: %f", sampling_time); if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name, - plan_info.link_name, plan_info.start_joint_position, st, joint_trajectory, error_code)) + 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"; From 9479007d9a2036780d1b97773ca0aa8576fa05ff Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 08:01:05 +0100 Subject: [PATCH 8/9] fix: removes unused `min_number_of_samples` interpolation parameter, adds `checkInterpolationParameters` testutils, adds `interpolationParameters` unit test for LIN generator --- .../src/interpolation_parameters.yaml | 24 ++++-- .../test/test_utils.cpp | 58 +++++++++++++ .../test/test_utils.hpp | 12 +++ .../src/unittest_trajectory_generator_lin.cpp | 85 +++++++++++++++++++ 4 files changed, 171 insertions(+), 8 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml index 44afc02d60..84b08bbfc1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml +++ b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml @@ -7,14 +7,6 @@ interpolation: gt: [ 0.0 ] } } - min_number_of_samples: { - type: int, - default_value: 0, - description: "Minimum number of samples for the generated trajectory", - validation: { - gt_eq: [ 0 ] - } - } max_translation_interpolation_distance: { type: double, default_value: 10000000.0, @@ -31,3 +23,19 @@ interpolation: gt: [ 0.0 ] } } + min_rotation_interpolation_distance: { + type: double, + default_value: 0.000001, # 1e-6 + description: "Min rotation distance between two consecutive samples", + validation: { + gt_eq: [ 0.000001 ] + } + } + min_translation_interpolation_distance: { + type: double, + default_value: 0.000001, # 1e-6 + description: "Min translation distance between two consecutive samples", + validation: { + gt_eq: [ 0.000001 ] + } + } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 2dff2e5a9c..94d6233351 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -1474,3 +1474,61 @@ testutils::checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryC return hasTrapezoidVelocity(accelerations, acc_tol); } + +::testing::AssertionResult +testutils::checkInterpolationParameters(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, + const std::string& link_name, + const interpolation::Params& interpolation_parameters, const double EPSILON) +{ + // Iterate over waypoints and check that interpolation parameters are met + for (size_t i = 1; i < trajectory->getWayPointCount(); ++i) + + { + auto waypoint_pose_0 = trajectory->getWayPoint(i - 1).getFrameTransform(link_name); + auto waypoint_pose_1 = trajectory->getWayPoint(i).getFrameTransform(link_name); + + auto translational_distance = (waypoint_pose_1.translation() - waypoint_pose_0.translation()).norm(); + if (translational_distance > interpolation_parameters.max_translation_interpolation_distance + EPSILON) + { + return ::testing::AssertionFailure() << "Translational distance between waypoints " << i - 1 << " and " << i + << " exceeds the maximum allowed distance of " + << interpolation_parameters.max_translation_interpolation_distance + << ". Actual distance: " << translational_distance; + } + + auto rotational_distance = + Eigen::AngleAxisd(waypoint_pose_0.rotation() * waypoint_pose_1.rotation().inverse()).angle(); + if (rotational_distance > interpolation_parameters.max_rotation_interpolation_distance + EPSILON) + { + return ::testing::AssertionFailure() << "Rotational distance between waypoints " << i - 1 << " and " << i + << " exceeds the maximum allowed distance of " + << interpolation_parameters.max_rotation_interpolation_distance + << ". Actual distance: " << rotational_distance; + } + + if (translational_distance < interpolation_parameters.min_translation_interpolation_distance - EPSILON && + rotational_distance < interpolation_parameters.min_rotation_interpolation_distance - EPSILON && i > 1) + { + return ::testing::AssertionFailure() << "Translational distance between waypoints " << i - 1 << " and " << i + << " is less than the minimum allowed distance of " + << interpolation_parameters.min_translation_interpolation_distance + << ". Actual translational distance: " << translational_distance + << ". Rotational distance between waypoints " << i - 1 << " and " << i + << " is less than the minimum allowed distance of " + << interpolation_parameters.min_rotation_interpolation_distance + << ". Actual rotational distance: " << rotational_distance; + } + + // This do not make sense as soon as the max_sample_time will be used as reference but increased + // if necessary to avoid inserting too close points + // auto time_between_waypoints = trajectory->getWayPointDurationFromPrevious(i); + // if (time_between_waypoints > interpolation_parameters.max_sample_time + EPSILON) + // { + // return ::testing::AssertionFailure() + // << "Time between waypoints " << i - 1 << " and " << i << " exceeds the maximum allowed time of " + // << interpolation_parameters.max_sample_time << ". Actual time: " << time_between_waypoints; + // } + } + + return ::testing::AssertionSuccess(); +} diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.hpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.hpp index 5adb93a6a9..8477755ebb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.hpp @@ -483,6 +483,18 @@ checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& tr const double rot_axis_tol = DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE, const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE); +/** + * @brief Check that the given interpolation parameters are fulfilled by the + * trajectory. + * @param trajectory: the trajectory to check + * @param link_name: the link for which to check the interpolation parameters + * @param interpolation_parameters: the parameters to check + */ +::testing::AssertionResult checkInterpolationParameters(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, + const std::string& link_name, + const interpolation::Params& interpolation_parameters, + const double EPSILON = 1e-05); + inline bool isMonotonouslyDecreasing(const std::vector& vec, double tol) { return std::is_sorted(vec.begin(), vec.end(), [tol](double a, double b) { 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 0b8c7b41a4..08240785d6 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 @@ -307,6 +307,91 @@ TEST_F(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile) } } +/** + * @brief Unit test for the TrajectoryGeneratorLIN class to verify interpolation parameters. + * + * This test constructs a motion plan request and sets various interpolation parameters to check + * the behavior of the linear trajectory generation. It verifies the following: + * - Successful trajectory generation with default parameters. + * - Failure of interpolation when the max translation interpolation distance is set to a very small value. + * - Failure of interpolation when the max rotation interpolation distance is set to a very small value. + * - Successful trajectory generation and interpolation with specific test parameters. + */ +TEST_F(TrajectoryGeneratorLINTest, interpolationParameters) +{ + // construct motion plan request + moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + + interpolation::Params default_params; + interpolation::Params interpolation_params; + interpolation::Params test_params; + test_params.max_sample_time = 0.05; + test_params.max_translation_interpolation_distance = 0.001; + test_params.max_rotation_interpolation_distance = 0.001; + + /// +++++++++++++++++++++++ + /// + plan LIN trajectory + + /// +++++++++++++++++++++++ + planning_interface::MotionPlanResponse res; + interpolation_params.max_sample_time = test_params.max_sample_time; + lin_->generate(planning_scene_, lin_joint_req, res, interpolation_params); + EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + + // set the max translation interpolation distance to a very small value + // and check that the interpolation fails + interpolation_params.max_translation_interpolation_distance = test_params.max_translation_interpolation_distance; + ASSERT_FALSE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params)); + interpolation_params.max_translation_interpolation_distance = default_params.max_translation_interpolation_distance; + + // set the max rotation interpolation distance to a very small value + // and check that the interpolation fails + interpolation_params.max_rotation_interpolation_distance = test_params.max_rotation_interpolation_distance; + ASSERT_FALSE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params)); + + // recompute the trajectory with the same interpolation parameters + // and check that the interpolation is successful + lin_->generate(planning_scene_, lin_joint_req, res, test_params); + ASSERT_TRUE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, test_params)); +} + +/** + * @brief Test case for verifying the interpolation parameters in the LIN trajectory generator. + * + * This test constructs a motion plan request for a linear joint motion and sets the interpolation parameters. + * It then generates a LIN trajectory and checks if the interpolation parameters are correctly applied. + * The test verifies the following: + * - The trajectory generation is successful with the initial interpolation parameters. + * - The interpolation parameters are correctly checked against the generated trajectory. + * - The trajectory generation is successful with modified interpolation parameters. + */ +// TEST_F(TrajectoryGeneratorLINTest, interpolationParametersNumericalIK) +// { +// // construct motion plan request +// moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint("lin2").toRequest() }; + +// interpolation::Params interpolation_params; +// interpolation_params.max_sample_time = 0.01; +// interpolation_params.max_translation_interpolation_distance = 0.001; +// interpolation_params.max_rotation_interpolation_distance = 1.0; + +// /// +++++++++++++++++++++++ +// /// + plan LIN trajectory + +// /// +++++++++++++++++++++++ +// planning_interface::MotionPlanResponse res; +// lin_->generate(planning_scene_, lin_joint_req, res, interpolation_params); +// EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS); +// ASSERT_TRUE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params)); + +// interpolation_params.min_translation_interpolation_distance = 5e-4; +// interpolation_params.min_rotation_interpolation_distance = 5e-4; +// ASSERT_FALSE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params)); + +// // recompute the trajectory with the same interpolation parameters +// // and check that the interpolation is successful +// lin_->generate(planning_scene_, lin_joint_req, res, interpolation_params); +// ASSERT_TRUE(testutils::checkInterpolationParameters(res.trajectory, target_link_hcd_, interpolation_params)); +// } + /** * @brief Check that lin planner returns 'false' if * calculated lin trajectory violates velocity/acceleration or deceleration From b25497e3017c4ec313b898f242d4d2608c735730 Mon Sep 17 00:00:00 2001 From: Marco Magri Date: Wed, 12 Feb 2025 09:22:15 +0100 Subject: [PATCH 9/9] fix: clang-tidy --- .../pilz_industrial_motion_planner/trajectory_functions.hpp | 4 ++-- .../src/trajectory_functions.cpp | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp index 4c97a6f061..07b62df1eb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.hpp @@ -123,8 +123,8 @@ bool verifySampleJointLimits(const std::map& position_last, * A value of 0 returns the start_pose, and a value of 1 returns the end_pose. * @param interpolated_pose The resulting interpolated pose as an Eigen::Isometry3d. */ -void interpolate(const Eigen::Isometry3d start_pose, const Eigen::Isometry3d end_pose, double interpolation_factor, - Eigen::Isometry3d& interpolated_pose); +void interpolate(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& end_pose, + const double& interpolation_factor, Eigen::Isometry3d& interpolated_pose); /** * @brief Generate joint trajectory from a KDL Cartesian trajectory diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index ea5e0f2151..7a4fb6def1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -202,8 +202,9 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( return true; } -void pilz_industrial_motion_planner::interpolate(const Eigen::Isometry3d start_pose, const Eigen::Isometry3d end_pose, - double interpolation_factor, Eigen::Isometry3d& interpolated_pose) +void pilz_industrial_motion_planner::interpolate(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& end_pose, + const double& interpolation_factor, + Eigen::Isometry3d& interpolated_pose) { interpolated_pose.translation() = start_pose.translation() + interpolation_factor * (end_pose.translation() - start_pose.translation());