andyz/squashed_improvements branch, merged w/master, clang tests fixed

.clang-format

@@ -10,7 +10,7 @@ AllowShortLoopsOnASingleLine: false
 AllowShortFunctionsOnASingleLine: None
 AllowShortLoopsOnASingleLine: false
 AlwaysBreakTemplateDeclarations: true
-AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakBeforeMultilineStrings: true
 BreakBeforeBinaryOperators: false
 BreakBeforeTernaryOperators: false
 BreakConstructorInitializersBeforeComma: true

.gitignore

@@ -18,4 +18,8 @@
 .DS_Store
 
 # SSH Keys (used for Docker)
-id_rsa
+id_rsa
+
+# Catkin tools
+/.catkin-tools/
+

README.md

@@ -19,7 +19,7 @@ TODO(andyze): fix Travis badge:
 
 ## Run
 
-1. rosrun trackjoint run_example
+1. rosrun trackjoint simple_example  (or streaming_example)
 
 (You don't need to start `roscore` because the executable doesn't use ROS)
 

include/trackjoint/error_codes.h

@@ -22,32 +22,33 @@ 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

@@ -43,7 +43,8 @@ class SingleJointGenerator
    *                          Should be set lower than the accuracy requirements for your task
    * input use_streaming_mode set to true for fast streaming applications. Returns a maximum of num_waypoints_threshold
    * waypoints.
-   * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust timestep
+   * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust
+   * timestep
    */
   SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state,
                        const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints,
@@ -83,6 +84,16 @@ class SingleJointGenerator
   void updateTrajectoryDuration(double new_trajectory_duration);
 
 private:
+  /** \brief Record the index when compensation first failed */
+  inline void recordFailureTime(size_t current_index, size_t* index_last_successful)
+  {
+    // Record the index when compensation first failed
+    if (current_index < *index_last_successful)
+    {
+      *index_last_successful = current_index;
+    }
+  };
+
   /** \brief interpolate from start to end state with a polynomial
    *
    * input times a vector of waypoint times.
@@ -101,9 +112,6 @@ class SingleJointGenerator
    * vector */
   ErrorCodeEnum positionVectorLimitLookAhead(size_t* index_last_successful);
 
-  /** \brief Record the index when compensation first failed */
-  inline void recordFailureTime(size_t current_index, size_t* index_last_successful);
-
   /** \brief Check whether the duration needs to be extended, and do it */
   ErrorCodeEnum predictTimeToReach();
 

include/trackjoint/trajectory_generator.h

@@ -78,7 +78,7 @@ class TrajectoryGenerator
    * input limits vector of kinematic limits for each degree of freedom
    * input nominal_timestep the user-requested time between waypoints
    * returna TrackJoint status code
-  */
+   */
   ErrorCodeEnum inputChecking(const std::vector<KinematicState>& current_joint_states,
                               const std::vector<KinematicState>& goal_joint_states, const std::vector<Limits>& limits,
                               double nominal_timestep);
@@ -87,7 +87,7 @@ class TrajectoryGenerator
   /** \brief Ensure limits are obeyed before outputting.
    *
    * input trajectory the calculated trajectories for n joints
-  */
+   */
   void clipVectorsForOutput(std::vector<JointTrajectory>* trajectory);
 
   /** \brief upsample if num. waypoints would be short. Helps with accuracy. */
@@ -100,15 +100,15 @@ class TrajectoryGenerator
    * input velocity_vector a vector of velocities
    * input acceleration_vector a vector of accelerations
    * input jerk_vector a vector of jerks
-  */
+   */
   void downSample(Eigen::VectorXd* time_vector, Eigen::VectorXd* position_vector, Eigen::VectorXd* velocity_vector,
                   Eigen::VectorXd* acceleration_vector, Eigen::VectorXd* jerk_vector);
 
   /** \brief Synchronize all trajectories with the one of longest duration.
    *
    * input output_trajectories the calculated trajectories for n joints
    * returna TrackJoint status code
-  */
+   */
   ErrorCodeEnum synchronizeTrajComponents(std::vector<JointTrajectory>* output_trajectories);
 
   // TODO(andyz): set this back to a small number when done testing

include/trackjoint/utilities.h

@@ -35,7 +35,7 @@ Eigen::VectorXd DiscreteDifferentiation(const Eigen::VectorXd& input_vector, dou
  * input joint the index of a joint
  * input output_trajectories the calculated trajectories for n joints
  * input desired_duration the user-requested duration of the trajectory
-*/
+ */
 void PrintJointTrajectory(const std::size_t joint, const std::vector<JointTrajectory>& output_trajectories,
                           const double desired_duration);
 

src/simple_example.cpp

@@ -29,8 +29,8 @@ int main(int argc, char** argv)
   // the whole trajectory at once.
   constexpr bool use_streaming_mode = false;
   // Optional logging of TrackJoint output
-  const std::string output_path_base = "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/"
-                                                                                "output_joint";
+  const std::string output_path_base =
+      "/home/" + std::string(getenv("USER")) + "/Downloads/trackjoint_data/output_joint";
 
   std::vector<trackjoint::KinematicState> current_joint_states(1);
   trackjoint::KinematicState joint_state;
@@ -73,9 +73,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;
   }
 
@@ -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);

src/single_joint_generator.cpp

@@ -113,15 +113,14 @@ void SingleJointGenerator::extendTrajectoryDuration()
     Eigen::RowVectorXd position(waypoints_.positions);
 
     const auto fit = SplineFitting1D::Interpolate(position, 2, new_times);
-    Spline1D spline(fit);
 
     // New times, with the extended duration
     waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_);
     // Retrieve new positions at the new times
     waypoints_.positions.resize(new_num_waypoints);
 
-    for (size_t idx = 0; idx < waypoints_.elapsed_times.size(); ++idx)
-      waypoints_.positions[idx] = spline(waypoints_.elapsed_times(idx)).coeff(0);
+    for (Eigen::Index idx = 0; idx < waypoints_.elapsed_times.size(); ++idx)
+      waypoints_.positions[idx] = fit(waypoints_.elapsed_times(idx)).coeff(0);
 
     calculateDerivativesFromPosition();
     return;
@@ -135,7 +134,7 @@ void SingleJointGenerator::extendTrajectoryDuration()
     waypoints_.elapsed_times.setLinSpaced(new_num_waypoints, 0., (new_num_waypoints - 1) * timestep_);
     waypoints_.positions = interpolate(waypoints_.elapsed_times);
     calculateDerivativesFromPosition();
-    ErrorCodeEnum error_code = forwardLimitCompensation(&index_last_successful_);
+    forwardLimitCompensation(&index_last_successful_);
   }
 
   return;
@@ -151,7 +150,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
@@ -182,7 +181,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
@@ -294,8 +293,6 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde
   position_error = 0;
   for (size_t index = 1; index < *index_last_successful; ++index)
   {
-    delta_v = 0;
-
     // If the velocity limit would be exceeded
     if (fabs(waypoints_.velocities(index)) > velocity_limit)
     {
@@ -329,19 +326,10 @@ inline ErrorCodeEnum SingleJointGenerator::forwardLimitCompensation(size_t* inde
   // Re-calculate derivatives from the updated velocity vector
   calculateDerivativesFromVelocity();
 
-  return ErrorCodeEnum::kNoError;
-}
-
-inline void SingleJointGenerator::recordFailureTime(size_t current_index, size_t* index_last_successful)
-{
-  // Record the index when compensation first failed
-  if (current_index < *index_last_successful)
-  {
-    *index_last_successful = current_index;
-  }
+  return ErrorCodeEnum::NO_ERROR;
 }
 
-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
@@ -445,13 +433,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_)
@@ -528,18 +516,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)
@@ -570,7 +558,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);
@@ -579,7 +567,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

@@ -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;
     }