From d541d5ae7cd79ae0c41df67f316d9868eb9f4c45 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Wed, 21 Jul 2021 16:01:35 +0200
Subject: [PATCH] Add the posibility to get the internal state of the
 controller

---
 .../LieGroupControllers/impl/ControllerBase.h | 36 +++++++++++++++++
 .../ProportionalController/ControllerBase.h   | 39 +++++++++++++++++++
 .../ControllerBase.h                          | 39 +++++++++++++++++++
 3 files changed, 114 insertions(+)

diff --git a/include/LieGroupControllers/impl/ControllerBase.h b/include/LieGroupControllers/impl/ControllerBase.h
index 734e92a..fb82c10 100644
--- a/include/LieGroupControllers/impl/ControllerBase.h
+++ b/include/LieGroupControllers/impl/ControllerBase.h
@@ -80,6 +80,24 @@ template <class _Derived> class ControllerBase
      * express the vector in a different tangent space.
      */
     const Vector& getControl() const;
+
+    /**
+     * Get the state of the system.
+     * @return the state of the system.
+     */
+    const State& getState() const;
+
+    /**
+     * Get the desired state of the system.
+     * @return the state of the system.
+     */
+    const State& getDesiredState() const;
+
+    /**
+     * Get the feedforward term.
+     * @return the controller feedforward.
+     */
+    const Vector& getFeedForward() const;
 };
 
 template <class _Derived>
@@ -118,6 +136,24 @@ const typename ControllerBase<_Derived>::Vector& ControllerBase<_Derived>::getCo
     return this->derived().getControl();
 }
 
+template <class _Derived>
+const typename ControllerBase<_Derived>::State& ControllerBase<_Derived>::getState() const
+{
+    return this->derived().getState();
+}
+
+template <class _Derived>
+const typename ControllerBase<_Derived>::State& ControllerBase<_Derived>::getDesiredState() const
+{
+    return this->derived().getDesiredState();
+}
+
+template <class _Derived>
+const typename ControllerBase<_Derived>::Vector& ControllerBase<_Derived>::getFeedForward() const
+{
+    return this->derived().getFeedForward();
+}
+
 } // namespace LieGroupControllers
 
 #endif // LIE_GROUP_CONTROLLERS_IMPL_CONTROLLER_BASE_H
diff --git a/include/LieGroupControllers/impl/ProportionalController/ControllerBase.h b/include/LieGroupControllers/impl/ProportionalController/ControllerBase.h
index a008448..448efe7 100644
--- a/include/LieGroupControllers/impl/ProportionalController/ControllerBase.h
+++ b/include/LieGroupControllers/impl/ProportionalController/ControllerBase.h
@@ -70,6 +70,24 @@ template <typename _Derived> class ProportionalControllerBase : public Controlle
      * express the vector in a different tangent space.
      */
     const Vector& getControl() const;
+
+    /**
+     * Get the state of the system.
+     * @return the state of the system.
+     */
+    const State& getState() const;
+
+    /**
+     * Get the desired state of the system.
+     * @return the state of the system.
+     */
+    const State& getDesiredState() const;
+
+    /**
+     * Get the feedforward term.
+     * @return the controller feedforward.
+     */
+    const Vector& getFeedForward() const;
 };
 
 template <typename _Derived> bool ProportionalControllerBase<_Derived>::setState(const State& state)
@@ -114,6 +132,27 @@ ProportionalControllerBase<_Derived>::getControl() const
     return m_controlOutput;
 }
 
+template <class _Derived>
+const typename ProportionalControllerBase<_Derived>::State&
+ProportionalControllerBase<_Derived>::getState() const
+{
+    return m_state;
+}
+
+template <class _Derived>
+const typename ProportionalControllerBase<_Derived>::State&
+ProportionalControllerBase<_Derived>::getDesiredState() const
+{
+    return m_desiredState;
+}
+
+template <class _Derived>
+const typename ProportionalControllerBase<_Derived>::Vector&
+ProportionalControllerBase<_Derived>::getFeedForward() const
+{
+    return m_feedForward;
+}
+
 } // namespace LieGroupControllers
 
 #endif // LIE_GROUP_CONTROLLERS_IMPL_PROPORTIONAL_CONTROLLER_CONTROLLER_BASE_H
diff --git a/include/LieGroupControllers/impl/ProportionalDerivativeController/ControllerBase.h b/include/LieGroupControllers/impl/ProportionalDerivativeController/ControllerBase.h
index c5f1f3e..2c5314e 100644
--- a/include/LieGroupControllers/impl/ProportionalDerivativeController/ControllerBase.h
+++ b/include/LieGroupControllers/impl/ProportionalDerivativeController/ControllerBase.h
@@ -74,6 +74,24 @@ class ProportionalDerivativeControllerBase : public ControllerBase<_Derived>
      * express the vector in a different tangent space.
      */
     const Vector& getControl() const;
+
+    /**
+     * Get the state of the system.
+     * @return the state of the system.
+     */
+    const State& getState() const;
+
+    /**
+     * Get the desired state of the system.
+     * @return the state of the system.
+     */
+    const State& getDesiredState() const;
+
+    /**
+     * Get the feedforward term.
+     * @return the controller feedforward.
+     */
+    const Vector& getFeedForward() const;
 };
 
 template <typename _Derived>
@@ -130,6 +148,27 @@ ProportionalDerivativeControllerBase<_Derived>::getControl() const
     return m_controlOutput;
 }
 
+template <class _Derived>
+const typename ProportionalDerivativeControllerBase<_Derived>::State&
+ProportionalDerivativeControllerBase<_Derived>::getState() const
+{
+    return m_state;
+}
+
+template <class _Derived>
+const typename ProportionalDerivativeControllerBase<_Derived>::State&
+ProportionalDerivativeControllerBase<_Derived>::getDesiredState() const
+{
+    return m_desiredState;
+}
+
+template <class _Derived>
+const typename ProportionalDerivativeControllerBase<_Derived>::Vector&
+ProportionalDerivativeControllerBase<_Derived>::getFeedForward() const
+{
+    return m_feedForward;
+}
+
 } // namespace LieGroupControllers
 
 #endif // LIE_GROUP_CONTROLLERS_IMPL_PROPORTIONAL_DERIVATIVE_CONTROLLER_CONTROLLER_BASE_H