Get contact forces from fulldyn mpc

Author: edantec

bindings/expose-mpc.cpp

@@ -64,9 +64,12 @@ namespace simple_mpc
     void exposeMPC()
     {
       using StageVec = std::vector<std::shared_ptr<StageModel>>;
+      using ForceVec = std::vector<Eigen::Vector3d>;
       using MapBool = std::map<std::string, bool>;
       StdVectorPythonVisitor<StageVec, true>::expose(
         "StdVec_StageModel", eigenpy::details::overload_base_get_item_for_std_vector<StageVec>());
+      StdVectorPythonVisitor<ForceVec, true>::expose(
+        "StdVec_Force", eigenpy::details::overload_base_get_item_for_std_vector<ForceVec>());
 
       StdVectorPythonVisitor<std::vector<MapBool>, true>::expose("StdVec_MapBool");
 
@@ -88,6 +91,7 @@ namespace simple_mpc
         .def("getFootTakeoffCycle", &MPC::getFootTakeoffCycle, ("self"_a, "ee_name"))
         .def("getFootLandCycle", &MPC::getFootLandCycle, ("self"_a, "ee_name"))
         .def("getStateDerivative", &MPC::getStateDerivative, ("self"_a, "t"))
+        .def("getContactForces", &MPC::getContactForces, ("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

@@ -44,7 +44,7 @@
 w_basepos = [0, 0, 0, 0, 0, 0]
 w_legpos = [10, 10, 10]
 
-w_basevel = [10, 10, 10, 10, 10, 10]
+w_basevel = [0, 0, 0, 0, 0, 0]
 w_legvel = [0.1, 0.1, 0.1]
 w_x = np.array(w_basepos + w_legpos * 4 + w_basevel + w_legvel * 4)
 w_cent_lin = np.array([0.0, 0.0, 0])
@@ -201,9 +201,9 @@
 L_measured = []
 
 v = np.zeros(6)
-v[0] = 0.
+v[5] = 0.2
 mpc.velocity_base = v
-for t in range(500):
+for t in range(1000):
     print("Time " + str(t))
     land_LF = mpc.getFootLandCycle("FL_foot")
     land_RF = mpc.getFootLandCycle("RL_foot")
@@ -214,7 +214,10 @@
         str(land_RF) + ", takeoff_LF = " + str(takeoff_LF) + ", landing_LF = ",
         str(land_LF),
     ) """
-
+    if t == 400:
+        v = np.zeros(6)
+        v[0] = 0.2
+        mpc.velocity_base = v
     """ if t == 200:
         for s in range(T):
             device.resetState(mpc.xs[s][:nq])
@@ -238,13 +241,14 @@
     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)
+    forces_vec = mpc.getContactForces(0)
     contact_states = mpc.ocp_handler.getContactState(0)
-    total_forces = np.concatenate((FL_f, FR_f, RL_f, RR_f))
-    force_FL.append(FL_f)
-    force_FR.append(FR_f)
-    force_RL.append(RL_f)
-    force_RR.append(RR_f)
+    total_forces = np.concatenate((forces_vec[0], forces_vec[1], forces_vec[2], forces_vec[3]))
+
+    force_FL.append(forces_vec[0])
+    force_FR.append(forces_vec[1])
+    force_RL.append(forces_vec[2])
+    force_RR.append(forces_vec[3])
 
     forces = [total_forces, total_forces]
     ddqs = [a0, a1]

include/simple-mpc/mpc.hpp

@@ -11,6 +11,7 @@
 #include <aligator/fwd.hpp>
 #include <aligator/modelling/dynamics/fwd.hpp>
 #include <aligator/modelling/dynamics/integrator-explicit.hpp>
+#include <aligator/modelling/dynamics/multibody-constraint-fwd.hpp>
 #include <aligator/solvers/proxddp/solver-proxddp.hpp>
 
 #include "simple-mpc/deprecated.hpp"
@@ -22,6 +23,8 @@
 namespace simple_mpc
 {
   using ExplicitIntegratorData = dynamics::ExplicitIntegratorDataTpl<double>;
+  using MultibodyConstraintFwdData = dynamics::MultibodyConstraintFwdDataTpl<double>;
+  using MultibodyConstraintFwdDynamics = dynamics::MultibodyConstraintFwdDynamicsTpl<double>;
 
   struct MPCSettings
   {
@@ -183,6 +186,33 @@ namespace simple_mpc
       return int_data->continuous_data->xdot_;
     }
 
+    const std::vector<Eigen::Vector3d> getContactForces(const std::size_t t)
+    {
+      std::vector<Eigen::Vector3d> contact_forces;
+      ExplicitIntegratorData * int_data =
+        dynamic_cast<ExplicitIntegratorData *>(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data);
+      assert(int_data != nullptr);
+      MultibodyConstraintFwdData * mc_data = dynamic_cast<MultibodyConstraintFwdData *>(&*int_data->continuous_data);
+      assert(mc_data != nullptr);
+
+      std::vector<bool> contact_state = ocp_handler_->getContactState(t);
+
+      size_t force_id = 0;
+      for (size_t i = 0; i < contact_state.size(); i++)
+      {
+        if (contact_state[i])
+        {
+          contact_forces.push_back(mc_data->constraint_datas_[force_id].contact_force.linear());
+          force_id += 1;
+        }
+        else
+        {
+          contact_forces.push_back(Eigen::Vector3d::Zero());
+        }
+      }
+      return contact_forces;
+    }
+
     void switchToWalk(const Vector6d & velocity_base);
 
     void switchToStand();

tests/mpc.cpp

@@ -90,6 +90,8 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics)
   BOOST_CHECK_EQUAL(mpc.foot_land_times_.at("right_sole_link")[0], 150);
 
   Eigen::VectorXd xdot = mpc.getStateDerivative(0);
+
+  std::vector<Eigen::Vector3d> forces = mpc.getContactForces(0);
 }
 
 BOOST_AUTO_TEST_CASE(mpc_kinodynamics)