PID direct implementation. Removed control_toolbox dependency. (#825)

Signed-off-by: Piotr Jaroszek <[email protected]>
Co-authored-by: Michał Pełka <[email protected]>

Gems/ROS2/Code/CMakeLists.txt

@@ -70,8 +70,7 @@ ly_add_target(
             Gem::LmbrCentral.API
 )
 
-target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs tf2_ros ackermann_msgs gazebo_msgs vision_msgs)
-target_depends_on_ros2_package(${gem_name}.Static control_toolbox 2.2.0 REQUIRED)
+target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs tf2_ros ackermann_msgs gazebo_msgs vision_msgs control_msgs)
 
 ly_add_target(
     NAME ${gem_name}.API HEADERONLY

Gems/ROS2/Code/Include/ROS2/Utilities/Controllers/PidConfiguration.h

@@ -8,11 +8,11 @@
 #pragma once
 
 #include <AzCore/Serialization/SerializeContext.h>
-#include <control_toolbox/pid.hpp>
 
 namespace ROS2::Controllers
 {
-    //! A wrapper for ROS 2 control_toolbox pid controller.
+    //! A PID controller.
+    //! Based on a ROS 2 control_toolbox implementation.
     //! @see <a href="https://github.com/ros-controls/control_toolbox">control_toolbox</a>.
     class PidConfiguration
     {
@@ -38,7 +38,7 @@ namespace ROS2::Controllers
             const bool antiWindup,
             const double outputLimit);
 
-        //! Initialize PID using member fields as set by the user.
+        //! Initialize the controller
         void InitializePid();
 
         //! Compute the value of PID command.
@@ -54,8 +54,9 @@ namespace ROS2::Controllers
         double m_iMax = 10.0; //!< maximal allowable integral term.
         double m_iMin = -10.0; //!< minimal allowable integral term.
         bool m_antiWindup = false; //!< prevents condition of integrator overflow in integral action.
+        bool m_initialized = false; //!< is PID initialized.
         double m_outputLimit = 0.0; //!< limit PID output; set to 0.0 to disable.
-
-        control_toolbox::Pid m_pid; //!< PID implementation object from control_toolbox (of ros2_control).
+        double m_previousError = 0.0; //!< previous recorded error.
+        double m_integral = 0.0; //!< integral accumulator.
     };
 } // namespace ROS2::Controllers

Gems/ROS2/Code/Source/Utilities/Controllers/PidConfiguration.cpp

@@ -6,6 +6,7 @@
  *
  */
 
+#include <AzCore/Math/MathUtils.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzCore/Serialization/SerializeContext.h>
@@ -70,15 +71,59 @@ namespace ROS2::Controllers
 
     void PidConfiguration::InitializePid()
     {
-        m_pid.initPid(m_p, m_i, m_d, m_iMax, m_iMin, m_antiWindup);
+        if (m_iMin > m_iMax)
+        {
+            AZ_Error("PidConfiguration", false, "Invalid PID configuration.");
+        }
+        else
+        {
+            m_initialized = true;
+        }
     }
 
     double PidConfiguration::ComputeCommand(double error, uint64_t deltaTimeNanoseconds)
     {
-        double output = m_pid.computeCommand(error, deltaTimeNanoseconds);
+        const double dt = aznumeric_cast<double>(deltaTimeNanoseconds) / 1.e9;
+
+        if (!m_initialized)
+        {
+            AZ_Error("PidConfiguration", false, "PID not initialized, ignoring.");
+            return 0.0;
+        }
+
+        if (dt <= 0.0 || !azisfinite(error))
+        {
+            AZ_Warning("PidConfiguration", false, "Invalid PID conditions.");
+            return 0.0;
+        }
+
+        const double proportionalTerm = m_p * error;
+
+        m_integral += error * dt;
+
+        if (m_antiWindup && m_i != 0)
+        {
+            AZStd::pair<double, double> bounds = AZStd::minmax<double>(m_iMin / m_i, m_iMax / m_i);
+            m_integral = AZStd::clamp<double>(m_integral, bounds.first, bounds.second);
+        }
+
+        double integralTerm = m_i * m_integral;
+
+        if (m_antiWindup)
+        {
+            integralTerm = AZStd::clamp<double>(integralTerm, m_iMin, m_iMax);
+        }
+
+        const double derivative = (error - m_previousError) / dt;
+        const double derivativeTerm = m_d * derivative;
+
+        m_previousError = error;
+
+        double output = proportionalTerm + integralTerm + derivativeTerm;
+
         if (m_outputLimit > 0.0)
         {
-            output = AZStd::clamp<float>(output, -m_outputLimit, m_outputLimit);
+            output = AZStd::clamp<double>(output, 0.0, m_outputLimit);
         }
         return output;
     }

Gems/ROS2/Code/Tests/PIDTest.cpp

@@ -0,0 +1,195 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include <AzCore/UnitTest/TestTypes.h>
+#include <AzTest/AzTest.h>
+
+#include <ROS2/Utilities/Controllers/PidConfiguration.h>
+
+namespace UnitTest
+{
+    static const uint64_t secToNanosec = 1e9;
+
+    class PIDTest : public LeakDetectionFixture
+    {
+    };
+
+    TEST_F(PIDTest, iClampAntiwindup)
+    {
+        double iGain = 1.0;
+        double iMin = -1.0;
+        double iMax = 1.0;
+
+        ROS2::Controllers::PidConfiguration pid(0.0, iGain, 0.0, iMax, iMin, true, 1.0);
+        pid.InitializePid();
+
+        double output = 0.0;
+
+        output = pid.ComputeCommand(-10.0, 1 * secToNanosec);
+        EXPECT_EQ(0.0, output);
+
+        output = pid.ComputeCommand(30.0, 1 * secToNanosec);
+        EXPECT_EQ(1.0, output);
+    }
+
+    TEST_F(PIDTest, iClampNoGain)
+    {
+        double iGain = 0.0;
+        double iMin = -1.0;
+        double iMax = 1.0;
+
+        ROS2::Controllers::PidConfiguration pid(0.0, iGain, 0.0, iMax, iMin, false, 0.0);
+        pid.InitializePid();
+
+        double output = 0.0;
+
+        output = pid.ComputeCommand(-1.0, 1 * secToNanosec);
+        EXPECT_LE(iMin, output);
+        EXPECT_LE(output, iMax);
+        EXPECT_EQ(0.0, output);
+
+        output = pid.ComputeCommand(-1.0, 1 * secToNanosec);
+        EXPECT_LE(iMin, output);
+        EXPECT_LE(output, iMax);
+        EXPECT_EQ(0.0, output);
+    }
+
+    TEST_F(PIDTest, iAntiwindup)
+    {
+        double iGain = 2.0;
+        double iMin = -1.0;
+        double iMax = 1.0;
+
+        ROS2::Controllers::PidConfiguration pid(0.0, iGain, 0.0, iMax, iMin, true, 0.0);
+        pid.InitializePid();
+
+        double output = 0.0;
+
+        output = pid.ComputeCommand(-1.0, 1 * secToNanosec);
+        EXPECT_EQ(-1.0, output);
+
+        output = pid.ComputeCommand(-1.0, 1 * secToNanosec);
+        EXPECT_EQ(-1.0, output);
+
+        output = pid.ComputeCommand(0.5, 1 * secToNanosec);
+        EXPECT_EQ(0.0, output);
+
+        output = pid.ComputeCommand(-1.0, 1 * secToNanosec);
+        EXPECT_EQ(-1.0, output);
+    }
+
+    TEST_F(PIDTest, negativeIAntiwindup)
+    {
+        double iGain = -2.5;
+        double iMin = -0.2;
+        double iMax = 0.5;
+
+        ROS2::Controllers::PidConfiguration pid(0.0, iGain, 0.0, iMax, iMin, true, 0.0);
+        pid.InitializePid();
+
+        double output = 0.0;
+
+        output = pid.ComputeCommand(0.1, 1 * secToNanosec);
+        EXPECT_EQ(-0.2, output);
+
+        output = pid.ComputeCommand(0.1, 1 * secToNanosec);
+        EXPECT_EQ(-0.2, output);
+
+        output = pid.ComputeCommand(-0.05, 1 * secToNanosec);
+        EXPECT_EQ(-0.075, output);
+
+        output = pid.ComputeCommand(0.1, 1 * secToNanosec);
+        EXPECT_EQ(-0.2, output);
+    }
+
+    TEST_F(PIDTest, pOnly)
+    {
+        ROS2::Controllers::PidConfiguration pid(1.0, 0.0, 0.0, 0.0, 0.0, false, 0.0);
+        pid.InitializePid();
+
+        double output = 0.0;
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(-0.5, output);
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(-0.5, output);
+
+        output = pid.ComputeCommand(-1.0, 1 * secToNanosec);
+        EXPECT_EQ(-1.0, output);
+
+        output = pid.ComputeCommand(0.5, 1 * secToNanosec);
+        EXPECT_EQ(0.5, output);
+    }
+
+    TEST_F(PIDTest, iOnly)
+    {
+        ROS2::Controllers::PidConfiguration pid(0.0, 1.0, 0.0, 5.0, -5.0, false, 0.0);
+        pid.InitializePid();
+
+        double output = 0.0;
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(-0.5, output);
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(-1.0, output);
+
+        output = pid.ComputeCommand(0.0, 1 * secToNanosec);
+        EXPECT_EQ(-1.0, output);
+
+        output = pid.ComputeCommand(0.0, 1 * secToNanosec);
+        EXPECT_EQ(-1.0, output);
+
+        output = pid.ComputeCommand(1.0, 1 * secToNanosec);
+        EXPECT_EQ(0.0, output);
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(-0.5, output);
+    }
+
+    TEST_F(PIDTest, dOnly)
+    {
+        ROS2::Controllers::PidConfiguration pid(0.0, 0.0, 1.0, 0.0, 0.0, false, 0.0);
+        pid.InitializePid();
+
+        double output = 0.0;
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(-0.5, output);
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(0.0, output);
+
+        output = pid.ComputeCommand(-0.5, 0 * secToNanosec);
+        EXPECT_EQ(0.0, output);
+
+        output = pid.ComputeCommand(-1.0, 1 * secToNanosec);
+        EXPECT_EQ(-0.5, output);
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(0.5, output);
+    }
+
+    TEST_F(PIDTest, completePID)
+    {
+        ROS2::Controllers::PidConfiguration pid(1.0, 1.0, 1.0, 5.0, -5.0, false, 0.0);
+        pid.InitializePid();
+
+        double output = 0.0;
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(-1.5, output);
+
+        output = pid.ComputeCommand(-0.5, 1 * secToNanosec);
+        EXPECT_EQ(-1.5, output);
+
+        output = pid.ComputeCommand(-1.0, 1 * secToNanosec);
+        EXPECT_EQ(-3.5, output);
+    }
+} // namespace UnitTest

Gems/ROS2/Code/ros2_tests_files.cmake

@@ -6,4 +6,5 @@
 set(FILES
     Tests/ROS2Test.cpp
     Tests/GNSSTest.cpp
+    Tests/PIDTest.cpp
 )