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 c5b6defa0d..a9222f7428 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 @@ -5,7 +5,6 @@ package edu.wpi.first.math.controller; import edu.wpi.first.math.DARE; -import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; @@ -40,28 +39,19 @@ import edu.wpi.first.math.trajectory.Trajectory; public class LTVDifferentialDriveController { private final double m_trackwidth; - // LUT from drivetrain linear velocity to LQR gain - private final InterpolatingMatrixTreeMap m_table = - new InterpolatingMatrixTreeMap<>(); + // Continuous velocity dynamics + private final Matrix m_A; + private final Matrix m_B; + + // LQR cost matrices + private final Matrix m_Q; + private final Matrix m_R; + + private final double m_dt; private Matrix m_error = new Matrix<>(Nat.N5(), Nat.N1()); private Matrix m_tolerance = new Matrix<>(Nat.N5(), Nat.N1()); - /** States of the drivetrain system. */ - private enum State { - kX(0), - kY(1), - kHeading(2), - kLeftVelocity(3), - kRightVelocity(4); - - public final int value; - - State(int i) { - this.value = i; - } - } - /** * Constructs a linear time-varying differential drive controller. * @@ -75,8 +65,6 @@ public class LTVDifferentialDriveController { * @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. - * @throws IllegalArgumentException if max velocity of plant with 12 V input <= 0 m/s or >= - * 15 m/s. */ public LTVDifferentialDriveController( LinearSystem plant, @@ -85,96 +73,11 @@ public class LTVDifferentialDriveController { Vector relems, 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 = - MatBuilder.fill( - Nat.N5(), - Nat.N5(), - 0.0, - 0.0, - 0.0, - 0.5, - 0.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -1.0 / m_trackwidth, - 1.0 / m_trackwidth, - 0.0, - 0.0, - 0.0, - plant.getA(0, 0), - plant.getA(0, 1), - 0.0, - 0.0, - 0.0, - plant.getA(1, 0), - plant.getA(1, 1)); - var B = - MatBuilder.fill( - Nat.N5(), - Nat.N2(), - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - plant.getB(0, 0), - plant.getB(0, 1), - plant.getB(1, 0), - plant.getB(1, 1)); - var Q = StateSpaceUtil.makeCostMatrix(qelems); - var R = StateSpaceUtil.makeCostMatrix(relems); - - // dx/dt = Ax + Bu - // 0 = Ax + Bu - // Ax = -Bu - // x = -A⁻¹Bu - double maxV = - plant.getA().solve(plant.getB().times(VecBuilder.fill(12.0, 12.0))).times(-1.0).get(0, 0); - - if (maxV <= 0.0) { - throw new IllegalArgumentException( - "Max velocity of plant with 12 V input must be greater than 0 m/s."); - } - if (maxV >= 15.0) { - throw new IllegalArgumentException( - "Max velocity of plant with 12 V input must be less than 15 m/s."); - } - - for (double velocity = -maxV; velocity < maxV; 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) { - A.set(State.kY.value, State.kHeading.value, 1e-4); - } else { - A.set(State.kY.value, State.kHeading.value, velocity); - } - - var discABPair = Discretization.discretizeAB(A, B, dt); - var discA = discABPair.getFirst(); - var discB = discABPair.getSecond(); - - var S = DARE.dareNoPrecond(discA, discB, Q, R); - - // K = (BᵀSB + R)⁻¹BᵀSA - m_table.put( - velocity, - discB - .transpose() - .times(S) - .times(discB) - .plus(R) - .solve(discB.transpose().times(S).times(discA))); - } + m_A = plant.getA(); + m_B = plant.getB(); + m_Q = StateSpaceUtil.makeCostMatrix(qelems); + m_R = StateSpaceUtil.makeCostMatrix(relems); + m_dt = dt; } /** @@ -230,20 +133,21 @@ public class LTVDifferentialDriveController { double leftVelocityRef, double rightVelocityRef) { // This implements the linear time-varying differential drive controller in - // theorem 9.6.3 of https://tavsys.net/controls-in-frc. - var x = - VecBuilder.fill( - currentPose.getX(), - currentPose.getY(), - currentPose.getRotation().getRadians(), - leftVelocity, - rightVelocity); + // theorem 8.7.4 of https://controls-in-frc.link/ + // + // [x ] + // [y ] [Vₗ] + // x = [θ ] u = [Vᵣ] + // [vₗ] + // [vᵣ] - var inRobotFrame = Matrix.eye(Nat.N5()); - inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0))); - inRobotFrame.set(0, 1, Math.sin(x.get(State.kHeading.value, 0))); - inRobotFrame.set(1, 0, -Math.sin(x.get(State.kHeading.value, 0))); - inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0))); + double velocity = (leftVelocity + rightVelocity) / 2.0; + + // 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) { + velocity = 1e-4; + } var r = VecBuilder.fill( @@ -252,12 +156,55 @@ public class LTVDifferentialDriveController { poseRef.getRotation().getRadians(), leftVelocityRef, rightVelocityRef); - m_error = r.minus(x); - m_error.set( - State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0))); + var x = + VecBuilder.fill( + currentPose.getX(), + currentPose.getY(), + currentPose.getRotation().getRadians(), + leftVelocity, + rightVelocity); - double velocity = (leftVelocity + rightVelocity) / 2.0; - var K = m_table.get(velocity); + m_error = r.minus(x); + m_error.set(2, 0, MathUtil.angleModulus(m_error.get(2, 0))); + + // spotless:off + var A = MatBuilder.fill(Nat.N5(), Nat.N5(), + 0.0, 0.0, 0.0, 0.5, 0.5, + 0.0, 0.0, velocity, 0.0, 0.0, + 0.0, 0.0, 0.0, -1.0 / m_trackwidth, 1.0 / m_trackwidth, + 0.0, 0.0, 0.0, m_A.get(0, 0), m_A.get(0, 1), + 0.0, 0.0, 0.0, m_A.get(1, 0), m_A.get(1, 1)); + var B = MatBuilder.fill(Nat.N5(), Nat.N2(), + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + m_B.get(0, 0), m_B.get(0, 1), + m_B.get(1, 0), m_B.get(1, 1)); + // spotless:on + + var discABPair = Discretization.discretizeAB(A, B, m_dt); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + var S = DARE.dareNoPrecond(discA, discB, m_Q, m_R); + + // K = (BᵀSB + R)⁻¹BᵀSA + var K = + discB + .transpose() + .times(S) + .times(discB) + .plus(m_R) + .solve(discB.transpose().times(S).times(discA)); + + // spotless:off + var inRobotFrame = MatBuilder.fill(Nat.N5(), Nat.N5(), + Math.cos(x.get(2, 0)), Math.sin(x.get(2, 0)), 0.0, 0.0, 0.0, + -Math.sin(x.get(2, 0)), Math.cos(x.get(2, 0)), 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0); + // spotless:on var u = K.times(inRobotFrame).times(m_error); 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 8437eafac8..387a013e2a 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 @@ -5,7 +5,6 @@ package edu.wpi.first.math.controller; import edu.wpi.first.math.DARE; -import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -31,27 +30,16 @@ import edu.wpi.first.math.trajectory.Trajectory; * shown in theorem 8.9.1. */ public class LTVUnicycleController { - // LUT from drivetrain linear velocity to LQR gain - private final InterpolatingMatrixTreeMap m_table = - new InterpolatingMatrixTreeMap<>(); + // LQR cost matrices + private Matrix m_Q; + private Matrix m_R; + + private final double m_dt; private Pose2d m_poseError; private Pose2d m_poseTolerance; private boolean m_enabled = true; - /** States of the drivetrain system. */ - private enum State { - kX(0), - kY(1), - kHeading(2); - - public final int value; - - State(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, @@ -60,21 +48,7 @@ public class LTVUnicycleController { * @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. - * @throws IllegalArgumentException if maxVelocity <= 0. - */ - public LTVUnicycleController(double dt, double maxVelocity) { - this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity); + this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt); } /** @@ -89,95 +63,9 @@ public class LTVUnicycleController { * @param dt Discretization timestep in seconds. */ public LTVUnicycleController(Vector qelems, Vector relems, double dt) { - this(qelems, relems, dt, 9.0); - } - - /** - * Constructs a linear time-varying unicycle controller. - * - *

See https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning - * for how to select the tolerances. - * - * @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. - * @throws IllegalArgumentException if maxVelocity <= 0 m/s or >= 15 m/s. - */ - public LTVUnicycleController( - Vector qelems, Vector relems, double dt, double maxVelocity) { - if (maxVelocity <= 0.0) { - throw new IllegalArgumentException("Max velocity must be greater than 0 m/s."); - } - if (maxVelocity >= 15.0) { - throw new IllegalArgumentException("Max velocity must be less than 15 m/s."); - } - - // 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 = MatBuilder.fill(Nat.N3(), Nat.N2(), 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) { - A.set(State.kY.value, State.kHeading.value, 1e-4); - } else { - A.set(State.kY.value, State.kHeading.value, velocity); - } - - var discABPair = Discretization.discretizeAB(A, B, dt); - var discA = discABPair.getFirst(); - var discB = discABPair.getSecond(); - - var S = DARE.dareNoPrecond(discA, discB, Q, R); - - // K = (BᵀSB + R)⁻¹BᵀSA - m_table.put( - velocity, - discB - .transpose() - .times(S) - .times(discB) - .plus(R) - .solve(discB.transpose().times(S).times(discA))); - } + m_Q = StateSpaceUtil.makeCostMatrix(qelems); + m_R = StateSpaceUtil.makeCostMatrix(relems); + m_dt = dt; } /** @@ -218,13 +106,78 @@ public class LTVUnicycleController { */ public ChassisSpeeds calculate( Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) { + // 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] + if (!m_enabled) { return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef); } + // The DARE is ill-conditioned if the velocity is close to zero, so don't + // let the system stop. + if (Math.abs(linearVelocityRef) < 1e-4) { + linearVelocityRef = 1e-4; + } + m_poseError = poseRef.relativeTo(currentPose); - var K = m_table.get(linearVelocityRef); + // spotless:off + var A = MatBuilder.fill(Nat.N3(), Nat.N3(), + 0.0, 0.0, 0.0, + 0.0, 0.0, linearVelocityRef, + 0.0, 0.0, 0.0); + var B = MatBuilder.fill(Nat.N3(), Nat.N2(), + 1.0, 0.0, + 0.0, 0.0, + 0.0, 1.0); + // spotless:on + + var discABPair = Discretization.discretizeAB(A, B, m_dt); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + var S = DARE.dareNoPrecond(discA, discB, m_Q, m_R); + + // K = (BᵀSB + R)⁻¹BᵀSA + var K = + discB + .transpose() + .times(S) + .times(discB) + .plus(m_R) + .solve(discB.transpose().times(S).times(discA)); + var e = MatBuilder.fill( Nat.N3(), diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp index f7e8f2bde7..0ddd2f6e84 100644 --- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp @@ -5,177 +5,77 @@ #include "frc/controller/LTVDifferentialDriveController.h" #include -#include - -#include #include "frc/DARE.h" #include "frc/MathUtil.h" -#include "frc/StateSpaceUtil.h" #include "frc/system/Discretization.h" using namespace frc; -namespace { - -/** - * States of the drivetrain system. - */ -class State { - public: - /// X position in global coordinate frame. - [[maybe_unused]] - static constexpr int kX = 0; - - /// Y position in global coordinate frame. - static constexpr int kY = 1; - - /// Heading in global coordinate frame. - static constexpr int kHeading = 2; - - /// Left encoder velocity. - [[maybe_unused]] - static constexpr int kLeftVelocity = 3; - - /// Right encoder velocity. - [[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}, - {0.0, 0.0, 0.0, -1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}, - {0.0, 0.0, 0.0, plant.A(0, 0), plant.A(0, 1)}, - {0.0, 0.0, 0.0, plant.A(1, 0), plant.A(1, 1)}}; - Matrixd<5, 2> B{{0.0, 0.0}, - {0.0, 0.0}, - {0.0, 0.0}, - {plant.B(0, 0), plant.B(0, 1)}, - {plant.B(1, 0), plant.B(1, 1)}}; - Matrixd<5, 5> Q = frc::MakeCostMatrix(Qelems); - Matrixd<2, 2> R = frc::MakeCostMatrix(Relems); - - // dx/dt = Ax + Bu - // 0 = Ax + Bu - // Ax = -Bu - // x = -A⁻¹Bu - units::meters_per_second_t maxV{ - // NOLINTNEXTLINE(clang-analyzer-unix.Malloc) - -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)}; - - if (maxV <= 0_mps) { - throw std::domain_error( - "Max velocity of plant with 12 V input must be greater than 0 m/s."); - } - if (maxV >= 15_mps) { - throw std::domain_error( - "Max velocity of plant with 12 V input must be less than 15 m/s."); - } - - auto R_llt = R.llt(); - - for (auto velocity = -maxV; velocity < maxV; 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) { - A(State::kY, State::kHeading) = 1e-4; - } else { - A(State::kY, State::kHeading) = velocity.value(); - } - - Matrixd<5, 5> discA; - Matrixd<5, 2> discB; - DiscretizeAB(A, B, dt, &discA, &discB); - - auto S = detail::DARE<5, 2>(discA, discB, Q, R_llt); - - // K = (BᵀSB + R)⁻¹BᵀSA - m_table.insert(velocity, (discB.transpose() * S * discB + R) - .llt() - .solve(discB.transpose() * S * discA)); - } -} - -bool LTVDifferentialDriveController::AtReference() const { - return std::abs(m_error(0)) < m_tolerance(0) && - std::abs(m_error(1)) < m_tolerance(1) && - std::abs(m_error(2)) < m_tolerance(2) && - std::abs(m_error(3)) < m_tolerance(3) && - std::abs(m_error(4)) < m_tolerance(4); -} - -void LTVDifferentialDriveController::SetTolerance( - const Pose2d& poseTolerance, - units::meters_per_second_t leftVelocityTolerance, - units::meters_per_second_t rightVelocityTolerance) { - m_tolerance = - Vectord<5>{poseTolerance.X().value(), poseTolerance.Y().value(), - poseTolerance.Rotation().Radians().value(), - leftVelocityTolerance.value(), rightVelocityTolerance.value()}; -} - DifferentialDriveWheelVoltages LTVDifferentialDriveController::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) { // This implements the linear time-varying differential drive controller in - // theorem 9.6.3 of https://tavsys.net/controls-in-frc. - Vectord<5> x{currentPose.X().value(), currentPose.Y().value(), - currentPose.Rotation().Radians().value(), leftVelocity.value(), - rightVelocity.value()}; - - Matrixd<5, 5> inRobotFrame = Matrixd<5, 5>::Identity(); - inRobotFrame(0, 0) = std::cos(x(State::kHeading)); - inRobotFrame(0, 1) = std::sin(x(State::kHeading)); - inRobotFrame(1, 0) = -std::sin(x(State::kHeading)); - inRobotFrame(1, 1) = std::cos(x(State::kHeading)); - - Vectord<5> r{poseRef.X().value(), poseRef.Y().value(), - poseRef.Rotation().Radians().value(), leftVelocityRef.value(), - rightVelocityRef.value()}; - m_error = r - x; - m_error(State::kHeading) = - frc::AngleModulus(units::radian_t{m_error(State::kHeading)}).value(); + // theorem 8.7.4 of https://controls-in-frc.link/ + // + // [x ] + // [y ] [Vₗ] + // x = [θ ] u = [Vᵣ] + // [vₗ] + // [vᵣ] units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0}; - const auto& K = m_table[velocity]; - Vectord<2> u = K * inRobotFrame * m_error; + // 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) { + velocity = 1e-4_mps; + } + + Eigen::Vector r{poseRef.X().value(), poseRef.Y().value(), + poseRef.Rotation().Radians().value(), + leftVelocityRef.value(), rightVelocityRef.value()}; + Eigen::Vector x{currentPose.X().value(), currentPose.Y().value(), + currentPose.Rotation().Radians().value(), + leftVelocity.value(), rightVelocity.value()}; + + m_error = r - x; + m_error(2) = frc::AngleModulus(units::radian_t{m_error(2)}).value(); + + Eigen::Matrix A{ + {0.0, 0.0, 0.0, 0.5, 0.5}, + {0.0, 0.0, velocity.value(), 0.0, 0.0}, + {0.0, 0.0, 0.0, -1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}, + {0.0, 0.0, 0.0, m_A(0, 0), m_A(0, 1)}, + {0.0, 0.0, 0.0, m_A(1, 0), m_A(1, 1)}}; + Eigen::Matrix B{{0.0, 0.0}, + {0.0, 0.0}, + {0.0, 0.0}, + {m_B(0, 0), m_B(0, 1)}, + {m_B(1, 0), m_B(1, 1)}}; + + Eigen::Matrix discA; + Eigen::Matrix discB; + DiscretizeAB(A, B, m_dt, &discA, &discB); + + auto S = DARE<5, 2>(discA, discB, m_Q, m_R, false).value(); + + // K = (BᵀSB + R)⁻¹BᵀSA + Eigen::Matrix K = (discB.transpose() * S * discB + m_R) + .llt() + .solve(discB.transpose() * S * discA); + + Eigen::Matrix inRobotFrame{ + {std::cos(x(2)), std::sin(x(2)), 0.0, 0.0, 0.0}, + {-std::sin(x(2)), std::cos(x(2)), 0.0, 0.0, 0.0}, + {0.0, 0.0, 1.0, 0.0, 0.0}, + {0.0, 0.0, 0.0, 1.0, 0.0}, + {0.0, 0.0, 0.0, 0.0, 1.0}}; + + Eigen::Vector2d u = K * inRobotFrame * m_error; return DifferentialDriveWheelVoltages{units::volt_t{u(0)}, units::volt_t{u(1)}}; } - -DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate( - const Pose2d& currentPose, units::meters_per_second_t leftVelocity, - units::meters_per_second_t rightVelocity, - const Trajectory::State& desiredState) { - // v = (v_r + v_l) / 2 (1) - // w = (v_r - v_l) / (2r) (2) - // k = w / v (3) - // - // v_l = v - wr - // v_l = v - (vk)r - // v_l = v(1 - kr) - // - // v_r = v + wr - // v_r = v + (vk)r - // v_r = v(1 + kr) - return Calculate( - currentPose, leftVelocity, rightVelocity, desiredState.pose, - desiredState.velocity * - (1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)), - desiredState.velocity * - (1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0))); -} diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp index 0e1f5d6591..b525e256e8 100644 --- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp @@ -4,52 +4,16 @@ #include "frc/controller/LTVUnicycleController.h" -#include - -#include - #include "frc/DARE.h" -#include "frc/StateSpaceUtil.h" #include "frc/system/Discretization.h" #include "units/math.h" using namespace frc; -namespace { - -/** - * States of the drivetrain system. - */ -class State { - public: - /// X position in global coordinate frame. - [[maybe_unused]] - static constexpr int kX = 0; - - /// Y position in global coordinate frame. - static constexpr int kY = 1; - - /// Heading in global coordinate frame. - static constexpr int kHeading = 2; -}; - -} // namespace - -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, units::meters_per_second_t maxVelocity) { - if (maxVelocity <= 0_mps) { - throw std::domain_error("Max velocity must be greater than 0 m/s."); - } - if (maxVelocity >= 15_mps) { - throw std::domain_error("Max velocity must be less than 15 m/s."); - } - +ChassisSpeeds LTVUnicycleController::Calculate( + const Pose2d& currentPose, const Pose2d& poseRef, + units::meters_per_second_t linearVelocityRef, + units::radians_per_second_t angularVelocityRef) { // The change in global pose for a unicycle is defined by the following three // equations. // @@ -82,76 +46,39 @@ LTVUnicycleController::LTVUnicycleController( // [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); - Matrixd<2, 2> R = frc::MakeCostMatrix(Relems); - auto R_llt = R.llt(); - - 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) { - A(State::kY, State::kHeading) = 1e-4; - } else { - A(State::kY, State::kHeading) = velocity.value(); - } - - Matrixd<3, 3> discA; - Matrixd<3, 2> discB; - DiscretizeAB(A, B, dt, &discA, &discB); - - auto S = detail::DARE<3, 2>(discA, discB, Q, R_llt); - - // K = (BᵀSB + R)⁻¹BᵀSA - m_table.insert(velocity, (discB.transpose() * S * discB + R) - .llt() - .solve(discB.transpose() * S * discA)); - } -} - -bool LTVUnicycleController::AtReference() const { - const auto& eTranslate = m_poseError.Translation(); - const auto& eRotate = m_poseError.Rotation(); - const auto& tolTranslate = m_poseTolerance.Translation(); - const auto& tolRotate = m_poseTolerance.Rotation(); - return units::math::abs(eTranslate.X()) < tolTranslate.X() && - units::math::abs(eTranslate.Y()) < tolTranslate.Y() && - units::math::abs(eRotate.Radians()) < tolRotate.Radians(); -} - -void LTVUnicycleController::SetTolerance(const Pose2d& poseTolerance) { - m_poseTolerance = poseTolerance; -} - -ChassisSpeeds LTVUnicycleController::Calculate( - const Pose2d& currentPose, const Pose2d& poseRef, - units::meters_per_second_t linearVelocityRef, - units::radians_per_second_t angularVelocityRef) { if (!m_enabled) { return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef}; } + // The DARE is ill-conditioned if the velocity is close to zero, so don't + // let the system stop. + if (units::math::abs(linearVelocityRef) < 1e-4_mps) { + linearVelocityRef = 1e-4_mps; + } + m_poseError = poseRef.RelativeTo(currentPose); - 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 = K * e; + Eigen::Matrix A{ + {0.0, 0.0, 0.0}, {0.0, 0.0, linearVelocityRef.value()}, {0.0, 0.0, 0.0}}; + constexpr Eigen::Matrix B{{1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}}; + + Eigen::Matrix discA; + Eigen::Matrix discB; + DiscretizeAB(A, B, m_dt, &discA, &discB); + + auto S = DARE<3, 2>(discA, discB, m_Q, m_R, false).value(); + + // K = (BᵀSB + R)⁻¹BᵀSA + Eigen::Matrix K = (discB.transpose() * S * discB + m_R) + .llt() + .solve(discB.transpose() * S * discA); + + Eigen::Vector3d e{m_poseError.X().value(), m_poseError.Y().value(), + m_poseError.Rotation().Radians().value()}; + Eigen::Vector2d u = K * e; return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)}, 0_mps, angularVelocityRef + units::radians_per_second_t{u(1)}}; } - -ChassisSpeeds LTVUnicycleController::Calculate( - const Pose2d& currentPose, const Trajectory::State& desiredState) { - return Calculate(currentPose, desiredState.pose, desiredState.velocity, - desiredState.velocity * desiredState.curvature); -} - -void LTVUnicycleController::SetEnabled(bool enabled) { - m_enabled = enabled; -} diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h index c07c1cef78..be0542413a 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h @@ -4,11 +4,13 @@ #pragma once +#include + +#include #include #include -#include -#include "frc/EigenCore.h" +#include "frc/StateSpaceUtil.h" #include "frc/controller/DifferentialDriveWheelVoltages.h" #include "frc/geometry/Pose2d.h" #include "frc/system/LinearSystem.h" @@ -52,14 +54,18 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { * @param Qelems The maximum desired error tolerance for each state. * @param Relems The maximum desired control effort for each input. * @param dt Discretization timestep. - * @throws std::domain_error if max velocity of plant with 12 V input <= 0 m/s - * or >= 15 m/s. */ LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth, const wpi::array& Qelems, const wpi::array& Relems, - units::second_t dt); + units::second_t dt) + : m_trackwidth{trackwidth}, + m_A{plant.A()}, + m_B{plant.B()}, + m_Q{frc::MakeCostMatrix(Qelems)}, + m_R{frc::MakeCostMatrix(Relems)}, + m_dt{dt} {} /** * Move constructor. @@ -75,7 +81,13 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { /** * Returns true if the pose error is within tolerance of the reference. */ - bool AtReference() const; + bool AtReference() const { + return std::abs(m_error(0)) < m_tolerance(0) && + std::abs(m_error(1)) < m_tolerance(1) && + std::abs(m_error(2)) < m_tolerance(2) && + std::abs(m_error(3)) < m_tolerance(3) && + std::abs(m_error(4)) < m_tolerance(4); + } /** * Sets the pose error which is considered tolerable for use with @@ -87,7 +99,12 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { */ void SetTolerance(const Pose2d& poseTolerance, units::meters_per_second_t leftVelocityTolerance, - units::meters_per_second_t rightVelocityTolerance); + units::meters_per_second_t rightVelocityTolerance) { + m_tolerance = Eigen::Vector{ + poseTolerance.X().value(), poseTolerance.Y().value(), + poseTolerance.Rotation().Radians().value(), + leftVelocityTolerance.value(), rightVelocityTolerance.value()}; + } /** * Returns the left and right output voltages of the LTV controller. @@ -123,16 +140,41 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { DifferentialDriveWheelVoltages Calculate( const Pose2d& currentPose, units::meters_per_second_t leftVelocity, units::meters_per_second_t rightVelocity, - const Trajectory::State& desiredState); + const Trajectory::State& desiredState) { + // v = (v_r + v_l) / 2 (1) + // w = (v_r - v_l) / (2r) (2) + // k = w / v (3) + // + // v_l = v - wr + // v_l = v - (vk)r + // v_l = v(1 - kr) + // + // v_r = v + wr + // v_r = v + (vk)r + // v_r = v(1 + kr) + return Calculate( + currentPose, leftVelocity, rightVelocity, desiredState.pose, + desiredState.velocity * + (1 - (desiredState.curvature / 1_rad * m_trackwidth / 2.0)), + desiredState.velocity * + (1 + (desiredState.curvature / 1_rad * m_trackwidth / 2.0))); + } private: units::meter_t m_trackwidth; - // LUT from drivetrain linear velocity to LQR gain - wpi::interpolating_map> m_table; + // Continuous velocity dynamics + Eigen::Matrix m_A; + Eigen::Matrix m_B; - Vectord<5> m_error; - Vectord<5> m_tolerance; + // LQR cost matrices + Eigen::Matrix m_Q; + Eigen::Matrix m_R; + + units::second_t m_dt; + + Eigen::Vector m_error; + Eigen::Vector m_tolerance; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h index 02f5271ad3..ae60ed0818 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -4,15 +4,16 @@ #pragma once +#include #include #include -#include -#include "frc/EigenCore.h" +#include "frc/StateSpaceUtil.h" #include "frc/geometry/Pose2d.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/trajectory/Trajectory.h" #include "units/angular_velocity.h" +#include "units/math.h" #include "units/time.h" #include "units/velocity.h" @@ -37,12 +38,9 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * 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. - * @throws std::domain_error if maxVelocity <= 0. */ - explicit LTVUnicycleController( - units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps); + explicit LTVUnicycleController(units::second_t dt) + : LTVUnicycleController{{0.0625, 0.125, 2.0}, {1.0, 2.0}, dt} {} /** * Constructs a linear time-varying unicycle controller. @@ -54,13 +52,12 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * @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. - * @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s. */ LTVUnicycleController(const wpi::array& Qelems, - const wpi::array& Relems, units::second_t dt, - units::meters_per_second_t maxVelocity = 9_mps); + const wpi::array& Relems, units::second_t dt) + : m_Q{frc::MakeCostMatrix(Qelems)}, + m_R{frc::MakeCostMatrix(Relems)}, + m_dt{dt} {} /** * Move constructor. @@ -75,7 +72,15 @@ class WPILIB_DLLEXPORT LTVUnicycleController { /** * Returns true if the pose error is within tolerance of the reference. */ - bool AtReference() const; + bool AtReference() const { + const auto& eTranslate = m_poseError.Translation(); + const auto& eRotate = m_poseError.Rotation(); + const auto& tolTranslate = m_poseTolerance.Translation(); + const auto& tolRotate = m_poseTolerance.Rotation(); + return units::math::abs(eTranslate.X()) < tolTranslate.X() && + units::math::abs(eTranslate.Y()) < tolTranslate.Y() && + units::math::abs(eRotate.Radians()) < tolRotate.Radians(); + } /** * Sets the pose error which is considered tolerable for use with @@ -83,7 +88,9 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * * @param poseTolerance Pose error which is tolerable. */ - void SetTolerance(const Pose2d& poseTolerance); + void SetTolerance(const Pose2d& poseTolerance) { + m_poseTolerance = poseTolerance; + } /** * Returns the linear and angular velocity outputs of the LTV controller. @@ -111,18 +118,24 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * from a trajectory. */ ChassisSpeeds Calculate(const Pose2d& currentPose, - const Trajectory::State& desiredState); + const Trajectory::State& desiredState) { + return Calculate(currentPose, desiredState.pose, desiredState.velocity, + desiredState.velocity * desiredState.curvature); + } /** * Enables and disables the controller for troubleshooting purposes. * * @param enabled If the controller is enabled or not. */ - void SetEnabled(bool enabled); + void SetEnabled(bool enabled) { m_enabled = enabled; } private: - // LUT from drivetrain linear velocity to LQR gain - wpi::interpolating_map> m_table; + // LQR cost matrices + Eigen::Matrix m_Q; + Eigen::Matrix m_R; + + units::second_t m_dt; Pose2d m_poseError; Pose2d m_poseTolerance;