diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index dfd8efd92e..92165c05e5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -4,10 +4,12 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.StateSpaceUtil; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -25,12 +27,9 @@ import edu.wpi.first.math.trajectory.Trajectory; */ @SuppressWarnings("MemberName") public class LTVUnicycleController { - private final Matrix m_A = new Matrix<>(Nat.N3(), Nat.N3()); - private final Matrix m_B = - new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0); - private final Matrix m_Q; - private final Matrix m_R; - private final double m_dt; + // LUT from drivetrain linear velocity to LQR gain + private final InterpolatingMatrixTreeMap m_table = + new InterpolatingMatrixTreeMap<>(); private Pose2d m_poseError; private Pose2d m_poseTolerance; @@ -65,6 +64,30 @@ public class LTVUnicycleController { } } + /** + * Constructs a linear time-varying unicycle controller with default maximum desired error + * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, + * 2 rad/s). + * + * @param dt Discretization timestep in seconds. + */ + public LTVUnicycleController(double dt) { + this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, 9.0); + } + + /** + * Constructs a linear time-varying unicycle controller with default maximum desired error + * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, + * 2 rad/s). + * + * @param dt Discretization timestep in seconds. + * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup + * table. The default is 9 m/s. + */ + public LTVUnicycleController(double dt, double maxVelocity) { + this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity); + } + /** * Constructs a linear time-varying unicycle controller. * @@ -73,9 +96,36 @@ public class LTVUnicycleController { * @param dt Discretization timestep in seconds. */ public LTVUnicycleController(Vector qelems, Vector relems, double dt) { - m_dt = dt; - m_Q = StateSpaceUtil.makeCostMatrix(qelems); - m_R = StateSpaceUtil.makeCostMatrix(relems); + this(qelems, relems, dt, 9.0); + } + + /** + * 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 in seconds. + * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup + * table. The default is 9 m/s. + */ + @SuppressWarnings("LocalVariableName") + public LTVUnicycleController( + Vector qelems, Vector relems, double dt, double maxVelocity) { + var A = new Matrix<>(Nat.N3(), Nat.N3()); + var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0); + var Q = StateSpaceUtil.makeCostMatrix(qelems); + var R = StateSpaceUtil.makeCostMatrix(relems); + + for (double velocity = -maxVelocity; velocity < maxVelocity; velocity += 0.01) { + // The DARE is ill-conditioned if the velocity is close to zero, so don't + // let the system stop. + if (Math.abs(velocity) < 1e-4) { + m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N3())); + } else { + A.set(State.kY.value, State.kHeading.value, velocity); + m_table.put(velocity, new LinearQuadraticRegulator(A, B, Q, R, dt).getK()); + } + } } /** @@ -122,15 +172,12 @@ public class LTVUnicycleController { m_poseError = poseRef.relativeTo(currentPose); - if (Math.abs(linearVelocityRef) < 1e-4) { - m_A.set(State.kY.value, State.kHeading.value, 1e-4); - } else { - m_A.set(State.kY.value, State.kHeading.value, linearVelocityRef); - } + @SuppressWarnings("LocalVariableName") + var K = m_table.get(linearVelocityRef); var e = new MatBuilder<>(Nat.N3(), Nat.N1()) .fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians()); - var u = new LinearQuadraticRegulator(m_A, m_B, m_Q, m_R, m_dt).getK().times(e); + var u = K.times(e); return new ChassisSpeeds( linearVelocityRef + u.get(0, 0), 0.0, angularVelocityRef + u.get(1, 0)); diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp index 59a11fa32a..dca1532f9d 100644 --- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp @@ -37,12 +37,31 @@ class Input { static constexpr int kAngularVelocity = 4; }; +LTVUnicycleController::LTVUnicycleController( + units::second_t dt, units::meters_per_second_t maxVelocity) + : LTVUnicycleController{{0.0625, 0.125, 2.0}, {1.0, 2.0}, dt, maxVelocity} { +} + LTVUnicycleController::LTVUnicycleController( const wpi::array& Qelems, const wpi::array& Relems, - units::second_t dt) - : m_dt{dt} { - m_Q = frc::MakeCostMatrix(Qelems); - m_R = frc::MakeCostMatrix(Relems); + units::second_t dt, units::meters_per_second_t maxVelocity) { + Matrixd<3, 3> A = Matrixd<3, 3>::Zero(); + Matrixd<3, 2> B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}}; + Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems); + Matrixd<2, 2> R = frc::MakeCostMatrix(Relems); + + for (auto velocity = -maxVelocity; velocity < maxVelocity; + velocity += 0.01_mps) { + // The DARE is ill-conditioned if the velocity is close to zero, so don't + // let the system stop. + if (units::math::abs(velocity) < 1e-4_mps) { + m_table.insert(velocity, Matrixd<2, 3>::Zero()); + } else { + A(State::kY, State::kHeading) = velocity.value(); + m_table.insert(velocity, + frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K()); + } + } } bool LTVUnicycleController::AtReference() const { @@ -69,15 +88,10 @@ ChassisSpeeds LTVUnicycleController::Calculate( m_poseError = poseRef.RelativeTo(currentPose); - if (units::math::abs(linearVelocityRef) < 1e-4_mps) { - m_A(State::kY, State::kHeading) = 1e-4; - } else { - m_A(State::kY, State::kHeading) = linearVelocityRef.value(); - } + const auto& K = m_table[linearVelocityRef]; Vectord<3> e{m_poseError.X().value(), m_poseError.Y().value(), m_poseError.Rotation().Radians().value()}; - Vectord<2> u = - frc::LinearQuadraticRegulator<3, 2>{m_A, m_B, m_Q, m_R, m_dt}.K() * e; + Vectord<2> u = K * e; return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)}, 0_mps, diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h index 80b8d05574..83cfe4bfd2 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -6,6 +6,7 @@ #include #include +#include #include "frc/EigenCore.h" #include "frc/geometry/Pose2d.h" @@ -27,16 +28,30 @@ namespace frc { */ class WPILIB_DLLEXPORT LTVUnicycleController { public: + /** + * Constructs a linear time-varying unicycle controller with default maximum + * desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum + * desired control effort of (1 m/s, 2 rad/s). + * + * @param dt Discretization timestep. + * @param maxVelocity The maximum velocity for the controller gain lookup + * table. + */ + explicit LTVUnicycleController( + units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps); + /** * 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. + * @param maxVelocity The maximum velocity for the controller gain lookup + * table. */ LTVUnicycleController(const wpi::array& Qelems, - const wpi::array& Relems, - units::second_t dt); + const wpi::array& Relems, units::second_t dt, + units::meters_per_second_t maxVelocity = 9_mps); /** * Move constructor. @@ -97,11 +112,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController { 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; + // LUT from drivetrain linear velocity to LQR gain + wpi::interpolating_map> m_table; Pose2d m_poseError; Pose2d m_poseTolerance;