From 759c2cbe529750e7d07e74b78f66d3d7541f1d71 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 8 Dec 2022 16:07:28 -0600 Subject: [PATCH] Comment all 3 versions of computeTimeStamps --- .../time_optimal_trajectory_generation.h | 62 ++++++++++--------- 1 file changed, 33 insertions(+), 29 deletions(-) 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 0859857457f..efbb00f4814 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 @@ -167,34 +167,39 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1, const double min_angle_change = 0.001); + // clang-format off +/** + * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). + * Resampling the trajectory doesn't change the start and goal point, + * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). + * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. + * path_tolerance_ is defined in configuration space, so the unit is usually radians. + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + // clang-format on bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const override; + // clang-format off +/** + * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). + * Resampling the trajectory doesn't change the start and goal point, + * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). + * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. + * path_tolerance_ is defined in configuration space, so the unit is usually radians. + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param velocity_limits Joint names and velocity limits in rad/s + * \param acceleration_limits Joint names and acceleration limits in rad/s^2 + */ + // clang-format on bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, const std::unordered_map& acceleration_limits) const override; -<<<<<<< HEAD -======= - // clang-format off - /** - * \brief Compute a trajectory with the given number of waypoints (plus or minus 1, due to rounding). - * Resampling the trajectory to get the desired num_waypoints doesn't change the start and goal point, - * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). - * path_tolerance_ is defined in configuration space, so the unit is usually radians. - * But controller execution may deviate from the intended path if waypoint spacing is too sparse. - * \param num_waypoints The desired number of waypoints. - * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been - * time-parameterized; this function will re-time-parameterize it. - * \param max_velocity_scaling_factor Joint velocity limits are scaled by this factor. - * \param max_acceleration_scaling_factor Joint acceleration limits are scaled by this factor. - */ - // clang-format on - bool computeTimeStamps(const size_t num_waypoints, robot_trajectory::RobotTrajectory& trajectory, - const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0); - ->>>>>>> faf71f41d (Increase understanding of TOTG path_tolerance_) private: bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory, const Eigen::VectorXd& max_velocity, @@ -207,16 +212,15 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization // clang-format off /** - * \brief Compute a trajectory with the given number of waypoints (plus or minus 1, due to rounding). - * Resampling the trajectory to get the desired num_waypoints doesn't change the start and goal point, - * and all re-sampled waypoints will be on the path of the original trajectory (within the configured tolerances) - * but controller execution may deviate from the intended path if waypoint spacing is too sparse. - * This is a free function because it needs to modify the const resample_dt_ member of TimeOptimalTrajectoryGeneration class. - * \param num_waypoints The desired number of waypoints. + * \brief Compute a trajectory with the desired number of waypoints. + * Resampling the trajectory doesn't change the start and goal point, + * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). + * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. + * path_tolerance_ is defined in configuration space, so the unit is usually radians. * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been * time-parameterized; this function will re-time-parameterize it. - * \param max_velocity_scaling_factor Joint velocity limits are scaled by this factor. - * \param max_acceleration_scaling_factor Joint acceleration limits are scaled by this factor. + * \param velocity_limits Joint names and velocity limits in rad/s + * \param acceleration_limits Joint names and acceleration limits in rad/s^2 */ // clang-format on bool totgComputeTimeStamps(const size_t num_waypoints, robot_trajectory::RobotTrajectory& trajectory,