Test example programs

.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/
+

CMakeLists.txt

@@ -155,6 +155,7 @@ if(CATKIN_ENABLE_TESTING)
     ${catkin_LIBRARIES}
   )
 
+  add_rostest(test/test_examples.test)
 endif()
 
 ## Test for correct C++ source code

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

@@ -12,7 +12,8 @@
 
 #pragma once
 
-#include <cmath>  // copysign
+#include <cmath>                      // copysign
+#include <unsupported/Eigen/Splines>  // Spline-fitting is used to extend trajectory duration
 #include "trackjoint/error_codes.h"
 #include "trackjoint/joint_trajectory.h"
 #include "trackjoint/kinematic_state.h"
@@ -21,13 +22,15 @@
 
 namespace trackjoint
 {
+typedef Eigen::Spline<double, 1, 2> Spline1D;
+typedef Eigen::SplineFitting<Spline1D> SplineFitting1D;
+
 class SingleJointGenerator
 {
 public:
   /** \brief Constructor
    *
    * input timestep desired time between waypoints
-   * input desired_duration total desired duration of the trajectory
    * input max_duration allow the trajectory to be extended up to this limit. Error if that cannot be done.
    * input current_joint_states vector of the initial kinematic states for each degree of freedom
    * input goal_joint_states vector of the target kinematic states for each degree of freedom
@@ -40,29 +43,27 @@ 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
    */
-  SingleJointGenerator(double timestep, double desired_duration, double max_duration,
-                       const KinematicState& current_joint_state, const KinematicState& goal_joint_state,
-                       const Limits& limits, size_t desired_num_waypoints, size_t num_waypoints_threshold,
-                       size_t max_num_waypoints_trajectory_mode, const double position_tolerance,
-                       bool use_streaming_mode);
+  SingleJointGenerator(double timestep, double max_duration, const KinematicState& current_joint_state,
+                       const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints,
+                       size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode,
+                       const double position_tolerance, bool use_streaming_mode, bool timestep_was_upsampled);
 
   /** \brief reset data members and prepare to generate a new trajectory */
-  void reset(double timestep, double desired_duration, double max_duration, const KinematicState& current_joint_state,
+  void reset(double timestep, double max_duration, const KinematicState& current_joint_state,
              const KinematicState& goal_joint_state, const Limits& limits, size_t desired_num_waypoints,
-             const double position_tolerance, bool use_streaming_mode);
+             const double position_tolerance, bool use_streaming_mode, bool timestep_was_upsampled);
 
   /** \brief Generate a jerk-limited trajectory for this joint
    *
    * return a TrackJoint status code
    */
   ErrorCodeEnum generateTrajectory();
 
-  /** \brief Calculate a trajectory once duration is known. Similar to generateTrajectory minus predictTimeToReach().
-   *
-   * return a TrackJoint status code
-   */
-  ErrorCodeEnum extendTrajectoryDuration();
+  /** \brief Extend a trajectory to a new duration. Magnitudes of vel/accel/jerk will be decreased. */
+  void extendTrajectoryDuration();
 
   /** \brief Get the generated trajectory
    *
@@ -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.
@@ -95,20 +106,20 @@ 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 */
   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();
 
   /** \brief Calculate vel/accel/jerk from position */
-  void calculateDerivatives();
+  void calculateDerivativesFromPosition();
+
+  /** \brief Calculate accel/jerk from velocity */
+  void calculateDerivativesFromVelocity();
 
   const size_t kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory;
 

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

@@ -21,42 +21,47 @@ int main(int argc, char** argv)
   // This example is for just one degree of freedom
   constexpr int num_dof = 1;
   // Timestep. Units don't matter as long as they're consistent
-  constexpr double timestep = 0.0075;
-  // Total desired trajectory duration
-  constexpr double desired_duration = 0.028322;
+  constexpr double timestep = 0.001;
   // TrackJoint is allowed to extend the trajectory up to this duration, if a solution at kDesiredDuration can't be
   // found
-  constexpr double max_duration = 10;
+  constexpr double max_duration = 5;
   // streaming mode returns just a few waypoints but executes very quickly. We won't use it here -- we'll calculate
   // 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")) + "/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;
-  joint_state.position = 0.00596041;
-  joint_state.velocity = -0.176232;
-  joint_state.acceleration = -3.06289;
+  joint_state.position = 1.16431;
+  joint_state.velocity = 2.66465;
+  joint_state.acceleration = 0;
   // This is the initial state of the joint
   current_joint_states[0] = joint_state;
 
   std::vector<trackjoint::KinematicState> goal_joint_states(1);
-  joint_state.position = -0.00121542;
-  joint_state.velocity = -0.289615;
-  joint_state.acceleration = -2.88021;
+  joint_state.position = 1.40264;
+  joint_state.velocity = 2.88556;
+  joint_state.acceleration = 0.0615633;
   goal_joint_states[0] = joint_state;
 
   trackjoint::Limits single_joint_limits;
   // Typically, jerk limit >> acceleration limit > velocity limit
   single_joint_limits.velocity_limit = 3.15;
   single_joint_limits.acceleration_limit = 5;
-  single_joint_limits.jerk_limit = 5000;
+  single_joint_limits.jerk_limit = 100;
   std::vector<trackjoint::Limits> limits(1, single_joint_limits);
 
+  // Estimate trajectory duration
+  // This is the fastest possible trajectory execution time, assuming the robot starts at full velocity.
+  double desired_duration =
+      fabs(goal_joint_states[0].position - current_joint_states[0].position) / single_joint_limits.velocity_limit;
+  std::cout << "Desired duration: " << desired_duration << std::endl;
+
   // This descibes how far TrackJoint can deviate from a smooth, interpolated polynomial.
   // It is used for calculations internally. It should be set to a smaller number than your task requires.
-  const double position_tolerance = 1e-6;
+  const double position_tolerance = 0.0001;
 
   // Instantiate a trajectory generation object
   trackjoint::TrajectoryGenerator traj_gen(num_dof, timestep, desired_duration, max_duration, current_joint_states,
@@ -68,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;
   }
 
@@ -81,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);