From f4d03f581072a1287ed3d769a5c1fb119d12007d Mon Sep 17 00:00:00 2001
From: Lorenzo Moretti <107630048+LoreMoretti@users.noreply.github.com>
Date: Fri, 31 Jan 2025 10:15:26 +0100
Subject: [PATCH] Enable the setting of the measured feet transform for the
 Unicycle Trajectory Generator (#927)

---
 CHANGELOG.md                                  |  1 +
 .../Planners/UnicycleTrajectoryGenerator.h    |  5 +++
 .../src/UnicycleTrajectoryGenerator.cpp       | 34 ++++++++++++++++---
 3 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7e6ddf187a..7b8dbfd179 100644
--- 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
index 3f67fefc20..5e4c0f14fb 100644
--- 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
index 3f66746f25..440e25cb3f 100644
--- 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;
+};