Export the CoM velocity and the angular momentum trajectory in the Ce…

…ntroidalMPC
ami-iit · Mar 11, 2024 · 0d62fcc · 0d62fcc
1 parent bb058aa
commit 0d62fcc
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 1 deletion.
diff --git a/bindings/python/ReducedModelControllers/src/CentroidalMPC.cpp b/bindings/python/ReducedModelControllers/src/CentroidalMPC.cpp
@@ -30,7 +30,10 @@ void CreateCentroidalMPC(pybind11::module& module)
         .def(py::init())
         .def_readwrite("contacts", &CentroidalMPCOutput::contacts)
         .def_readwrite("contact_phase_list", &CentroidalMPCOutput::contactPhaseList)
-        .def_readwrite("com_trajectory", &CentroidalMPCOutput::comTrajectory);
+        .def_readwrite("com_trajectory", &CentroidalMPCOutput::comTrajectory)
+        .def_readwrite("com_velocity_trajectory", &CentroidalMPCOutput::comVelocityTrajectory)
+        .def_readwrite("angular_momentum_trajectory", &CentroidalMPCOutput::angularMomentumTrajectory);
+
 
     BipedalLocomotion::bindings::System::CreateSource<CentroidalMPCOutput>(module,
                                                                            "CentroidalMPCOutput");

diff --git a/...ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h b/...ReducedModelControllers/include/BipedalLocomotion/ReducedModelControllers/CentroidalMPC.h
@@ -35,6 +35,10 @@ struct CentroidalMPCOutput
                                                     CentroidalMPC. */
     std::vector<Eigen::Vector3d> comTrajectory; /**< Desired CoM trajectory generated by the
                                                     CentroidalMPC. */
+    std::vector<Eigen::Vector3d> comVelocityTrajectory; /**< Desired CoM velocity trajectory
+                                                           generated by the CentroidalMPC. */
+    std::vector<Eigen::Vector3d> angularMomentumTrajectory; /**< Desired angular momentum trajectory
+                                                               generated by the CentroidalMPC. */
 };
 
 /**

diff --git a/src/ReducedModelControllers/src/CentroidalMPC.cpp b/src/ReducedModelControllers/src/CentroidalMPC.cpp
@@ -556,6 +556,8 @@ struct CentroidalMPC::Impl
 
         // resize the CoM Trajectory
         this->output.comTrajectory.resize(stateHorizon);
+        this->output.comVelocityTrajectory.resize(stateHorizon);
+        this->output.angularMomentumTrajectory.resize(stateHorizon);
 
         // In case of no warmstart the variables are:
         // - centroidalVariables = 7: external force + external torque + com current + dcom current
@@ -1071,6 +1073,8 @@ struct CentroidalMPC::Impl
         }
 
         concatenateOutput(this->optiVariables.com, "com");
+        concatenateOutput(this->optiVariables.dcom, "dcom");
+        concatenateOutput(this->optiVariables.angularMomentum, "angular_momentum");
 
         casadi::Dict toFunctionOptions, jitOptions;
         if (casadiVersionIsAtLeast360())
@@ -1263,6 +1267,21 @@ bool CentroidalMPC::advance()
         using namespace BipedalLocomotion::Conversions;
         m_pimpl->output.comTrajectory[i] = toEigen(*it).col(i);
     }
+
+    std::advance(it, 1);
+    for (int i = 0; i < m_pimpl->output.comVelocityTrajectory.size(); i++)
+    {
+        using namespace BipedalLocomotion::Conversions;
+        m_pimpl->output.comVelocityTrajectory[i] = toEigen(*it).col(i);
+    }
+
+    std::advance(it, 1);
+    for (int i = 0; i < m_pimpl->output.angularMomentumTrajectory.size(); i++)
+    {
+        using namespace BipedalLocomotion::Conversions;
+        m_pimpl->output.angularMomentumTrajectory[i] = toEigen(*it).col(i);
+    }
+
     // advance the time
     m_pimpl->currentTime += m_pimpl->optiSettings.samplingTime;