Implement PID/Position/Orientation controllers

src/software/embedded/motion_control/BUILD

@@ -0,0 +1,52 @@
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "position_controller",
+    srcs = [
+        "position_controller.cpp",
+    ],
+    hdrs = [
+        "position_controller.h",
+    ],
+    deps = [
+        ":pid_controller",
+        "//software/ai/navigator/trajectory:trajectory_path",
+    ],
+)
+
+cc_library(
+    name = "orientation_controller",
+    srcs = [
+        "orientation_controller.cpp",
+    ],
+    hdrs = [
+        "orientation_controller.h",
+    ],
+    deps = [
+        ":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",
+    ],
+)

src/software/embedded/motion_control/orientation_controller.cpp

@@ -0,0 +1,33 @@
+#include "software/embedded/motion_control/orientation_controller.h"
+
+AngularVelocity OrientationController::step(
+    Angle orientation, const BangBangTrajectory1DAngular& target_trajectory,
+    Duration elapsed_time, double 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));
+    }
+    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));
+        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,42 @@
+#pragma once
+
+#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.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:
+    /**
+     * 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(Angle orientation,
+                         const BangBangTrajectory1DAngular& target_trajectory,
+                         Duration elapsed_time, double delta_time = 1.0);
+
+    /**
+     * Resets the state of this orientation controller.
+     */
+    void reset();
+
+   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/pid_controller.cpp

@@ -0,0 +1,36 @@
+#include "software/embedded/motion_control/pid_controller.h"
+
+#include <algorithm>
+#include <stdexcept>
+
+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)
+{
+    if (max_integral < 0.0)
+    {
+        throw std::invalid_argument("PidController max_integral must be >= 0.0");
+    }
+};
+
+double PidController::step(double error, double delta_time)
+{
+    // if sign of error swaps, reset integrator
+    if (last_error_.value_or(0.0) * error <= 0.0)
+    {
+        integral_ = 0.0;
+    }
+
+    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,48 @@
+#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
+     * @throws std::invalid_argument if max_integral < 0.0
+     **/
+    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
+     * integrator and derivative. If this function is calling in invariant intervals,
+     * delta_time is by default set to 1 and any effects it would have can be handled
+     * by tuning k_i and k_d.
+     **/
+    double step(double error, double delta_time = 1.0);
+
+    /**
+     * Resets the integrator and clears the last error used for derivative calculation.
+     **/
+    void reset();
+
+    double k_p;
+    double k_i;
+    double k_d;
+    double max_integral;
+
+   private:
+    double integral_                  = 0.0;
+    std::optional<double> last_error_ = std::nullopt;
+};

src/software/embedded/motion_control/pid_controller_test.cpp

@@ -0,0 +1,86 @@
+#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), 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0), 1.0);
+    EXPECT_DOUBLE_EQ(pid.step(8.0), 8.0);
+    EXPECT_DOUBLE_EQ(pid.step(-5.0), -5.0);
+    EXPECT_DOUBLE_EQ(pid.step(-1.0), -1.0);
+
+    pid = PidController{5.0, 0.0, 0.0, 0.0};
+
+    EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0), 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0), 5.0);
+    EXPECT_DOUBLE_EQ(pid.step(8.0), 40.0);
+    EXPECT_DOUBLE_EQ(pid.step(-5.0), -25.0);
+    EXPECT_DOUBLE_EQ(pid.step(-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), k_i * 1.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 2.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0), k_i * 3.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.5), k_i * 3.5);
+
+    // switch error direction, integral term should reset
+    EXPECT_DOUBLE_EQ(pid.step(-0.2), k_i * -0.2);
+    EXPECT_DOUBLE_EQ(pid.step(-1.0), k_i * -1.2);
+    EXPECT_DOUBLE_EQ(pid.step(0.0), k_i * 0.0);
+
+    // should not accumulate integral term above max_integral
+    EXPECT_DOUBLE_EQ(pid.step(9.0), k_i * 9.0);
+    EXPECT_DOUBLE_EQ(pid.step(5.0), k_i * 10.0);
+    EXPECT_DOUBLE_EQ(pid.step(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), k_d * 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0), k_d * 0.0);
+    EXPECT_DOUBLE_EQ(pid.step(1.0), k_d * 1.0);
+    EXPECT_DOUBLE_EQ(pid.step(8.0), k_d * 7.0);
+    EXPECT_DOUBLE_EQ(pid.step(-5.0), k_d * -13.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.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), k_p * 4.0 + k_i * 10.0 + k_d * -20.0);
+    EXPECT_DOUBLE_EQ(pid.step(0.0), k_p * 0.0 + k_i * 0.0 + k_d * -4.0);
+    EXPECT_DOUBLE_EQ(pid.step(2.0), k_p * 2.0 + k_i * 2.0 + k_d * 2.0);
+    EXPECT_DOUBLE_EQ(pid.step(-2.0), k_p * -2.0 + k_i * -2.0 + k_d * -4.0);
+}
+
+TEST(PidControllerTest, InvalidArgumentsToConstructor)
+{
+    EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 9.0));
+    EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 1.0));
+    EXPECT_NO_THROW(PidController(0.0, 0.0, 0.0, 0.0));
+    EXPECT_THROW(PidController(0.0, 0.0, 0.0, -0.5), std::invalid_argument);
+    EXPECT_THROW(PidController(0.0, 0.0, 0.0, -1.0), std::invalid_argument);
+    EXPECT_THROW(PidController(0.0, 0.0, 0.0, -3.0), std::invalid_argument);
+}

src/software/embedded/motion_control/position_controller.cpp

@@ -0,0 +1,33 @@
+#include "software/embedded/motion_control/position_controller.h"
+
+Vector PositionController::step(const Point& position,
+                                const TrajectoryPath& target_trajectory,
+                                Duration elapsed_time, double delta_time)
+{
+    const Vector distance_from_destination =
+        target_trajectory.getDestination() - position;
+
+    if (distance_from_destination.lengthSquared() < LINEAR_PURE_PID_THRESHOLD_METERS)
+    {
+        // if target destination is close enough, use pure PID for velocity
+        return Vector{x_pid_close_.step(distance_from_destination.x(), delta_time),
+                      y_pid_close_.step(distance_from_destination.y(), delta_time)};
+    }
+    else
+    {
+        // feedforward trajectory velocity with small pid control effort
+        const Vector error =
+            target_trajectory.getPosition(elapsed_time.toSeconds()) - position;
+        const Vector control_effort{x_pid_.step(error.x(), delta_time),
+                                    y_pid_.step(error.y(), delta_time)};
+        return target_trajectory.getVelocity(elapsed_time.toSeconds()) + control_effort;
+    }
+}
+
+void PositionController::reset()
+{
+    x_pid_.reset();
+    y_pid_.reset();
+    x_pid_close_.reset();
+    y_pid_close_.reset();
+}

src/software/embedded/motion_control/position_controller.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include "software/ai/navigator/trajectory/trajectory_path.h"
+#include "software/embedded/motion_control/pid_controller.h"
+#include "software/geom/point.h"
+#include "software/geom/vector.h"
+#include "software/time/duration.h"
+
+class PositionController
+{
+   public:
+    /**
+     * Constructs a position controller that uses measurements over multiple time
+     * intervals to calculate the target velocity to minimize error.
+     */
+    PositionController() = default;
+
+    /**
+     * Given a position and target trajectory, returns a target global velocity to
+     * minimize error between the two.
+     *
+     * @param position The actual position.
+     * @param target_trajectory The target 2D trajectory path.
+     * @param elapsed_time The elapsed time since the trajectory was created.
+     * @param delta_time The time passed since last time step.
+     */
+    Vector step(const Point& position, const TrajectoryPath& target_trajectory,
+                Duration elapsed_time, double delta_time = 1.0);
+
+    /**
+     * Resets the state of this position controller.
+     */
+    void reset();
+
+   private:
+    // TODO(#3737): tune constants
+    PidController x_pid_{0.8, 0.0, 0.0, 0.0};
+    PidController y_pid_{0.8, 0.0, 0.0, 0.0};
+
+    PidController x_pid_close_{2.0, 0.0, 0.0, 0.0};
+    PidController y_pid_close_{2.0, 0.0, 0.0, 0.0};
+
+    static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5;
+};