[WIP] Rapid robotics test

.travis.yml

@@ -22,6 +22,7 @@ env:
       - TEST=clang-format    # check code formatting for compliance to .clang-format rules
       - TEST=clang-tidy-fix  # perform static code analysis and compliance check against .clang-tidy rules
       - TEST=catkin_lint     # perform catkin_lint checks
+      - TEST=code-coverage   # check test coverage
       # pull in packages from a local .rosinstall file
       - UPSTREAM_WORKSPACE=trackjoint.rosinstall
 

CMakeLists.txt

@@ -69,7 +69,6 @@ add_executable(
   ${PROJECT_NAME}_simple_example
   src/simple_example.cpp
 )
-# Rename C++ executable without namespace
 set_target_properties(
   ${PROJECT_NAME}_simple_example
   PROPERTIES
@@ -81,36 +80,32 @@ target_link_libraries(
   ${catkin_LIBRARIES}
 )
 
-# A streaming example (update within a loop)
 add_executable(
-  ${PROJECT_NAME}_streaming_example
-  src/streaming_example.cpp
+  ${PROJECT_NAME}_six_dof_examples
+  src/six_dof_examples.cpp
 )
-# Rename C++ executable without namespace
 set_target_properties(
-  ${PROJECT_NAME}_streaming_example
+  ${PROJECT_NAME}_six_dof_examples
   PROPERTIES
-    OUTPUT_NAME streaming_example PREFIX ""
+    OUTPUT_NAME six_dof_examples PREFIX ""
 )
 target_link_libraries(
-  ${PROJECT_NAME}_streaming_example
+  ${PROJECT_NAME}_six_dof_examples
   ${LIBRARY_NAME}
   ${catkin_LIBRARIES}
 )
 
 add_executable(
-  ${PROJECT_NAME}_three_dof_examples
-  src/three_dof_examples.cpp
+  ${PROJECT_NAME}_streaming_example
+  src/streaming_example.cpp
 )
-
 set_target_properties(
-  ${PROJECT_NAME}_three_dof_examples
+  ${PROJECT_NAME}_streaming_example
   PROPERTIES
-    OUTPUT_NAME three_dof_examples PREFIX ""
+    OUTPUT_NAME streaming_example PREFIX ""
 )
-
 target_link_libraries(
-  ${PROJECT_NAME}_three_dof_examples
+  ${PROJECT_NAME}_streaming_example
   ${LIBRARY_NAME}
   ${catkin_LIBRARIES}
 )
@@ -122,7 +117,7 @@ target_link_libraries(
 # Mark executables and/or libraries for installation
 install(
   TARGETS
-    ${PROJECT_NAME} ${PROJECT_NAME}_simple_example ${PROJECT_NAME}_streaming_example
+    ${PROJECT_NAME} ${PROJECT_NAME}_simple_example
   ARCHIVE DESTINATION
     ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION
@@ -155,6 +150,15 @@ if(CATKIN_ENABLE_TESTING)
     ${catkin_LIBRARIES}
   )
 
+  add_rostest_gtest(single_joint_generator_test
+    test/single_joint_generator_test.test
+    test/single_joint_generator_test.cpp
+  )
+  target_link_libraries(single_joint_generator_test
+    ${LIBRARY_NAME}
+    ${catkin_LIBRARIES}
+  )
+
 endif()
 
 ## Test for correct C++ source code

README.md

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

include/trackjoint/configuration.h

@@ -21,13 +21,11 @@ namespace trackjoint
  */
 struct Configuration
 {
-  Configuration(double timestep, double max_duration, const Limits& limits, const double position_tolerance,
-                bool use_streaming_mode)
+  Configuration(double timestep, double max_duration, const Limits& limits, const double position_tolerance)
     : timestep(timestep)
     , max_duration(max_duration)
     , limits(limits)
     , position_tolerance(position_tolerance)
-    , use_streaming_mode(use_streaming_mode)
   {
   }
 
@@ -39,16 +37,5 @@ struct Configuration
   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,8 +31,8 @@ enum ErrorCodeEnum
   LIMIT_NOT_POSITIVE = 6,
   GOAL_POSITION_MISMATCH = 7,
   FAILURE_TO_GENERATE_SINGLE_WAYPOINT = 8,
-  LESS_THAN_TEN_TIMESTEPS_FOR_STREAMING_MODE = 9,
-  OBJECT_NOT_RESET = 10,
+  OBJECT_NOT_RESET = 9,
+  ERROR_IN_TIMESTEP_STRETCHING = 10
 };
 
 /**
@@ -50,9 +50,7 @@ const std::unordered_map<uint, std::string> ERROR_CODE_MAP({
     { 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" },
-
+    { ERROR_IN_TIMESTEP_STRETCHING, "Error during timestep stretching" }
 });
 }  // end namespace trackjoint

include/trackjoint/single_joint_generator.h

@@ -31,7 +31,7 @@ class SingleJointGenerator
 public:
   /** \brief Constructor
    *
-   * input num_waypoints_threshold minimum/maximum number of waypoints for full trajectory/streaming modes, respectively
+   * input num_waypoints_threshold minimum/maximum number of waypoints
    * input max_num_waypoints_trajectory_mode to maintain determinism, return an error if more than this many waypoints
    * is required
    */
@@ -60,15 +60,13 @@ class SingleJointGenerator
    * input desired_num_waypoints nominal number of waypoints, calculated from user-supplied duration and timestep
    * 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.
    * input timestep_was_upsampled If upsampling happened (we are working with very few waypoints), do not adjust
    * timestep
    *
    */
   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);
+             const double position_tolerance, bool timestep_was_upsampled);
 
   /** \brief Generate a jerk-limited trajectory for this joint
    *
@@ -77,37 +75,29 @@ class SingleJointGenerator
   ErrorCodeEnum generateTrajectory();
 
   /** \brief Extend a trajectory to a new duration. Magnitudes of vel/accel/jerk will be decreased. */
-  void extendTrajectoryDuration();
+  void extendTrajectoryDuration(const Eigen::VectorXd stretched_times);
 
   /** \brief Get the generated trajectory
    *
    * return a vector of kinematic states for the joint
    */
   JointTrajectory getTrajectory();
 
-  /** \brief Get the last waypoint that successfully matched the polynomial interpolation
-   *
-   * return the waypoint index
-   */
-  size_t getLastSuccessfulIndex();
+  /** \brief Get the number of waypoints */
+  size_t getTrajectoryLength();
 
   /** \brief Update desired_duration_ for this joint
    *
    * input new_trajectory_duration the new desired duration. Units not important as long as they are consistent
    */
   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 Gradually stretch timesteps so trajectory duration is extended to new_duration.
+   *
+   */
+  ErrorCodeEnum calculateStretchedTimes(const double new_duration, Eigen::VectorXd& stretched_times);
 
+private:
   /** \brief interpolate from start to end state with a polynomial
    *
    * input times a vector of waypoint times.
@@ -116,25 +106,22 @@ class SingleJointGenerator
   Eigen::VectorXd interpolate(Eigen::VectorXd& times);
 
   /** \brief Step through a vector of velocities, compensating for limits. Start from the beginning. */
-  ErrorCodeEnum forwardLimitCompensation(size_t* index_last_successful);
+  ErrorCodeEnum forwardLimitCompensation(bool& successful_limit_comp);
 
   /** \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);
 
   /** \brief This uses backwardLimitCompensation() but it starts from a position
    * vector */
-  ErrorCodeEnum positionVectorLimitLookAhead(size_t* index_last_successful);
+  ErrorCodeEnum positionVectorLimitLookAhead(bool& successful_limit_comp);
 
   /** \brief Check whether the duration needs to be extended, and do it */
   ErrorCodeEnum predictTimeToReach();
 
   /** \brief Calculate vel/accel/jerk from position */
   void calculateDerivativesFromPosition();
 
-  /** \brief Calculate accel/jerk from velocity */
-  void calculateDerivativesFromVelocity();
-
   const size_t kNumWaypointsThreshold, kMaxNumWaypointsFullTrajectory;
 
   Configuration configuration_;
@@ -143,7 +130,7 @@ class SingleJointGenerator
   KinematicState goal_joint_state_;
   Eigen::VectorXd nominal_times_;
   JointTrajectory waypoints_;
-  size_t index_last_successful_;
+  bool successful_limit_comp_;
   bool is_reset_;
 };  // end class SingleJointGenerator
 }  // namespace trackjoint

include/trackjoint/trajectory_generator.h

@@ -43,19 +43,17 @@ class TrajectoryGenerator
    * input limits vector of kinematic limits for each degree of freedom
    * 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 kNumWaypointsThreshold
-   * waypoints.
    */
   TrajectoryGenerator(uint num_dof, double timestep, double desired_duration, double max_duration,
                       const std::vector<KinematicState>& current_joint_states,
                       const std::vector<KinematicState>& goal_joint_states, const std::vector<Limits>& limits,
-                      const double position_tolerance, bool use_streaming_mode);
+                      const double position_tolerance);
 
   /** \brief reset the member variables of the object and prepare to generate a new trajectory */
   void reset(double timestep, double desired_duration, double max_duration,
              const std::vector<KinematicState>& current_joint_states,
              const std::vector<KinematicState>& goal_joint_states, const std::vector<Limits>& limits,
-             const double position_tolerance, bool use_streaming_mode);
+             const double position_tolerance);
 
   /** \brief Generate and return trajectories for every joint
    *
@@ -85,12 +83,6 @@ class TrajectoryGenerator
                               double nominal_timestep);
 
 private:
-  /** \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. */
   void upsample();
 
@@ -113,18 +105,15 @@ class TrajectoryGenerator
   ErrorCodeEnum synchronizeTrajComponents(std::vector<JointTrajectory>* output_trajectories);
 
   // TODO(andyz): set this back to a small number when done testing
-  // TODO(709): Remove kMaxNumWaypointsFullTrajectory - not needed now that we have streaming mode
   const size_t kMaxNumWaypointsFullTrajectory = 10000;  // A relatively small number, to run fast
-  // Upsample for better accuracy if num waypoints is below threshold in full trajectory mode
-  // Clip trajectories to threshold in streaming mode
+  // Upsample for better accuracy if num waypoints is below threshold
   const size_t kNumWaypointsThreshold = 10;
 
   const uint kNumDof;
   double desired_timestep_, upsampled_timestep_;
   double desired_duration_, max_duration_;
   std::vector<KinematicState> current_joint_states_;
   std::vector<Limits> limits_;
-  bool use_streaming_mode_;
   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.

scripts/plot

@@ -0,0 +1,19 @@
+#!/usr/bin/env python3
+#
+# sudo apt-get install python3-pandas
+import pandas
+import matplotlib.pyplot as plt
+import sys
+
+# Read command-line args
+if len(sys.argv) < 2:
+    sys.stderr.write(f"Usage: {sys.argv[0]} FILE.csv [YLIM_MIN YLIM_MAX]\n")
+    sys.exit(1)
+fname = sys.argv[1]
+ylim = tuple([int(i) for i in (sys.argv[2:] + [-2000, 2000])[:2]])
+
+# Generate and show plot
+data = pandas.read_csv(fname, sep=' ')
+data.columns=['t', 'd', 'v', 'a', 'j']
+ax = data.plot(x='t', ylim=ylim)
+plt.show()