Set the resample_dt_ member of TOTG back to const (#1776)

* Set the resample_dt_ member of TOTG back to const

* Remove unused TOTG instance in test

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>

* Add "totg" to function name

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>

moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h

@@ -174,30 +174,31 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
                          const std::unordered_map<std::string, double>& velocity_limits,
                          const std::unordered_map<std::string, double>& acceleration_limits) const override;
 
-  // 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.
-   * \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);
-
 private:
   bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
                                           const Eigen::VectorXd& max_velocity,
                                           const Eigen::VectorXd& max_acceleration) const;
 
   const double path_tolerance_;
-  double resample_dt_;
+  const double resample_dt_;
   const double min_angle_change_;
 };
+
+// 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.
+  * \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 totgComputeTimeStamps(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);
 }  // namespace trajectory_processing

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -1057,27 +1057,28 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
   return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
 }
 
-bool TimeOptimalTrajectoryGeneration::computeTimeStamps(const size_t num_waypoints,
-                                                        robot_trajectory::RobotTrajectory& trajectory,
-                                                        const double max_velocity_scaling_factor,
-                                                        const double max_acceleration_scaling_factor)
+bool totgComputeTimeStamps(const size_t num_waypoints, robot_trajectory::RobotTrajectory& trajectory,
+                           const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor)
 {
   // The algorithm is:
-  // 1. Run TOTG once to figure out the optimal trajectory duration
+  // 1. Run TOTG with default settings once to find the optimal trajectory duration
   // 2. Calculate the timestep to get the desired num_waypoints:
   //      new_delta_t = duration/(n-1)     // subtract one for the initial waypoint
-  // 3. Run TOTG again with the new timestep. This should give the exact num_waypoints you want
+  // 3. Run TOTG again with the new timestep. This gives the exact num_waypoints you want (plus or minus one due to
+  // numerical rounding)
 
   if (num_waypoints < 2)
   {
     RCLCPP_ERROR(LOGGER, "computeTimeStamps() requires num_waypoints > 1");
     return false;
   }
 
-  computeTimeStamps(trajectory, max_velocity_scaling_factor, max_acceleration_scaling_factor);
+  TimeOptimalTrajectoryGeneration default_totg(0.1 /* default path tolerance */, 0.1 /* default resample_dt */);
+  default_totg.computeTimeStamps(trajectory, max_velocity_scaling_factor, max_acceleration_scaling_factor);
   double optimal_duration = trajectory.getDuration();
-  resample_dt_ = optimal_duration / (num_waypoints - 1);
-  computeTimeStamps(trajectory, max_velocity_scaling_factor, max_acceleration_scaling_factor);
+  double new_resample_dt = optimal_duration / (num_waypoints - 1);
+  TimeOptimalTrajectoryGeneration resample_totg(0.1 /* path tolerance */, new_resample_dt);
+  resample_totg.computeTimeStamps(trajectory, max_velocity_scaling_factor, max_acceleration_scaling_factor);
   return true;
 }
 

moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp

@@ -518,8 +518,8 @@ TEST(time_optimal_trajectory_generation, testFixedNumWaypoints)
   waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.45, -3.2, 1.2, -2.4, -0.8, 0.6, 0.0 });
   trajectory.addSuffixWayPoint(waypoint_state, delta_t);
 
-  TimeOptimalTrajectoryGeneration totg;
-  ASSERT_TRUE(totg.computeTimeStamps(desired_num_waypoints, trajectory)) << "Failed to compute time stamps";
+  ASSERT_TRUE(trajectory_processing::totgComputeTimeStamps(desired_num_waypoints, trajectory))
+      << "Failed to compute time stamps";
   // Allow +/-1 waypoint due to floating point error
   EXPECT_NEAR(trajectory.getWayPointCount(), desired_num_waypoints, 1);
 }