Bind getAccelerations

Author: earlaud

bindings/expose-inverse-dynamics.cpp

@@ -14,11 +14,18 @@ namespace simple_mpc
       const Eigen::Ref<const Eigen::VectorXd> & q_meas,
       const Eigen::Ref<const Eigen::VectorXd> & v_meas)
     {
-      Eigen::VectorXd tau_res(v_meas.size());
+      Eigen::VectorXd tau_res(self.model_handler_.getModel().nv - 6);
       self.solve(t, q_meas, v_meas, tau_res);
       return tau_res;
     }
 
+    Eigen::VectorXd getAccelerationsProxy(KinodynamicsID & self)
+    {
+      Eigen::VectorXd a(self.model_handler_.getModel().nv - 6);
+      self.getAccelerations(a);
+      return a;
+    }
+
     void exposeInverseDynamics()
     {
       bp::class_<KinodynamicsID::Settings>("KinodynamicsIDSettings", bp::init<>(bp::args("self")))
@@ -37,7 +44,8 @@ namespace simple_mpc
         "KinodynamicsID", bp::init<const simple_mpc::RobotModelHandler &, double, const KinodynamicsID::Settings>(
                             bp::args("self", "model_handler", "control_dt", "settings")))
         .def("setTarget", &KinodynamicsID::setTarget)
-        .def("solve", &solveProxy);
+        .def("solve", &solveProxy)
+        .def("getAccelerations", &getAccelerationsProxy);
     }
   } // namespace python
 } // namespace simple_mpc

include/simple-mpc/inverse-dynamics.hpp

@@ -69,12 +69,15 @@ namespace simple_mpc
       const Eigen::Ref<const Eigen::VectorXd> & v_meas,
       Eigen::Ref<Eigen::VectorXd> tau_res);
 
-    const Eigen::VectorXd & getAccelerations();
+    void getAccelerations(Eigen::Ref<Eigen::VectorXd> a);
 
-  private:
-    // Order matters to be instanciated in the right order
+  public:
+    // Order matters to be instantiated in the right order
     const Settings settings_;
     const simple_mpc::RobotModelHandler & model_handler_;
+
+  private:
+    // Order still matter here
     simple_mpc::RobotDataHandler data_handler_;
     tsid::robots::RobotWrapper robot_;
     tsid::InverseDynamicsFormulationAccForce formulation_;

src/inverse-dynamics.cpp

@@ -156,7 +156,7 @@ void KinodynamicsID::solve(
   tau_res = formulation_.getActuatorForces(last_solution_);
 }
 
-const Eigen::VectorXd & KinodynamicsID::getAccelerations()
+void KinodynamicsID::getAccelerations(Eigen::Ref<Eigen::VectorXd> ddq)
 {
-  return formulation_.getAccelerations(last_solution_);
+  ddq = formulation_.getAccelerations(last_solution_);
 }

tests/inverse_dynamics.cpp

@@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
   {
     // Solve and get solution
     solver.solve(t, q, v, tau);
-    a = solver.getAccelerations();
+    solver.getAccelerations(a);
 
     // Integrate
     t += dt;
@@ -133,7 +133,7 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_contact)
   {
     // Solve and get solution
     solver.solve(t, q, v, tau);
-    a = solver.getAccelerations();
+    solver.getAccelerations(a);
 
     // Integrate
     t += dt;
@@ -191,7 +191,7 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_baseTask)
   {
     // Solve and get solution
     solver.solve(t, q, v, tau);
-    a = solver.getAccelerations();
+    solver.getAccelerations(a);
 
     // Integrate
     t += dt;
@@ -253,7 +253,7 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
   {
     // Solve and get solution
     solver.solve(t, q, v, tau);
-    a = solver.getAccelerations();
+    solver.getAccelerations(a);
 
     // Integrate
     t += dt;