Add zmp generator and populate all outputs of the unycycle trajectory…

… generator (#916)
ami-iit · Jan 23, 2025 · a2ed322 · a2ed322
1 parent 1721c3e
commit a2ed322
Show file tree

Hide file tree

Showing 7 changed files with 223 additions and 56 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -3,10 +3,14 @@ All notable changes to this project are documented in this file.
 
 ## [unreleased]
 ### Added
+- Add `ZMPgenerator` to `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916)
+
 ### Changed
 - Some improvements on the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910)
 - Removed ROS1 device publisher and the corresponding example (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910)
+
 ### Fixed
+- Fix outputs of `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916)
 - Add missing `ContinuousDynamicalSystem` row to `Exported components` in the README (https://github.com/ami-iit/bipedal-locomotion-framework/pull/919)
 - Avoid using ``iCubGazeboV3`` model in ``BaseEstimatorFromFootIMU`` test (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910)
 

diff --git a/examples/UnicycleTrajectoryGenerator/main.cpp b/examples/UnicycleTrajectoryGenerator/main.cpp
@@ -31,6 +31,7 @@ std::shared_ptr<ParametersHandler::IParametersHandler> getUnicycleParametersHand
     handler->setParameter("mergePointRatios", Eigen::Vector2d(0.4, 0.4));
     handler->setParameter("leftContactFrameName", "l_sole");
     handler->setParameter("rightContactFrameName", "r_sole");
+    handler->setParameter("use_zmp_generator", false);
 
     return handler;
 }
@@ -205,23 +206,24 @@ int main(int argc, char* argv[])
         w_H_left = leftFootPlanner.getOutput().transform;
         w_H_right = rightFootPlanner.getOutput().transform;
 
+        // get the DCM trajectory from the unicycle trajectory generator
+        const auto& dcmPosition = output.dcmTrajectory.position;
+        const auto& dcmVelocity = output.dcmTrajectory.velocity;
+
         if (saveResults)
         {
             positionLeftFoot.push_back(w_H_left.translation());
             positionRightFoot.push_back(w_H_right.translation());
             positionCOM.push_back(output.comTrajectory.position.front());
             velocityCOM.push_back(output.comTrajectory.velocity.front());
-            DCMposition.push_back(output.dcmTrajectory.position.front());
-            DCMvelocity.push_back(output.dcmTrajectory.velocity.front());
+            DCMposition.push_back(dcmPosition.front());
+            DCMvelocity.push_back(dcmVelocity.front());
 
             i++;
             time.push_back(
                 std::chrono::duration_cast<std::chrono::milliseconds>(currentTime - timeStart)
                     .count());
         }
-        // get the DCM trajectory from the unicycle trajectory generator
-        auto dcmPosition = output.dcmTrajectory.position;
-        auto dcmVelocity = output.dcmTrajectory.velocity;
 
         Eigen::VectorXd Xdcm;
         Xdcm.resize(dcmPosition.size());
@@ -239,8 +241,6 @@ int main(int argc, char* argv[])
             Ydcm(i) = dcmPosition[i][1];
         }
 
-        // log()->info("[main] DCM x: {}", Xdcm.transpose());
-        // log()->info("[main] DCM y: {}", Ydcm.transpose());
 
         BipedalLocomotion::clock().sleepUntil(currentTime + dtChrono);
 

diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h
@@ -101,12 +101,13 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final
      * Initialize the planner.
      *
      * @note The following parameters are required by the class:
-     * |           Name              |      Type      |      Default      |     Example     |                     Description                           |
-     * | :-------------------------: | :------------: | :---------------: | :-------------: | :-------------------------------------------------------: |
-     * | `planner_advance_time_in_s` |     double     |       0.08        |       -         |     The time in advance at which the planner is called    |
-     * |           `dt`              |     double     |      0.002        |       -         |     The sampling time of the trajectory generator         |
-     * | `leftContactFrameName`      |     string     |         -         |    "l_sole"     |     Name of the left foot contact frame                   |
-     * | `rightContactFrameName`     |     string     |         -         |    "r_sole"     |     Name of the right foot contact frame                  |
+     * |           Name              |      Type      |      Default      |     Example     |                     Description                         |
+     * | :-------------------------: | :------------: | :---------------: | :-------------: | :-----------------------------------------------------: |
+     * | `planner_advance_time_in_s` |     double     |       0.08        |       -         |     The time in advance at which the planner is called  |
+     * |           `dt`              |     double     |      0.002        |       -         |     The sampling time of the trajectory generator       |
+     * | `leftContactFrameName`      |     string     |         -         |    "l_sole"     |     Name of the left foot contact frame                 |
+     * | `rightContactFrameName`     |     string     |         -         |    "r_sole"     |     Name of the right foot contact frame                |
+     * | `use_zmp_generator`         |     bool       |       false       |       -         |     If true enables the zmp generator                   |
      *
      * Implicitely, the class needs also all the parameters required by the Bipedalocotion::Planner::UnicyclePlanner class.
     // clang-format on
@@ -182,6 +183,8 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final
      * @return True in case of success, false otherwise.
      */
     bool generateFirstTrajectory();
+
+    bool m_useZMPGenerator{false}; /**< True if the ZMP generator is enabled. False otherwise. */
 };
 
 #endif // BIPEDAL_LOCOMOTION_PLANNERS_UNICYCLE_TRAJECTORY_GENERATOR_H