Skip to content

Commit

Permalink
Merge branch 'main' into trajectory_iterator
Browse files Browse the repository at this point in the history
  • Loading branch information
@DLu
DLu authored Nov 1, 2021
2 parents 8ea09a3 + 48625f6 commit f3d7964
Show file tree
Hide file tree
Showing 4 changed files with 272 additions and 72 deletions.
8 changes: 6 additions & 2 deletions moveit_core/trajectory_processing/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,10 +36,14 @@ if(BUILD_TESTING)

ament_add_gtest(test_time_parameterization test/test_time_parameterization.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")

target_link_libraries(test_time_parameterization moveit_test_utils ${MOVEIT_LIB_NAME})

ament_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp)

target_link_libraries(test_time_optimal_trajectory_generation ${MOVEIT_LIB_NAME})

ament_add_gtest(test_ruckig_traj_smoothing test/test_ruckig_traj_smoothing.cpp)
target_link_libraries(test_ruckig_traj_smoothing
${MOVEIT_LIB_NAME}
moveit_test_utils
)
endif()
42 changes: 36 additions & 6 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 @@ -52,23 +52,53 @@ class RuckigSmoothing
private:
/**
* \brief Feed previous output back as input for next iteration. Get next target state from the next waypoint.
* \param ruckig_output The previous output from Ruckig
* \param next_waypoint The nominal, desired state at the next waypoint
* \param num_dof Number of actuated joints
* \param idx MoveIt list of joint group indices
* \param ruckig_input Output. The Rucking parameters for the next iteration
*/
static void getNextCurrentTargetStates(ruckig::InputParameter<0>& ruckig_input,
ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
const std::vector<int>& idx);
static void getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
const std::vector<int>& idx, ruckig::InputParameter<0>& ruckig_input);

/**
* \brief Check for lagging motion of any joint at a waypoint.
*
* \return true if lagging motion is detected on any joint
* \param num_dof Number of actuated joints
* \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 size_t num_dof, 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 num_dof Number of actuated joints
*/
static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof);

/**
* \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.
* \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 num_dof Number of actuated joints
* \param joint_idx MoveIt list of joint group indices
*/
static void initializeRuckigState(ruckig::InputParameter<0>& ruckig_input, ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotState& first_waypoint, size_t num_dof,
const std::vector<int>& joint_idx);
};
} // namespace trajectory_processing
186 changes: 122 additions & 64 deletions moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,14 @@ namespace trajectory_processing
{
namespace
{
static 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
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 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,
Expand Down Expand Up @@ -76,31 +80,14 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto

// Instantiate the smoother
double timestep = trajectory.getAverageSegmentDuration();
ruckig::Ruckig<0> ruckig{ num_dof, timestep };
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 };

// Initialize the smoother
// ruckig_input.synchronization = ruckig::Synchronization::Time;
// ruckig_input.interface = ruckig::Interface::Velocity;
const std::vector<int>& idx = group->getVariableIndexList();
std::vector<double> current_positions_vector(num_dof);
std::vector<double> current_velocities_vector(num_dof);
std::vector<double> current_accelerations_vector(num_dof);
moveit::core::RobotStatePtr waypoint = trajectory.getFirstWayPointPtr();
for (size_t i = 0; i < num_dof; ++i)
{
current_positions_vector.at(i) = waypoint->getVariablePosition(idx.at(i));
current_velocities_vector.at(i) = waypoint->getVariableVelocity(idx.at(i));
current_accelerations_vector.at(i) = waypoint->getVariableAcceleration(idx.at(i));
}
std::copy_n(current_positions_vector.begin(), num_dof, ruckig_input.current_position.begin());
std::copy_n(current_velocities_vector.begin(), num_dof, ruckig_input.current_velocity.begin());
std::copy_n(current_accelerations_vector.begin(), num_dof, ruckig_input.current_acceleration.begin());
// Initialize output data struct
ruckig_output.new_position = ruckig_input.current_position;
ruckig_output.new_velocity = ruckig_input.current_velocity;
ruckig_output.new_acceleration = ruckig_input.current_acceleration;
initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, idx);

// Kinematic limits (vel/accel/jerk)
const std::vector<std::string>& vars = group->getVariableNames();
Expand Down Expand Up @@ -132,61 +119,130 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto
}

ruckig::Result ruckig_result;
for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
bool smoothing_complete = false;
double duration_extension_factor = 1;
while ((duration_extension_factor < MAX_DURATION_EXTENSION_FACTOR) && !smoothing_complete)
{
moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);
for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
{
moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);

getNextCurrentTargetStates(ruckig_input, ruckig_output, next_waypoint, num_dof, idx);
getNextRuckigInput(ruckig_output, next_waypoint, num_dof, idx, ruckig_input);

// Run Ruckig
ruckig_result = ruckig.update(ruckig_input, ruckig_output);
// 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(num_dof, 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(num_dof, ruckig_input, ruckig_output);

double minimum_velocity_magnitude = 0.01; // rad/s
double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
while (backward_motion_detected && rclcpp::ok() && (velocity_magnitude > minimum_velocity_magnitude))
{
// decrease target velocity
double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
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;
// Propogate 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, num_dof);
// Run Ruckig
ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);

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

// Overwrite pos/vel/accel of the target waypoint
for (size_t joint = 0; joint < num_dof; ++joint)
{
ruckig_input.target_velocity.at(joint) *= 0.9;
// Propogate the change in velocity to position and acceleration, too.
// This derives from a first-order approximation, e.g. x_i+1 = x_i + v_i * delta_t
// and 0.9 * x_i+1 = 0.9 * (x_i + v_i * delta_t)
ruckig_input.target_position.at(joint) -= (0.9 * 0.9 - 1) * timestep * ruckig_input.target_velocity.at(joint);
ruckig_input.target_acceleration.at(joint) =
(ruckig_input.target_velocity.at(joint) - ruckig_output.new_velocity.at(joint)) / timestep;
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));
}
velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof);
// Run Ruckig
ruckig_result = ruckig.update(ruckig_input, ruckig_output);
// check for backward motion
backward_motion_detected = checkForLaggingMotion(num_dof, ruckig_input, ruckig_output);
next_waypoint->update();
}

if (ruckig_result != ruckig::Result::Working)
// 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)
{
RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed at waypoint " << waypoint_idx);
RCLCPP_ERROR_STREAM(LOGGER, "Ruckig error: " << ruckig_result);
return false;
smoothing_complete = true;
}

// Overwrite pos/vel/accel of the target waypoint
for (size_t joint = 0; joint < num_dof; ++joint)
else
{
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));
// 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(), num_dof, idx);
duration_extension_factor *= DURATION_EXTENSION_FRACTION;
for (size_t waypoint_idx = 1; waypoint_idx < num_waypoints; ++waypoint_idx)
{
trajectory.setWayPointDurationFromPrevious(
waypoint_idx, DURATION_EXTENSION_FRACTION * trajectory.getWayPointDurationFromPrevious(waypoint_idx));
// TODO(andyz): re-calculate waypoint velocity and acceleration here?
}

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

// 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))
{
RCLCPP_ERROR_STREAM(LOGGER, "Ruckig trajectory smoothing failed. Ruckig error: " << ruckig_result);
return false;
}

return true;
}

void RuckigSmoothing::initializeRuckigState(ruckig::InputParameter<0>& ruckig_input,
ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotState& first_waypoint, size_t num_dof,
const std::vector<int>& idx)
{
std::vector<double> current_positions_vector(num_dof);
std::vector<double> current_velocities_vector(num_dof);
std::vector<double> current_accelerations_vector(num_dof);

for (size_t i = 0; i < num_dof; ++i)
{
current_positions_vector.at(i) = first_waypoint.getVariablePosition(idx.at(i));
current_velocities_vector.at(i) = first_waypoint.getVariableVelocity(idx.at(i));
current_accelerations_vector.at(i) = first_waypoint.getVariableAcceleration(idx.at(i));
}
std::copy_n(current_positions_vector.begin(), num_dof, ruckig_input.current_position.begin());
std::copy_n(current_velocities_vector.begin(), num_dof, ruckig_input.current_velocity.begin());
std::copy_n(current_accelerations_vector.begin(), num_dof, ruckig_input.current_acceleration.begin());
// Initialize output data struct
ruckig_output.new_position = ruckig_input.current_position;
ruckig_output.new_velocity = ruckig_input.current_velocity;
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, size_t num_dof)
{
double vel_magnitude = 0;
Expand All @@ -212,11 +268,13 @@ bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig::
return false;
}

void RuckigSmoothing::getNextCurrentTargetStates(ruckig::InputParameter<0>& ruckig_input,
ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
const std::vector<int>& idx)
void RuckigSmoothing::getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output,
const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof,
const std::vector<int>& idx, ruckig::InputParameter<0>& ruckig_input)
{
// TODO(andyz): https://github.com/ros-planning/moveit2/issues/766
// ruckig_output.pass_to_input(ruckig_input);

for (size_t joint = 0; joint < num_dof; ++joint)
{
// Feed output from the previous timestep back as input
Expand Down
Loading

0 comments on commit f3d7964

Please sign in to comment.