Merge pull request #687 from ami-iit/mann_trajectory_generator_com

Expose the CoM trajectory and the angular momentum trajectory as std::vector in MANNTrajectoryGenerator
ami-iit · Jun 14, 2023 · 5a28374 · 5a28374
2 parents 128facb + 86d4357
commit 5a28374
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 18 deletions.
diff --git a/src/ML/include/BipedalLocomotion/ML/MANNTrajectoryGenerator.h b/src/ML/include/BipedalLocomotion/ML/MANNTrajectoryGenerator.h
@@ -13,8 +13,8 @@
 #include <Eigen/Dense>
 
 #include <BipedalLocomotion/Contacts/ContactPhaseList.h>
-#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/ML/MANNAutoregressive.h>
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/System/Advanceable.h>
 
 namespace BipedalLocomotion
@@ -36,14 +36,14 @@ struct MANNTrajectoryGeneratorInput : public MANNAutoregressiveInput
  */
 struct MANNTrajectoryGeneratorOutput
 {
-    Eigen::Matrix3Xd comTrajectory; /**< CoM trajectory expressed in the inertial frame. */
-    Eigen::Matrix3Xd angularMomentumTrajectory; /**< Centroidal angular momentum trajectory
-                                                   expressed in the mixed representation. */
-    std::vector<Eigen::VectorXd> jointPositions; /**< Vector cointaining the joint positions for
-                                                    each instant. The order is the one associated to
-                                                    the model provided with
-                                                    MANNTrajectoryGenerator::setRobotModel. */
-    std::vector<manif::SE3d> basePoses; /**< Vector cointaining the base pose for each instant. */
+    /** CoM trajectory expressed in the inertial frame */
+    std::vector<Eigen::Vector3d> comTrajectory;
+
+    /** Centroidal angular momentum trajectory expressed in the mixed representation. */
+    std::vector<Eigen::Vector3d> angularMomentumTrajectory;
+
+    std::vector<Eigen::VectorXd> jointPositions; /**< Joints position in radians */
+    std::vector<manif::SE3d> basePoses; /**< Vector containing the base pose for each instant. */
     Contacts::ContactPhaseList phaseList; /**< List of the contact phases */
 };
 
@@ -78,7 +78,6 @@ class MANNTrajectoryGenerator
     : public System::Advanceable<MANNTrajectoryGeneratorInput, MANNTrajectoryGeneratorOutput>
 {
 public:
-
     /**
      * Constructor
      */

diff --git a/src/ML/src/MANNTrajectoryGenerator.cpp b/src/ML/src/MANNTrajectoryGenerator.cpp
@@ -219,8 +219,8 @@ bool MANNTrajectoryGenerator::initialize(
     m_pimpl->mergePointStates.resize(m_pimpl->horizon / m_pimpl->dT);
     m_pimpl->output.basePoses.resize(m_pimpl->horizon / m_pimpl->dT);
     m_pimpl->output.jointPositions.resize(m_pimpl->horizon / m_pimpl->dT);
-    m_pimpl->output.angularMomentumTrajectory.resize(3, m_pimpl->horizon / m_pimpl->dT);
-    m_pimpl->output.comTrajectory.resize(3, m_pimpl->horizon / m_pimpl->dT);
+    m_pimpl->output.angularMomentumTrajectory.resize(m_pimpl->horizon / m_pimpl->dT);
+    m_pimpl->output.comTrajectory.resize(m_pimpl->horizon / m_pimpl->dT);
 
     return m_pimpl->mann.initialize(paramHandler);
 }
@@ -349,8 +349,8 @@ bool MANNTrajectoryGenerator::advance()
         // populate the output of the trajectory generator
         m_pimpl->output.basePoses[i] = MANNOutput.basePose;
         m_pimpl->output.jointPositions[i] = MANNOutput.jointsPosition;
-        m_pimpl->output.angularMomentumTrajectory.col(i) = MANNOutput.angularMomentum;
-        m_pimpl->output.comTrajectory.col(i) = MANNOutput.comPosition;
+        m_pimpl->output.angularMomentumTrajectory[i] = MANNOutput.angularMomentum;
+        m_pimpl->output.comTrajectory[i] = MANNOutput.comPosition;
 
         if (!m_pimpl->updateContactList("left_foot", MANNOutput.currentTime, MANNOutput.leftFoot))
         {

diff --git a/src/ML/tests/MANNTrajectoryGeneratorTest.cpp b/src/ML/tests/MANNTrajectoryGeneratorTest.cpp
@@ -129,8 +129,8 @@ TEST_CASE("MANNTrajectoryGenerator")
     REQUIRE(generator.advance());
 
     // here we check that the robot was able to walk straight
-    Eigen::Vector3d finalCoMPosition = generator.getOutput().comTrajectory.rightCols<1>();
-    Eigen::Vector3d initialCoMPosition = generator.getOutput().comTrajectory.leftCols<1>();
+    Eigen::Vector3d finalCoMPosition = generator.getOutput().comTrajectory.back();
+    Eigen::Vector3d initialCoMPosition = generator.getOutput().comTrajectory.front();
 
     constexpr double forwardTolerance = 2;
     constexpr double lateralTolerance = 0.6;
@@ -145,8 +145,8 @@ TEST_CASE("MANNTrajectoryGenerator")
     REQUIRE(generator.advance());
 
     // here we check that the robot was able to walk straight
-    finalCoMPosition = generator.getOutput().comTrajectory.rightCols<1>();
-    initialCoMPosition = generator.getOutput().comTrajectory.leftCols<1>();
+    finalCoMPosition = generator.getOutput().comTrajectory.back();
+    initialCoMPosition = generator.getOutput().comTrajectory.front();
     REQUIRE(std::abs(finalCoMPosition[0] - initialCoMPosition[0]) > forwardTolerance);
     REQUIRE(std::abs(finalCoMPosition[1] - initialCoMPosition[1]) < lateralTolerance);
     REQUIRE(std::abs(finalCoMPosition[2] - initialCoMPosition[2]) < verticalTolerance);