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

Jump to bottom

Ruckig smoothing cleanup #1111

Merged
merged 12 commits into from
Apr 12, 2022
Merged

Ruckig smoothing cleanup #1111

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
3d87e58
Simplify. Use Ruckig as a check if duration needs extension.
Mar 16, 2022
865888d
Add a test for a single waypoint
Mar 16, 2022
eb328ce
Add a unit test that actually requires math
Mar 16, 2022
3c62c13
Use a traj deep copy. Right-const everywhere. Minor cleanup.
Mar 17, 2022
7db9e49
Fix the test timestep
Mar 18, 2022
d7fbc3d
Read jerk from the robot model
Mar 18, 2022
87d473f
Revert to right-const everywhere
Apr 1, 2022
100ad5f
Simplify jerk limit expression
AndyZe Apr 1, 2022
070b159
Add a helpful hint if no vel/accel/jerk limits are defined.
Apr 3, 2022
c4afbae
Update test conditions since I changed the jerk limit
Apr 4, 2022
05afc04
Rebase cleanup
Apr 12, 2022
4a510cc
Merge branch 'main' into andyz/duration_calc
Apr 12, 2022
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
31 changes: 1 addition & 30 deletions ...t_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

/* Author: Jack Center, Wyatt Rees, Andy Zelenak */
/* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */

#pragma once

Expand Down Expand Up @@ -62,35 +62,6 @@ class RuckigSmoothing
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<0>& ruckig_input);

/**
* \brief Check for lagging motion of any joint at a waypoint.
* \param joint_group The MoveIt JointModelGroup of interest
* \param ruckig_input Input parameters to Ruckig
* \param ruckig_output Output parameters from Ruckig
* \return true if lagging motion is detected on any joint
*/
static bool checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group,
const ruckig::InputParameter<0>& ruckig_input,
const ruckig::OutputParameter<0>& ruckig_output);

/**
* \brief Return L2-norm of velocity, taking all joints into account.
* \param ruckig_input Input parameters to Ruckig
* \param joint_group The MoveIt JointModelGroup of interest
*/
static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input,
const moveit::core::JointModelGroup* joint_group);

/**
* \brief Check if the joint positions of two waypoints are very similar.
* \param prev_waypoint State at waypoint i-1
* \param prev_waypoint State at waypoint i
* \joint_group The MoveIt JointModelGroup of interest
*/
static bool checkForIdenticalWaypoints(const moveit::core::RobotState& prev_waypoint,
const moveit::core::RobotState& next_waypoint,
const moveit::core::JointModelGroup* joint_group);

/**
* \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.
Expand Down
192 changes: 73 additions & 119 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

/* Author: Jack Center, Wyatt Rees, Andy Zelenak */
/* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */

#include <algorithm>
#include <cmath>
Expand All @@ -46,20 +46,18 @@ namespace trajectory_processing
namespace
{
const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.ruckig_traj_smoothing");
constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s
constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2
constexpr double DEFAULT_MAX_JERK = 20; // rad/s^3
constexpr double IDENTICAL_POSITION_EPSILON = 1e-3; // rad
constexpr double MAX_DURATION_EXTENSION_FACTOR = 5.0;
constexpr double DEFAULT_MAX_VELOCITY = 5; // rad/s
constexpr double DEFAULT_MAX_ACCELERATION = 10; // rad/s^2
constexpr double DEFAULT_MAX_JERK = 1000; // rad/s^3
constexpr double MAX_DURATION_EXTENSION_FACTOR = 10.0;
constexpr double DURATION_EXTENSION_FRACTION = 1.1;
constexpr double MINIMUM_VELOCITY_SEARCH_MAGNITUDE = 0.01; // rad/s. Stop searching when velocity drops below this
} // namespace

bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
const double max_velocity_scaling_factor,
const double max_acceleration_scaling_factor)
{
const moveit::core::JointModelGroup* group = trajectory.getGroup();
moveit::core::JointModelGroup const* const group = trajectory.getGroup();
if (!group)
{
RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
Expand All @@ -69,34 +67,36 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
const size_t num_waypoints = trajectory.getWayPointCount();
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
if (num_waypoints < 2)
{
RCLCPP_ERROR(LOGGER, "Trajectory does not have enough points to smooth with Ruckig");
return false;
RCLCPP_WARN(LOGGER,
"Trajectory does not have enough points to smooth with Ruckig. Returning an unmodified trajectory.");
return true;
}

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

const size_t num_dof = group->getVariableCount();
AndyZe marked this conversation as resolved.
Show resolved Hide resolved

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

// Instantiate the smoother
double timestep = trajectory.getAverageSegmentDuration();
std::unique_ptr<ruckig::Ruckig<0>> ruckig_ptr;
ruckig_ptr = std::make_unique<ruckig::Ruckig<0>>(num_dof, timestep);
ruckig::InputParameter<0> ruckig_input{ num_dof };
ruckig::OutputParameter<0> ruckig_output{ num_dof };
std::unique_ptr<ruckig::Ruckig<ruckig::DynamicDOFs>> ruckig_ptr;
ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
ruckig::InputParameter<ruckig::DynamicDOFs> ruckig_input{ num_dof };
ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };

// Initialize the smoother
const std::vector<int>& idx = group->getVariableIndexList();
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);

// Kinematic limits (vel/accel/jerk)
const std::vector<std::string>& vars = group->getVariableNames();
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
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)
{
// TODO(andyz): read this from the joint group if/when jerk limits are added to the JointModel
ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;

const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars.at(i));

// This assumes min/max bounds are symmetric
Expand All @@ -106,6 +106,10 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
}
else
{
RCLCPP_WARN_STREAM_ONCE(LOGGER,
"Joint velocity limits are not defined. Using the default "
<< DEFAULT_MAX_VELOCITY
<< " rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
ruckig_input.max_velocity.at(i) = max_velocity_scaling_factor * DEFAULT_MAX_VELOCITY;
}
if (bounds.acceleration_bounded_)
Expand All @@ -114,13 +118,29 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
}
else
{
RCLCPP_WARN_STREAM_ONCE(LOGGER,
"Joint acceleration limits are not defined. Using the default "
<< DEFAULT_MAX_ACCELERATION
<< " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
ruckig_input.max_acceleration.at(i) = max_acceleration_scaling_factor * DEFAULT_MAX_ACCELERATION;
}
ruckig_input.max_jerk.at(i) = bounds.jerk_bounded_ ? bounds.max_jerk_ : DEFAULT_MAX_JERK;
if (bounds.jerk_bounded_)
{
ruckig_input.max_jerk.at(i) = bounds.max_jerk_;
}
else
{
RCLCPP_WARN_STREAM_ONCE(LOGGER, "Joint jerk limits are not defined. Using the default "
<< DEFAULT_MAX_JERK
<< " rad/s^3. You can define jerk limits in joint_limits.yaml.");
ruckig_input.max_jerk.at(i) = DEFAULT_MAX_JERK;
}
}

ruckig::Result ruckig_result;
bool smoothing_complete = false;
double duration_extension_factor = 1;
bool smoothing_complete = false;
while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
{
for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
Expand All @@ -132,76 +152,48 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
// Run Ruckig
ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);

// If the requested velocity is too great, a joint can actually "move backward" to give itself more time to
// accelerate to the target velocity. Iterate and decrease velocities until that behavior is gone.
bool backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output);

double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group);
while (backward_motion_detected && (velocity_magnitude > MINIMUM_VELOCITY_SEARCH_MAGNITUDE))
{
// Skip repeated waypoints with no change in position. Ruckig does not handle this well and there's really no
// need to smooth it Simply set it equal to the previous (identical) waypoint.
if (checkForIdenticalWaypoints(*trajectory.getWayPointPtr(waypoint_idx), *next_waypoint, trajectory.getGroup()))
{
*next_waypoint = trajectory.getWayPoint(waypoint_idx);
continue;
}

// decrease target velocity
for (size_t joint = 0; joint < num_dof; ++joint)
{
ruckig_input.target_velocity.at(joint) *= 0.9;
// Propagate the change in velocity to acceleration, too.
// We don't change the position to ensure the exact target position is achieved.
ruckig_input.target_acceleration.at(joint) =
(ruckig_input.target_velocity.at(joint) - ruckig_output.new_velocity.at(joint)) / timestep;
}
velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group);
// Run Ruckig
ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);

// check for backward motion
backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output);
}

// Overwrite pos/vel/accel of the target waypoint
for (size_t joint = 0; joint < num_dof; ++joint)
if ((waypoint_idx == num_waypoints - 2) && ruckig_result == ruckig::Result::Finished)
{
next_waypoint->setVariablePosition(idx.at(joint), ruckig_output.new_position.at(joint));
next_waypoint->setVariableVelocity(idx.at(joint), ruckig_output.new_velocity.at(joint));
next_waypoint->setVariableAcceleration(idx.at(joint), ruckig_output.new_acceleration.at(joint));
smoothing_complete = true;
break;
}
next_waypoint->update();
}

// If ruckig failed, the duration of the seed trajectory likely wasn't long enough.
// Try duration extension several times.
// TODO: see issue 767. (https://github.com/ros-planning/moveit2/issues/767)
if (ruckig_result == ruckig::Result::Working)
{
smoothing_complete = true;
}
else
{
// If Ruckig failed, it's likely because the original seed trajectory did not have a long enough duration when
// jerk is taken into account. Extend the duration and try again.
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
duration_extension_factor *= DURATION_EXTENSION_FRACTION;
for (size_t waypoint_idx = 1; waypoint_idx < num_waypoints; ++waypoint_idx)
// Extend the trajectory duration if Ruckig could not reach the waypoint successfully
if (ruckig_result != ruckig::Result::Finished)
{
trajectory.setWayPointDurationFromPrevious(
waypoint_idx, DURATION_EXTENSION_FRACTION * trajectory.getWayPointDurationFromPrevious(waypoint_idx));
// TODO(andyz): re-calculate waypoint velocity and acceleration here?
duration_extension_factor *= DURATION_EXTENSION_FRACTION;
// Reset the trajectory
trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */);
for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx)
Copy link
Member

Choose a reason for hiding this comment

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

wouldn't it make sense to only stretch until the current waypoint_idx?

Copy link
Member Author

@AndyZe AndyZe Apr 1, 2022 •
edited
Loading

Choose a reason for hiding this comment

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

I think that's good logic and it should work but it complicates the code for a couple reasons:

  • You have to worry about the index zero edge case. (waypoint_idx == 0)
  • The tracking of duration_extension_factor gets tricky

Overall I don't think it's worth the complication. Usually MoveIt trajectories are pretty small, like 100 waypoints or less. If you're working with very large trajectories like the ~7,000 waypoints I showed above, you should feed 10 at a time or so into this algorithm.

Your suggestion is even a pessimization w.r.t. run time for most cases. Increasing the timestep for every waypoint one at a time is computationally expensive.

Anyway, I did try this replacement but it often fails. Likely due to duration_extension_factor exceeding the limit but I didn't dig into it too much.

        // Reset the trajectory
        trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */);

        // If failure occurred at index 0, stretch up to index 1.
        // (Stretching index 0 is meaningless)
        // Else, stretch up to (current waypoint)
        size_t stretch_to_idx = (waypoint_idx == 0) ? 1 : waypoint_idx + 1;

        for (size_t time_stretch_idx = 1; time_stretch_idx <= stretch_to_idx; ++time_stretch_idx)
        {
          // Do stretching here...

{
trajectory.setWayPointDurationFromPrevious(
time_stretch_idx,
duration_extension_factor * original_trajectory.getWayPointDurationFromPrevious(time_stretch_idx));
// re-calculate waypoint velocity and acceleration
auto target_state = trajectory.getWayPointPtr(time_stretch_idx);
const auto prev_state = trajectory.getWayPointPtr(time_stretch_idx - 1);
timestep = trajectory.getAverageSegmentDuration();
for (size_t joint = 0; joint < num_dof; ++joint)
{
target_state->setVariableVelocity(move_group_idx.at(joint),
(1 / duration_extension_factor) *
target_state->getVariableVelocity(move_group_idx.at(joint)));

double prev_velocity = prev_state->getVariableVelocity(move_group_idx.at(joint));
double curr_velocity = target_state->getVariableVelocity(move_group_idx.at(joint));
target_state->setVariableAcceleration(move_group_idx.at(joint), (curr_velocity - prev_velocity) / timestep);
}
target_state->update();
}
ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group);
// Begin the while() loop again
break;
}

timestep = trajectory.getAverageSegmentDuration();
ruckig_ptr = std::make_unique<ruckig::Ruckig<0>>(num_dof, timestep);
}
}

// Either of these results is acceptable.
// Working means smoothing worked well but the final target position wasn't exactly achieved (I think) -- Andy Z.
if ((ruckig_result != ruckig::Result::Working) && (ruckig_result != ruckig::Result::Finished))
if (ruckig_result != ruckig::Result::Finished)
{
RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
return false;
Expand Down Expand Up @@ -237,44 +229,6 @@ void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_in
ruckig_output.new_acceleration = ruckig_input.current_acceleration;
}

bool RuckigSmoothing::checkForIdenticalWaypoints(const moveit::core::RobotState& prev_waypoint,
const moveit::core::RobotState& next_waypoint,
const moveit::core::JointModelGroup* joint_group)
{
double magnitude_position_difference = prev_waypoint.distance(next_waypoint, joint_group);

return (magnitude_position_difference <= IDENTICAL_POSITION_EPSILON);
}

double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input,
const moveit::core::JointModelGroup* joint_group)
{
const size_t num_dof = joint_group->getVariableCount();
double vel_magnitude = 0;
for (size_t joint = 0; joint < num_dof; ++joint)
{
vel_magnitude += ruckig_input.target_velocity.at(joint) * ruckig_input.target_velocity.at(joint);
}
return sqrt(vel_magnitude);
}

bool RuckigSmoothing::checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group,
const ruckig::InputParameter<0>& ruckig_input,
const ruckig::OutputParameter<0>& ruckig_output)
{
const size_t num_dof = joint_group->getVariableCount();
// Check for backward motion of any joint
for (size_t joint = 0; joint < num_dof; ++joint)
{
// This indicates the jerk-limited output lags the target output
if ((ruckig_output.new_velocity.at(joint) / ruckig_input.target_velocity.at(joint)) < 1)
{
return true;
}
}
return false;
}

void RuckigSmoothing::getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotStatePtr& next_waypoint,
const moveit::core::JointModelGroup* joint_group,
Expand Down
Loading