From 5872d23dc6509e999d177e832843081b8ae35b7b Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 20 Mar 2023 08:25:07 -0500 Subject: [PATCH] Delete the Ruckig "batches" option, deprecated by #1990 --- .../ruckig_traj_smoothing.h | 15 ----- .../src/ruckig_traj_smoothing.cpp | 61 ------------------- 2 files changed, 76 deletions(-) diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h index dd280473f8..a079e8b026 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h @@ -127,21 +127,6 @@ class RuckigSmoothing ruckig::InputParameter& ruckig_input, ruckig::OutputParameter& ruckig_output); - /** - * \brief Break the `trajectory` parameter into batches of reasonable size (~100), run Ruckig on each of them, then - * re-combine into a single trajectory again. - * runRuckig() stretches all input waypoints in time until all kinematic limits are obeyed. This works but it can - * slow the trajectory more than necessary. It's better to feed in just a few waypoints at once, so that only the - * waypoints needing it get stretched. - * Here, break the trajectory waypoints into batches so the output is closer to time-optimal. - * There is a trade-off between time-optimality of the output trajectory and runtime of the smoothing algorithm. - * \param[in, out] trajectory Trajectory to smooth. - * \param[in, out] ruckig_input Necessary input for Ruckig smoothing. Contains kinematic limits (vel, accel, jerk) - */ - static std::optional - runRuckigInBatches(const size_t num_waypoints, const robot_trajectory::RobotTrajectory& trajectory, - ruckig::InputParameter& ruckig_input, size_t batch_size = 100); - /** * \brief A utility function to instantiate and run Ruckig for a series of waypoints. * \param[in, out] trajectory Trajectory to smooth. diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index c281d303e7..3f4798a696 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -142,67 +142,6 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto return runRuckig(trajectory, ruckig_input); } -std::optional -RuckigSmoothing::runRuckigInBatches(const size_t num_waypoints, const robot_trajectory::RobotTrajectory& trajectory, - ruckig::InputParameter& ruckig_input, size_t batch_size) -{ - // We take the batch size as the lesser of 0.1*num_waypoints or batch_size, to keep a balance between run time and - // time-optimality. - batch_size = [num_waypoints, batch_size]() { - const size_t temp_batch_size = std::min(size_t(0.1 * num_waypoints), size_t(batch_size)); - // We need at least 2 waypoints - return std::max(size_t(2), temp_batch_size); - }(); - size_t batch_start_idx = 0; - size_t batch_end_idx = batch_size - 1; - const size_t full_traj_final_idx = num_waypoints - 1; - // A deep copy is not needed since the waypoints are cleared immediately - robot_trajectory::RobotTrajectory sub_trajectory = - robot_trajectory::RobotTrajectory(trajectory, false /* deep copy */); - robot_trajectory::RobotTrajectory output_trajectory = - robot_trajectory::RobotTrajectory(trajectory, false /* deep copy */); - output_trajectory.clear(); - - while (batch_end_idx <= full_traj_final_idx) - { - sub_trajectory.clear(); - for (size_t waypoint_idx = batch_start_idx; waypoint_idx <= batch_end_idx; ++waypoint_idx) - { - sub_trajectory.addSuffixWayPoint(trajectory.getWayPoint(waypoint_idx), - trajectory.getWayPointDurationFromPrevious(waypoint_idx)); - } - - // When starting a new batch, set the last Ruckig output equal to the new, starting robot state - bool first_point_previously_smoothed = false; - if (output_trajectory.getWayPointCount() > 0) - { - sub_trajectory.addPrefixWayPoint(output_trajectory.getLastWayPoint(), 0); - first_point_previously_smoothed = true; - } - - if (!runRuckig(sub_trajectory, ruckig_input)) - { - return std::nullopt; - } - - // Skip appending the first waypoint in sub_trajectory if it was smoothed in - // the previous iteration - size_t first_new_waypoint = first_point_previously_smoothed ? 1 : 0; - - // Add smoothed waypoints to the output - for (size_t waypoint_idx = first_new_waypoint; waypoint_idx < sub_trajectory.getWayPointCount(); ++waypoint_idx) - { - output_trajectory.addSuffixWayPoint(sub_trajectory.getWayPoint(waypoint_idx), - sub_trajectory.getWayPointDurationFromPrevious(waypoint_idx)); - } - - batch_start_idx += batch_size; - batch_end_idx += batch_size; - } - - return std::make_optional(output_trajectory, true /* deep copy */); -} - bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, const std::vector& joint_limits, const double max_velocity_scaling_factor,