Squashed improvements

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

include/trackjoint/configuration.h

@@ -0,0 +1,54 @@
+/*********************************************************************
+ * Copyright (c) 2019, PickNik Consulting
+ * All rights reserved.
+ *
+ * Unauthorized copying of this file, via any medium is strictly prohibited
+ * Proprietary and confidential
+ *********************************************************************/
+
+/* Author: John Morris
+   Desc: Trajectory generator configuration.
+*/
+
+#pragma once
+
+#include "trackjoint/limits.h"
+
+namespace trackjoint
+{
+/**
+ * \brief Trajectory generator configuration.
+ */
+struct Configuration
+{
+  Configuration(double timestep, double max_duration, const Limits& limits, const double position_tolerance,
+                bool use_streaming_mode)
+    : timestep(timestep)
+    , max_duration(max_duration)
+    , limits(limits)
+    , position_tolerance(position_tolerance)
+    , use_streaming_mode(use_streaming_mode)
+  {
+  }
+
+  Configuration()
+  {
+  }
+
+  double timestep;
+  double max_duration;
+  Limits limits;
+  double position_tolerance;
+  // If streaming mode is enabled, trajectories are clipped at
+  // kNumWaypointsThreshold so the algorithm runs very quickly.
+  //
+  // Streaming mode is intended for realtime streaming applications.
+  //
+  // There could be even fewer waypoints than that if the goal is very
+  // close or the algorithm only finds a few waypoints successfully.
+  //
+  // In streaming mode, trajectory duration is not extended until it
+  // successfully reaches the goal.
+  bool use_streaming_mode;
+};
+}  // namespace trackjoint

include/trackjoint/error_codes.h

@@ -31,23 +31,28 @@ enum ErrorCodeEnum
   LIMIT_NOT_POSITIVE = 6,
   GOAL_POSITION_MISMATCH = 7,
   FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8,
-  LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9
+  LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9,
+  OBJECT_NOT_RESET = 10,
 };
 
 /**
  * \brief Use this map to look up human-readable strings for each error code
  */
-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" } });
+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" },
+    { OBJECT_NOT_RESET, "Must call reset() before generating trajectory" },
+
+});
 }  // end namespace trackjoint

include/trackjoint/single_joint_generator.h

@@ -12,57 +12,72 @@
 
 #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"
+#include "trackjoint/configuration.h"
 #include "trackjoint/limits.h"
 #include "trackjoint/utilities.h"
 
 namespace trackjoint
 {
+typedef Eigen::Spline<double, 1, 2> Spline1D;
+typedef Eigen::SplineFitting<Spline1D> SplineFitting1D;
+
 class SingleJointGenerator
 {
 public:
   /** \brief Constructor
+   *
+   * input num_waypoints_threshold minimum/maximum number of waypoints for full trajectory/streaming modes, respectively
+   * input max_num_waypoints_trajectory_mode to maintain determinism, return an error if more than this many waypoints
+   * is required
+   */
+  SingleJointGenerator(size_t num_waypoints_threshold, size_t max_num_waypoints_trajectory_mode);
+
+  /** \brief reset data members and prepare to generate a new trajectory
+   *
+   * input configuration A `Configuration` object
+   * 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
+   * input desired_num_waypoints nominal number of waypoints, calculated from user-supplied duration and timestep
+   * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust
+   * timestep
+   *
+   */
+  void reset(const Configuration& configuration, const KinematicState& current_joint_state,
+             const KinematicState& goal_joint_state, size_t desired_num_waypoints, bool timestep_was_upsampled);
+
+  /** \brief reset data members and prepare to generate a new trajectory
    *
    * 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
    * input limits vector of kinematic limits for each degree of freedom
    * input desired_num_waypoints nominal number of waypoints, calculated from user-supplied duration and timestep
-   * input num_waypoints_threshold minimum/maximum number of waypoints for full trajectory/streaming modes, respectively
-   * input max_num_waypoints_trajectory_mode to maintain determinism, return an error if more than this many waypoints
-   * is required
    * input position_tolerance tolerance for how close the final trajectory should follow a smooth interpolation.
    *                          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.
+   *                          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);
-
-  /** \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
    *
@@ -105,7 +120,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 */
@@ -115,26 +130,20 @@ class SingleJointGenerator
   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;
 
-  double timestep_;
-  double desired_duration_, max_duration_;
+  Configuration configuration_;
+  double desired_duration_;
   KinematicState current_joint_state_;
   KinematicState goal_joint_state_;
-  Limits limits_;
-  double position_tolerance_;
   Eigen::VectorXd nominal_times_;
   JointTrajectory waypoints_;
   size_t index_last_successful_;
-
-  // If streaming mode is enabled, trajectories are clipped at kNumWaypointsThreshold so the algorithm runs very
-  // quickly.
-  // streaming mode is intended for realtime streaming applications.
-  // There could be even fewer waypoints than that if the goal is very close or the algorithm only finds a few waypoints
-  // successfully.
-  // In streaming mode, trajectory duration is not extended until it successfully reaches the goal.
-  bool use_streaming_mode_;
+  bool is_reset_;
 };  // end class SingleJointGenerator
 }  // namespace trackjoint

include/trackjoint/trajectory_generator.h

@@ -21,6 +21,7 @@
 #include "trackjoint/utilities.h"
 
 // C++
+#include <boost/filesystem.hpp>
 #include <Eigen/Geometry>
 #include <memory>  // shared_ptr
 #include <vector>
@@ -78,7 +79,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 +88,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 +101,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,46 +21,53 @@ 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,
                                            goal_joint_states, limits, position_tolerance, use_streaming_mode);
+  traj_gen.reset(timestep, desired_duration, max_duration, current_joint_states, goal_joint_states, limits,
+                 position_tolerance, use_streaming_mode);
   // This vector holds the trajectories for each DOF
   std::vector<trackjoint::JointTrajectory> output_trajectories(num_dof);
   // Optionally, check user input for common errors, like current velocities being less than velocity limits