Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Solving velocity deviation in Pilz #3158

Open
wants to merge 4 commits into
base: humble
Choose a base branch
from
Open

Solving velocity deviation in Pilz #3158

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
0833d19
Update trajectory_generator.h to use rect velocity profile
vivekcdavid Oct 30, 2024
ed90f7a
Update trajectory_generator.cpp and creating a new function for using…
vivekcdavid Oct 30, 2024
a35a3c9
Update trajectory_generator_lin.cpp
vivekcdavid Oct 30, 2024
e142a50
reducing the minimum velicity scaling factor to enable slower motions
vivekcdavid Nov 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 10 additions & 2 deletions ...z_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -136,9 +136,17 @@ class TrajectoryGenerator
* The trap profile returns uses the longer distance of translational and
* rotational motion.
*/
std::unique_ptr<KDL::VelocityProfile> cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor,
std::unique_ptr<KDL::VelocityProfile> cartesianTrapVelocityProfile(const double& max_velocity_scaling_factor,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this indentation may break the formatting -- accidental change?

const double& max_acceleration_scaling_factor,
const std::unique_ptr<KDL::Path>& 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<KDL::VelocityProfile> cartesianRectVelocityProfile(const double& max_velocity_scaling_factor,
const std::unique_ptr<KDL::Path>& path) const;

private:
virtual void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const;
Expand Down Expand Up @@ -271,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 };
Copy link
Contributor

@sea-bass sea-bass Dec 12, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This may end up changing default behavior -- but as a question to you, did you find this made a difference in other cases? Maybe this should also be a parameter that users can configure at runtime, whose default remains the former 1e-4?

Also minor nitpick, with all the zeros consider using scientific notation like 1e-7.

static constexpr double MAX_SCALING_FACTOR{ 1. };
static constexpr double VELOCITY_TOLERANCE{ 1e-8 };
const std::unique_ptr<rclcpp::Clock> clock_;
Expand Down
32 changes: 32 additions & 0 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <boost/range/combine.hpp>

#include <kdl/velocityprofile_trap.hpp>
#include <kdl/velocityprofile_rect.hpp>
#include <moveit/robot_state/conversions.h>

#include <pilz_industrial_motion_planner/limits_container.h>
Expand Down Expand Up @@ -300,6 +301,37 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_sca
return vp_trans;
}

std::unique_ptr<KDL::VelocityProfile>
TrajectoryGenerator::cartesianRectVelocityProfile(const double& max_velocity_scaling_factor,
const std::unique_ptr<KDL::Path>& path) const
{
// Create a rectangular velocity profile with the calculated max velocity
//std::unique_ptr<KDL::VelocityProfile_Rectangular> vp_trans(new KDL::VelocityProfile_Rectangular());
std::unique_ptr<KDL::VelocityProfile> vp_trans = std::make_unique<KDL::VelocityProfile_Rectangular>(
max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity());
//std::unique_ptr<KDL::VelocityProfile> vp_trans = std::make_unique<KDL::VelocityProfile_Trap>(
// max_velocity_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalVelocity(),
// max_acceleration_scaling_factor * planner_limits_.getCartesianLimits().getMaxTranslationalAcceleration());

if (path->PathLength() > std::numeric_limits<double>::epsilon()) // avoid division by zero
{
//vp_trans->SetProfile(0, path->PathLength());
Comment on lines +309 to +318
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

make sure all these commented out lines are removed

// 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<double>::epsilon());
}

return vp_trans;
}

bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res, double sampling_time)
Expand Down
13 changes: 8 additions & 5 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -154,10 +154,12 @@ void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& s
// create Cartesian path for lin
std::unique_ptr<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose));

// create velocity profile
// create velocity profile using trap profile
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should be rect?

//std::unique_ptr<KDL::VelocityProfile> vp(
// cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path));
// create velocity profile using rect profile
Comment on lines +158 to +160
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove these?

std::unique_ptr<KDL::VelocityProfile> vp(
cartesianTrapVelocityProfile(req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, path));

cartesianRectVelocityProfile(req.max_velocity_scaling_factor, path));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This cannot be just changed, it has to be a runtime parameter configurable by the user, whose default still retains trapezoidal for compatibility.

// combine path and velocity profile into Cartesian trajectory
// with the third parameter set to false, KDL::Trajectory_Segment does not
// take
Expand Down Expand Up @@ -185,8 +187,9 @@ std::unique_ptr<KDL::Path> 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;
Comment on lines +190 to +192
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same here -- probably whatever parameter is used to switch from trapezoidal to rectangle can also zero out the eq_radius.

KDL::RotationalInterpolation* rot_interpo = new KDL::RotationalInterpolation_SingleAxis();

return std::unique_ptr<KDL::Path>(
Expand Down