From c03e2fab6b8f25e505610b0df6b764cfd85b9d5c Mon Sep 17 00:00:00 2001
From: Niels Dehio <niels.dehio@mailbox.org>
Date: Tue, 22 Sep 2020 17:57:15 +0200
Subject: [PATCH] add c++ method 'plot_parametrization()' that creates a python
 file to plot parametrization similar to the toppra python interface

---
 cpp/src/toppra/parametrizer.cpp             |  2 +-
 cpp/src/toppra/parametrizer.hpp             |  2 +
 cpp/src/toppra/parametrizer/const_accel.cpp | 90 +++++++++++++++++++++
 cpp/src/toppra/parametrizer/const_accel.hpp |  9 +++
 cpp/tests/test_toppra_approach.cpp          |  3 +
 5 files changed, 105 insertions(+), 1 deletion(-)

diff --git a/cpp/src/toppra/parametrizer.cpp b/cpp/src/toppra/parametrizer.cpp
index 67764b91..2a947113 100644
--- a/cpp/src/toppra/parametrizer.cpp
+++ b/cpp/src/toppra/parametrizer.cpp
@@ -5,7 +5,7 @@ namespace toppra {
 
 Parametrizer::Parametrizer(GeometricPathPtr path, const Vector& gridpoints,
                            const Vector& vsquared)
-    : m_path(path), m_gridpoints(gridpoints) {
+    : m_path(path), m_gridpoints(gridpoints), m_vsquared(vsquared) {
   assert(gridpoints.size() == vsquared.size());
   m_vs.resize(vsquared.size());
   for (std::size_t i = 0; i < gridpoints.size(); i++) {
diff --git a/cpp/src/toppra/parametrizer.hpp b/cpp/src/toppra/parametrizer.hpp
index 3aa67680..df9bccf1 100644
--- a/cpp/src/toppra/parametrizer.hpp
+++ b/cpp/src/toppra/parametrizer.hpp
@@ -57,6 +57,8 @@ class Parametrizer : public GeometricPath {
   GeometricPathPtr m_path;
   // Input gridpoints
   Vector m_gridpoints;
+  // Input vsquared
+  Vector m_vsquared;
   // Input path velocities (not squared)
   Vector m_vs;
 
diff --git a/cpp/src/toppra/parametrizer/const_accel.cpp b/cpp/src/toppra/parametrizer/const_accel.cpp
index a5e8ad54..e1c2f11b 100644
--- a/cpp/src/toppra/parametrizer/const_accel.cpp
+++ b/cpp/src/toppra/parametrizer/const_accel.cpp
@@ -1,4 +1,5 @@
 #include <algorithm>
+#include <fstream>
 #include <cmath>
 #include <cstddef>
 #include <cstdlib>
@@ -119,5 +120,94 @@ Bound ConstAccel::pathInterval_impl() const {
   return b;
 }
 
+bool ConstAccel::plot_parametrization(const int n_sample) {
+  // reimplements the function plot_parametrization() from the file toppra/parametrizer.py 
+  Vector _ss = this->m_gridpoints;
+  Vector _velocities = this->m_vsquared;
+  Bound pi = this->pathInterval();
+  Vector ts = Vector::LinSpaced(n_sample, pi(0), pi(1));
+  Vector ss, vs, us;
+  bool ok = this->evalParams(ts, ss, vs, us);
+  if(!ok){
+    return false;
+  }
+  Vectors qs = m_path->eval(ss);
+  Vector ss_dense = Vector::LinSpaced(n_sample, _ss(0), _ss(_ss.size()-1));
+  Vectors _ss_dense = m_path->eval(ss_dense);
+
+  //write python code to the file
+  std::ofstream myfile;
+  myfile.open("plot_parametrization.py");
+  myfile << "import numpy as np\n";
+  myfile << "import matplotlib.pyplot as plt\n";
+  writeVectorToFile(myfile, ts, "ts");
+  writeVectorToFile(myfile, ss, "ss");
+  writeVectorToFile(myfile, vs, "vs");
+  writeVectorsToFile(myfile, qs, "qs");
+  writeVectorToFile(myfile, this->m_ts, "_ts");
+  writeVectorToFile(myfile, _ss, "_ss");
+  writeVectorToFile(myfile, _velocities, "_velocities");
+  writeVectorToFile(myfile, ss_dense, "ss_dense");
+  writeVectorsToFile(myfile, _ss_dense, "_ss_dense");
+  myfile << "plt.subplot(2, 2, 1)\n";
+  myfile << "plt.plot(ts, ss, label=\"s(t)\")\n";
+  myfile << "plt.plot(_ts, _ss, \"o\", label=\"input\")\n";
+  myfile << "plt.title(\"path(time)\")\n";
+  myfile << "plt.legend()\n";
+  myfile << "plt.subplot(2, 2, 2)\n";
+  myfile << "plt.plot(ss, vs, label=\"v(s)\")\n";
+  myfile << "plt.plot(_ss, _velocities, \"o\", label=\"input\")\n";
+  myfile << "plt.title(\"velocity(path)\")\n";
+  myfile << "plt.legend()\n";
+  myfile << "plt.subplot(2, 2, 3)\n";
+  myfile << "plt.plot(ts, qs)\n";
+  myfile << "plt.title(\"retimed path\")\n";
+  myfile << "plt.subplot(2, 2, 4)\n";
+  myfile << "plt.plot(ss_dense, _ss_dense)\n";
+  myfile << "plt.title(\"original path\")\n";
+  myfile << "plt.tight_layout()\n";
+  myfile << "plt.show()\n";
+  myfile.close();
+
+  //execute python file
+  system("python plot_parametrization.py");
+
+  return true;
+}
+
+void ConstAccel::writeVectorToFile(std::ofstream& myfile, const Vector& vector, const std::string & name) const {
+  const double factor = 10000;
+  myfile << name << " = np.array([";
+  for(size_t i=0; i<vector.size(); i++){
+    myfile << factor*vector(i);
+    if(i<vector.size()-1){
+      myfile << ", ";
+    }
+  }
+  myfile << "])\n";
+  myfile << name << " = " << name << " / " << std::to_string(factor) << "\n";
+}
+
+void ConstAccel::writeVectorsToFile(std::ofstream& myfile, const Vectors& vectors, const std::string & name) const {
+  const double factor = 10000;
+  myfile << name << " = np.array([";
+  for (size_t i = 0; i < vectors.size(); i++) {
+    Vector vector = vectors.at(i);
+    myfile << "[";
+    for (size_t j = 0; j < vector.size(); j++) {
+      myfile << factor*vector(j);
+      if(j<vector.size()-1){
+        myfile << ", ";
+      }
+    }
+    myfile << "]";
+    if(i<vectors.size()-1){
+      myfile << ", ";
+    }
+  }
+  myfile << "])\n";
+  myfile << name << " = " << name << " / " << std::to_string(factor) << "\n";
+}
+
 }  // namespace parametrizer
 }  // namespace toppra
diff --git a/cpp/src/toppra/parametrizer/const_accel.hpp b/cpp/src/toppra/parametrizer/const_accel.hpp
index e7e1daf2..e812b642 100644
--- a/cpp/src/toppra/parametrizer/const_accel.hpp
+++ b/cpp/src/toppra/parametrizer/const_accel.hpp
@@ -16,11 +16,20 @@ class ConstAccel : public Parametrizer {
  public:
   ConstAccel(GeometricPathPtr path, const Vector &gridpoints, const Vector &vsquared);
 
+  /** \brief Create a python file and execute it to plot the output parametrization and show it.
+   *
+   * \param[in] n_sample Number of samples.
+   *
+   */
+  bool plot_parametrization(const int n_sample);
+
  private:
   /** Return joint derivatives at specified times. */
   Vectors eval_impl(const Vector &times, int order = 0) const override;
   bool validate_impl() const override;
   Bound pathInterval_impl() const override;
+  void writeVectorToFile(std::ofstream& myfile, const Vector& vector, const std::string & name) const;
+  void writeVectorsToFile(std::ofstream& myfile, const Vectors& vectors, const std::string & name) const;
 
   /** \brief Evaluate path variables ss, vs, and us at the input time instances.
    *
diff --git a/cpp/tests/test_toppra_approach.cpp b/cpp/tests/test_toppra_approach.cpp
index 8eb0a52b..ba6016a7 100644
--- a/cpp/tests/test_toppra_approach.cpp
+++ b/cpp/tests/test_toppra_approach.cpp
@@ -149,4 +149,7 @@ TEST_F(Approach, ToppraCompleteExample) {
       ASSERT_LE(path_acc2_(jointID, i), accLimitUpper(jointID) * (1 + RTOL));
     }
   }
+
+  bool ok = ca->plot_parametrization(41);
+  ASSERT_TRUE(ok);
 }