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

Sync Ruckig with MoveIt1 #2596

Merged
merged 4 commits into from Dec 12, 2023
Merged

Sync Ruckig with MoveIt1 #2596

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
20e7ce6
Debug Ruckig tests (MoveIt1 3300)
AndyZe Dec 11, 2023
ff327bf
Add a test, termination condition bugfix (MoveIt1 3348)
AndyZe Dec 11, 2023
f878e3b
Mitigate Ruckig overshoot (MoveIt1 3376)
AndyZe Dec 11, 2023
89a6be2
Small variable fixup
AndyZe Dec 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
24 changes: 15 additions & 9 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Open in desktop
Expand Up @@ -50,7 +50,7 @@ namespace
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 MAX_DURATION_EXTENSION_FACTOR = 50.0;
constexpr double DURATION_EXTENSION_FRACTION = 1.1;
// If "mitigate_overshoot" is enabled, overshoot is checked with this timestep
constexpr double OVERSHOOT_CHECK_PERIOD = 0.01; // sec
Expand Down Expand Up @@ -251,7 +251,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
const size_t num_waypoints = trajectory.getWayPointCount();
const moveit::core::JointModelGroup* const group = trajectory.getGroup();
const size_t num_dof = group->getVariableCount();
ruckig::OutputParameter<ruckig::DynamicDOFs> ruckig_output{ num_dof };
ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector> ruckig_output(num_dof);

// This lib does not work properly when angles wrap, so we need to unwind the path first
trajectory.unwind();
Expand All @@ -268,7 +268,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
double duration_extension_factor = 1;
bool smoothing_complete = false;
size_t waypoint_idx = 0;
while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
while ((duration_extension_factor <= MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
{
while (waypoint_idx < num_waypoints - 1)
{
Expand All @@ -278,15 +278,14 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
getNextRuckigInput(curr_waypoint, next_waypoint, group, ruckig_input);

// Run Ruckig
ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector> ruckig_trajectory(num_dof);
ruckig_result = ruckig.calculate(ruckig_input, ruckig_trajectory);
ruckig_result = ruckig.calculate(ruckig_input, ruckig_output);

// Step through the trajectory at the given OVERSHOOT_CHECK_PERIOD and check for overshoot.
// We will extend the duration to mitigate it.
bool overshoots = false;
if (mitigate_overshoot)
{
overshoots = checkOvershoot(ruckig_trajectory, num_dof, ruckig_input, overshoot_threshold);
overshoots = checkOvershoot(ruckig_output, num_dof, ruckig_input, overshoot_threshold);
}

// The difference between Result::Working and Result::Finished is that Finished can be reached in one
Expand All @@ -297,7 +296,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
if (!overshoots && (waypoint_idx == num_waypoints - 2) &&
(ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished))
{
trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_trajectory.get_duration());
trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.get_duration());
smoothing_complete = true;
break;
}
Expand All @@ -306,19 +305,27 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
if (overshoots || (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished))
{
duration_extension_factor *= DURATION_EXTENSION_FRACTION;
// Reset the trajectory
trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */);

const std::vector<int>& move_group_idx = group->getVariableIndexList();
extendTrajectoryDuration(duration_extension_factor, waypoint_idx, num_dof, move_group_idx, original_trajectory,
trajectory);

initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input);
// Continue the loop from failed segment, but with increased duration extension factor
// Begin the for() loop again
break;
}
++waypoint_idx;
}
}

if (duration_extension_factor > MAX_DURATION_EXTENSION_FACTOR)
{
RCLCPP_ERROR_STREAM(getLogger(),
"Ruckig extended the trajectory duration to its maximum and still did not find a solution");
}

if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)
{
RCLCPP_ERROR_STREAM(getLogger(), "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
Expand Down Expand Up @@ -347,7 +354,6 @@ void RuckigSmoothing::extendTrajectoryDuration(const double duration_extension_f
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);
Expand Down