Merge pull request #129 from hungpham2511/feat-parametrizer

[cpp]Implement trajectory parametrizer

.circleci/config.yml

@@ -118,15 +118,15 @@ jobs:
           name: Install Python API with bindings
           command: |
             pip install numpy cython invoke  # absolute minimum to run setup.py
-            invoke install-solvers
+            python -m invoke install-solvers  # require python because of circleci glitch
             pip install -e .[dev]
 
       - run:
           name: Test Python API with bindings
           command: |
             export ROS_PACKAGE_PATH=/opt/openrobots/share
             export PYTHONPATH=/opt/openrobots/lib/python3.6/site-packages:$PYTHONPATH
-            pytest tests --durations=3 --junitxml=test-reports/junit.xml
+            python -m pytest tests --durations=3 --junitxml=test-reports/junit.xml
 
       - store_test_results:
           path: test-reports

.clang-format

@@ -112,6 +112,4 @@ SpacesInParentheses: false
 SpacesInSquareBrackets: false
 Standard:        Auto
 TabWidth:        8
-UseTab:          Never
-...
-
+UseTab:          Never

.gitignore

@@ -107,4 +107,8 @@ venv.bak/
 .mypy_cache/
 
 # emacs temp file
-*~
+*~
+cpp/.clangd
+build*
+cpp/doc/CMakeFiles/*
+cpp/doc/doc/*

HISTORY.md

@@ -1,5 +1,10 @@
 # History
 
+## Unrelease
+
+### Added
+- [cpp] [#129][#129] Implement constant-acceleration trajectory parametrizer
+
 ## 0.3.1 (Aug 23 2020)
 
 ### Added
@@ -58,4 +63,5 @@ Major release! Implement TOPPRA in C++ and several improvements to Python codeba
 [gh-78]: https://github.com/hungpham2511/toppra/pull/78
 [gh-79]: https://github.com/hungpham2511/toppra/pull/79
 [#117]: https://github.com/hungpham2511/toppra/pull/117
+[#129]: https://github.com/hungpham2511/toppra/pull/129
 [cm-0e022c]: https://github.com/hungpham2511/toppra/commit/0e022c53ab9db473485bd9fb6b8f34a7364efdf8

cpp/README.md

@@ -28,32 +28,64 @@ export PYTHONPATH=/opt/openrobots/lib/python3.6/site-packages:$PYTHONPATH
 # or any python version that you install
 ```
 
-
-# Using TOPPRA in CMake-based project
-
-In your CMakeLists.txt,
-```cmake
-# The following line defines cmake target toppra::toppra
-find_package(toppra)
-...
-target_link_library(foo PUBLIC toppra::toppra)
-```
-
 ## How to install optional dependencies:
 
 - GLPK: `sudo apt install libglpk-dev`
 - qpOASES: `sudo apt install robotpkg-qpoases` (follow http://robotpkg.openrobots.org/debian.html for robotpkg)
 - pinocchio: `sudo apt install robotpkg-pinocchio` (follow http://robotpkg.openrobots.org/debian.html for robotpkg)
 
 
-# Building bindings
+## Building with Python bindings
 
 - Install pybind11 2.5.0
 - Build toppra-cpp with bindings on
 ``` sh
 cmake -GNinja -DCMAKE_EXPORT_COMPILE_COMMANDS=1 -DBUILD_WITH_PINOCCHIO=ON -DBUILD_WITH_qpOASES=ON -DBUILD_WITH_GLPK=ON -DPYTHON_BINDINGS=ON -DPYBIND11_PYTHON_VERSION=3.7 ..
 # change to 3.8 or other version as needed.
+# build and test
+ninja 
 ```
 - Install toppra python normally.
 
+## Issues during build
+
+``` sh
+/usr/local/include/pybind11/detail/common.h:112:10: fatal error: Python.h: No such file or directory
+ #include <Python.h>
+          ^~~~~~~~~~
+compilation terminated.
+```
+This is because during compilation, `pybind11` can not find the Python header files. 
+Check that you have `libpython-dev` or `libpython3-dev` installed and whether the 
+correct Python executable  is found during cmake step. You should see something similar to
+below:
+
+``` sh
+-- Found PythonInterp: /usr/bin/python3.7 (found suitable version "3.7.4", minimum required is "3.7")
+-- Found PythonLibs: /usr/lib/x86_64-linux-gnu/libpython3.7m.so
+-- Found /usr/include/python3.7m /usr/bin/python3.7
+```
+
+## Building doxygen doc
+
+Make sure that `doxygen` is installed and available. Running cmake
+normally will create a rule for building doc with doxygen.
+
+``` sh
+# Run cmake normally
+ninja doc # or make doc
+# The documentation is available at doc/doc/html/index.html
+```
+
+# Using TOPPRA in CMake-based project
+
+In your CMakeLists.txt,
+```cmake
+# The following line defines cmake target toppra::toppra
+find_package(toppra)
+...
+target_link_library(foo PUBLIC toppra::toppra)
+```
+
+
 

cpp/doc/Doxyfile

@@ -336,4 +336,4 @@ MAX_DOT_GRAPH_DEPTH    = 0
 DOT_TRANSPARENT        = NO
 DOT_MULTI_TARGETS      = NO
 GENERATE_LEGEND        = YES
-DOT_CLEANUP            = YES
+DOT_CLEANUP            = YES

cpp/src/CMakeLists.txt

@@ -10,6 +10,8 @@ set(SOURCE_FILES
 
   toppra/geometric_path.cpp
   toppra/geometric_path/piecewise_poly_path.cpp
+  toppra/parametrizer/const_accel.cpp
+  toppra/parametrizer.cpp
 
   toppra/algorithm.cpp
   toppra/algorithm/toppra.cpp
@@ -65,6 +67,7 @@ install(FILES
   ${CMAKE_CURRENT_BINARY_DIR}/toppra/export.hpp
 
   toppra/geometric_path.hpp
+  toppra/parametrizer.hpp
   toppra/toppra.hpp
 
   toppra/solver.hpp
@@ -81,7 +84,9 @@ install(FILES
 install(FILES
   toppra/geometric_path/piecewise_poly_path.hpp
   DESTINATION include/toppra/geometric_path)
-
+install(FILES
+  toppra/parametrizer/const_accel.hpp
+  DESTINATION include/toppra/parametrizer)
 install(FILES
   toppra/constraint/joint_torque/pinocchio.hpp
   DESTINATION include/toppra/constraint/joint_torque)

cpp/src/toppra/algorithm.cpp

@@ -13,17 +13,16 @@ PathParametrizationAlgorithm::PathParametrizationAlgorithm(
 
 ReturnCode PathParametrizationAlgorithm::computePathParametrization(value_type vel_start,
                                                                     value_type vel_end) {
-  ReturnCode ret;
   initialize();
   m_solver->setupSolver();
   Bound vel_ends;
   vel_ends.setConstant(vel_end);
-  ret = computeControllableSets(vel_ends);
-  if ((int)ret > 0) {
-    return ret;
+  m_data.ret_code = computeControllableSets(vel_ends);
+  if ((int)m_data.ret_code > 0) {
+    return m_data.ret_code;
   }
-  ret = computeForwardPass(vel_start);
-  return ret;
+  m_data.ret_code = computeForwardPass(vel_start);
+  return m_data.ret_code;
 };
 
 ReturnCode PathParametrizationAlgorithm::computeControllableSets(

cpp/src/toppra/geometric_path.hpp

@@ -31,19 +31,19 @@ class GeometricPath {
   GeometricPath(int configSize, int nDof) : m_configSize(configSize), m_dof (nDof) {}
 
   /**
-   * /brief Evaluate the path at given position.
+   * \brief Evaluate the path at given position.
    */
   virtual Vector eval_single(value_type, int order = 0) const = 0;
 
   /**
-   * /brief Evaluate the path at given positions (vector).
+   * \brief Evaluate the path at given positions (vector).
    *
    * Default implementation: Evaluation each point one-by-one.
    */
   virtual Vectors eval(const Vector &positions, int order = 0) const;
 
   /**
-   * \return the dimension of the configuration space
+   * \brief Dimension of the configuration space
    */
   int configSize() const
   {
@@ -59,17 +59,17 @@ class GeometricPath {
   }
 
   /**
-   * Serialize path to stream.
+   * \brief Serialize path to stream.
    */
   virtual void serialize(std::ostream &O) const {};
 
   /**
-   * Deserialize stream to construct path.
+   * \brief Deserialize stream to construct path.
    */
   virtual void deserialize(std::istream &I){};
 
   /**
-   * \return the starting and ending path positions.
+   * \brief Starting and ending path positions.
    */
   virtual Bound pathInterval() const = 0;
 

cpp/src/toppra/geometric_path/piecewise_poly_path.hpp

@@ -37,17 +37,17 @@ class PiecewisePolyPath : public GeometricPath {
   /**
    * /brief Evaluate the path at given position.
    */
-  Vector eval_single(value_type, int order = 0) const;
+  Vector eval_single(value_type, int order = 0) const override;
 
   /**
    * /brief Evaluate the path at given positions (vector).
    */
-  Vectors eval(const Vector &, int order = 0) const;
+  Vectors eval(const Vector &, int order = 0) const override;
 
   /**
    * Return the starting and ending path positions.
    */
-  Bound pathInterval() const;
+  Bound pathInterval() const override;
   void serialize(std::ostream &O) const override;
   void deserialize(std::istream &I) override;
 

cpp/src/toppra/parametrizer.cpp

@@ -0,0 +1,37 @@
+#include <toppra/parametrizer.hpp>
+#include <toppra/toppra.hpp>
+
+namespace toppra {
+
+Parametrizer::Parametrizer(GeometricPathPtr path, const Vector& gridpoints,
+                           const Vector& vsquared)
+    : m_path(path), m_gridpoints(gridpoints) {
+  assert(gridpoints.size() == vsquared.size());
+  m_vs.resize(vsquared.size());
+  for (std::size_t i = 0; i < gridpoints.size(); i++) {
+    assert(vsquared[i] >= 0);
+    m_vs[i] = std::sqrt(vsquared[i]);
+    if (i == gridpoints.size() - 1) continue;
+    assert(gridpoints[i + 1] > gridpoints[i]);
+  }
+
+  assert(std::abs(path->pathInterval()[0] - gridpoints[0]) < TOPPRA_NEARLY_ZERO);
+  assert(std::abs(path->pathInterval()[1] - gridpoints[gridpoints.size() - 1]) <
+         TOPPRA_NEARLY_ZERO);
+}
+
+Vector Parametrizer::eval_single(value_type val, int order) const {
+  Vector v{1};
+  v << val;
+  auto results = eval_impl(v, order);
+  return results[0];
+}
+
+Vectors Parametrizer::eval(const Vector& positions, int order) const {
+  return eval_impl(positions, order);
+}
+
+Bound Parametrizer::pathInterval() const { return pathInterval_impl(); }
+
+bool Parametrizer::validate() const { return validate_impl(); }
+}  // namespace toppra

cpp/src/toppra/parametrizer.hpp

@@ -0,0 +1,71 @@
+#ifndef TOPPRA_PARAMETRIZER_HPP
+#define TOPPRA_PARAMETRIZER_HPP
+
+#include <toppra/geometric_path.hpp>
+#include <toppra/toppra.hpp>
+
+namespace toppra {
+
+/**
+ * \brief Abstract output trajectory parametrizers.
+ *
+ * A parametrizer has the same interface as a geometric path. It
+ * receives as input the original geometric path and two arrays of
+ * gridpoints and parametrization (i.e. squared path velocities). A
+ * parametrizer should validate the input data. If not validated,
+ * evaluation results are not defined.
+ *
+ * Requirements: https://github.com/hungpham2511/toppra/issues/102
+ *
+ * Sub-classes should override the virtual private methods *_impl.
+ */
+class Parametrizer : public GeometricPath {
+ public:
+  /** Construct the parametrizer.
+   *
+   * \param path Input geometric path.
+   * \param gridpoints Shape (N+1,). Gridpoints of the parametrization, should be
+   * compatible with the path domain. \param vsquared Shape (N+1,). Path velocity
+   * squared, should have same shape as gridpoints as vsquared[i] corresponds to the
+   * velocity at gridpoints[i].
+   */
+  Parametrizer(GeometricPathPtr path, const Vector &gridpoints, const Vector &vsquared);
+
+  /** \brief Evaluate the path at given position.
+   */
+  Vector eval_single(value_type, int order = 0) const override;
+
+  /** \brief Evaluate the path at given positions (vector).
+   */
+  Vectors eval(const Vector &, int order = 0) const override;
+
+  /** \brief Return the starting and ending path positions.
+   */
+  Bound pathInterval() const override;
+
+  /** \brief Validate input data.
+   *
+   * Return false if something is wrong with the output
+   * trajectory. Only use the trajectory if validation successes.
+   */
+  bool validate() const;
+
+  virtual ~Parametrizer() {}
+
+ protected:
+  // Input geometric path
+  GeometricPathPtr m_path;
+  // Input gridpoints
+  Vector m_gridpoints;
+  // Input path velocities (not squared)
+  Vector m_vs;
+
+ private:
+  /// To be overwriten by derived classes
+  virtual Vectors eval_impl(const Vector &, int order = 0) const = 0;
+  virtual bool validate_impl() const = 0;
+  virtual Bound pathInterval_impl() const = 0;
+};
+};  // namespace toppra
+
+#endif