From 0833d195c8aff763fe872f91db6a96808446b307 Mon Sep 17 00:00:00 2001 From: vivekcdavid <111999394+vivekcdavid@users.noreply.github.com> Date: Wed, 30 Oct 2024 14:07:39 +0100 Subject: [PATCH 1/4] Update trajectory_generator.h to use rect velocity profile --- .../trajectory_generator.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index a24a7395c6..cdbf91babf 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -136,9 +136,17 @@ class TrajectoryGenerator * The trap profile returns uses the longer distance of translational and * rotational motion. */ - std::unique_ptr cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, + std::unique_ptr cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor, const double& max_acceleration_scaling_factor, const std::unique_ptr& path) const; + /** + * @brief build cartesian velocity profile for the path + * + * Uses the path to get the cartesian length from start to goal. + * The rect profile returns a uniform velocity profile without considering the acceleration + */ + std::unique_ptr cartesianRectVelocityProfile(const double& max_velocity_scaling_factor, + const std::unique_ptr& path) const; private: virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const; From ed90f7ab3a79ee334a4a2a219004cc80b0158564 Mon Sep 17 00:00:00 2001 From: vivekcdavid <111999394+vivekcdavid@users.noreply.github.com> Date: Wed, 30 Oct 2024 14:09:28 +0100 Subject: [PATCH 2/4] Update trajectory_generator.cpp and creating a new function for using velocityprofile_rect. --- .../src/trajectory_generator.cpp | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) 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 5a8c9753e3..b3e50a7ba8 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -300,6 +301,37 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_sca return vp_trans; } +std::unique_ptr +TrajectoryGenerator::cartesianRectVelocityProfile(const double& max_velocity_scaling_factor, + const std::unique_ptr& path) const +{ + // Create a rectangular velocity profile with the calculated max velocity + //std::unique_ptr vp_trans(new KDL::VelocityProfile_Rectangular()); + std::unique_ptr vp_trans = std::make_unique( + max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity()); + //std::unique_ptr vp_trans = std::make_unique( + // max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(), + // max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration()); + + if (path->PathLength() > std::numeric_limits::epsilon()) // avoid division by zero + { + //vp_trans->SetProfile(0, path->PathLength()); + // Use the PathLength function to get the total path distance + double actual_distance = path->PathLength(); + // getting the actual velocity based on the max_velocity_scaling_factor + double actual_velocity = max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(); + // Calculate the desired time based on max_velocity + double actual_duration = actual_distance / actual_velocity; + vp_trans->SetProfileDuration(0, actual_distance, actual_duration); + } + else + { + vp_trans->SetProfile(0, std::numeric_limits::epsilon()); + } + + return vp_trans; +} + bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, double sampling_time) From a35a3c99091031af4872f400bff60407938eabfe Mon Sep 17 00:00:00 2001 From: vivekcdavid <111999394+vivekcdavid@users.noreply.github.com> Date: Wed, 30 Oct 2024 14:18:47 +0100 Subject: [PATCH 3/4] Update trajectory_generator_lin.cpp setting eqradius = 0.0 and replacing the cartesianTrapVelocityProfile() function with cartesianRectVelocityProfile for generating the velocity profile of the KDL path --- .../src/trajectory_generator_lin.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) 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 0a7cb7340c..0564cfa7d6 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 @@ -154,10 +154,12 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s // create Cartesian path for lin std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); - // create velocity profile + // create velocity profile using trap profile + //std::unique_ptr vp( + // cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); + // create velocity profile using rect profile std::unique_ptr vp( - cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path)); - + cartesianRectVelocityProfile(req.max_velocity_scaling_factor, path)); // combine path and velocity profile into Cartesian trajectory // with the third parameter set to false, KDL::Trajectory_Segment does not // take @@ -185,8 +187,9 @@ std::unique_ptr TrajectoryGeneratorLIN::setPathLIN(const Eigen::Affin KDL::Frame kdl_start_pose, kdl_goal_pose; tf2::transformEigenToKDL(start_pose, kdl_start_pose); tf2::transformEigenToKDL(goal_pose, kdl_goal_pose); - double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / - planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); + //double eqradius = planner_limits_.getCartesianLimits().getMaxTranslationalVelocity() / + // planner_limits_.getCartesianLimits().getMaxRotationalVelocity(); + double eqradius = 0.0; KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis(); return std::unique_ptr( From e142a502a27aabb53be12769f03e0a40bad2c25e Mon Sep 17 00:00:00 2001 From: vivekcdavid <111999394+vivekcdavid@users.noreply.github.com> Date: Fri, 29 Nov 2024 14:09:24 +0100 Subject: [PATCH 4/4] reducing the minimum velicity scaling factor to enable slower motions --- .../pilz_industrial_motion_planner/trajectory_generator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index cdbf91babf..6df353aeb1 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -279,7 +279,7 @@ class TrajectoryGenerator protected: const moveit::core::RobotModelConstPtr robot_model_; const pilz_industrial_motion_planner::LimitsContainer planner_limits_; - static constexpr double MIN_SCALING_FACTOR{ 0.0001 }; + static constexpr double MIN_SCALING_FACTOR{ 0.0000001 }; static constexpr double MAX_SCALING_FACTOR{ 1. }; static constexpr double VELOCITY_TOLERANCE{ 1e-8 }; const std::unique_ptr clock_;