From b2a3b16b2a3572bde9fa5378b246a5305da30a5d Mon Sep 17 00:00:00 2001 From: AndyZe Date: Tue, 12 Dec 2023 06:59:28 -0600 Subject: [PATCH] Sync Ruckig with MoveIt1 (#2596) * Debug Ruckig tests (MoveIt1 3300) * Add a test, termination condition bugfix (MoveIt1 3348) * Mitigate Ruckig overshoot (MoveIt1 3376) * Small variable fixup --- .../src/ruckig_traj_smoothing.cpp | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index bfa88e450d..31a72d3265 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -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 @@ -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_output{ num_dof }; + ruckig::Trajectory ruckig_output(num_dof); // This lib does not work properly when angles wrap, so we need to unwind the path first trajectory.unwind(); @@ -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) { @@ -278,15 +278,14 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, getNextRuckigInput(curr_waypoint, next_waypoint, group, ruckig_input); // Run Ruckig - ruckig::Trajectory 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 @@ -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; } @@ -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& 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); @@ -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);