Fix endpoint discontinuity in LimitCompensation test

CMakeLists.txt

@@ -155,6 +155,15 @@ if(CATKIN_ENABLE_TESTING)
     ${catkin_LIBRARIES}
   )
 
+  add_rostest_gtest(algorithm_test
+    test/algorithm_test.test
+    test/algorithm_test.cpp
+  )
+  target_link_libraries(algorithm_test
+    ${LIBRARY_NAME}
+    ${catkin_LIBRARIES}
+  )
+
 endif()
 
 ## Test for correct C++ source code

include/trackjoint/error_codes.h

@@ -22,32 +22,32 @@ namespace trackjoint
  */
 enum ErrorCodeEnum
 {
-  kNoError = 0,
-  kDesiredDurationTooShort = 1,
-  kMaxDurationExceeded = 2,
-  kVelocityExceedsLimit = 3,
-  kAccelExceedsLimit = 4,
-  kMaxDurationLessThanDesiredDuration = 5,
-  kLimitNotPositive = 6,
-  kGoalPositionMismatch = 7,
-  kFailureToGenerateSingleWaypoint = 8,
-  kLessThanTenTimestepsForStreamingMode = 9
+  NO_ERROR = 0,
+  DESIRED_DURATION_TOO_SHORT = 1,
+  MAX_DURATION_EXCEEDED = 2,
+  VELOCITY_EXCEEDS_LIMIT = 3,
+  ACCEL_EXCEEDS_LIMIT = 4,
+  MAX_DURATION_LESS_THAN_DESIRED_DURATION = 5,
+  LIMIT_NOT_POSITIVE = 6,
+  GOAL_POSITION_MISMATCH = 7,
+  FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8,
+  LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9
 };
 
 /**
  * \brief Use this map to look up human-readable strings for each error code
  */
-const std::unordered_map<uint, std::string> kErrorCodeMap(
-    { { kNoError, "No error, trajectory generation was successful" },
-      { kDesiredDurationTooShort, "Desired duration is too short, cannot have less than one timestep in a "
-                                  "trajectory" },
-      { kMaxDurationExceeded, "Max duration was exceeded" },
-      { kVelocityExceedsLimit, "A velocity input exceeds the velocity limit" },
-      { kAccelExceedsLimit, "An acceleration input exceeds the acceleration limit" },
-      { kMaxDurationLessThanDesiredDuration, "max_duration should not be less than desired_duration" },
-      { kLimitNotPositive, "Vel/accel/jerk limits should be greater than zero" },
-      { kGoalPositionMismatch, "Mismatch between the final position and the goal position" },
-      { kFailureToGenerateSingleWaypoint, "Failed to generate even a single new waypoint" },
-      { kLessThanTenTimestepsForStreamingMode, "In streaming mode, desired duration should be at least 10 "
-                                               "timesteps" } });
+const std::unordered_map<uint, std::string> ERROR_CODE_MAP(
+    { { NO_ERROR, "No error, trajectory generation was successful" },
+      { DESIRED_DURATION_TOO_SHORT, "Desired duration is too short, cannot have less than one timestep in a "
+                                    "trajectory" },
+      { MAX_DURATION_EXCEEDED, "Max duration was exceeded" },
+      { VELOCITY_EXCEEDS_LIMIT, "A velocity input exceeds the velocity limit" },
+      { ACCEL_EXCEEDS_LIMIT, "An acceleration input exceeds the acceleration limit" },
+      { MAX_DURATION_LESS_THAN_DESIRED_DURATION, "max_duration should not be less than desired_duration" },
+      { LIMIT_NOT_POSITIVE, "Vel/accel/jerk limits should be greater than zero" },
+      { GOAL_POSITION_MISMATCH, "Mismatch between the final position and the goal position" },
+      { FAILURE_TO_GENERATE_SINGLE_WAYPOINT, "Failed to generate even a single new waypoint" },
+      { LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE, "In streaming mode, desired duration should be at least 10 "
+                                                    "timesteps" } });
 }  // end namespace trackjoint

include/trackjoint/single_joint_generator.h

@@ -95,7 +95,7 @@ class SingleJointGenerator
 
   /** \brief Start looking back through a velocity vector to calculate for an
    * excess velocity at limited_index. */
-  bool backwardLimitCompensation(size_t limited_index, double* excess_velocity);
+  bool backwardLimitCompensation(size_t limited_index, double excess_velocity);
 
   /** \brief This uses backwardLimitCompensation() but it starts from a position
    * vector */
@@ -132,5 +132,7 @@ class SingleJointGenerator
   // successfully.
   // In streaming mode, trajectory duration is not extended until it successfully reaches the goal.
   bool use_streaming_mode_;
+
+  friend class AlgorithmTest;
 };  // end class SingleJointGenerator
 }  // namespace trackjoint

include/trackjoint/trajectory_generator.h

@@ -127,5 +127,7 @@ class TrajectoryGenerator
   std::vector<SingleJointGenerator> single_joint_generators_;
   size_t upsampled_num_waypoints_;
   size_t upsample_rounds_ = 0;  // Every time we upsample, timestep is halved. Track this.
-};                              // end class TrajectoryGenerator
+
+  friend class AlgorithmTest;
+};  // end class TrajectoryGenerator
 }  // namespace trackjoint

src/simple_example.cpp

@@ -68,9 +68,9 @@ int main(int argc, char** argv)
       traj_gen.inputChecking(current_joint_states, goal_joint_states, limits, timestep);
 
   // Input error handling - if an error is found, the trajectory is not generated.
-  if (error_code != trackjoint::ErrorCodeEnum::kNoError)
+  if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR)
   {
-    std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+    std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
     return -1;
   }
 
@@ -81,17 +81,17 @@ int main(int argc, char** argv)
   auto end = std::chrono::system_clock::now();
 
   // Trajectory generation error handling
-  if (error_code != trackjoint::ErrorCodeEnum::kNoError)
+  if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR)
   {
-    std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+    std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
     return -1;
   }
 
   std::chrono::duration<double> elapsed_seconds = end - start;
 
   std::cout << "Runtime: " << elapsed_seconds.count() << std::endl;
   std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl;
-  std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+  std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
 
   // Save the synchronized trajectories to .csv files
   traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base);

src/single_joint_generator.cpp

@@ -91,7 +91,7 @@ size_t SingleJointGenerator::getLastSuccessfulIndex()
   return index_last_successful_;
 }
 
-inline Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times)
+Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times)
 {
   // See De Luca, "Trajectory Planning" pdf, slide 19
   // Interpolate a smooth trajectory from initial to final state while matching
@@ -122,7 +122,7 @@ inline Eigen::VectorXd SingleJointGenerator::interpolate(Eigen::VectorXd& times)
   return interpolated_position;
 }
 
-inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_successful)
+ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* index_last_successful)
 {
   // This is the indexing convention.
   // 1. accel(i) = accel(i-1) + jerk(i) * dt
@@ -167,7 +167,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde
       delta_v = 0.5 * delta_j * timestep_ * timestep_;
 
       // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed
-      successful_compensation = backwardLimitCompensation(index, &delta_v);
+      successful_compensation = backwardLimitCompensation(index, -delta_v);
       if (!successful_compensation)
       {
         position_error = position_error + delta_v * timestep_;
@@ -218,7 +218,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde
 
       // Try adjusting the velocity in previous timesteps to compensate for this limit, if needed
       delta_v = delta_a * timestep_;
-      successful_compensation = backwardLimitCompensation(index, &delta_v);
+      successful_compensation = backwardLimitCompensation(index, -delta_v);
       if (!successful_compensation)
       {
         position_error = position_error + delta_v * timestep_;
@@ -252,7 +252,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde
       // Try adjusting the velocity in previous timesteps to compensate for this limit.
       // Try to account for position error, too.
       delta_v += position_error / timestep_;
-      successful_compensation = backwardLimitCompensation(index, &delta_v);
+      successful_compensation = backwardLimitCompensation(index, -delta_v);
       if (!successful_compensation)
       {
         position_error = position_error + delta_v * timestep_;
@@ -273,7 +273,7 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde
     }
   }
 
-  return ErrorCodeEnum::kNoError;
+  return ErrorCodeEnum::NO_ERROR;
 }
 
 inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t* index_last_successful)
@@ -285,7 +285,7 @@ inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t
   }
 }
 
-inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double* excess_velocity)
+bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index, double excess_velocity)
 {
   // The algorithm:
   // 1) check jerk limits, from beginning to end of trajectory. Don't bother
@@ -306,10 +306,10 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index
     if (fabs(waypoints_.velocities(index)) < limits_.velocity_limit)
     {
       // If the full change can be made in this timestep
-      if ((*excess_velocity > 0 && waypoints_.velocities(index) <= limits_.velocity_limit - *excess_velocity) ||
-          (*excess_velocity < 0 && waypoints_.velocities(index) >= -limits_.velocity_limit - *excess_velocity))
+      if ((excess_velocity > 0 && waypoints_.velocities(index) <= limits_.velocity_limit - excess_velocity) ||
+          (excess_velocity < 0 && waypoints_.velocities(index) >= -limits_.velocity_limit - excess_velocity))
       {
-        double new_velocity = waypoints_.velocities(index) + *excess_velocity;
+        double new_velocity = waypoints_.velocities(index) + excess_velocity;
         // Accel and jerk, calculated from the previous waypoints
         double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_;
         double backward_jerk =
@@ -348,7 +348,7 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index
       {
         // This is what accel and jerk would be if we set velocity(index) to the
         // limit
-        double new_velocity = std::copysign(1.0, *excess_velocity) * limits_.velocity_limit;
+        double new_velocity = std::copysign(1.0, excess_velocity) * limits_.velocity_limit;
         // Accel and jerk, calculated from the previous waypoints
         double backward_accel = (new_velocity - waypoints_.velocities(index - 1)) / timestep_;
         double backward_jerk =
@@ -378,7 +378,7 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index
             waypoints_.accelerations(index) = backward_accel;
             waypoints_.jerks(index) = backward_jerk;
           }
-          *excess_velocity = *excess_velocity - delta_v;
+          excess_velocity -= delta_v;
         }
       }
     }
@@ -390,13 +390,13 @@ inline bool SingleJointGenerator::backwardLimitCompensation(size_t limited_index
   return successful_compensation;
 }
 
-inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach()
+ErrorCodeEnum SingleJointGenerator::predictTimeToReach()
 {
   // Take a trajectory that could not reach the desired position in time.
   // Try increasing the duration until it is interpolated without violating limits.
   // This gives a new duration estimate.
 
-  ErrorCodeEnum error_code = ErrorCodeEnum::kNoError;
+  ErrorCodeEnum error_code = ErrorCodeEnum::NO_ERROR;
 
   // If in normal mode, we can extend trajectories
   if (!use_streaming_mode_)
@@ -473,18 +473,18 @@ inline ErrorCodeEnum SingleJointGenerator::predictTimeToReach()
   // Normal mode: Error if we extended the duration to the maximum and it still wasn't successful
   if (!use_streaming_mode_ && index_last_successful_ < static_cast<size_t>(waypoints_.elapsed_times.size() - 1))
   {
-    error_code = ErrorCodeEnum::kMaxDurationExceeded;
+    error_code = ErrorCodeEnum::MAX_DURATION_EXCEEDED;
   }
   // Error if not even a single waypoint could be generated
   if (waypoints_.positions.size() < 2)
   {
-    error_code = ErrorCodeEnum::kFailureToGenerateSingleWaypoint;
+    error_code = ErrorCodeEnum::FAILURE_TO_GENERATE_SINGLE_WAYPOINT;
   }
 
   return error_code;
 }
 
-inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_last_successful)
+ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t* index_last_successful)
 {
   ErrorCodeEnum error_code = forwardLimitCompensation(index_last_successful);
   if (error_code)
@@ -508,7 +508,7 @@ inline ErrorCodeEnum SingleJointGenerator::positionVectorLimitLookAhead(size_t*
   return error_code;
 }
 
-inline void SingleJointGenerator::calculateDerivativesFromPosition()
+void SingleJointGenerator::calculateDerivativesFromPosition()
 {
   // From position vector, approximate vel/accel/jerk.
   waypoints_.velocities = DiscreteDifferentiation(waypoints_.positions, timestep_, current_joint_state_.velocity);
@@ -517,7 +517,7 @@ inline void SingleJointGenerator::calculateDerivativesFromPosition()
   waypoints_.jerks = DiscreteDifferentiation(waypoints_.accelerations, timestep_, 0);
 }
 
-inline void SingleJointGenerator::calculateDerivativesFromVelocity()
+void SingleJointGenerator::calculateDerivativesFromVelocity()
 {
   // From velocity vector, approximate accel/jerk.
   waypoints_.accelerations =

src/streaming_example.cpp

@@ -39,7 +39,7 @@ int main(int argc, char** argv)
   constexpr double final_acceleration_tolerance = 1e-1;
   // For streaming mode, it is important to keep the desired duration >=10 timesteps.
   // Otherwise, an error will be thrown. This helps with accuracy
-  constexpr double min_desired_duration = 10 * timestep;
+  constexpr double min_desired_duration = 1 * timestep;
   // Between TrackJoint iterations, move ahead this many waypoints along the trajectory.
   constexpr std::size_t next_waypoint = 1;
 
@@ -70,15 +70,15 @@ int main(int argc, char** argv)
   trackjoint::ErrorCodeEnum error_code = traj_gen.inputChecking(start_state, goal_joint_states, limits, timestep);
   if (error_code)
   {
-    std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+    std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
     return -1;
   }
 
   // Generate the initial trajectory
   error_code = traj_gen.generateTrajectories(&output_trajectories);
-  if (error_code != trackjoint::ErrorCodeEnum::kNoError)
+  if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR)
   {
-    std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+    std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
     return -1;
   }
   std::cout << "Initial trajectory calculation:" << std::endl;
@@ -110,9 +110,9 @@ int main(int argc, char** argv)
     std::cout << "Run time (microseconds): "
               << std::chrono::duration_cast<std::chrono::microseconds>(end_time - start_time).count() << std::endl;
 
-    if (error_code != trackjoint::ErrorCodeEnum::kNoError)
+    if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR)
     {
-      std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+      std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
       return -1;
     }
 

src/three_dof_examples.cpp

@@ -74,9 +74,9 @@ int main(int argc, char** argv)
 
   // Input error handling - if an error is found, the trajectory is not
   // generated.
-  if (error_code != trackjoint::ErrorCodeEnum::kNoError)
+  if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR)
   {
-    std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+    std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
     return -1;
   }
 
@@ -86,17 +86,17 @@ int main(int argc, char** argv)
   auto end = std::chrono::system_clock::now();
 
   // Trajectory generation error handling
-  if (error_code != trackjoint::ErrorCodeEnum::kNoError)
+  if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR)
   {
-    std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+    std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
     return -1;
   }
 
   std::chrono::duration<double> elapsed_seconds = end - start;
 
   std::cout << "Runtime: " << elapsed_seconds.count() << std::endl;
   std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl;
-  std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+  std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
 
   // Save the synchronized trajectories to .csv files
   traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base);
@@ -167,9 +167,9 @@ int main(int argc, char** argv)
 
   // Input error handling - if an error is found, the trajectory is not
   // generated.
-  if (error_code != trackjoint::ErrorCodeEnum::kNoError)
+  if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR)
   {
-    std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+    std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
     return -1;
   }
 
@@ -179,17 +179,17 @@ int main(int argc, char** argv)
   end = std::chrono::system_clock::now();
 
   // Trajectory generation error handling
-  if (error_code != trackjoint::ErrorCodeEnum::kNoError)
+  if (error_code != trackjoint::ErrorCodeEnum::NO_ERROR)
   {
-    std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+    std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
     return -1;
   }
 
   elapsed_seconds = end - start;
 
   std::cout << "Runtime: " << elapsed_seconds.count() << std::endl;
   std::cout << "Num waypoints: " << output_trajectories.at(0).positions.size() << std::endl;
-  std::cout << "Error code: " << trackjoint::kErrorCodeMap.at(error_code) << std::endl;
+  std::cout << "Error code: " << trackjoint::ERROR_CODE_MAP.at(error_code) << std::endl;
 
   // Save the synchronized trajectories to .csv files
   traj_gen.saveTrajectoriesToFile(output_trajectories, output_path_base);