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 Feb 22, 2023
1 parent cc63547 commit 003d748
Show file tree
Hide file tree
Showing 3 changed files with 44 additions and 32 deletions.
4 changes: 2 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
47 changes: 25 additions & 22 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 @@ -320,8 +310,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,

// 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, timestep);
initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output);

// Cache the trajectory in case we need to reset it
Expand All @@ -340,16 +329,31 @@ 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);

// 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 ((waypoint_idx == num_waypoints - 2) && ruckig_result == ruckig::Result::Finished)
// 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, 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
Expand All @@ -375,15 +379,14 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
}
target_state->update();
}
ruckig_ptr = std::make_unique<ruckig::Ruckig<ruckig::DynamicDOFs>>(num_dof, timestep);
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 Down Expand Up @@ -424,8 +427,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
25 changes: 17 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,31 @@ 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;
const double ideal_duration = 0.310;

moveit::core::RobotState robot_state(robot_model_);
robot_state.setToDefaultValues();
// 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 +142,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 003d748

Please sign in to comment.