[wpimath] Add LTV controllers (#4094)

This adds a unicycle controller that's a drop-in replacement for Ramsete
and a differential drive controller that controls the full pose and
outputs voltages. The main benefit is LQR-like tuning knobs using a
system model.

wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h

@@ -0,0 +1,132 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+#include <wpi/interpolating_map.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+#include "units/voltage.h"
+
+namespace frc {
+
+/**
+ * The linear time-varying differential drive controller has a similar form to
+ * the LQR, but the model used to compute the controller gain is the nonlinear
+ * model linearized around the drivetrain's current state. We precomputed gains
+ * for important places in our state-space, then interpolated between them with
+ * a LUT to save computational resources.
+ *
+ * See section 8.7 in Controls Engineering in FRC for a derivation of the
+ * control law we used shown in theorem 8.7.4.
+ */
+class WPILIB_DLLEXPORT LTVDifferentialDriveController {
+ public:
+  /**
+   * Motor voltages for a differential drive.
+   */
+  struct WheelVoltages {
+    units::volt_t left = 0_V;
+    units::volt_t right = 0_V;
+  };
+
+  /**
+   * Constructs a linear time-varying differential drive controller.
+   *
+   * @param plant      The drivetrain velocity plant.
+   * @param trackwidth The drivetrain's trackwidth.
+   * @param Qelems     The maximum desired error tolerance for each state.
+   * @param Relems     The maximum desired control effort for each input.
+   * @param dt         Discretization timestep.
+   */
+  LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
+                                 units::meter_t trackwidth,
+                                 const wpi::array<double, 5>& Qelems,
+                                 const wpi::array<double, 2>& Relems,
+                                 units::second_t dt);
+
+  /**
+   * Move constructor.
+   */
+  LTVDifferentialDriveController(LTVDifferentialDriveController&&) = default;
+
+  /**
+   * Move assignment operator.
+   */
+  LTVDifferentialDriveController& operator=(LTVDifferentialDriveController&&) =
+      default;
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const;
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   * @param leftVelocityTolerance Left velocity error which is tolerable.
+   * @param rightVelocityTolerance Right velocity error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& poseTolerance,
+                    units::meters_per_second_t leftVelocityTolerance,
+                    units::meters_per_second_t rightVelocityTolerance);
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose      The current pose.
+   * @param leftVelocity     The current left velocity.
+   * @param rightVelocity    The current right velocity.
+   * @param poseRef          The desired pose.
+   * @param leftVelocityRef  The desired left velocity.
+   * @param rightVelocityRef The desired right velocity.
+   */
+  WheelVoltages Calculate(const Pose2d& currentPose,
+                          units::meters_per_second_t leftVelocity,
+                          units::meters_per_second_t rightVelocity,
+                          const Pose2d& poseRef,
+                          units::meters_per_second_t leftVelocityRef,
+                          units::meters_per_second_t rightVelocityRef);
+
+  /**
+   * Returns the left and right output voltages of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param leftVelocity The left velocity.
+   * @param rightVelocity The right velocity.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   */
+  WheelVoltages Calculate(const Pose2d& currentPose,
+                          units::meters_per_second_t leftVelocity,
+                          units::meters_per_second_t rightVelocity,
+                          const Trajectory::State& desiredState);
+
+ private:
+  units::meter_t m_trackwidth;
+
+  // LUT from drivetrain linear velocity to LQR gain
+  wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 5>> m_table;
+
+  Vectord<5> m_error;
+  Vectord<5> m_tolerance;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h

@@ -0,0 +1,111 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "frc/EigenCore.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/angular_velocity.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+/**
+ * The linear time-varying unicycle controller has a similar form to the LQR,
+ * but the model used to compute the controller gain is the nonlinear model
+ * linearized around the drivetrain's current state.
+ *
+ * See section 8.9 in Controls Engineering in FRC for a derivation of the
+ * control law we used shown in theorem 8.9.1.
+ */
+class WPILIB_DLLEXPORT LTVUnicycleController {
+ public:
+  /**
+   * Constructs a linear time-varying unicycle controller.
+   *
+   * @param Qelems The maximum desired error tolerance for each state.
+   * @param Relems The maximum desired control effort for each input.
+   * @param dt     Discretization timestep.
+   */
+  LTVUnicycleController(const wpi::array<double, 3>& Qelems,
+                        const wpi::array<double, 2>& Relems,
+                        units::second_t dt);
+
+  /**
+   * Move constructor.
+   */
+  LTVUnicycleController(LTVUnicycleController&&) = default;
+
+  /**
+   * Move assignment operator.
+   */
+  LTVUnicycleController& operator=(LTVUnicycleController&&) = default;
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const;
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& poseTolerance);
+
+  /**
+   * Returns the linear and angular velocity outputs of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose        The current pose.
+   * @param poseRef            The desired pose.
+   * @param linearVelocityRef  The desired linear velocity.
+   * @param angularVelocityRef The desired angular velocity.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+                          units::meters_per_second_t linearVelocityRef,
+                          units::radians_per_second_t angularVelocityRef);
+
+  /**
+   * Returns the linear and angular velocity outputs of the LTV controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
+                          const Trajectory::State& desiredState);
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
+ private:
+  Matrixd<3, 3> m_A = Matrixd<3, 3>::Zero();
+  Matrixd<3, 2> m_B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}};
+  Matrixd<3, 3> m_Q;
+  Matrixd<2, 2> m_R;
+  units::second_t m_dt;
+
+  Pose2d m_poseError;
+  Pose2d m_poseTolerance;
+  bool m_enabled = true;
+};
+
+}  // namespace frc