diff --git a/README.md b/README.md index ec1a4167..fdddbfe8 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting | +| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB) | | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD | diff --git a/ROADMAP.md b/ROADMAP.md index 7e3e1aa5..5786f348 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -227,12 +227,6 @@ the library does not yet expose. Detailed below under - **Algorithm / paper:** P. Kaminski, A. Bryson, S. Schmidt, "Discrete Square Root Filtering: A Survey of Current Techniques," *IEEE Trans. AC*, 16(6), 1971. - **Reuses:** [KalmanFilterBase.hpp](numerical/filters/active/KalmanFilterBase.hpp), [Cholesky](numerical/solvers/CholeskyDecomposition.hpp), item 27. -### 40. Feedback linearization *(float-first)* -- **What:** Cancel a control-affine system's known nonlinear dynamics via a coordinate transform + inner control law, leaving an equivalent linear system that an outer loop (PD/LQR) can drive. -- **Embedded value:** One linear gain set works across the whole operating envelope of any structurally-known nonlinear plant (robot arms, quadrotors, electromechanical drives) — no gain scheduling, no lookup tables. -- **Algorithm / paper:** A. Isidori, *Nonlinear Control Systems* (1995); Slotine & Li, *Applied Nonlinear Control*. -- **Reuses:** an injected control-affine plant model, `math::Matrix`, new `nonlinear_control/` module. (The manipulator computed-torque instance lives in robotics-toolbox-cpp.) - ### 41. Backstepping controller *(float-first)* - **What:** Recursive Lyapunov-based design for strict-feedback systems, stabilizing one integrator stage at a time. - **Embedded value:** Systematic, provably-stable control for cascaded nonlinear plants (electromechanical, flight). diff --git a/doc/nonlinear_control/FeedbackLinearization.md b/doc/nonlinear_control/FeedbackLinearization.md new file mode 100644 index 00000000..be3c5a97 --- /dev/null +++ b/doc/nonlinear_control/FeedbackLinearization.md @@ -0,0 +1,106 @@ +# Feedback Linearization + +## Overview & Motivation + +Nonlinear plants such as robot arms, quadrotors, and electromechanical drives are only well-controlled by a fixed linear gain over a narrow operating range. Feedback linearization resolves this by exploiting a known model of the plant's nonlinearity to cancel it exactly in the closed loop, leaving an equivalent linear system — decoupled integrator chains — that a single outer-loop gain set can drive correctly across the full operating envelope. No gain scheduling, no lookup tables, no re-tuning when the operating point changes. + +## Mathematical Theory + +### Control-Affine Plant + +The technique applies to plants whose output $y \in \mathbb{R}^m$ satisfies, after $r$ differentiations, + +$$y^{(r)} = a(x) + B(x)\, u$$ + +where $x \in \mathbb{R}^n$ is the state, $u \in \mathbb{R}^m$ is the input, $a(x) \in \mathbb{R}^m$ is the **drift term** (known nonlinear dynamics), and $B(x) \in \mathbb{R}^{m \times m}$ is the **decoupling matrix** (state-dependent input gain). The integer $r$ is the relative degree. For mechanical systems ($r = 2$), $B(x) = M(q)$ is the inertia matrix and $a(x) = C(q, \dot{q})\dot{q} + g(q)$ is the Coriolis-plus-gravity term. + +### Inner Control Law (Cancellation) + +The inner law selects $u$ so that the term $a(x)$ is cancelled and the decoupling matrix is factored out: + +$$u = B(x)\, v + a(x)$$ + +Substituting into the plant equation yields + +$$y^{(r)} = a(x) + B(x)\bigl(B(x)\,v + a(x)\bigr) - a(x) = v$$ + +leaving pure integrator chains $y^{(r)} = v$, provided $B(x)$ is nonsingular. + +### Outer Control Law (Linear Outer Loop) + +With the plant reduced to integrators, a PD outer loop commands the virtual input: + +$$v = y_d^{(r)} + K_d\,\dot{e} + K_p\, e, \quad e = y_d - y, \quad \dot{e} = \dot{y}_d - \dot{y}$$ + +The closed-loop error satisfies the linear ODE + +$$e^{(r)} + K_d\,\dot{e} + K_p\, e = 0$$ + +whose eigenvalues are set by choosing $K_p, K_d$. Critical damping per channel requires $K_d = 2\sqrt{K_p}$. + +### Combined Law + +Expanding yields the single expression evaluated on the hot path: + +$$u = B(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e\bigr) + a(x)$$ + +No matrix inversion appears on the hot path: the law multiplies by $B(x)$, not by $B(x)^{-1}$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|--------------|--------------------|--------------|----------------------------------------| +| Construction | $O(m^2)$ | $O(m^2)$ | Copy two gain matrices | +| ComputeInput | $O(m^2)$ | $O(m)$ extra | Two matrix-vector products dominate | +| Model query | $O(m^2)$–$O(nm^2)$ | $O(m^2)$ | Implementation-defined; injected model | + +All storage is in fixed-size stack arrays; the law itself performs no heap allocation. + +## Step-by-Step Walkthrough + +Consider a 2-DOF planar arm with $m = 2$, $K_p = 100 I$, $K_d = 20 I$, and at one instant: + +- State $x = [0.1, 0.2]^\top$, $\dot{x} = [0, 0]^\top$. +- Reference $y_d = [0.5, 0.5]^\top$, $\dot{y}_d = [0, 0]^\top$, $\ddot{y}_d = [0, 0]^\top$. +- Model returns $B(x) = I$ and $a(x) = [0.3, 0.1]^\top$. + +1. Compute error: $e = [0.4, 0.3]^\top$, $\dot{e} = [0, 0]^\top$. +2. Compute virtual input: $v = 0 + 20 \cdot 0 + 100 \cdot [0.4, 0.3]^\top = [40, 30]^\top$. +3. Inner law: $u = I \cdot [40, 30]^\top + [0.3, 0.1]^\top = [40.3, 30.1]^\top$. + +The gravity-like drift $a(x)$ is added directly; the outer PD term drives position error to zero. + +## Pitfalls & Edge Cases + +- **Singular decoupling matrix**: if $B(x)$ is rank-deficient the inner law is undefined. The condition $\det B(x) \neq 0$ must hold throughout the operating region. +- **Model mismatch**: cancellation is only as exact as the model. Unmodelled dynamics or parameter error leaves a residual nonlinearity; pair with a robust or adaptive outer term to bound the error. +- **Zero dynamics**: exact linearisation of the output may leave internal states unobservable. These zero dynamics can be unstable even when the output tracks perfectly. Verify stability of the internal dynamics before deployment. +- **Actuator limits**: the inner law can command arbitrarily large $u$ near the start of a transient. Saturation on $u$ breaks the exact cancellation argument; scale $K_p$, $K_d$ or add a reference pre-filter to keep the command within actuator bounds. +- **Float precision**: for large $m$, matrix products accumulate rounding error proportional to $m \cdot \epsilon_\text{float}$. Verify the gain matrices are well-conditioned. + +## Variants & Generalizations + +- **Input-output linearization (SISO)**: for scalar output with relative degree $r > 1$, the cancellation uses Lie derivatives $L_f^r h(x)$ and $L_g L_f^{r-1} h(x)$, and the input is $u = (v - L_f^r h(x)) / L_g L_f^{r-1} h(x)$. The singularity condition $L_g L_f^{r-1} h \neq 0$ replaces $\det B \neq 0$. +- **Computed-torque control**: the mechanical specialisation with $B = M(q)$ and $a = C(q,\dot{q})\dot{q} + g(q)$. The canonical instantiation lives in robotics-toolbox-cpp. +- **Partial feedback linearization**: linearizes only the input-output channels, leaving the rest of the state dynamics (zero dynamics) uncontrolled by the outer loop. +- **Adaptive feedback linearization / MRAC**: replaces the fixed model with an online-adapted estimate, enabling cancellation under parametric uncertainty. + +## Applications + +- Robot manipulators: decoupled Cartesian impedance or position control across the full joint-space workspace. +- Quadrotor UAVs: attitude and altitude decoupling for independent channel control. +- Electromechanical drives: cancellation of back-EMF and friction in torque-controlled axes. +- Chemical process control: inversion of Hammerstein-type nonlinear input maps. + +## Connections to Other Algorithms + +- **Backstepping**: recursive alternative for strict-feedback systems; tolerates drift terms that cannot be directly cancelled. +- **Model Reference Adaptive Control (MRAC)**: adapts the model online; complements feedback linearization when the plant parameters are unknown. +- **LQR**: natural choice for the outer linear loop once the plant has been linearized. +- **Sliding Mode Control**: robustifies the outer loop against residual model mismatch by adding a discontinuous reaching term. + +## References & Further Reading + +- A. Isidori, *Nonlinear Control Systems*, 3rd ed., Springer, 1995. +- J.-J. Slotine, W. Li, *Applied Nonlinear Control*, Prentice-Hall, 1991, Chapter 6. +- H. K. Khalil, *Nonlinear Systems*, 3rd ed., Prentice-Hall, 2002, Chapter 13. diff --git a/doc/nonlinear_control/README.md b/doc/nonlinear_control/README.md new file mode 100644 index 00000000..3e55cae6 --- /dev/null +++ b/doc/nonlinear_control/README.md @@ -0,0 +1,9 @@ +# Nonlinear Control + +Algorithms for nonlinear control design: controllers that exploit a known plant model to cancel or structurally transform nonlinear dynamics. + +## Algorithms + +| Algorithm | Description | +|----------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Feedback Linearization](FeedbackLinearization.md) | Cancels a control-affine plant's known nonlinear dynamics via an inner control law, leaving decoupled integrator chains that a simple outer PD/LQR loop drives | diff --git a/numerical/CMakeLists.txt b/numerical/CMakeLists.txt index 1d4650c1..2f392cd0 100644 --- a/numerical/CMakeLists.txt +++ b/numerical/CMakeLists.txt @@ -7,5 +7,6 @@ add_subdirectory(math) add_subdirectory(neural_network) add_subdirectory(optimization) add_subdirectory(regularization) +add_subdirectory(nonlinear_control) add_subdirectory(robust_control) add_subdirectory(solvers) diff --git a/numerical/nonlinear_control/CMakeLists.txt b/numerical/nonlinear_control/CMakeLists.txt new file mode 100644 index 00000000..46403be2 --- /dev/null +++ b/numerical/nonlinear_control/CMakeLists.txt @@ -0,0 +1,21 @@ +numerical_add_header_library(numerical.nonlinear_control STATIC) + +target_include_directories(numerical.nonlinear_control ${NUMERICAL_VISIBILITY} + "$" + "$" +) + +target_link_libraries(numerical.nonlinear_control ${NUMERICAL_VISIBILITY} + numerical.math + infra.util +) + +target_sources(numerical.nonlinear_control PRIVATE + FeedbackLinearization.hpp +) + +numerical_add_coverage_sources(numerical.nonlinear_control + FeedbackLinearization.cpp +) + +add_subdirectory(test) diff --git a/numerical/nonlinear_control/FeedbackLinearization.cpp b/numerical/nonlinear_control/FeedbackLinearization.cpp new file mode 100644 index 00000000..a3b97631 --- /dev/null +++ b/numerical/nonlinear_control/FeedbackLinearization.cpp @@ -0,0 +1,9 @@ +// Copyright (c) 2024 Numerical Toolbox Contributors +// SPDX-License-Identifier: MIT + +#include "numerical/nonlinear_control/FeedbackLinearization.hpp" + +namespace nonlinear_control +{ + template class FeedbackLinearization; +} diff --git a/numerical/nonlinear_control/FeedbackLinearization.hpp b/numerical/nonlinear_control/FeedbackLinearization.hpp new file mode 100644 index 00000000..3fc62012 --- /dev/null +++ b/numerical/nonlinear_control/FeedbackLinearization.hpp @@ -0,0 +1,75 @@ +#pragma once + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC optimize("O3", "fast-math") +#endif + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace nonlinear_control +{ + template + class ControlAffineModel + { + static_assert(std::is_floating_point_v, "ControlAffineModel supports floating-point types"); + static_assert(Dim > 0, "ControlAffineModel requires Dim > 0"); + + public: + using StateVector = math::Vector; + using DecouplingMatrix = math::SquareMatrix; + + virtual ~ControlAffineModel() = default; + + [[nodiscard]] virtual DecouplingMatrix DecouplingMatrixAt(const StateVector& x) const = 0; + [[nodiscard]] virtual StateVector DriftTerm(const StateVector& x) const = 0; + }; + + template + class FeedbackLinearization + { + static_assert(std::is_floating_point_v, "FeedbackLinearization supports floating-point types"); + static_assert(Dim > 0, "FeedbackLinearization requires Dim > 0"); + + public: + using StateVector = math::Vector; + using GainMatrix = math::SquareMatrix; + + FeedbackLinearization(const ControlAffineModel& model, const GainMatrix& kp, const GainMatrix& kd); + + OPTIMIZE_FOR_SPEED StateVector ComputeInput(const StateVector& x, const StateVector& xDot, + const StateVector& yd, const StateVector& ydDot, const StateVector& ydDdot); + + private: + const ControlAffineModel& model; + GainMatrix kp; + GainMatrix kd; + }; + + template + FeedbackLinearization::FeedbackLinearization( + const ControlAffineModel& model, const GainMatrix& kp, const GainMatrix& kd) + : model{ model } + , kp{ kp } + , kd{ kd } + {} + + template + OPTIMIZE_FOR_SPEED typename FeedbackLinearization::StateVector + FeedbackLinearization::ComputeInput(const StateVector& x, const StateVector& xDot, + const StateVector& yd, const StateVector& ydDot, const StateVector& ydDdot) + { + const StateVector e{ yd - x }; + const StateVector eDot{ ydDot - xDot }; + const StateVector v{ ydDdot + kd * eDot + kp * e }; + const auto B{ model.DecouplingMatrixAt(x) }; + const StateVector a{ model.DriftTerm(x) }; + return B * v + a; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class FeedbackLinearization; +#endif +} diff --git a/numerical/nonlinear_control/test/CMakeLists.txt b/numerical/nonlinear_control/test/CMakeLists.txt new file mode 100644 index 00000000..c3530222 --- /dev/null +++ b/numerical/nonlinear_control/test/CMakeLists.txt @@ -0,0 +1,12 @@ +add_executable(numerical.nonlinear_control_test) +emil_build_for(numerical.nonlinear_control_test BOOL NUMERICAL_TOOLBOX_BUILD_TESTS) +emil_add_test(numerical.nonlinear_control_test) + +target_link_libraries(numerical.nonlinear_control_test PUBLIC + gmock_main + numerical.nonlinear_control +) + +target_sources(numerical.nonlinear_control_test PRIVATE + TestFeedbackLinearization.cpp +) diff --git a/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp b/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp new file mode 100644 index 00000000..d0a323be --- /dev/null +++ b/numerical/nonlinear_control/test/TestFeedbackLinearization.cpp @@ -0,0 +1,281 @@ +#include "numerical/math/Matrix.hpp" +#include "numerical/math/Tolerance.hpp" +#include "numerical/nonlinear_control/FeedbackLinearization.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include + +namespace +{ + template + class MockControlAffineModel + : public nonlinear_control::ControlAffineModel + { + public: + using StateVector = math::Vector; + using DecouplingMatrix = math::SquareMatrix; + + MOCK_METHOD(DecouplingMatrix, DecouplingMatrixAt, (const StateVector& x), (const, override)); + MOCK_METHOD(StateVector, DriftTerm, (const StateVector& x), (const, override)); + }; + + template + class ManipulatorPlant + : public nonlinear_control::ControlAffineModel + { + public: + using StateVector = math::Vector; + using DecouplingMatrix = math::SquareMatrix; + + [[nodiscard]] DecouplingMatrix DecouplingMatrixAt(const StateVector& q) const override + { + return InertiaMatrix(q); + } + + [[nodiscard]] StateVector DriftTerm(const StateVector& q) const override + { + return GravityTerm(q); + } + + void Step(const StateVector& u, T dt) + { + const StateVector qDdot{ SolveSpd(InertiaMatrix(q), u - GravityTerm(q)) }; + qDot = qDot + qDdot * dt; + q = q + qDot * dt; + } + + [[nodiscard]] StateVector Position() const + { + return q; + } + + [[nodiscard]] StateVector Velocity() const + { + return qDot; + } + + private: + static DecouplingMatrix InertiaMatrix(const StateVector& q) + { + const T c{ std::cos(q.at(1, 0)) }; + return DecouplingMatrix{ static_cast(2) + c, static_cast(0.5), + static_cast(0.5), static_cast(1) }; + } + + static StateVector GravityTerm(const StateVector& q) + { + return StateVector{ { std::sin(q.at(0, 0)) }, { std::sin(q.at(1, 0)) } }; + } + + static StateVector SolveSpd(const DecouplingMatrix& m, const StateVector& b) + { + const T det{ m.at(0, 0) * m.at(1, 1) - m.at(0, 1) * m.at(1, 0) }; + return StateVector{ + { (m.at(1, 1) * b.at(0, 0) - m.at(0, 1) * b.at(1, 0)) / det }, + { (m.at(0, 0) * b.at(1, 0) - m.at(1, 0) * b.at(0, 0)) / det } + }; + } + + StateVector q{ { T{} }, { T{} } }; + StateVector qDot{ { T{} }, { T{} } }; + }; + + class TestFeedbackLinearization + : public ::testing::Test + { + protected: + ::testing::StrictMock> model; + + math::SquareMatrix kp{ 100.0f, 0.0f, 0.0f, 100.0f }; + math::SquareMatrix kd{ 20.0f, 0.0f, 0.0f, 20.0f }; + + nonlinear_control::FeedbackLinearization controller{ model, kp, kd }; + + math::Vector zero{ { 0.0f }, { 0.0f } }; + }; +} + +TEST_F(TestFeedbackLinearization, cancels_to_integrator_chain) +{ + const math::SquareMatrix identity{ math::SquareMatrix::Identity() }; + const math::Vector a{ { 0.0f }, { 0.0f } }; + const math::Vector c{ { 3.0f }, { -2.0f } }; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(identity)); + EXPECT_CALL(model, DriftTerm(::testing::_)).WillOnce(::testing::Return(a)); + + const auto u{ controller.ComputeInput(zero, zero, zero, zero, c) }; + + EXPECT_NEAR(u.at(0, 0), c.at(0, 0), math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), c.at(1, 0), math::Tolerance()); +} + +TEST_F(TestFeedbackLinearization, adds_drift_compensation) +{ + const math::SquareMatrix identity{ math::SquareMatrix::Identity() }; + const math::Vector drift{ { 0.0f }, { 5.0f } }; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(identity)); + EXPECT_CALL(model, DriftTerm(::testing::_)).WillOnce(::testing::Return(drift)); + + const auto u{ controller.ComputeInput(zero, zero, zero, zero, zero) }; + + EXPECT_NEAR(u.at(0, 0), drift.at(0, 0), math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), drift.at(1, 0), math::Tolerance()); +} + +TEST_F(TestFeedbackLinearization, pd_law_drives_position_error) +{ + const math::SquareMatrix identity{ math::SquareMatrix::Identity() }; + const math::Vector a{ { 0.0f }, { 0.0f } }; + const math::Vector x{ { 1.0f }, { 2.0f } }; + const math::Vector yd{ { 3.0f }, { 5.0f } }; + const math::Vector e{ yd - x }; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(identity)); + EXPECT_CALL(model, DriftTerm(::testing::_)).WillOnce(::testing::Return(a)); + + const auto u{ controller.ComputeInput(x, zero, yd, zero, zero) }; + const auto expected{ kp * e }; + + EXPECT_NEAR(u.at(0, 0), expected.at(0, 0), math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), expected.at(1, 0), math::Tolerance()); +} + +TEST_F(TestFeedbackLinearization, pd_law_drives_velocity_error) +{ + const math::SquareMatrix identity{ math::SquareMatrix::Identity() }; + const math::Vector a{ { 0.0f }, { 0.0f } }; + const math::Vector xDot{ { 0.5f }, { -1.0f } }; + const math::Vector ydDot{ { 1.5f }, { 2.0f } }; + const math::Vector eDot{ ydDot - xDot }; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(identity)); + EXPECT_CALL(model, DriftTerm(::testing::_)).WillOnce(::testing::Return(a)); + + const auto u{ controller.ComputeInput(zero, xDot, zero, ydDot, zero) }; + const auto expected{ kd * eDot }; + + EXPECT_NEAR(u.at(0, 0), expected.at(0, 0), math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), expected.at(1, 0), math::Tolerance()); +} + +TEST_F(TestFeedbackLinearization, decoupling_matrix_scales_virtual_input) +{ + const math::SquareMatrix B{ 2.0f, 0.0f, 0.0f, 3.0f }; + const math::Vector a{ { 0.0f }, { 0.0f } }; + const math::Vector v{ { 1.0f }, { 1.0f } }; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(B)); + EXPECT_CALL(model, DriftTerm(::testing::_)).WillOnce(::testing::Return(a)); + + const auto u{ controller.ComputeInput(zero, zero, zero, zero, v) }; + + EXPECT_NEAR(u.at(0, 0), 2.0f, math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), 3.0f, math::Tolerance()); +} + +TEST_F(TestFeedbackLinearization, closed_loop_error_decays) +{ + const math::SquareMatrix identity{ math::SquareMatrix::Identity() }; + const math::Vector a{ { 0.0f }, { 0.0f } }; + const math::Vector yd{ { 1.0f }, { 0.5f } }; + math::Vector x{ { 0.0f }, { 0.0f } }; + math::Vector xDot{ { 0.0f }, { 0.0f } }; + const float dt{ 0.01f }; + const int steps{ 300 }; + + const math::Vector initError{ yd - x }; + const float prevErrorNorm{ initError.at(0, 0) * initError.at(0, 0) + initError.at(1, 0) * initError.at(1, 0) }; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).Times(steps).WillRepeatedly(::testing::Return(identity)); + EXPECT_CALL(model, DriftTerm(::testing::_)).Times(steps).WillRepeatedly(::testing::Return(a)); + + for (int i = 0; i < steps; ++i) + { + const auto u{ controller.ComputeInput(x, xDot, yd, zero, zero) }; + xDot = xDot + u * dt; + x = x + xDot * dt; + } + + const math::Vector finalError{ yd - x }; + const float finalNorm{ finalError.at(0, 0) * finalError.at(0, 0) + finalError.at(1, 0) * finalError.at(1, 0) }; + EXPECT_LT(finalNorm, prevErrorNorm); +} + +TEST_F(TestFeedbackLinearization, reference_feedforward_used) +{ + const math::SquareMatrix identity{ math::SquareMatrix::Identity() }; + const math::Vector a{ { 0.0f }, { 0.0f } }; + const math::Vector c{ { 7.0f }, { -4.0f } }; + + EXPECT_CALL(model, DecouplingMatrixAt(::testing::_)).WillOnce(::testing::Return(identity)); + EXPECT_CALL(model, DriftTerm(::testing::_)).WillOnce(::testing::Return(a)); + + const auto u{ controller.ComputeInput(zero, zero, zero, zero, c) }; + + EXPECT_NEAR(u.at(0, 0), c.at(0, 0), math::Tolerance()); + EXPECT_NEAR(u.at(1, 0), c.at(1, 0), math::Tolerance()); +} + +namespace +{ + class TestFeedbackLinearizationPlant + : public ::testing::Test + { + protected: + ManipulatorPlant plant; + + math::SquareMatrix kp{ 100.0f, 0.0f, 0.0f, 100.0f }; + math::SquareMatrix kd{ 20.0f, 0.0f, 0.0f, 20.0f }; + + nonlinear_control::FeedbackLinearization controller{ plant, kp, kd }; + + math::Vector zero{ { 0.0f }, { 0.0f } }; + }; +} + +TEST_F(TestFeedbackLinearizationPlant, regulates_to_setpoint_despite_nonlinear_dynamics) +{ + const math::Vector yd{ { 1.0f }, { -0.5f } }; + const float dt{ 0.001f }; + const int steps{ 2000 }; + + for (int i = 0; i < steps; ++i) + { + const auto u{ controller.ComputeInput(plant.Position(), plant.Velocity(), yd, zero, zero) }; + plant.Step(u, dt); + } + + EXPECT_NEAR(plant.Position().at(0, 0), yd.at(0, 0), 1.0e-2f); + EXPECT_NEAR(plant.Position().at(1, 0), yd.at(1, 0), 1.0e-2f); + EXPECT_NEAR(plant.Velocity().at(0, 0), 0.0f, 1.0e-2f); + EXPECT_NEAR(plant.Velocity().at(1, 0), 0.0f, 1.0e-2f); +} + +TEST_F(TestFeedbackLinearizationPlant, tracks_sinusoidal_trajectory_with_feedforward) +{ + const float amplitude{ 0.3f }; + const float omega{ 3.0f }; + const float dt{ 0.001f }; + const int steps{ 3000 }; + + for (int i = 0; i < steps; ++i) + { + const float t{ static_cast(i) * dt }; + const float s{ std::sin(omega * t) }; + const float cc{ std::cos(omega * t) }; + const math::Vector yd{ { amplitude * s }, { amplitude * s } }; + const math::Vector ydDot{ { amplitude * omega * cc }, { amplitude * omega * cc } }; + const math::Vector ydDdot{ { -amplitude * omega * omega * s }, { -amplitude * omega * omega * s } }; + + const auto u{ controller.ComputeInput(plant.Position(), plant.Velocity(), yd, ydDot, ydDdot) }; + plant.Step(u, dt); + } + + const float tFinal{ static_cast(steps) * dt }; + const float ydFinal{ amplitude * std::sin(omega * tFinal) }; + + EXPECT_NEAR(plant.Position().at(0, 0), ydFinal, 1.0e-2f); + EXPECT_NEAR(plant.Position().at(1, 0), ydFinal, 1.0e-2f); +} diff --git a/roadmap/nonlinear_control/FeedbackLinearization/explanation.md b/roadmap/nonlinear_control/FeedbackLinearization/explanation.md deleted file mode 100644 index bb501eaa..00000000 --- a/roadmap/nonlinear_control/FeedbackLinearization/explanation.md +++ /dev/null @@ -1,41 +0,0 @@ -# Feedback Linearization — Overview - -## What it is -A control technique that **cancels** a control-affine plant's known nonlinear dynamics with an inner -control law, leaving an equivalent linear system that a simple outer loop (PD, LQR) can drive. For a -system whose input enters through a state-dependent decoupling matrix, the cancelling law is -`u = B(x)·v + a(x)`, which turns the plant into decoupled integrator chains `ÿ = v`. The mechanical -*computed-torque* method `τ = M(q)·v + C(q,q̇)q̇ + g(q)` is the canonical instance (`B = M`, -`a = Cq̇ + g`). - -## Why it matters (embedded) -Robot arms, quadrotors, and other structurally-known machines are strongly nonlinear — a fixed PID -tuned at one operating point misbehaves at another. Feedback linearization uses the *model you -already have* to erase that nonlinearity, so one linear gain set works across the whole operating -envelope. No gain scheduling, no lookup tables. - -## How it works (intuition) -Split the controller in two. The **inner** law evaluates the injected model at the current state and -injects exactly the input needed to cancel the drift `a(x)`. What remains behaves like plain -integrator chains. The **outer** law then commands a virtual input `v = y_d^{(r)} + Kd·ė + Kp·e` -as if controlling those trivial integrators. In the mechanical (input-state) case the cancellation -multiplies by `B(x) = M(q)` — never inverts it — so the hot path stays well-conditioned. - -## Key parameters -- **nonlinear model** — an injected control-affine model supplying the decoupling matrix `B(x)` and - the drift term `a(x)` to be cancelled. (The manipulator instance — `M(q)`, `C(q,q̇)q̇`, `g(q)` — - lives in robotics-toolbox-cpp.) -- **Kp, Kd** — outer-loop gains for the linearized integrator chain; pick `Kd = 2√Kp` for - critical damping. -- **relative degree** (general form) — how many times to differentiate the output before the input - appears; sets the structure of the cancellation. - -## Reference -A. Isidori, *Nonlinear Control Systems*, 3rd ed. (1995); J.-J. Slotine, W. Li, -*Applied Nonlinear Control* (1991), computed-torque chapter. - -## See also -`BacksteppingControl` (recursive alternative that tolerates non-cancellable terms); -`ModelReferenceAdaptiveControl` (adapts the model online when parameters are unknown); -`Lqr` (a natural outer loop). The manipulator computed-torque instantiation is -`controllers/manipulator/ComputedTorqueControl` in robotics-toolbox-cpp. diff --git a/roadmap/nonlinear_control/FeedbackLinearization/implementation.md b/roadmap/nonlinear_control/FeedbackLinearization/implementation.md deleted file mode 100644 index b5f776f2..00000000 --- a/roadmap/nonlinear_control/FeedbackLinearization/implementation.md +++ /dev/null @@ -1,88 +0,0 @@ -# Feedback Linearization — Implementation Pseudocode - -> Roadmap ref: #40 (Tier 4) · Target: `numerical/nonlinear_control` · Namespace `nonlinear_control` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -# Injected abstract model of a control-affine plant y^(r) = a(x) + B(x)·u : -template -class ControlAffineModel: # pure-virtual interface (DIP) - virtual ~ControlAffineModel() = default - virtual SquareMatrix DecouplingMatrix(const StateVector& x) const # B(x) - virtual Vector DriftTerm(const StateVector& x) const # a(x) to cancel - -template # static_assert(std::is_floating_point_v); instantiated for float -class FeedbackLinearization: - const ControlAffineModel& model # supplies B(x), a(x) - math::SquareMatrix Kp # outer proportional gain - math::SquareMatrix Kd # outer derivative gain -``` - -The mechanical *computed-torque* instance sets `B(x) = M(q)` and `a(x) = C(q,q̇)q̇ + g(q)`; that -manipulator model lives in robotics-toolbox-cpp and is injected here through `ControlAffineModel`. - -## Interface - -``` -# Nonlinear model injected (DIP); outer linear gains chosen for the integrator chain: -FeedbackLinearization(const ControlAffineModel& model, - const SquareMatrix& Kp, const SquareMatrix& Kd) - -Vector ComputeInput(const StateVector& x, const StateVector& xDot, - const StateVector& yd, const StateVector& ydDot, - const StateVector& ydDdot) # hot path -``` - -## Algorithm (pseudocode) - -``` -function ComputeInput(x, xDot, yd, ydDot, ydDdot): # OPTIMIZE_FOR_SPEED - # --- outer loop: linear control on the linearized plant ÿ = v --- - e = yd - x - eDot = ydDot - xDot - v = ydDdot + Kd * eDot + Kp * e # virtual input command - - # --- inner loop: cancel the known drift (input-state form) --- - # u = B(x)·v + a(x) ⇒ ÿ = v exactly - B = model.DecouplingMatrix(x) - a = model.DriftTerm(x) - return B * v + a - -# General SISO input-output form (relative degree r), for non-mechanical plants: -# y^(r) = L_f^r h(x) + L_g L_f^(r-1) h(x) · u -# u = ( v - L_f^r h(x) ) / ( L_g L_f^(r-1) h(x) ) # requires L_g L_f^(r-1) h ≠ 0 -``` - -## Complexity & memory - -- `ComputeInput`: `O(Dim²)` for `B·v`; model evaluation is `O(Dim)`–`O(Dim²)`. -- Memory: `O(Dim²)` for the two gains; no dynamic state — all static, no heap. - -## Numerical / embedded notes - -- The input-state form **multiplies** by `B(x)` — it never inverts it, so no ill-conditioned solve - on the hot path (unlike forward dynamics). For mechanical plants `B(x) = M(q)` is SPD. -- Cancellation is only as good as the model: parameter mismatch leaves a residual nonlinearity — - pair with a robust (sliding-mode) or adaptive (MRAC) outer term to mop up the error. -- The general input-output form loses well-posedness where `L_g L_f^(r-1) h → 0` (a singularity); - keep the operating region away from it, and watch for unstable internal dynamics (zero dynamics). -- Choose `Kp`, `Kd` for a critically-damped integrator chain (`Kd = 2√Kp`) per channel. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/nonlinear_control/FeedbackLinearization.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `ComputeInput`, and - `extern template class FeedbackLinearization;` - under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/nonlinear_control/FeedbackLinearization.cpp` → - `template class FeedbackLinearization;` -- Test: `numerical/nonlinear_control/test/TestFeedbackLinearization.cpp` -- Doc: `doc/nonlinear_control/FeedbackLinearization.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestFeedbackLinearization.cpp` → the `_test` target. -- New module: create `numerical/nonlinear_control/CMakeLists.txt` via `numerical_add_header_library(...)`, - add `test/`, register in `numerical/CMakeLists.txt`, add `doc/nonlinear_control/`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/nonlinear_control/FeedbackLinearization/tests.md b/roadmap/nonlinear_control/FeedbackLinearization/tests.md deleted file mode 100644 index 7b434948..00000000 --- a/roadmap/nonlinear_control/FeedbackLinearization/tests.md +++ /dev/null @@ -1,61 +0,0 @@ -# Feedback Linearization — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestFeedbackLinearization : public ::testing::Test: - # Injected control-affine model is mocked so the law is verified in isolation: - StrictMock> model - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - FeedbackLinearization controller{ model, Kp, Kd } -# each case below is a TEST_F(TestFeedbackLinearization, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -cancels_to_integrator_chain: - Arrange: model returns B=I, a=0; error and error-rate = 0, ydDdot = c - Act: u = ComputeInput(...) - Assert: u == c (u = B·v reduces to the commanded virtual input) - -adds_drift_compensation: - Arrange: model a(x) = [0, d]; all setpoints match state, v = 0 - Assert: u == a(x) (pure drift-cancelling input) - -pd_law_drives_position_error: - Arrange: e = yd - x != 0, eDot = 0, feedforward = 0, B=I - Assert: u == Kp · e (proportional term appears through B·v) - -pd_law_drives_velocity_error: - Arrange: eDot != 0, e = 0, B=I - Assert: u == Kd · eDot - -decoupling_matrix_scales_virtual_input: - Arrange: B = diag(2,3), v = [1,1] - Assert: u == [2,3] (B·v applied, not v alone) - -closed_loop_error_decays: - Arrange: wrap a plant that integrates ÿ = B⁻¹(u − a); run K steps - Assert: ||yd − x|| -> 0 monotonically (exact linearization) - -reference_feedforward_used: - Arrange: ydDdot = c, all errors 0, B=I - Assert: u == c (feedforward passes through) -``` - -## Reference vectors - -- Ideal cancellation: with the exact model, closed-loop error obeys `ë + Kd·ė + Kp·e = 0` — a - linear ODE whose decay rate is hand-computable from `Kp`, `Kd`. -- Computed-torque instance (single pendulum): with `B = M`, `a = g(q)`, the gravity-hold input at - zero acceleration is the golden `τ = m g L sin(q)`. - -## Edge cases - -- Model mismatch (mock `B` scaled by 1.2): closed loop stays stable but shows bounded steady error. -- `ydDdot` large: input must not saturate the (documented) actuator model in tests. -- Near-singular `B` / decoupling matrix in the general input-output form: division guard exercised.