Adding another kinodynamic timer (#443)

* add kinodynamic retiming

* integrate retiming code

* add unit tests

* finish unit tests

* add docstrings and tests for post-processing

* code format

* fix tests for post processor

* fix code style

* code clean

* add CHANGELOG

* address Brian's comments

* address AVK's comments

* rename folder

* rename namespace

* Fix formatting.

* rename functions

* fix doc strings

* refactor the functions in detail namespace

* Rename OptimalRetimer to KunzRetimer.

* Speed up KunzRetimer tests.

Reducing the maxDeviation from 10 to 0.1 brings the runtime down from 8
seconds to 3 seconds.

* modify test values for speeding

CHANGELOG.md

@@ -47,6 +47,7 @@
   * Added flags to WorldStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339)
   * Changed interface for TrajectoryPostProcessor: [#341](https://github.com/personalrobotics/aikido/pull/341)
   * Planning calls with InverseKinematicsSampleable constraints explicitly set MetaSkeleton to solve IK with: [#379](https://github.com/personalrobotics/aikido/pull/379)
+  * Added a kinodynamic timer that generates a time-optimal smooth trajectory without completely stopping at each waypoint: [#443](https://github.com/personalrobotics/aikido/pull/443)
 
 * Robot
 

cmake/FindEIGEN3.cmake

@@ -0,0 +1,36 @@
+# Copyright (c) 2011-2018, The DART development contributors
+# All rights reserved.
+#
+# The list of contributors can be found at:
+#   https://github.com/dartsim/dart/blob/master/LICENSE
+#
+# This file is provided under the "BSD-style" License
+
+# Find Eigen
+#
+# This sets the following variables:
+# EIGEN3_FOUND
+# EIGEN3_INCLUDE_DIRS
+# EIGEN3_VERSION
+
+find_package(PkgConfig QUIET)
+
+# Check to see if pkgconfig is installed.
+pkg_check_modules(PC_EIGEN3 eigen3 QUIET)
+
+# Include directories
+find_path(EIGEN3_INCLUDE_DIRS
+    NAMES Eigen/Core
+    PATHS "${CMAKE_INSTALL_PREFIX}/include"
+    PATH_SUFFIXES eigen3 eigen)
+
+# Version
+set(EIGEN3_VERSION ${PC_EIGEN3_VERSION})
+
+# Set (NAME)_FOUND if all the variables and the version are satisfied.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(EIGEN3
+    FAIL_MESSAGE  DEFAULT_MSG
+    REQUIRED_VARS EIGEN3_INCLUDE_DIRS
+    VERSION_VAR   EIGEN3_VERSION)
+

include/aikido/planner/kunzretimer/KunzRetimer.hpp

@@ -0,0 +1,140 @@
+#ifndef AIKIDO_PLANNER_KUNZRETIMER_KUNZRETIMER_HPP_
+#define AIKIDO_PLANNER_KUNZRETIMER_KUNZRETIMER_HPP_
+
+#include <Eigen/Dense>
+#include "aikido/planner/TrajectoryPostProcessor.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+#include "aikido/trajectory/Spline.hpp"
+
+namespace aikido {
+namespace planner {
+namespace kunzretimer {
+
+/// Computes the time-optimal timing of a trajectory consisting of a sequence
+/// Geodesic interpolations between states under velocity and acceleration
+/// bounds. The output is a parabolic spline, encoded in cubic polynomials.
+/// It firstly preprocesses a non-differentiable path to a differentiable one
+/// by adding circular blends; and then \b exactly follows the preprocessed
+/// path.
+///
+/// The output trajectory consists of a sequence of trapezoidal velocity
+/// profiles that implement bang-bang control. This function curently only
+/// supports \c RealVector, \c SO2, and compound state spaces of those types.
+/// Additionally, this function requires that \c inputTrajectory to be
+/// interpolated using a \c GeodesicInterpolator.
+///
+/// \param[in] inputTrajectory Input piecewise Geodesic trajectory
+/// \param[in] maxVelocity Maximum velocity for each dimension
+/// \param[in] maxAcceleration Maximum acceleration for each dimension
+/// \param[in] maxDeviation Maximum deviation from a waypoint in doing circular
+/// blending around the waypoint
+/// \param[in] timeStep Time step in following the path
+/// \return Time optimal trajectory that satisfies velocity and acceleration
+/// constraints
+std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
+    const aikido::trajectory::Interpolated& inputTrajectory,
+    const Eigen::VectorXd& maxVelocity,
+    const Eigen::VectorXd& maxAcceleration,
+    double maxDeviation = 1e-2,
+    double timeStep = 0.1);
+
+/// Computes the time-optimal timing of a spline trajectory under velocity
+/// and acceleration bounds. The output is another parabolic spline, encoded
+/// in cubic polynomials. In retiming, the input trajectory is used as a
+/// piecewise Geodesic trajectory (only the geometry of the input trajectory
+/// is considered; and the velocity information is ignored. It firstly
+/// preprocesses a non-differentiable path to a differentiable one by adding
+/// circular blends; and then \b exactly follows the preprocessed path.
+///
+/// The output trajectory consists of a sequence of trapezoidal velocity
+/// profiles that implement bang-bang control. This function curently only
+/// supports \c RealVector, \c SO2, and compound state spaces of those types.
+/// Additionally, this function requires that \c inputTrajectory to be
+/// a spline (only the geometry of the trajectory is considered in retiming).
+///
+/// \param[in] inputTrajectory Input piecewise Geodesic trajectory
+/// \param[in] maxVelocity Maximum velocity for each dimension
+/// \param[in] maxAcceleration Maximum acceleration for each dimension
+/// \param[in] maxDeviation Maximum deviation from a waypoint in doing circular
+/// blending around the waypoint
+/// \param[in] timeStep Time step in following the path
+/// \return Time optimal trajectory that satisfies acceleration constraints
+std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
+    const aikido::trajectory::Spline& inputTrajectory,
+    const Eigen::VectorXd& maxVelocity,
+    const Eigen::VectorXd& maxAcceleration,
+    double maxDeviation = 1e-2,
+    double timeStep = 0.1);
+
+/// Class for performing time-optimal trajectory retiming following subject to
+/// velocity and acceleration limits.
+class KunzRetimer : public aikido::planner::TrajectoryPostProcessor
+{
+public:
+  /// \param[in] velocityLimits Maximum velocity for each dimension.
+  /// \param[in] accelerationLimits Maximum acceleration for each dimension.
+  /// \param[in] maxDeviation Maximum deviation in circular blending
+  /// \param[in] timeStep Time step in following the path
+  KunzRetimer(
+      const Eigen::VectorXd& velocityLimits,
+      const Eigen::VectorXd& accelerationLimits,
+      double maxDeviation,
+      double timeStep);
+
+  /// Performs parabolic retiming on an input trajectory.
+  /// \copydoc TrajectoryPostProcessor::postprocess
+  std::unique_ptr<aikido::trajectory::Spline> postprocess(
+      const aikido::trajectory::Interpolated& inputTraj,
+      const aikido::common::RNG& rng,
+      const aikido::constraint::TestablePtr& constraint = nullptr) override;
+
+  /// Performs parabolic retiming on an input *spline* trajectory.
+  /// \copydoc TrajectoryPostProcessor::postprocess
+  std::unique_ptr<aikido::trajectory::Spline> postprocess(
+      const aikido::trajectory::Spline& inputTraj,
+      const aikido::common::RNG& rng,
+      const aikido::constraint::TestablePtr& constraint = nullptr) override;
+
+  /// Returns the velocity limits of the dimensions
+  const Eigen::VectorXd& getVelocityLimits() const;
+
+  /// Returns the acceleration limits of the dimensions
+  const Eigen::VectorXd& getAccelerationLimits() const;
+
+  /// Sets the velocity limits of the dimensions
+  void setVelocityLimits(const Eigen::VectorXd& velocityLimits);
+
+  /// Sets the acceleration limits of the dimensions
+  void setAccelerationLimits(const Eigen::VectorXd& accelerationLimits);
+
+  /// Returns the time step in following a path
+  double getTimeStep() const;
+
+  /// Sets the time step in following a path
+  void setTimeStep(double timeStep);
+
+  /// Returns the max deviation of circular blending
+  double getMaxDeviation() const;
+
+  /// Sets the max deviation of circular blending
+  void setMaxDeviation(double maxDeviation);
+
+private:
+  /// Set to the value of \c velocityLimits.
+  Eigen::VectorXd mVelocityLimits;
+
+  /// Set to the value of \c accelerationLimits.
+  Eigen::VectorXd mAccelerationLimits;
+
+  /// Set to the value of \c maxDeviation
+  double mMaxDeviation;
+
+  /// Set to the value of \c timeStep
+  double mTimeStep;
+};
+
+} // namespace kunzretimer
+} // namespace planner
+} // namespace aikido
+
+#endif // ifndef AIKIDO_PLANNER_KUNZRETIMER_KUNZRETIMER_HPP_

src/CMakeLists.txt

@@ -2,6 +2,7 @@
 # Aikido targets are marked as [target_name].
 
 add_subdirectory("external/hauser_parabolic_smoother")
+add_subdirectory("external/kunz_retimer")
 
 add_subdirectory("common")     # boost, dart
 add_subdirectory("io")         # [common], boost, dart, tinyxml2, yaml-cpp

src/external/kunz_retimer/CMakeLists.txt

@@ -0,0 +1,18 @@
+find_package(EIGEN3 REQUIRED)
+
+add_library("${PROJECT_NAME}_external_kunzretimer" STATIC
+  Path.cpp
+  Trajectory.cpp
+)
+
+target_include_directories("${PROJECT_NAME}_external_kunzretimer"
+  PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}"
+          ${EIGEN3_INCLUDE_DIRS}
+)
+set_target_properties("${PROJECT_NAME}_external_kunzretimer"
+  PROPERTIES POSITION_INDEPENDENT_CODE TRUE
+)
+target_compile_options("${PROJECT_NAME}_external_kunzretimer"
+  PUBLIC ${AIKIDO_CXX_STANDARD_FLAGS}
+  PRIVATE -w
+)