Skip to content

Commit

Permalink
Add velocity and acceleration scaling when using custom limits in Tim…
Browse files Browse the repository at this point in the history 
…e Parameterization (#1832)

* add velocity and accelerations scaling when using custom limits for time parameterization

* add scaling when passing in vecotor of joint-limits

* add function descriptions

* add verifyScalingFactor helper function

* make map const

* address feedback

* add comment

Co-authored-by: Michael Wiznitzer <[email protected]>
  • Loading branch information
@mechwiz
mechwiz and Michael Wiznitzer authored Jan 18, 2023
1 parent 0642ef0 commit d1aab8c
Show file tree
Hide file tree
Showing 5 changed files with 202 additions and 44 deletions.
32 changes: 31 additions & 1 deletion ...t_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,14 +45,44 @@ namespace trajectory_processing
class RuckigSmoothing
{
public:
/**
* \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated.
* \param[in,out] trajectory A path which needs smoothing.
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0);

/**
* \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated.
* \param[in,out] trajectory A path which needs smoothing.
* \param velocity_limits Joint names and velocity limits in rad/s
* \param acceleration_limits Joint names and acceleration limits in rad/s^2
* \param jerk_limits Joint names and jerk limits in rad/s^3
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits,
const std::unordered_map<std::string, double>& jerk_limits);
const std::unordered_map<std::string, double>& jerk_limits,
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0);

/**
* \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated.
* \param[in,out] trajectory A path which needs smoothing.
* \param joint_limits Joint names and corresponding velocity limits in rad/s, acceleration limits in rad/s^2,
* and jerk limits in rad/s^3
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0);

private:
/**
Expand Down
42 changes: 41 additions & 1 deletion ...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,14 @@

namespace trajectory_processing
{
enum LimitType
{
VELOCITY,
ACCELERATION
};

const std::unordered_map<LimitType, std::string> LIMIT_TYPES = { { VELOCITY, "velocity" },
{ ACCELERATION, "acceleration" } };
class PathSegment
{
public:
Expand Down Expand Up @@ -196,11 +204,35 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
* time-parameterized; this function will re-time-parameterize it.
* \param velocity_limits Joint names and velocity limits in rad/s
* \param acceleration_limits Joint names and acceleration limits in rad/s^2
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
// clang-format on
bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits) const override;
const std::unordered_map<std::string, double>& acceleration_limits,
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const override;

// clang-format off
/**
* \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_).
* Resampling the trajectory doesn't change the start and goal point,
* and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_).
* However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints.
* path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints,
* meters for prismatic joints.
* \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been
* time-parameterized; this function will re-time-parameterize it.
* \param joint_limits Joint names and corresponding velocity limits in rad/s and acceleration limits in rad/s^2
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
// clang-format on
bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const override;

private:
bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
Expand All @@ -214,6 +246,14 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
*/
bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) const;

/**
* @brief Check if the requested scaling factor is valid and if not, return 1.0.
* \param requested_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits
* \param limit_type Whether the velocity or acceleration scaling factor is being verified
* \return The user requested scaling factor, if it is valid. Otherwise, return 1.0.
*/
double verifyScalingFactor(const double requested_scaling_factor, const LimitType limit_type) const;

const double path_tolerance_;
const double resample_dt_;
const double min_angle_change_;
Expand Down
35 changes: 34 additions & 1 deletion ...t_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,44 @@ class TimeParameterization
{
public:
virtual ~TimeParameterization() = default;

/**
* \brief Compute a trajectory with waypoints spaced equally in time
* \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been
* time-parameterized; this function will re-time-parameterize it.
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const = 0;

/**
* \brief Compute a trajectory with waypoints spaced equally in time
* \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been
* time-parameterized; this function will re-time-parameterize it.
* \param velocity_limits Joint names and velocity limits in rad/s
* \param acceleration_limits Joint names and acceleration limits in rad/s^2
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits) const = 0;
const std::unordered_map<std::string, double>& acceleration_limits,
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const = 0;

/**
* \brief Compute a trajectory with waypoints spaced equally in time
* \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been
* time-parameterized; this function will re-time-parameterize it.
* \param joint_limits Joint names and corresponding velocity limits in rad/s and acceleration limits in rad/s^2
* \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
* \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory.
*/
virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const = 0;
};
} // namespace trajectory_processing
38 changes: 33 additions & 5 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,9 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits,
const std::unordered_map<std::string, double>& jerk_limits)
const std::unordered_map<std::string, double>& jerk_limits,
const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor)
{
if (!validateGroup(trajectory))
{
Expand All @@ -111,8 +113,6 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
moveit::core::JointModelGroup const* const group = trajectory.getGroup();
const size_t num_dof = group->getVariableCount();
ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
double max_velocity_scaling_factor = 1.0;
double max_acceleration_scaling_factor = 1.0;
if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input))
{
RCLCPP_ERROR(LOGGER, "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel.");
Expand All @@ -128,13 +128,13 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
auto it = velocity_limits.find(vars[j]);
if (it != velocity_limits.end())
{
ruckig_input.max_velocity.at(j) = it->second;
ruckig_input.max_velocity.at(j) = it->second * max_velocity_scaling_factor;
}
// Acceleration
it = acceleration_limits.find(vars[j]);
if (it != acceleration_limits.end())
{
ruckig_input.max_acceleration.at(j) = it->second;
ruckig_input.max_acceleration.at(j) = it->second * max_acceleration_scaling_factor;
}
// Jerk
it = jerk_limits.find(vars[j]);
Expand Down Expand Up @@ -213,6 +213,34 @@ RuckigSmoothing::runRuckigInBatches(const size_t num_waypoints, const robot_traj
return std::make_optional<robot_trajectory::RobotTrajectory>(output_trajectory, true /* deep copy */);
}

bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor)
{
std::unordered_map<std::string, double> velocity_limits;
std::unordered_map<std::string, double> acceleration_limits;
std::unordered_map<std::string, double> jerk_limits;
for (const auto& limit : joint_limits)
{
// If custom limits are not defined here, they will be supplied from getRobotModelBounds() later
if (limit.has_velocity_limits)
{
velocity_limits[limit.joint_name] = limit.max_velocity;
}
if (limit.has_acceleration_limits)
{
acceleration_limits[limit.joint_name] = limit.max_acceleration;
}
if (limit.has_jerk_limits)
{
jerk_limits[limit.joint_name] = limit.max_jerk;
}
}
return applySmoothing(trajectory, velocity_limits, acceleration_limits, jerk_limits, max_velocity_scaling_factor,
max_acceleration_scaling_factor);
}

bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& trajectory)
{
moveit::core::JointModelGroup const* const group = trajectory.getGroup();
Expand Down
99 changes: 63 additions & 36 deletions moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ static const rclcpp::Logger LOGGER =
rclcpp::get_logger("moveit_trajectory_processing.time_optimal_trajectory_generation");
constexpr double DEFAULT_TIMESTEP = 1e-3;
constexpr double EPS = 1e-6;
constexpr double DEFAULT_SCALING_FACTOR = 1.0;
} // namespace

class LinearPathSegment : public PathSegment
Expand Down Expand Up @@ -880,37 +881,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
}

// Validate scaling
double velocity_scaling_factor = 1.0;
if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
{
velocity_scaling_factor = max_velocity_scaling_factor;
}
else if (max_velocity_scaling_factor == 0.0)
{
RCLCPP_DEBUG(LOGGER, "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
velocity_scaling_factor);
}
else
{
RCLCPP_WARN(LOGGER, "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
max_velocity_scaling_factor, velocity_scaling_factor);
}

double acceleration_scaling_factor = 1.0;
if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
{
acceleration_scaling_factor = max_acceleration_scaling_factor;
}
else if (max_acceleration_scaling_factor == 0.0)
{
RCLCPP_DEBUG(LOGGER, "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
acceleration_scaling_factor);
}
else
{
RCLCPP_WARN(LOGGER, "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
max_acceleration_scaling_factor, acceleration_scaling_factor);
}
double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor, VELOCITY);
double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor, ACCELERATION);

const std::vector<std::string>& vars = group->getVariableNames();
const moveit::core::RobotModel& rmodel = group->getParentModel();
Expand Down Expand Up @@ -967,9 +939,33 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
}

bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor) const
{
std::unordered_map<std::string, double> velocity_limits;
std::unordered_map<std::string, double> acceleration_limits;
for (const auto& limit : joint_limits)
{
// If custom limits are not defined here, they will be supplied from getRobotModelBounds() later
if (limit.has_velocity_limits)
{
velocity_limits[limit.joint_name] = limit.max_velocity;
}
if (limit.has_acceleration_limits)
{
acceleration_limits[limit.joint_name] = limit.max_acceleration;
}
}
return computeTimeStamps(trajectory, velocity_limits, acceleration_limits, max_velocity_scaling_factor,
max_acceleration_scaling_factor);
}

bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits) const
const std::unordered_map<std::string, double>& acceleration_limits, const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor) const
{
if (trajectory.empty())
return true;
Expand All @@ -981,6 +977,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
return false;
}

// Validate scaling
double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor, VELOCITY);
double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor, ACCELERATION);

const unsigned num_joints = group->getVariableCount();
const std::vector<std::string>& vars = group->getVariableNames();
const moveit::core::RobotModel& rmodel = group->getParentModel();
Expand All @@ -997,7 +998,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
auto it = velocity_limits.find(vars[j]);
if (it != velocity_limits.end())
{
max_velocity[j] = it->second;
max_velocity[j] = it->second * velocity_scaling_factor;
set_velocity_limit = true;
}

Expand All @@ -1010,7 +1011,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
bounds.max_velocity_, vars[j].c_str());
return false;
}
max_velocity[j] = std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_));
max_velocity[j] =
std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor;
set_velocity_limit = true;
}

Expand All @@ -1029,7 +1031,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
it = acceleration_limits.find(vars[j]);
if (it != acceleration_limits.end())
{
max_acceleration[j] = it->second;
max_acceleration[j] = it->second * acceleration_scaling_factor;
set_acceleration_limit = true;
}

Expand All @@ -1042,7 +1044,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
bounds.max_acceleration_, vars[j].c_str());
return false;
}
max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_));
max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
acceleration_scaling_factor;
set_acceleration_limit = true;
}
if (!set_acceleration_limit)
Expand Down Expand Up @@ -1203,4 +1206,28 @@ bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::Joi

return have_prismatic && have_revolute;
}

double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double requested_scaling_factor,
const LimitType limit_type) const
{
std::string limit_type_str;
double scaling_factor = DEFAULT_SCALING_FACTOR;

const auto limit_type_it = LIMIT_TYPES.find(limit_type);
if (limit_type_it != LIMIT_TYPES.end())
{
limit_type_str = limit_type_it->second + "_";
}

if (requested_scaling_factor > 0.0 && requested_scaling_factor <= 1.0)
{
scaling_factor = requested_scaling_factor;
}
else
{
RCLCPP_WARN(LOGGER, "Invalid max_%sscaling_factor %f specified, defaulting to %f instead.", limit_type_str.c_str(),
requested_scaling_factor, scaling_factor);
}
return scaling_factor;
}
} // namespace trajectory_processing

0 comments on commit d1aab8c

Please sign in to comment.