Enable the setting of the measured feet transform for the Unicycle Tr…

…ajectory Generator (#927)
ami-iit · Jan 31, 2025 · f4d03f5 · f4d03f5
1 parent f4ed878
commit f4d03f5
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 5 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,7 @@ All notable changes to this project are documented in this file.
 ### Added
 - Add `ZMPgenerator` to `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/916)
 - Add `VariableFeasibleRegionTaskVariableFeasibleRegionTask` in the TSID controller (https://github.com/ami-iit/bipedal-locomotion-framework/pull/922)
+- Add `setFeetTransform` in the `UnicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/927)
 - Add `getCurrentTime` to the `FixedFootDetector` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/928)
 
 ### Changed

diff --git a/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h b/src/Planners/include/BipedalLocomotion/Planners/UnicycleTrajectoryGenerator.h
@@ -174,6 +174,11 @@ class BipedalLocomotion::Planners::UnicycleTrajectoryGenerator final
      */
     std::chrono::nanoseconds getSamplingTime() const;
 
+    /**
+     * @brief Set the feet transforms to use when regenerating the trajectory.
+     */
+    void setFeetTransform(const manif::SE3d& leftFootTransform, const manif::SE3d& rightFootTransform);
+
 private:
     class Impl;
     std::unique_ptr<Impl> m_pImpl;

diff --git a/src/Planners/src/UnicycleTrajectoryGenerator.cpp b/src/Planners/src/UnicycleTrajectoryGenerator.cpp
@@ -99,6 +99,10 @@ class Planners::UnicycleTrajectoryGenerator::Impl
 
     BipedalLocomotion::Planners::UnicycleTrajectoryPlanner unicycleTrajectoryPlanner;
 
+    manif::SE3d measuredLeftFootTransform;
+    manif::SE3d measuredRightFootTransform;
+    bool useMeasuredFeetTransforms;
+
     /**
      * ask for a new trajectory to the unicycle trajectory planner
      */
@@ -558,12 +562,24 @@ bool Planners::UnicycleTrajectoryGenerator::advance()
 
             std::chrono::nanoseconds initTimeTrajectory
                 = m_pImpl->time + m_pImpl->newTrajectoryMergeCounter * m_pImpl->parameters.dt;
+            manif::SE3d measuredTransform;
+
+            if (m_pImpl->useMeasuredFeetTransforms)
+            {
+                measuredTransform = m_pImpl->trajectory.isLeftFootLastSwinging.front()
+                                        ? m_pImpl->measuredRightFootTransform
+                                        : m_pImpl->measuredLeftFootTransform;
+                m_pImpl->useMeasuredFeetTransforms = false; // once used, it is no more usable. User
+                                                            // should reset it again.
+            } else
+            {
 
-            manif::SE3d measuredTransform = m_pImpl->trajectory.isLeftFootLastSwinging.front()
-                                                ? m_pImpl->trajectory.rightFootTransform.at(
-                                                    m_pImpl->newTrajectoryMergeCounter)
-                                                : m_pImpl->trajectory.leftFootTransform.at(
-                                                    m_pImpl->newTrajectoryMergeCounter);
+                measuredTransform = m_pImpl->trajectory.isLeftFootLastSwinging.front()
+                                        ? m_pImpl->trajectory.rightFootTransform.at(
+                                              m_pImpl->newTrajectoryMergeCounter)
+                                        : m_pImpl->trajectory.leftFootTransform.at(
+                                              m_pImpl->newTrajectoryMergeCounter);
+            }
 
             // ask for a new trajectory (and spawn an asynchronous thread to compute it)
             {
@@ -958,3 +974,11 @@ BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::getSamplingTime() cons
 {
     return m_pImpl->parameters.dt;
 };
+
+void BipedalLocomotion::Planners::UnicycleTrajectoryGenerator::setFeetTransform(
+    const manif::SE3d& leftFootTransform, const manif::SE3d& rightFootTransform)
+{
+    m_pImpl->measuredLeftFootTransform = leftFootTransform;
+    m_pImpl->measuredRightFootTransform = rightFootTransform;
+    m_pImpl->useMeasuredFeetTransforms = true;
+};