Implement PID/Position/Orientation controllers (#3730)

* Add PID controller class and basic test cases

* Rename directory to position_controller

* Create position controller class specs

* Add missing BUILD file

* Fix BUILD file

* Add position controller basic test cases

* Implement position controller

* Re-write position controller for feedforward

* Rename directory to motion_control

* Add specification for OrientationController

* Implement OrientationController

* Remove orientation and position controller tests

* Add motion_control BUILD file

* Add reset() method

* Update parameter names and doc comments

* Remove old tests

* Tag issue in TODO

* Formatting

* Remove default argument for delta_time

* Remove integrator reset on error sign swap

* Add doc comment

* Use length instead of lengthSquared checking for pure pid

* Add generic motion controller interface

* Fix include

* Add test placeholders for position and orientation controller

* Use assertion instead of throwing exception

* Remove exception tests

* Make pid controller gain variables private

Author: nycrat

src/software/embedded/motion_control/BUILD

@@ -0,0 +1,86 @@
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "controller",
+    hdrs = [
+        "controller.h",
+    ],
+    deps = [
+        "//software/time:duration",
+    ],
+)
+
+cc_library(
+    name = "position_controller",
+    srcs = [
+        "position_controller.cpp",
+    ],
+    hdrs = [
+        "position_controller.h",
+    ],
+    deps = [
+        ":controller",
+        ":pid_controller",
+        "//software/ai/navigator/trajectory:trajectory_path",
+    ],
+)
+
+cc_library(
+    name = "orientation_controller",
+    srcs = [
+        "orientation_controller.cpp",
+    ],
+    hdrs = [
+        "orientation_controller.h",
+    ],
+    deps = [
+        ":controller",
+        ":pid_controller",
+        "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular",
+        "//software/geom:angle",
+        "//software/geom:angular_velocity",
+    ],
+)
+
+cc_library(
+    name = "pid_controller",
+    srcs = [
+        "pid_controller.cpp",
+    ],
+    hdrs = [
+        "pid_controller.h",
+    ],
+)
+
+cc_test(
+    name = "pid_controller_test",
+    srcs = [
+        "pid_controller_test.cpp",
+    ],
+    deps = [
+        ":pid_controller",
+        "//shared/test_util:tbots_gtest_main",
+    ],
+)
+
+cc_test(
+    name = "position_controller_test",
+    srcs = [
+        "position_controller_test.cpp",
+    ],
+    deps = [
+        ":position_controller",
+        "//shared/test_util:tbots_gtest_main",
+    ],
+)
+
+cc_test(
+    name = "orientation_controller_test",
+    srcs = [
+        "orientation_controller_test.cpp",
+    ],
+    deps = [
+        ":orientation_controller",
+        "//shared/test_util:tbots_gtest_main",
+    ],
+)

src/software/embedded/motion_control/controller.h

@@ -0,0 +1,32 @@
+#include "software/time/duration.h"
+
+/**
+ * A generic interface for a motion controller.
+ *
+ * @tparam StateType The type of the system's current feedback state
+ * @tparam TrajectoryType The type of the state's trajectory
+ * @tparam OutputType The type of the calculated control effort
+ */
+template <typename StateType, typename TrajectoryType, typename OutputType>
+class MotionController
+{
+   public:
+    /**
+     * Evaluates a single time step of the control loop and returns an output to minimize
+     * error between the current state and target trajectory.
+     *
+     * @param current_state The actual measured current state.
+     * @param target_trajectory The target trajectory of motion.
+     * @param elapsed_time The total elapsed time since the trajectory was created.
+     * @param delta_time The elapsed time since the last time step.
+     */
+    virtual OutputType step(const StateType& current_state,
+                            const TrajectoryType& target_trajectory,
+                            Duration elapsed_time, Duration delta_time) = 0;
+
+    /**
+     * Resets the internal state of the motion controller. For example, any
+     * accumulated error terms used for calculating control effort.
+     */
+    virtual void reset() = 0;
+};

src/software/embedded/motion_control/orientation_controller.cpp

@@ -0,0 +1,33 @@
+#include "software/embedded/motion_control/orientation_controller.h"
+
+AngularVelocity OrientationController::step(
+    const Angle& orientation, const BangBangTrajectory1DAngular& target_trajectory,
+    Duration elapsed_time, Duration delta_time)
+{
+    const Angle difference_from_target =
+        (target_trajectory.getDestination() - orientation).clamp();
+
+    if (difference_from_target.abs().toDegrees() < ANGULAR_PURE_PID_THRESHOLD_DEGREES)
+    {
+        // if target orientation is close enough, use pure PID for angular velocity
+        return AngularVelocity::fromRadians(w_pid_close_.step(
+            difference_from_target.toRadians(), delta_time.toSeconds()));
+    }
+    else
+    {
+        // feedforward trajectory angular velocity with small pid control effort
+        const Angle error_angular =
+            (target_trajectory.getPosition(elapsed_time.toSeconds()) - orientation)
+                .clamp();
+        const AngularVelocity pid_effort_angular = AngularVelocity::fromRadians(
+            w_pid_.step(error_angular.toRadians(), delta_time.toSeconds()));
+        return target_trajectory.getVelocity(elapsed_time.toSeconds()) +
+               pid_effort_angular;
+    }
+}
+
+void OrientationController::reset()
+{
+    w_pid_.reset();
+    w_pid_close_.reset();
+}

src/software/embedded/motion_control/orientation_controller.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
+#include "software/embedded/motion_control/controller.h"
+#include "software/embedded/motion_control/pid_controller.h"
+#include "software/geom/angle.h"
+#include "software/geom/angular_velocity.h"
+#include "software/time/duration.h"
+
+class OrientationController
+    : public MotionController<Angle, BangBangTrajectory1DAngular, AngularVelocity>
+{
+   public:
+    /**
+     * Constructs an orientation controller that uses measurements over multiple
+     * time intervals to calculate the target angular velocity to minimize error.
+     */
+    OrientationController() = default;
+
+    /**
+     * Given an orientation and target orientation, returns a target angular
+     * velocity to minimize the error between the two.
+     *
+     * @param orientation The actual orientation.
+     * @param target_trajectory The target angular trajectory.
+     * @param elapsed_time The elapsed time since the trajectory was created.
+     * @param delta_time The time passed since last time step.
+     */
+    AngularVelocity step(const Angle& orientation,
+                         const BangBangTrajectory1DAngular& target_trajectory,
+                         Duration elapsed_time, Duration delta_time) override;
+
+    /**
+     * Resets the state of this orientation controller.
+     */
+    void reset() override;
+
+   private:
+    // TODO(#3737): tune constants
+    PidController w_pid_{0.7, 0.0, 2.0, 0.0};
+    PidController w_pid_close_{2.0, 0.0, 4.0, 0.0};
+
+    static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0;
+};

src/software/embedded/motion_control/orientation_controller_test.cpp

@@ -0,0 +1,14 @@
+#include "software/embedded/motion_control/orientation_controller.h"
+
+#include <gtest/gtest.h>
+
+// TODO(#3743): write proper tests after pid constants have been tuned
+
+TEST(OrientationControllerTest, BasicTest)
+{
+    OrientationController controller;
+    BangBangTrajectory1DAngular trajectory;
+    controller.step(Angle::zero(), trajectory, Duration::fromSeconds(1.0),
+                    Duration::fromSeconds(0.01));
+    controller.reset();
+}

src/software/embedded/motion_control/pid_controller.cpp

@@ -0,0 +1,27 @@
+#include "software/embedded/motion_control/pid_controller.h"
+
+#include <algorithm>
+#include <cassert>
+
+PidController::PidController(double k_p, double k_i, double k_d, double max_integral)
+    : k_p_(k_p), k_i_(k_i), k_d_(k_d), max_integral_(max_integral)
+{
+    assert(max_integral >= 0.0);
+};
+
+double PidController::step(double error, double delta_time)
+{
+    integral_ = std::clamp(integral_ + error * delta_time, -max_integral_, max_integral_);
+
+    const double derivative = (error - last_error_.value_or(error)) / delta_time;
+
+    last_error_ = error;
+
+    return error * k_p_ + integral_ * k_i_ + derivative * k_d_;
+}
+
+void PidController::reset()
+{
+    integral_   = 0.0;
+    last_error_ = std::nullopt;
+}

src/software/embedded/motion_control/pid_controller.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <optional>
+
+class PidController
+{
+   public:
+    /**
+     * Constructs a new PID controller.
+     *
+     * A PID controller is used to calculate corrections based on error values over
+     * time as the difference between a desired value and the actual measured value.
+     * This PID controller also limits integral windup using a max_integral value.
+     *
+     * Resources:
+     * - https://raw.org/book/control-theory/introduction-to-pid-controllers/
+     * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/
+     *
+     * @pre max_integral must be >= 0.0
+     *
+     * @param k_p The proportional gain.
+     * @param k_i The integral gain.
+     * @param k_d The derivative gain.
+     * @param max_integral The maximum absolute value that the integrator term can
+     * accumulate to.
+     **/
+    PidController(double k_p, double k_i, double k_d, double max_integral);
+
+    /**
+     * Given an error, returns the control effort to minimize it.
+     *
+     * @param error The amount of error between desired and actual output
+     * @param delta_time The time passed since last step, for calculating the integrator
+     * and derivative.
+     **/
+    double step(double error, double delta_time);
+
+    /**
+     * Resets the integrator and clears the last error used for derivative calculation.
+     **/
+    void reset();
+
+   private:
+    double k_p_;
+    double k_i_;
+    double k_d_;
+    double max_integral_;
+
+    double integral_                  = 0.0;
+    std::optional<double> last_error_ = std::nullopt;
+};

src/software/embedded/motion_control/pid_controller_test.cpp

@@ -0,0 +1,75 @@
+#include "software/embedded/motion_control/pid_controller.h"
+
+#include <gtest/gtest.h>
+
+TEST(PidControllerTest, OnlyProportionTermNonZero)
+{
+    PidController pid{1.0, 0.0, 0.0, 0.0};
+
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), 1.0);
+    EXPECT_DOUBLE_EQ(pid.step(8.0, 1.0), 8.0);
+    EXPECT_DOUBLE_EQ(pid.step(-5.0, 1.0), -5.0);
+    EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), -1.0);
+
+    pid = PidController{5.0, 0.0, 0.0, 0.0};
+
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), 5.0);
+    EXPECT_DOUBLE_EQ(pid.step(8.0, 1.0), 40.0);
+    EXPECT_DOUBLE_EQ(pid.step(-5.0, 1.0), -25.0);
+    EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), -5.0);
+}
+
+TEST(PidControllerTest, OnlyIntegralTermNonZero)
+{
+    constexpr double k_i = 2.0;
+    PidController pid{0.0, k_i, 0.0, 10.0};
+
+    EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 1.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 2.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 3.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.5, 1.0), k_i * 3.5);
+
+    EXPECT_DOUBLE_EQ(pid.step(-0.2, 1.0), k_i * 3.3);
+    EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), k_i * 2.3);
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_i * 2.3);
+
+    // should not accumulate integral term above max_integral
+    EXPECT_DOUBLE_EQ(pid.step(6.7, 1.0), k_i * 9.0);
+    EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), k_i * 10.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 10.0);
+}
+
+TEST(PidControllerTest, OnlyDerivativeTermNonZero)
+{
+    constexpr double k_d = 3.0;
+    PidController pid{0.0, 0.0, k_d, 0.0};
+
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_d * 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_d * 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_d * 1.0);
+    EXPECT_DOUBLE_EQ(pid.step(8.0, 1.0), k_d * 7.0);
+    EXPECT_DOUBLE_EQ(pid.step(-5.0, 1.0), k_d * -13.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_d * 5.0);
+    EXPECT_DOUBLE_EQ(pid.step(5.0, 0.5), k_d * 5.0 / 0.5);
+    EXPECT_DOUBLE_EQ(pid.step(8.0, 2.0), k_d * 3.0 / 2.0);
+}
+
+TEST(PidControllerTest, GeneralApplication)
+{
+    constexpr double k_p          = 10.0;
+    constexpr double k_i          = 3.0;
+    constexpr double k_d          = -1.0;
+    constexpr double max_integral = 10.0;
+    PidController pid{k_p, k_i, k_d, max_integral};
+
+    EXPECT_DOUBLE_EQ(pid.step(12.0, 0.75), k_p * 12.0 + k_i * 9.0 + k_d * 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(24.0, 0.75), k_p * 24.0 + k_i * 10.0 + k_d * 12.0 / 0.75);
+    EXPECT_DOUBLE_EQ(pid.step(4.0, 1.0), k_p * 4.0 + k_i * 10.0 + k_d * -20.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_p * 0.0 + k_i * 10.0 + k_d * -4.0);
+    EXPECT_DOUBLE_EQ(pid.step(2.0, 1.0), k_p * 2.0 + k_i * 10.0 + k_d * 2.0);
+    EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * 8.0 + k_d * -4.0);
+}