Zultron/tracksuite refactor pt1

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,24 +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

@@ -17,6 +17,7 @@
 #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"
 
@@ -29,29 +30,42 @@ 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 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 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 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, bool timestep_was_upsampled);
@@ -123,22 +137,13 @@ class SingleJointGenerator
 
   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

src/simple_example.cpp

@@ -66,6 +66,8 @@ int main(int argc, char** argv)
   // 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