Access to step_nb and step_progress

Author: edantec

bindings/expose-interpolate.cpp

@@ -27,7 +27,9 @@ namespace simple_mpc
         .add_property("x_interpolated", &Interpolator::x_interpolated_)
         .add_property("u_interpolated", &Interpolator::u_interpolated_)
         .add_property("a_interpolated", &Interpolator::a_interpolated_)
-        .add_property("forces_interpolated", &Interpolator::forces_interpolated_);
+        .add_property("forces_interpolated", &Interpolator::forces_interpolated_)
+        .add_property("step_nb", &Interpolator::step_nb_)
+        .add_property("step_progress", &Interpolator::step_progress_);
     }
 
   } // namespace python

include/simple-mpc/interpolator.hpp

@@ -29,11 +29,20 @@ namespace simple_mpc
       std::vector<Eigen::VectorXd> ddqs,
       std::vector<Eigen::VectorXd> forces);
 
+    // Interpolated trajectories
     Eigen::VectorXd x_interpolated_;
     Eigen::VectorXd u_interpolated_;
     Eigen::VectorXd a_interpolated_;
     Eigen::VectorXd forces_interpolated_;
+
+    // Timestep of trajectories
     double MPC_timestep_;
+
+    // Time knot associated with current delay
+    size_t step_nb_;
+
+    // Interpolation time between two knots
+    double step_progress_;
   };
 } // namespace simple_mpc
 

src/interpolator.cpp

@@ -27,25 +27,25 @@ namespace simple_mpc
     std::vector<Eigen::VectorXd> forces)
   {
     // Compute the time knot corresponding to the current delay
-    size_t step_nb = static_cast<size_t>(delay / MPC_timestep_);
-    double step_progress = (delay - (double)step_nb * MPC_timestep_) / MPC_timestep_;
+    step_nb_ = static_cast<size_t>(delay / MPC_timestep_);
+    step_progress_ = (delay - (double)step_nb_ * MPC_timestep_) / MPC_timestep_;
 
     // Interpolate state and command trajectories
-    if (step_nb >= xs.size() - 1)
+    if (step_nb_ >= xs.size() - 1)
     {
-      step_nb = xs.size() - 1;
-      step_progress = 0.0;
-      x_interpolated_ = xs[step_nb];
-      u_interpolated_ = us[step_nb];
-      a_interpolated_ = ddqs[step_nb];
-      forces_interpolated_ = forces[step_nb];
+      step_nb_ = xs.size() - 1;
+      step_progress_ = 0.0;
+      x_interpolated_ = xs[step_nb_];
+      u_interpolated_ = us[step_nb_];
+      a_interpolated_ = ddqs[step_nb_];
+      forces_interpolated_ = forces[step_nb_];
     }
     else
     {
-      x_interpolated_ = xs[step_nb + 1] * step_progress + xs[step_nb] * (1. - step_progress);
-      u_interpolated_ = us[step_nb + 1] * step_progress + us[step_nb] * (1. - step_progress);
-      a_interpolated_ = ddqs[step_nb + 1] * step_progress + ddqs[step_nb] * (1. - step_progress);
-      forces_interpolated_ = forces[step_nb + 1] * step_progress + forces[step_nb] * (1. - step_progress);
+      x_interpolated_ = xs[step_nb_ + 1] * step_progress_ + xs[step_nb_] * (1. - step_progress_);
+      u_interpolated_ = us[step_nb_ + 1] * step_progress_ + us[step_nb_] * (1. - step_progress_);
+      a_interpolated_ = ddqs[step_nb_ + 1] * step_progress_ + ddqs[step_nb_] * (1. - step_progress_);
+      forces_interpolated_ = forces[step_nb_ + 1] * step_progress_ + forces[step_nb_] * (1. - step_progress_);
     }
   }
 

tests/interpolator.cpp

@@ -36,6 +36,8 @@ BOOST_AUTO_TEST_CASE(interpolate)
   BOOST_CHECK(us[2].isApprox(interpolator.u_interpolated_));
   BOOST_CHECK(ddqs[2].isApprox(interpolator.a_interpolated_));
   BOOST_CHECK(forces[2].isApprox(interpolator.forces_interpolated_));
+  BOOST_CHECK(interpolator.step_nb_ == 2);
+  BOOST_CHECK(interpolator.step_progress_ == 0.);
 }
 
 BOOST_AUTO_TEST_SUITE_END()