Skip to content

Commit

Permalink
Allow custom velocity/acceleration limits for TOTG time-parameterizat…
Browse files Browse the repository at this point in the history 
…ion algorithm (#1195)

* A new function for common time-param calculations

* Flesh out the second computeTimeStamps function

* Reading of the custom limits

* Add a unit test

* Revert comment changes about exceptions

* Small efficiency improvements

* Add computeTimeStamps() overload to the base class

Co-authored-by: Jafar <cafer.abdi@gmail.com>
  • Loading branch information
@JafarAbdi
AndyZe and JafarAbdi authored May 10, 2022
1 parent 05aedac commit 9e08880
Show file tree
Hide file tree
Showing 8 changed files with 176 additions and 9 deletions.
4 changes: 4 additions & 0 deletions ...ctory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,10 @@ class IterativeSplineParameterization : public TimeParameterization
bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const override;

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;

private:
bool add_points_; /// @brief If true, add two points to trajectory (first and last segments).
/// If false, move the 2nd and 2nd-last points.
Expand Down
4 changes: 4 additions & 0 deletions ...jectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,10 @@ class IterativeParabolicTimeParameterization : public TimeParameterization
bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const override;

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;

private:
unsigned int max_iterations_; /// @brief maximum number of iterations to find solution
double max_time_change_per_it_; /// @brief maximum allowed time change per iteration in seconds
Expand Down
8 changes: 8 additions & 0 deletions ...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 @@ -169,7 +169,15 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0) const override;

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;

private:
bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
const Eigen::VectorXd& max_velocity,
const Eigen::VectorXd& max_acceleration) const;

const double path_tolerance_;
const double resample_dt_;
const double min_angle_change_;
Expand Down
3 changes: 3 additions & 0 deletions ...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 @@ -14,5 +14,8 @@ class TimeParameterization
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;
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;
};
} // namespace trajectory_processing
8 changes: 8 additions & 0 deletions moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -346,6 +346,14 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT
return true;
}

bool IterativeSplineParameterization::computeTimeStamps(
robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits) const
{
RCLCPP_ERROR(LOGGER, "ISTP does not support this version of computeTimeStamps. Try TOTG instead?");
return false;
}

//////// Internal functions //////////////

/*
Expand Down
8 changes: 8 additions & 0 deletions moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -485,4 +485,12 @@ bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory:
updateTrajectory(trajectory, time_diff);
return true;
}

bool IterativeParabolicTimeParameterization::computeTimeStamps(
robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map<std::string, double>& velocity_limits,
const std::unordered_map<std::string, double>& acceleration_limits) const
{
RCLCPP_ERROR(LOGGER, "IPTP does not support this version of computeTimeStamps. Try TOTG instead?");
return false;
}
} // namespace trajectory_processing
123 changes: 114 additions & 9 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 @@ -911,17 +911,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
max_acceleration_scaling_factor, acceleration_scaling_factor);
}

// This lib does not actually work properly when angles wrap around, so we need to unwind the path first
trajectory.unwind();

// This is pretty much copied from IterativeParabolicTimeParameterization::applyVelocityConstraints
const std::vector<std::string>& vars = group->getVariableNames();
const std::vector<int>& idx = group->getVariableIndexList();
const moveit::core::RobotModel& rmodel = group->getParentModel();
const unsigned num_joints = group->getVariableCount();
const unsigned num_points = trajectory.getWayPointCount();

// Get the limits (we do this at same time, unlike IterativeParabolicTimeParameterization)
// Get the vel/accel limits
Eigen::VectorXd max_velocity(num_joints);
Eigen::VectorXd max_acceleration(num_joints);
for (size_t j = 0; j < num_joints; ++j)
Expand All @@ -932,7 +926,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
max_velocity[j] = 1.0;
if (bounds.velocity_bounded_)
{
if (bounds.max_velocity_ < std::numeric_limits<double>::epsilon())
if (bounds.max_velocity_ <= 0.0)
{
RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
bounds.max_velocity_, vars[j].c_str());
Expand All @@ -951,7 +945,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
max_acceleration[j] = 1.0;
if (bounds.acceleration_bounded_)
{
if (bounds.max_acceleration_ < std::numeric_limits<double>::epsilon())
if (bounds.max_acceleration_ < 0.0)
{
RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
bounds.max_acceleration_, vars[j].c_str());
Expand All @@ -969,6 +963,117 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
}
}

return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
}

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
{
if (trajectory.empty())
return true;

// Get the default joint limits from the robot model, then overwrite any that are provided as arguments
const moveit::core::JointModelGroup* group = trajectory.getGroup();
if (!group)
{
RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
return false;
}
const unsigned num_joints = group->getVariableCount();
const std::vector<std::string>& vars = group->getVariableNames();
const moveit::core::RobotModel& rmodel = group->getParentModel();

Eigen::VectorXd max_velocity(num_joints);
Eigen::VectorXd max_acceleration(num_joints);
for (size_t j = 0; j < num_joints; ++j)
{
const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]);

// VELOCITY LIMIT
// Check if a custom limit was specified as an argument
bool set_velocity_limit = false;
auto it = velocity_limits.find(vars[j]);
if (it != velocity_limits.end())
{
max_velocity[j] = it->second;
set_velocity_limit = true;
}

if (bounds.velocity_bounded_ && !set_velocity_limit)
{
// Set the default velocity limit, from robot model
if (bounds.max_velocity_ < 0.0)
{
RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
bounds.max_velocity_, vars[j].c_str());
return false;
}
max_velocity[j] = std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_));
set_velocity_limit = true;
}

if (!set_velocity_limit)
{
max_velocity[j] = 1.0;
RCLCPP_WARN_STREAM_ONCE(
LOGGER, "Joint velocity limits are not defined. Using the default "
<< max_velocity[j] << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
}

// ACCELERATION LIMIT
// Check if a custom limit was specified as an argument
bool set_acceleration_limit = false;
it = acceleration_limits.find(vars[j]);
if (it != acceleration_limits.end())
{
max_acceleration[j] = it->second;
set_acceleration_limit = true;
}

if (bounds.acceleration_bounded_ && !set_acceleration_limit)
{
// Set the default acceleration limit, from robot model
if (bounds.max_acceleration_ < 0.0)
{
RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
bounds.max_acceleration_, vars[j].c_str());
return false;
}
max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_));
set_acceleration_limit = true;
}
if (!set_acceleration_limit)
{
max_acceleration[j] = 1.0;
RCLCPP_WARN_STREAM_ONCE(LOGGER,
"Joint acceleration limits are not defined. Using the default "
<< max_acceleration[j]
<< " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
}
}

return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
}

bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
const Eigen::VectorXd& max_velocity,
const Eigen::VectorXd& max_acceleration) const
{
// This lib does not actually work properly when angles wrap around, so we need to unwind the path first
trajectory.unwind();

const moveit::core::JointModelGroup* group = trajectory.getGroup();
if (!group)
{
RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
return false;
}

const unsigned num_points = trajectory.getWayPointCount();
const std::vector<int>& idx = group->getVariableIndexList();
const unsigned num_joints = group->getVariableCount();

// Have to convert into Eigen data structs and remove repeated points
// (https://github.com/tobiaskunz/trajectories/issues/3)
std::list<Eigen::VectorXd> points;
Expand Down
27 changes: 27 additions & 0 deletions moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,33 @@ TEST(time_optimal_trajectory_generation, test3)
EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);
}

// Test the version of computeTimeStamps that takes custom velocity/acceleration limits
TEST(time_optimal_trajectory_generation, test_custom_limits)
{
constexpr auto robot_name{ "panda" };
constexpr auto group_name{ "panda_arm" };

auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
ASSERT_TRUE((bool)robot_model) << "Failed to load robot model" << robot_name;
auto group = robot_model->getJointModelGroup(group_name);
ASSERT_TRUE((bool)group) << "Failed to load joint model group " << group_name;
moveit::core::RobotState waypoint_state(robot_model);
waypoint_state.setToDefaultValues();

const double delta_t = 0.1;
robot_trajectory::RobotTrajectory trajectory(robot_model, group);
waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
trajectory.addSuffixWayPoint(waypoint_state, delta_t);
waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.45, -3.2, 1.2, -2.4, -0.8, 0.6, 0.0 });
trajectory.addSuffixWayPoint(waypoint_state, delta_t);

TimeOptimalTrajectoryGeneration totg;
// Custom velocity & acceleration limits for some joints
std::unordered_map<std::string, double> vel_limits{ { "panda_joint1", 1.3 } };
std::unordered_map<std::string, double> accel_limits{ { "panda_joint2", 2.3 }, { "panda_joint3", 3.3 } };
ASSERT_TRUE(totg.computeTimeStamps(trajectory, vel_limits, accel_limits)) << "Failed to compute time stamps";
}

// Test that totg algorithm doesn't give large acceleration
TEST(time_optimal_trajectory_generation, testLargeAccel)
{
Expand Down

0 comments on commit 9e08880

Please sign in to comment.