diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index bd5de7412f..b31d146d81 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -38,7 +38,7 @@ public class LTVDifferentialDriveController { private Matrix m_tolerance = new Matrix<>(Nat.N5(), Nat.N1()); /** States of the drivetrain system. */ - enum State { + private enum State { kX(0), kY(1), kHeading(2), @@ -73,6 +73,8 @@ public class LTVDifferentialDriveController { double dt) { m_trackwidth = trackwidth; + // Control law derivation is in section 8.7 of + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf var A = new MatBuilder<>(Nat.N5(), Nat.N5()) .fill( 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 92165c05e5..2f55be798d 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 @@ -36,7 +36,7 @@ public class LTVUnicycleController { private boolean m_enabled = true; /** States of the drivetrain system. */ - enum State { + private enum State { kX(0), kY(1), kHeading(2); @@ -50,20 +50,6 @@ public class LTVUnicycleController { } } - /** Inputs of the drivetrain system. */ - enum Input { - kLeftVelocity(3), - kRightVelocity(4); - - @SuppressWarnings("MemberName") - public final int value; - - @SuppressWarnings("ParameterName") - Input(int i) { - this.value = i; - } - } - /** * 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, @@ -111,6 +97,39 @@ public class LTVUnicycleController { @SuppressWarnings("LocalVariableName") public LTVUnicycleController( Vector qelems, Vector relems, double dt, double maxVelocity) { + // The change in global pose for a unicycle is defined by the following + // three equations. + // + // ẋ = v cosθ + // ẏ = v sinθ + // θ̇ = ω + // + // Here's the model as a vector function where x = [x y θ]ᵀ and + // u = [v ω]ᵀ. + // + // [v cosθ] + // f(x, u) = [v sinθ] + // [ ω ] + // + // To create an LQR, we need to linearize this. + // + // [0 0 −v sinθ] [cosθ 0] + // ∂f(x, u)/∂x = [0 0 v cosθ] ∂f(x, u)/∂u = [sinθ 0] + // [0 0 0 ] [ 0 1] + // + // We're going to make a cross-track error controller, so we'll apply a + // clockwise rotation matrix to the global tracking error to transform it + // into the robot's coordinate frame. Since the cross-track error is always + // measured from the robot's coordinate frame, the model used to compute the + // LQR should be linearized around θ = 0 at all times. + // + // [0 0 −v sin0] [cos0 0] + // A = [0 0 v cos0] B = [sin0 0] + // [0 0 0 ] [ 0 1] + // + // [0 0 0] [1 0] + // A = [0 0 v] B = [0 0] + // [0 0 0] [0 1] 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); diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp index a249344607..17a0c56a95 100644 --- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp @@ -12,13 +12,15 @@ using namespace frc; +namespace { + /** * States of the drivetrain system. */ class State { public: /// X position in global coordinate frame. - static constexpr int kX = 0; + [[maybe_unused]] static constexpr int kX = 0; /// Y position in global coordinate frame. static constexpr int kY = 1; @@ -27,17 +29,21 @@ class State { static constexpr int kHeading = 2; /// Left encoder velocity. - static constexpr int kLeftVelocity = 3; + [[maybe_unused]] static constexpr int kLeftVelocity = 3; /// Right encoder velocity. - static constexpr int kRightVelocity = 4; + [[maybe_unused]] static constexpr int kRightVelocity = 4; }; +} // namespace + LTVDifferentialDriveController::LTVDifferentialDriveController( const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth, const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt) : m_trackwidth{trackwidth} { + // Control law derivation is in section 8.7 of + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf Matrixd<5, 5> A{ {0.0, 0.0, 0.0, 0.5, 0.5}, {0.0, 0.0, 0.0, 0.0, 0.0}, diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp index dca1532f9d..256b757192 100644 --- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp @@ -10,13 +10,15 @@ using namespace frc; +namespace { + /** * States of the drivetrain system. */ class State { public: /// X position in global coordinate frame. - static constexpr int kX = 0; + [[maybe_unused]] static constexpr int kX = 0; /// Y position in global coordinate frame. static constexpr int kY = 1; @@ -25,17 +27,7 @@ class State { static constexpr int kHeading = 2; }; -/** - * Inputs of the drivetrain system. - */ -class Input { - public: - /// Linear velocity. - static constexpr int kLinearVelocity = 3; - - /// Angular velocity. - static constexpr int kAngularVelocity = 4; -}; +} // namespace LTVUnicycleController::LTVUnicycleController( units::second_t dt, units::meters_per_second_t maxVelocity) @@ -45,6 +37,38 @@ LTVUnicycleController::LTVUnicycleController( LTVUnicycleController::LTVUnicycleController( const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt, units::meters_per_second_t maxVelocity) { + // The change in global pose for a unicycle is defined by the following three + // equations. + // + // ẋ = v cosθ + // ẏ = v sinθ + // θ̇ = ω + // + // Here's the model as a vector function where x = [x y θ]ᵀ and u = [v ω]ᵀ. + // + // [v cosθ] + // f(x, u) = [v sinθ] + // [ ω ] + // + // To create an LQR, we need to linearize this. + // + // [0 0 −v sinθ] [cosθ 0] + // ∂f(x, u)/∂x = [0 0 v cosθ] ∂f(x, u)/∂u = [sinθ 0] + // [0 0 0 ] [ 0 1] + // + // We're going to make a cross-track error controller, so we'll apply a + // clockwise rotation matrix to the global tracking error to transform it into + // the robot's coordinate frame. Since the cross-track error is always + // measured from the robot's coordinate frame, the model used to compute the + // LQR should be linearized around θ = 0 at all times. + // + // [0 0 −v sin0] [cos0 0] + // A = [0 0 v cos0] B = [sin0 0] + // [0 0 0 ] [ 0 1] + // + // [0 0 0] [1 0] + // A = [0 0 v] B = [0 0] + // [0 0 0] [0 1] 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);