From aff395c7e3ae58aa49c12357ba7138a52982bb81 Mon Sep 17 00:00:00 2001 From: Andy Zelenak Date: Wed, 22 Feb 2023 15:06:33 -0600 Subject: [PATCH] Refactor to reduce nested for-loops --- .../ruckig_traj_smoothing.h | 14 +++++ .../src/ruckig_traj_smoothing.cpp | 58 +++++++++++-------- 2 files changed, 48 insertions(+), 24 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 5cc3c5b67d..4f5075ce83 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 @@ -121,5 +121,19 @@ class RuckigSmoothing */ [[nodiscard]] static bool runRuckig(robot_trajectory::RobotTrajectory& trajectory, ruckig::InputParameter& 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& move_group_idx, + const robot_trajectory::RobotTrajectory& original_trajectory, + robot_trajectory::RobotTrajectory& trajectory); }; } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index 5f64368664..c6e43f19c6 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -267,14 +267,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_output{ num_dof }; - const std::vector& 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(); - ruckig::Ruckig ruckig(num_dof, timestep); + ruckig::Ruckig ruckig(num_dof, trajectory.getAverageSegmentDuration()); initializeRuckigState(*trajectory.getFirstWayPointPtr(), group, ruckig_input, ruckig_output); // Cache the trajectory in case we need to reset it @@ -322,27 +320,11 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, 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(); - } + + const std::vector& 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 for() loop again break; @@ -365,6 +347,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& 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_input,