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 6eb1ac581b3..b8723f463c7 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 @@ -207,8 +207,12 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration) const; - /// @brief Check if a combination of revolute and prismatic joints is used. path_tolerance_ is not valid, if so. - bool checkMixedUnits(const moveit::core::JointModelGroup* group) const; + /** + * @brief Check if a combination of revolute and prismatic joints is used. path_tolerance_ is not valid, if so. + * \param group The JointModelGroup to check. + * \return true if there are mixed joints. + */ + bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) 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 e819289fb65..4285f36acf0 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -1096,7 +1096,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t return false; } - if (!checkMixedUnits(group)) + if (hasMixedJointTypes(group)) { RCLCPP_WARN(LOGGER, "There is a combination of revolute and prismatic joints in the robot model. TOTG's `path_tolerance` will not function correctly."); } @@ -1185,7 +1185,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t return true; } -bool TimeOptimalTrajectoryGeneration::checkMixedUnits(const moveit::core::JointModelGroup* group) const +bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::JointModelGroup* group) const { const std::vector& joint_models = group->getJointModels(); @@ -1199,8 +1199,6 @@ bool TimeOptimalTrajectoryGeneration::checkMixedUnits(const moveit::core::JointM return joint_model->getType() == moveit::core::JointModel::JointType::REVOLUTE; }); - if (have_prismatic && have_revolute) - return false; - return true; + return have_prismatic && have_revolute; } } // namespace trajectory_processing