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_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/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..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 @@ -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, + const 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/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..84b08bbfc1 --- /dev/null +++ b/moveit_planners/pilz_industrial_motion_planner/src/interpolation_parameters.yaml @@ -0,0 +1,41 @@ +interpolation: + max_sample_time: { + type: double, + default_value: 0.1, + description: "Max time interval between two consecutive samples", + validation: { + gt: [ 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 ] + } + } + 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/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_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index 41deb7ccd7..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 @@ -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,12 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( req.second_trajectory->getWayPointDurationFromPrevious(i)); } - // adjust the time from start - res.second_trajectory->setWayPointDurationFromPrevious(0, sampling_time); - 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 +179,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 +195,62 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validate return true; } +Eigen::Isometry3d interpolatePose(const robot_trajectory::RobotTrajectoryPtr& trajectory, const std::string& link_name, + const double& time, size_t& found_index, const size_t& start_index = 0) +{ + found_index = 0; + for (std::size_t i = start_index; i < trajectory->getWayPointCount() - 1; ++i) + { + if (trajectory->getWayPointDurationFromStart(i + 1) >= time) + { + found_index = i; + break; + } + } + + // If time is outside known waypoints, return the closest available pose + if (found_index == 0) + { + return trajectory->getWayPoint(0).getFrameTransform(link_name); + } + if (found_index == trajectory->getWayPointCount() - 1) + { + return trajectory->getWayPoint(found_index).getFrameTransform(link_name); + } + + // Get timestamps and transformations + 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 interpolation_factor = (time - t1) / (t2 - t1); + + // Linear interpolation for position + Eigen::Isometry3d interpolated_pose; + pilz_industrial_motion_planner::interpolate(pose1, pose2, interpolation_factor, interpolated_pose); + 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,42 +260,64 @@ 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.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; + + double blend_time = 0.0; + Eigen::Isometry3d last_blend_sample_pose = blend_sample_pose1; + + // 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 ((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, + blend_sample_pose1_index, blend_sample_pose1_index); } // 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, + blend_sample_pose2_index, blend_sample_pose2_index); } - 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 - blend_sample_pose.translation() = blend_sample_pose1.translation() + - alpha * (blend_sample_pose2.translation() - blend_sample_pose1.translation()); + 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 + 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; + } - // 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(); + // 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(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 01a7f11b59..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,6 +202,19 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits( return true; } +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()); + + // 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, @@ -217,11 +230,13 @@ 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; + time_samples.reserve(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()); @@ -361,6 +376,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/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..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 @@ -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,6 +216,13 @@ 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 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, sampling_time, joint_trajectory, error_code)) 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..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 @@ -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,6 +163,13 @@ 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 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, sampling_time, joint_trajectory, error_code)) 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..94d6233351 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) { @@ -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; @@ -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; @@ -1076,7 +1099,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 +1121,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'; @@ -1447,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_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..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 @@ -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."); @@ -294,91 +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 - planning_interface::MotionPlanResponse resp; - lin_generator_->generate(planning_scene_, req, resp, sampling_time_); - 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. @@ -696,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); 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..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 @@ -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_)); @@ -305,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