diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index d575e6d575e..527b0c6c8d9 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -247,13 +247,12 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) const; /** - * @brief Check if the max scaling factor is valid and if not, set it to the specified default value - * \param[in,out] scaling_factor The default scaling factor that should be applied - * if the `max_scaling_factor` is invalid - * \param max_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits + * @brief Check if the requested scaling factor is valid and if not, return 1.0. + * \param requested_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits * \param limit_type Whether the velocity or acceleration scaling factor is being verified + * \return The user requested scaling factor, if it is valid. Otherwise, return 1.0. */ - void verifyScalingFactor(double& scaling_factor, const double max_scaling_factor, const LimitType limit_type) const; + double verifyScalingFactor(const double requested_scaling_factor, const LimitType limit_type) const; const double path_tolerance_; const double resample_dt_; diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 3f58f766d0a..95484552039 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -53,6 +53,7 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.time_optimal_trajectory_generation"); constexpr double DEFAULT_TIMESTEP = 1e-3; constexpr double EPS = 1e-6; +constexpr double DEFAULT_SCALING_FACTOR = 1.0; } // namespace class LinearPathSegment : public PathSegment @@ -880,11 +881,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } // Validate scaling - double velocity_scaling_factor = 1.0; - verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY); + double velocity_scaling_factor = verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY); - double acceleration_scaling_factor = 1.0; - verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION); + double acceleration_scaling_factor = + verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION); const std::vector& vars = group->getVariableNames(); const moveit::core::RobotModel& rmodel = group->getParentModel(); @@ -980,11 +980,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( } // Validate scaling - double velocity_scaling_factor = 1.0; - verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY); + double velocity_scaling_factor = verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY); - double acceleration_scaling_factor = 1.0; - verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION); + double acceleration_scaling_factor = + verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION); const unsigned num_joints = group->getVariableCount(); const std::vector& vars = group->getVariableNames(); @@ -1211,30 +1210,27 @@ bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::Joi return have_prismatic && have_revolute; } -void TimeOptimalTrajectoryGeneration::verifyScalingFactor(double& scaling_factor, const double max_scaling_factor, - const LimitType limit_type) const +double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double requested_scaling_factor, + const LimitType limit_type) const { std::string limit_type_str; - auto limit_type_it = LIMIT_TYPES.find(limit_type); + double scaling_factor = DEFAULT_SCALING_FACTOR; + const auto limit_type_it = LIMIT_TYPES.find(limit_type); if (limit_type_it != LIMIT_TYPES.end()) { limit_type_str = limit_type_it->second + "_"; } - if (max_scaling_factor > 0.0 && max_scaling_factor <= 1.0) + if (requested_scaling_factor > 0.0 && requested_scaling_factor <= 1.0) { - scaling_factor = max_scaling_factor; - } - else if (max_scaling_factor == 0.0) - { - RCLCPP_DEBUG(LOGGER, "A max_%sscaling_factor of 0.0 was specified, defaulting to %f instead.", - limit_type_str.c_str(), scaling_factor); + scaling_factor = requested_scaling_factor; } else { RCLCPP_WARN(LOGGER, "Invalid max_%sscaling_factor %f specified, defaulting to %f instead.", limit_type_str.c_str(), - max_scaling_factor, scaling_factor); + requested_scaling_factor, scaling_factor); } + return scaling_factor; } } // namespace trajectory_processing