From 3fe5b8221d1739ab4a54e58d6b85eeb3b2d85c27 Mon Sep 17 00:00:00 2001 From: Burak Payzun Date: Tue, 12 Apr 2022 03:55:53 +0300 Subject: [PATCH] Replace num_dof and idx variables with JointGroup API (#1152) --- .../ruckig_traj_smoothing.h | 25 +++++++------ .../src/iterative_spline_parameterization.cpp | 17 ++++++--- .../src/ruckig_traj_smoothing.cpp | 37 ++++++++++++------- 3 files changed, 48 insertions(+), 31 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 cc22f4d0d7..bb90df1f2c 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 @@ -54,30 +54,32 @@ class RuckigSmoothing * \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 joint_group The MoveIt JointModelGroup of interest * \param ruckig_input Output. The Rucking parameters for the next iteration */ static void getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output, - const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof, - const std::vector& idx, ruckig::InputParameter<0>& ruckig_input); + const moveit::core::RobotStatePtr& next_waypoint, + const moveit::core::JointModelGroup* joint_group, + ruckig::InputParameter<0>& ruckig_input); /** * \brief Check for lagging motion of any joint at a waypoint. - * \param num_dof Number of actuated joints + * \param joint_group The MoveIt JointModelGroup of interest * \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, + static bool checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group, + 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 + * \param joint_group The MoveIt JointModelGroup of interest */ - static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof); + static double getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, + const moveit::core::JointModelGroup* joint_group); /** * \brief Check if the joint positions of two waypoints are very similar. @@ -94,11 +96,10 @@ class RuckigSmoothing * \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 + * \param joint_group The MoveIt JointModelGroup of interest */ 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& joint_idx); + const moveit::core::RobotState& first_waypoint, + const moveit::core::JointModelGroup* joint_group); }; } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp index 49c23b931a..1b1646e6a9 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -68,7 +68,7 @@ struct SingleJointTrajectory double max_acceleration_; }; -void globalAdjustment(std::vector& t2, int num_joints, const int num_points, +void globalAdjustment(std::vector& t2, robot_trajectory::RobotTrajectory& trajectory, std::vector& time_diff); IterativeSplineParameterization::IterativeSplineParameterization(bool add_points) : add_points_(add_points) @@ -321,7 +321,7 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT } // Final adjustment forces the trajectory within bounds - globalAdjustment(t2, num_joints, num_points, time_diff); + globalAdjustment(t2, trajectory, time_diff); // Convert back to JointTrajectory form for (unsigned int i = 1; i < num_points; ++i) @@ -570,11 +570,16 @@ static double global_adjustment_factor(const int n, double x1[], double x2[], co } // Expands the entire trajectory to fit exactly within bounds -void globalAdjustment(std::vector& t2, int num_joints, const int num_points, +void globalAdjustment(std::vector& t2, robot_trajectory::RobotTrajectory& trajectory, std::vector& time_diff) { + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + + const unsigned int num_points = trajectory.getWayPointCount(); + const unsigned int num_joints = group->getVariableCount(); + double gtfactor = 1.0; - for (int j = 0; j < num_joints; ++j) + for (unsigned int j = 0; j < num_joints; ++j) { double tfactor; tfactor = global_adjustment_factor(num_points, &t2[j].velocities_[0], &t2[j].accelerations_[0], t2[j].max_velocity_, @@ -584,10 +589,10 @@ void globalAdjustment(std::vector& t2, int num_joints, co } // printf("# Global adjustment: %0.4f%%\n", 100.0 * (gtfactor - 1.0)); - for (int i = 0; i < num_points - 1; ++i) + for (unsigned int i = 0; i < num_points - 1; ++i) time_diff[i] *= gtfactor; - for (int j = 0; j < num_joints; ++j) + for (unsigned int j = 0; j < num_joints; ++j) { fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]); } diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index 6f74994bf3..c485396e75 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -87,7 +87,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto // Initialize the smoother const std::vector& idx = group->getVariableIndexList(); - initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), num_dof, idx); + initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group); // Kinematic limits (vel/accel/jerk) const std::vector& vars = group->getVariableNames(); @@ -127,16 +127,16 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto { moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1); - getNextRuckigInput(ruckig_output, next_waypoint, num_dof, idx, ruckig_input); + getNextRuckigInput(ruckig_output, next_waypoint, group, ruckig_input); // 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); + bool backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output); - double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, num_dof); + double velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group); 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 @@ -156,12 +156,12 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto 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); + velocity_magnitude = getTargetVelocityMagnitude(ruckig_input, group); // 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); + backward_motion_detected = checkForLaggingMotion(group, ruckig_input, ruckig_output); } // Overwrite pos/vel/accel of the target waypoint @@ -185,7 +185,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto { // 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); + initializeRuckigState(ruckig_input, ruckig_output, *trajectory.getFirstWayPointPtr(), group); duration_extension_factor *= DURATION_EXTENSION_FRACTION; for (size_t waypoint_idx = 1; waypoint_idx < num_waypoints; ++waypoint_idx) { @@ -212,9 +212,12 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto 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& idx) + const moveit::core::RobotState& first_waypoint, + const moveit::core::JointModelGroup* joint_group) { + const size_t num_dof = joint_group->getVariableCount(); + const std::vector& idx = joint_group->getVariableIndexList(); + std::vector current_positions_vector(num_dof); std::vector current_velocities_vector(num_dof); std::vector current_accelerations_vector(num_dof); @@ -243,8 +246,10 @@ bool RuckigSmoothing::checkForIdenticalWaypoints(const moveit::core::RobotState& return (magnitude_position_difference <= IDENTICAL_POSITION_EPSILON); } -double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, size_t num_dof) +double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter<0>& ruckig_input, + const moveit::core::JointModelGroup* joint_group) { + const size_t num_dof = joint_group->getVariableCount(); double vel_magnitude = 0; for (size_t joint = 0; joint < num_dof; ++joint) { @@ -253,9 +258,11 @@ double RuckigSmoothing::getTargetVelocityMagnitude(const ruckig::InputParameter< return sqrt(vel_magnitude); } -bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig::InputParameter<0>& ruckig_input, +bool RuckigSmoothing::checkForLaggingMotion(const moveit::core::JointModelGroup* joint_group, + const ruckig::InputParameter<0>& ruckig_input, const ruckig::OutputParameter<0>& ruckig_output) { + const size_t num_dof = joint_group->getVariableCount(); // Check for backward motion of any joint for (size_t joint = 0; joint < num_dof; ++joint) { @@ -269,12 +276,16 @@ bool RuckigSmoothing::checkForLaggingMotion(const size_t num_dof, const ruckig:: } void RuckigSmoothing::getNextRuckigInput(const ruckig::OutputParameter<0>& ruckig_output, - const moveit::core::RobotStatePtr& next_waypoint, size_t num_dof, - const std::vector& idx, ruckig::InputParameter<0>& ruckig_input) + const moveit::core::RobotStatePtr& next_waypoint, + const moveit::core::JointModelGroup* joint_group, + ruckig::InputParameter<0>& ruckig_input) { // TODO(andyz): https://github.com/ros-planning/moveit2/issues/766 // ruckig_output.pass_to_input(ruckig_input); + const size_t num_dof = joint_group->getVariableCount(); + const std::vector& idx = joint_group->getVariableIndexList(); + for (size_t joint = 0; joint < num_dof; ++joint) { // Feed output from the previous timestep back as input