Add getStateDerivative function

Author: edantec

bindings/expose-mpc.cpp

@@ -90,6 +90,7 @@ namespace simple_mpc
         .def("switchToStand", &MPC::switchToStand, "self"_a)
         .def("getFootTakeoffCycle", &MPC::getFootTakeoffCycle, ("self"_a, "ee_name"))
         .def("getFootLandCycle", &MPC::getFootLandCycle, ("self"_a, "ee_name"))
+        .def("getStateDerivative", &MPC::getStateDerivative, ("self"_a, "t"))
         .def("getCyclingContactState", &MPC::getCyclingContactState, ("self"_a, "t", "ee_name"))
         .def(
           "getModelHandler", &MPC::getModelHandler, "self"_a, bp::return_internal_reference<>(),

examples/go2_fulldynamics.py

@@ -221,16 +221,8 @@
     end = time.time()
     solve_time.append(end - start)
 
-    a0 = (
-        mpc.solver
-        .workspace.problem_data.stage_data[0]
-        .dynamics_data.continuous_data.xdot[nv:]
-    )
-    a1 = (
-        mpc.solver
-        .workspace.problem_data.stage_data[1]
-        .dynamics_data.continuous_data.xdot[nv:]
-    )
+    a0 = mpc.getStateDerivative(0)[nv:]
+    a1 = mpc.getStateDerivative(1)[nv:]
 
     FL_f, FR_f, RL_f, RR_f = extract_forces(mpc.getTrajOptProblem(), mpc.solver.workspace, 0)
     contact_states = mpc.ocp_handler.getContactState(0)

examples/go2_kinodynamics.py

@@ -225,16 +225,9 @@
     com_measured.append(mpc.getDataHandler().getData().com[0].copy())
     L_measured.append(mpc.getDataHandler().getData().hg.angular.copy())
 
-    a0 = (
-        mpc.solver
-        .workspace.problem_data.stage_data[0]
-        .dynamics_data.continuous_data.xdot[nv:]
-    )
-    a1 = (
-        mpc.solver
-        .workspace.problem_data.stage_data[1]
-        .dynamics_data.continuous_data.xdot[nv:]
-    )
+    a0 = mpc.getStateDerivative(0)[nv:]
+    a1 = mpc.getStateDerivative(1)[nv:]
+
     a0[6:] = mpc.us[0][nk * force_size :]
     a1[6:] = mpc.us[1][nk * force_size :]
     forces0 = mpc.us[0][: nk * force_size]

examples/talos_centroidal.py

@@ -204,11 +204,7 @@
     contact_states = mpc.ocp_handler.getContactState(0)
     foot_ref = [mpc.getReferencePose(0, name) for name in model_handler.getFeetNames()]
     foot_ref_next = [mpc.getReferencePose(1, name) for name in model_handler.getFeetNames()]
-    dH = (
-        mpc.solver
-        .workspace.problem_data.stage_data[0]
-        .dynamics_data.continuous_data.xdot[3:9]
-    )
+    dH = mpc.getStateDerivative(0)[3:9]
     qp.computeDifferences(
         mpc.getDataHandler().getData(), x_measured, foot_ref, foot_ref_next
     )

examples/talos_kinodynamics.py

@@ -210,16 +210,9 @@
     mpc.iterate(x_measured)
     end = time.time()
     print("MPC iterate = " + str(end - start))
-    a0 = (
-        mpc.solver
-        .workspace.problem_data.stage_data[0]
-        .dynamics_data.continuous_data.xdot[nv:]
-    )
-    a1 = (
-        mpc.solver
-        .workspace.problem_data.stage_data[1]
-        .dynamics_data.continuous_data.xdot[nv:]
-    )
+
+    a0 = mpc.getStateDerivative(0)[nv:]
+    a1 = mpc.getStateDerivative(1)[nv:]
     a0[6:] = mpc.us[0][nk * force_size :]
     a1[6:] = mpc.us[1][nk * force_size :]
     forces0 = mpc.us[0][: nk * force_size]

include/simple-mpc/mpc.hpp

@@ -7,6 +7,10 @@
 ///////////////////////////////////////////////////////////////////////////////
 #pragma once
 
+#include <aligator/core/stage-data.hpp>
+#include <aligator/fwd.hpp>
+#include <aligator/modelling/dynamics/fwd.hpp>
+#include <aligator/modelling/dynamics/integrator-explicit.hpp>
 #include <aligator/solvers/proxddp/solver-proxddp.hpp>
 
 #include "simple-mpc/deprecated.hpp"
@@ -17,6 +21,7 @@
 
 namespace simple_mpc
 {
+  using ExplicitIntegratorData = dynamics::ExplicitIntegratorDataTpl<double>;
 
   struct MPCSettings
   {
@@ -171,6 +176,13 @@ namespace simple_mpc
       }
     }
 
+    const Eigen::VectorXd getStateDerivative(const std::size_t t)
+    {
+      ExplicitIntegratorData * int_data =
+        dynamic_cast<ExplicitIntegratorData *>(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data);
+      return int_data->continuous_data->xdot_;
+    }
+
     void switchToWalk(const Vector6d & velocity_base);
 
     void switchToStand();

tests/mpc.cpp

@@ -88,6 +88,8 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics)
   BOOST_CHECK_EQUAL(mpc.foot_takeoff_times_.at("right_sole_link")[0], 100);
   BOOST_CHECK_EQUAL(mpc.foot_land_times_.at("left_sole_link")[0], 209);
   BOOST_CHECK_EQUAL(mpc.foot_land_times_.at("right_sole_link")[0], 150);
+
+  Eigen::VectorXd xdot = mpc.getStateDerivative(0);
 }
 
 BOOST_AUTO_TEST_CASE(mpc_kinodynamics)
@@ -161,6 +163,8 @@ BOOST_AUTO_TEST_CASE(mpc_kinodynamics)
   {
     mpc.iterate(model_handler.getReferenceState());
   }
+
+  Eigen::VectorXd xdot = mpc.getStateDerivative(0);
 }
 
 BOOST_AUTO_TEST_CASE(mpc_centroidal)
@@ -236,6 +240,8 @@ BOOST_AUTO_TEST_CASE(mpc_centroidal)
   {
     mpc.iterate(x_multibody);
   }
+
+  Eigen::VectorXd xdot = mpc.getStateDerivative(0);
 }
 
 BOOST_AUTO_TEST_SUITE_END()