Skip to content

Commit

Permalink
Allow custom velocity/accel/jerk limits for Ruckig smoothing (#1221)
Browse files Browse the repository at this point in the history 
* Move common functionality to utility functions
* Add a utility function to read RobotModel bounds
* Add a unit test for the new version of computeTimeStamps()
* Clean up Doxygen
* Use ruckig::DynamicDOFs everywhere
  • Loading branch information
AndyZe authored May 10, 2022
1 parent 9e08880 commit e4c7b71
Show file tree
Hide file tree
Showing 3 changed files with 177 additions and 30 deletions.
46 changes: 39 additions & 7 deletions ...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 @@ -49,28 +49,60 @@ class RuckigSmoothing
const double max_velocity_scaling_factor = 1.0,
const double max_acceleration_scaling_factor = 1.0);

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);

private:
/**
* \brief A utility function to check if the group is defined.
* \param trajectory Trajectory to smooth.
*/
[[nodiscard]] static bool validateGroup(const robot_trajectory::RobotTrajectory& trajectory);

/**
* \brief A utility function to get bounds from a JointModelGroup and save them for Ruckig.
* \param max_velocity_scaling_factor Scale all joint velocity limits by this factor. Usually 1.0.
* \param max_acceleration_scaling_factor Scale all joint acceleration limits by this factor. Usually 1.0.
* \param group The RobotModel and the limits are retrieved from this group.
* \param[out] ruckig_input The limits are stored in this ruckig::InputParameter, for use in Ruckig.
*/
[[nodiscard]] static bool getRobotModelBounds(const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor,
moveit::core::JointModelGroup const* const group,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);

/**
* \brief Feed previous output back as input for next iteration. Get next target state from the next waypoint.
* \param current_waypoint The nominal current state
* \param next_waypoint The nominal, desired state at the next waypoint
* \param joint_group The MoveIt JointModelGroup of interest
* \param ruckig_input Output. The Rucking parameters for the next iteration
* \param[out] ruckig_input The Rucking parameters for the next iteration
*/
static void getNextRuckigInput(const moveit::core::RobotStatePtr& current_waypoint,
const moveit::core::RobotStatePtr& next_waypoint,
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<0>& ruckig_input);
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);

/**
* \brief Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same values
* \param rucking_input Input parameters to Ruckig. Initialized here.
* \param ruckig_output Output from the Ruckig algorithm. Initialized here.
* \param first_waypoint The Ruckig input/output parameters are initialized to the values at this waypoint
* \param joint_group The MoveIt JointModelGroup of interest
* \param[out] rucking_input Input parameters to Ruckig. Initialized here.
* \param[out] ruckig_output Output from the Ruckig algorithm. Initialized here.
*/
static void initializeRuckigState(const moveit::core::RobotState& first_waypoint,
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
ruckig::OutputParameter<ruckig::DynamicDOFs>& ruckig_output);

/**
* \brief A utility function to instantiate and run Ruckig for a series of waypoints.
* \param[in, out] trajectory Trajectory to smooth.
* \param[in, out] ruckig_input Necessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk)
*/
static void initializeRuckigState(ruckig::InputParameter<0>& ruckig_input, ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotState& first_waypoint,
const moveit::core::JointModelGroup* joint_group);
[[nodiscard]] static bool runRuckig(robot_trajectory::RobotTrajectory& trajectory,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);
};
} // namespace trajectory_processing
136 changes: 113 additions & 23 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,8 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor)
{
moveit::core::JointModelGroup const* const group = trajectory.getGroup();
if (!group)
if (!validateGroup(trajectory))
{
RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
return false;
}

Expand All @@ -72,26 +70,96 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
return true;
}

// Cache the trajectory in case we need to reset it
robot_trajectory::RobotTrajectory original_trajectory =
robot_trajectory::RobotTrajectory(trajectory, true /* deep copy */);

// Kinematic limits (vels/accels/jerks) from RobotModel
moveit::core::JointModelGroup const* const group = trajectory.getGroup();
const size_t num_dof = group->getVariableCount();
ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
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.");
return false;
}

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

// Instantiate the smoother
double timestep = trajectory.getAverageSegmentDuration();
std::unique_ptr<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_ptr;
ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
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)
{
if (!validateGroup(trajectory))
{
return false;
}

const size_t num_waypoints = trajectory.getWayPointCount();
if (num_waypoints < 2)
{
RCLCPP_WARN(LOGGER,
"Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
return true;
}

// Set default kinematic limits (vels/accels/jerks)
moveit::core::JointModelGroup const* const group = trajectory.getGroup();
const size_t num_dof = group->getVariableCount();
ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ 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.");
return false;
}

// Kinematic limits (vel/accel/jerk)
// Check if custom limits were supplied as arguments to overwrite the defaults
const std::vector<std::string>& vars = group->getVariableNames();
const unsigned num_joints = group->getVariableCount();
for (size_t j = 0; j < num_joints; ++j)
{
// Velocity
auto it = velocity_limits.find(vars[j]);
if (it != velocity_limits.end())
{
ruckig_input.max_velocity.at(j) = it->second;
}
// Acceleration
it = acceleration_limits.find(vars[j]);
if (it != acceleration_limits.end())
{
ruckig_input.max_acceleration.at(j) = it->second;
}
// Jerk
it = jerk_limits.find(vars[j]);
if (it != jerk_limits.end())
{
ruckig_input.max_jerk.at(j) = it->second;
}
}

return runRuckig(trajectory, ruckig_input);
}

bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& trajectory)
{
moveit::core::JointModelGroup const* const group = trajectory.getGroup();
if (!group)
{
RCLCPP_ERROR(LOGGER, "The planner did not set the group the plan was computed for");
return false;
}
return true;
}

bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor,
moveit::core::JointModelGroup const* const group,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
{
const size_t num_dof = group->getVariableCount();
const std::vector<std::string>& vars = group->getVariableNames();
const moveit::core::RobotModel& rmodel = group->getParentModel();
const std::vector<int>& move_group_idx = group->getVariableIndexList();
for (size_t i = 0; i < num_dof; ++i)
{
const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));
Expand Down Expand Up @@ -135,8 +203,30 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
}
}

return true;
}

bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
{
const size_t num_waypoints = trajectory.getWayPointCount();
moveit::core::JointModelGroup const* const group = trajectory.getGroup();
const size_t num_dof = group->getVariableCount();
ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };
const std::vector<int>& move_group_idx = group->getVariableIndexList();

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

// Initialize the smoother
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
double timestep = trajectory.getAverageSegmentDuration();
std::unique_ptr<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_ptr;
ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output);

// Cache the trajectory in case we need to reset it
robot_trajectory::RobotTrajectory original_trajectory =
robot_trajectory::RobotTrajectory(trajectory, true /* deep copy */);

ruckig::Result ruckig_result;
double duration_extension_factor = 1;
Expand Down Expand Up @@ -186,7 +276,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
target_state->update();
}
ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output);
// Begin the while() loop again
break;
}
Expand All @@ -202,10 +292,10 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
return true;
}

void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_input,
ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotState& first_waypoint,
const moveit::core::JointModelGroup* joint_group)
void RuckigSmoothing::initializeRuckigState(const moveit::core::RobotState& first_waypoint,
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
ruckig::OutputParameter<ruckig::DynamicDOFs>& ruckig_output)
{
const size_t num_dof = joint_group->getVariableCount();
const std::vector<int>& idx = joint_group->getVariableIndexList();
Expand Down Expand Up @@ -237,7 +327,7 @@ void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_in
void RuckigSmoothing::getNextRuckigInput(const moveit::core::RobotStatePtr& current_waypoint,
const moveit::core::RobotStatePtr& next_waypoint,
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<0>& ruckig_input)
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
{
const size_t num_dof = joint_group->getVariableCount();
const std::vector<int>& idx = joint_group->getVariableIndexList();
Expand Down
25 changes: 25 additions & 0 deletions moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,31 @@ TEST_F(RuckigTests, basic_trajectory)
smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));
}

TEST_F(RuckigTests, basic_trajectory_with_custom_limits)
{
// Check the version of computeTimeStamps that takes custom velocity/acceleration limits

moveit::core::RobotState robot_state(robot_model_);
robot_state.setToDefaultValues();
// First waypoint is default joint positions
trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);

// Second waypoint has slightly-different joint positions
std::vector<double> joint_positions;
robot_state.copyJointGroupPositions(JOINT_GROUP, joint_positions);
joint_positions.at(0) += 0.05;
robot_state.setJointGroupPositions(JOINT_GROUP, joint_positions);
robot_state.update();
trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);

// 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 } };
std::unordered_map<std::string, double> jerk_limits{ { "panda_joint5", 100.0 } };

EXPECT_TRUE(smoother_.applySmoothing(*trajectory_, vel_limits, accel_limits, jerk_limits));
}

TEST_F(RuckigTests, trajectory_duration)
{
// Compare against the OJET online trajectory generator: https://www.trajectorygenerator.com/ojet-online/
Expand Down

0 comments on commit e4c7b71

Please sign in to comment.