Merge pull request #9 from dic-iit/feature/get

Add the posibility to get the internal state of the controller
ami-iit · Jul 22, 2021 · 8a80a00 · 8a80a00
2 parents 5204bd8 + d541d5a
commit 8a80a00
Show file tree

Hide file tree

Showing 3 changed files with 114 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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