Skip to content

Commit

Permalink
Fix Ruckig termination condition
Browse files Browse the repository at this point in the history
  • Loading branch information
@AndyZe
AndyZe committed Mar 2, 2023
1 parent c27cb30 commit 572014b
Show file tree
Hide file tree
Showing 3 changed files with 95 additions and 55 deletions.
18 changes: 16 additions & 2 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 @@ -110,8 +110,8 @@ class RuckigSmoothing
* \param joint_group The MoveIt JointModelGroup of interest
* \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,
static void getNextRuckigInput(const moveit::core::RobotStateConstPtr& current_waypoint,
const moveit::core::RobotStateConstPtr& next_waypoint,
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);

Expand Down Expand Up @@ -149,5 +149,19 @@ class RuckigSmoothing
*/
[[nodiscard]] static bool runRuckig(robot_trajectory::RobotTrajectory& trajectory,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);

/**
* \brief Extend the duration of every trajectory segment
* \param[in] duration_extension_factor A number greater than 1. Extend every timestep by this much.
* \param[in] num_waypoints Number of waypoints in the trajectory.
* \param[in] num_dof Degrees of freedom in the manipulator.
* \param[in] move_group_idx For accessing the joints of interest out of the full RobotState.
* \param[in] original_trajectory Durations are extended based on the data in this original trajectory.
* \param[in, out] trajectory This trajectory will be returned with modified waypoint durations.
*/
static void extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints,
const size_t num_dof, const std::vector<int>& move_group_idx,
const robot_trajectory::RobotTrajectory& original_trajectory,
robot_trajectory::RobotTrajectory& trajectory);
};
} // namespace trajectory_processing
103 changes: 58 additions & 45 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,12 +81,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
return false;
}

auto ruckig_result = runRuckigInBatches(num_waypoints, trajectory, ruckig_input);
if (ruckig_result.has_value())
{
trajectory = ruckig_result.value();
}
return ruckig_result.has_value(); // Ruckig failed to smooth the trajectory
return runRuckig(trajectory, ruckig_input);
}

bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
Expand Down Expand Up @@ -144,12 +139,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
}
}

auto ruckig_result = runRuckigInBatches(num_waypoints, trajectory, ruckig_input);
if (ruckig_result.has_value())
{
trajectory = ruckig_result.value();
}
return ruckig_result.has_value(); // Ruckig failed to smooth the trajectory
return runRuckig(trajectory, ruckig_input);
}

std::optional<robot_trajectory::RobotTrajectory>
Expand Down Expand Up @@ -313,15 +303,12 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
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
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);
ruckig::Ruckig<ruckig::DynamicDOFs> ruckig(num_dof, trajectory.getAverageSegmentDuration());
initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output);

// Cache the trajectory in case we need to reset it
Expand All @@ -340,50 +327,48 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
getNextRuckigInput(trajectory.getWayPointPtr(waypoint_idx), next_waypoint, group, ruckig_input);

// Run Ruckig
ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);
ruckig_result = ruckig.update(ruckig_input, ruckig_output);

if ((waypoint_idx == num_waypoints - 2) && ruckig_result == ruckig::Result::Finished)
// The difference between Result::Working and Result::Finished is that Finished can be reached in one
// Ruckig timestep (constructor parameter). Both are acceptable for trajectories.
// (The difference is only relevant for streaming mode.)

// If successful and at the last trajectory segment
if ((waypoint_idx == num_waypoints - 2) &&
(ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished))
{
trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration());
smoothing_complete = true;
break;
}

// TODO(andyz): why doesn't this work?
// // If successful, on to the next waypoint
// if (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished)
// {
// trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration());
// continue;
// }

// Extend the trajectory duration if Ruckig could not reach the waypoint successfully
if (ruckig_result != ruckig::Result::Finished)
if (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 */);
for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx)
{
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);

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

initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output);
// Begin the while() loop again
// Begin the for() loop again
break;
}
}
}

if (ruckig_result != ruckig::Result::Finished)
if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)
{
RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
return false;
Expand All @@ -392,6 +377,34 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
return true;
}

void RuckigSmoothing::extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints,
const size_t num_dof, const std::vector<int>& move_group_idx,
const robot_trajectory::RobotTrajectory& original_trajectory,
robot_trajectory::RobotTrajectory& trajectory)
{
for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx)
{
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);
double 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();
}
}

void RuckigSmoothing::initializeRuckigState(const moveit::core::RobotState& first_waypoint,
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
Expand Down Expand Up @@ -424,8 +437,8 @@ void RuckigSmoothing::initializeRuckigState(const moveit::core::RobotState& firs
ruckig_output.new_acceleration = ruckig_input.current_acceleration;
}

void RuckigSmoothing::getNextRuckigInput(const moveit::core::RobotStatePtr& current_waypoint,
const moveit::core::RobotStatePtr& next_waypoint,
void RuckigSmoothing::getNextRuckigInput(const moveit::core::RobotStateConstPtr& current_waypoint,
const moveit::core::RobotStateConstPtr& next_waypoint,
const moveit::core::JointModelGroup* joint_group,
ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input)
{
Expand Down
29 changes: 21 additions & 8 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 @@ -63,6 +63,8 @@ TEST_F(RuckigTests, basic_trajectory)
{
moveit::core::RobotState robot_state(robot_model_);
robot_state.setToDefaultValues();
robot_state.zeroVelocities();
robot_state.zeroAccelerations();
// First waypoint is default joint positions
trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);

Expand All @@ -71,7 +73,6 @@ TEST_F(RuckigTests, basic_trajectory)
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);

EXPECT_TRUE(
Expand All @@ -84,6 +85,8 @@ TEST_F(RuckigTests, basic_trajectory_with_custom_limits)

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

Expand All @@ -92,7 +95,6 @@ TEST_F(RuckigTests, basic_trajectory_with_custom_limits)
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
Expand All @@ -106,24 +108,35 @@ TEST_F(RuckigTests, basic_trajectory_with_custom_limits)
TEST_F(RuckigTests, trajectory_duration)
{
// Compare against the OJET online trajectory generator: https://www.trajectorygenerator.com/ojet-online/
const double ideal_duration = 0.21025;
// Limits can be verified like this. Note that Ruckig applies defaults if the RobotModel has none.
// robot_model_->printModelInfo(std::cerr);
const double ideal_duration = 0.210;

moveit::core::RobotState robot_state(robot_model_);
robot_state.setToDefaultValues();
robot_state.zeroVelocities();
robot_state.zeroAccelerations();
// Special attention to Joint 0. It is the only joint to move in this test.
// Zero velocities and accelerations at the endpoints
robot_state.setVariablePosition("panda_joint1", 0.0);
robot_state.update();
trajectory_->addSuffixWayPoint(robot_state, 0.0);

robot_state.setVariablePosition("panda_joint1", 0.1);
robot_state.update();
trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);

EXPECT_TRUE(
smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */));

// No waypoint durations of zero except the first
for (size_t waypoint_idx = 1; waypoint_idx < trajectory_->getWayPointCount() - 1; ++waypoint_idx)
{
EXPECT_NE(trajectory_->getWayPointDurationFromPrevious(waypoint_idx), 0);
}

// The trajectory duration should be within 10% of the analytical solution since the implementation here extends
// the duration by 10% at every iteration.
EXPECT_GT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 0.9999 * ideal_duration);
EXPECT_LT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 1.5 * ideal_duration);
EXPECT_LT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 1.11 * ideal_duration);
}

TEST_F(RuckigTests, single_waypoint)
Expand All @@ -133,11 +146,11 @@ TEST_F(RuckigTests, single_waypoint)

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

robot_state.update();

// Trajectory should not change
auto first_waypoint_input = robot_state;

Expand Down

0 comments on commit 572014b

Please sign in to comment.