From ee5fc11e90d1afd0d16bf0f50001a7b92012af61 Mon Sep 17 00:00:00 2001
From: gfs <gabriel.fra.santos@gmail.com>
Date: Fri, 14 Aug 2026 14:43:09 +0000
Subject: [PATCH] fix(controllers): allow stateful feedforward in
 Feedforward2Dof (#277)

Remove const from Feedforward::Evaluate(), add pure-virtual Reset() to
the interface (consistent with FeedbackLaw::Reset()), and delegate
feedforward.Reset() from Feedforward2Dof::Reset() so dynamic pre-filters
that carry internal state are correctly cleared on reset.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 doc/controllers/Feedforward2Dof.md                          | 1 +
 numerical/controllers/implementations/Feedforward2Dof.hpp   | 4 +++-
 .../implementations/test/TestFeedforward2Dof.cpp            | 6 ++++--
 3 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/doc/controllers/Feedforward2Dof.md b/doc/controllers/Feedforward2Dof.md
index e6eea376..4d6c0a8e 100644
--- a/doc/controllers/Feedforward2Dof.md
+++ b/doc/controllers/Feedforward2Dof.md
@@ -55,6 +55,7 @@ If the system were already tracking perfectly ($y = r = 0.5$), then $e = 0$, $u_
 - **Feedforward does not aid disturbance rejection.** If $u_{ff}$ is tuned for tracking and a disturbance enters the plant, only $u_{fb}$ responds.
 - **Inverse-plant feedforward for non-minimum-phase systems** is non-causal; use a truncated or approximate inverse.
 - **Sign convention.** The error passed to feedback is $r - y$, not $y - r$. Swapping signs destabilises positive-gain plants.
+- **Stateful feedforward must implement `Reset()`.** Dynamic pre-filters (e.g. reference model filters) carry internal state that must be cleared on `Reset()`. Both the feedforward and feedback components are reset when `Feedforward2Dof::Reset()` is called; any feedforward implementation that holds state must override `Reset()` to clear it.
 
 ## Variants & Generalizations
 
diff --git a/numerical/controllers/implementations/Feedforward2Dof.hpp b/numerical/controllers/implementations/Feedforward2Dof.hpp
index 1e9d7c7e..a2091251 100644
--- a/numerical/controllers/implementations/Feedforward2Dof.hpp
+++ b/numerical/controllers/implementations/Feedforward2Dof.hpp
@@ -17,7 +17,8 @@ namespace controllers
         static_assert(std::is_floating_point_v<T>, "Feedforward supports floating-point types");
 
         virtual ~Feedforward() = default;
-        virtual T Evaluate(T reference) const = 0;
+        virtual T Evaluate(T reference) = 0;
+        virtual void Reset() = 0;
     };
 
     template<typename T>
@@ -68,6 +69,7 @@ namespace controllers
     template<typename T>
     void Feedforward2Dof<T>::Reset()
     {
+        feedforward.Reset();
         feedback.Reset();
     }
 
diff --git a/numerical/controllers/implementations/test/TestFeedforward2Dof.cpp b/numerical/controllers/implementations/test/TestFeedforward2Dof.cpp
index 53d4d59a..2ba0168e 100644
--- a/numerical/controllers/implementations/test/TestFeedforward2Dof.cpp
+++ b/numerical/controllers/implementations/test/TestFeedforward2Dof.cpp
@@ -9,7 +9,8 @@ namespace
         : public controllers::Feedforward<float>
     {
     public:
-        MOCK_METHOD(float, Evaluate, (float), (const, override));
+        MOCK_METHOD(float, Evaluate, (float), (override));
+        MOCK_METHOD(void, Reset, (), (override));
     };
 
     class MockFeedbackLaw
@@ -109,8 +110,9 @@ TEST_F(TestFeedforward2Dof, zero_feedback_reduces_to_feedforward)
     EXPECT_NEAR(result, 0.4f, math::Tolerance<float>());
 }
 
-TEST_F(TestFeedforward2Dof, reset_delegates_to_feedback)
+TEST_F(TestFeedforward2Dof, reset_delegates_to_feedforward_and_feedback)
 {
+    EXPECT_CALL(ff, Reset()).Times(1);
     EXPECT_CALL(fb, Reset()).Times(1);
 
     controller.Reset();