001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.controller;
006
007import edu.wpi.first.math.DARE;
008import edu.wpi.first.math.MatBuilder;
009import edu.wpi.first.math.MathUtil;
010import edu.wpi.first.math.Matrix;
011import edu.wpi.first.math.Nat;
012import edu.wpi.first.math.StateSpaceUtil;
013import edu.wpi.first.math.VecBuilder;
014import edu.wpi.first.math.Vector;
015import edu.wpi.first.math.geometry.Pose2d;
016import edu.wpi.first.math.numbers.N1;
017import edu.wpi.first.math.numbers.N2;
018import edu.wpi.first.math.numbers.N5;
019import edu.wpi.first.math.system.Discretization;
020import edu.wpi.first.math.system.LinearSystem;
021import edu.wpi.first.math.trajectory.Trajectory;
022
023/**
024 * The linear time-varying differential drive controller has a similar form to the LQR, but the
025 * model used to compute the controller gain is the nonlinear differential drive model linearized
026 * around the drivetrain's current state. We precompute gains for important places in our
027 * state-space, then interpolate between them with a lookup table to save computational resources.
028 *
029 * <p>This controller has a flat hierarchy with pose and wheel velocity references and voltage
030 * outputs. This is different from a unicycle controller's nested hierarchy where the top-level
031 * controller has a pose reference and chassis velocity command outputs, and the low-level
032 * controller has wheel velocity references and voltage outputs. Flat hierarchies are easier to tune
033 * in one shot.
034 *
035 * <p>See section 8.7 in Controls Engineering in FRC for a derivation of the control law we used
036 * shown in theorem 8.7.4.
037 */
038public class LTVDifferentialDriveController {
039  private final double m_trackwidth;
040
041  // Continuous velocity dynamics
042  private final Matrix<N2, N2> m_A;
043  private final Matrix<N2, N2> m_B;
044
045  // LQR cost matrices
046  private final Matrix<N5, N5> m_Q;
047  private final Matrix<N2, N2> m_R;
048
049  private final double m_dt;
050
051  private Matrix<N5, N1> m_error = new Matrix<>(Nat.N5(), Nat.N1());
052  private Matrix<N5, N1> m_tolerance = new Matrix<>(Nat.N5(), Nat.N1());
053
054  /**
055   * Constructs a linear time-varying differential drive controller.
056   *
057   * <p>See <a
058   * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning</a>
059   * for how to select the tolerances.
060   *
061   * @param plant The differential drive velocity plant.
062   * @param trackwidth The distance between the differential drive's left and right wheels in
063   *     meters.
064   * @param qelems The maximum desired error tolerance for each state.
065   * @param relems The maximum desired control effort for each input.
066   * @param dt Discretization timestep in seconds.
067   */
068  public LTVDifferentialDriveController(
069      LinearSystem<N2, N2, N2> plant,
070      double trackwidth,
071      Vector<N5> qelems,
072      Vector<N2> relems,
073      double dt) {
074    m_trackwidth = trackwidth;
075    m_A = plant.getA();
076    m_B = plant.getB();
077    m_Q = StateSpaceUtil.makeCostMatrix(qelems);
078    m_R = StateSpaceUtil.makeCostMatrix(relems);
079    m_dt = dt;
080  }
081
082  /**
083   * Returns true if the pose error is within tolerance of the reference.
084   *
085   * @return True if the pose error is within tolerance of the reference.
086   */
087  public boolean atReference() {
088    return Math.abs(m_error.get(0, 0)) < m_tolerance.get(0, 0)
089        && Math.abs(m_error.get(1, 0)) < m_tolerance.get(1, 0)
090        && Math.abs(m_error.get(2, 0)) < m_tolerance.get(2, 0)
091        && Math.abs(m_error.get(3, 0)) < m_tolerance.get(3, 0)
092        && Math.abs(m_error.get(4, 0)) < m_tolerance.get(4, 0);
093  }
094
095  /**
096   * Sets the pose error which is considered tolerable for use with atReference().
097   *
098   * @param poseTolerance Pose error which is tolerable.
099   * @param leftVelocityTolerance Left velocity error which is tolerable in meters per second.
100   * @param rightVelocityTolerance Right velocity error which is tolerable in meters per second.
101   */
102  public void setTolerance(
103      Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) {
104    m_tolerance =
105        VecBuilder.fill(
106            poseTolerance.getX(),
107            poseTolerance.getY(),
108            poseTolerance.getRotation().getRadians(),
109            leftVelocityTolerance,
110            rightVelocityTolerance);
111  }
112
113  /**
114   * Returns the left and right output voltages of the LTV controller.
115   *
116   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
117   * trajectory.
118   *
119   * @param currentPose The current pose.
120   * @param leftVelocity The current left velocity in meters per second.
121   * @param rightVelocity The current right velocity in meters per second.
122   * @param poseRef The desired pose.
123   * @param leftVelocityRef The desired left velocity in meters per second.
124   * @param rightVelocityRef The desired right velocity in meters per second.
125   * @return Left and right output voltages of the LTV controller.
126   */
127  public DifferentialDriveWheelVoltages calculate(
128      Pose2d currentPose,
129      double leftVelocity,
130      double rightVelocity,
131      Pose2d poseRef,
132      double leftVelocityRef,
133      double rightVelocityRef) {
134    // This implements the linear time-varying differential drive controller in
135    // theorem 8.7.4 of https://controls-in-frc.link/
136    //
137    //     [x ]
138    //     [y ]       [Vₗ]
139    // x = [θ ]   u = [Vᵣ]
140    //     [vₗ]
141    //     [vᵣ]
142
143    double velocity = (leftVelocity + rightVelocity) / 2.0;
144
145    // The DARE is ill-conditioned if the velocity is close to zero, so don't
146    // let the system stop.
147    if (Math.abs(velocity) < 1e-4) {
148      velocity = 1e-4;
149    }
150
151    var r =
152        VecBuilder.fill(
153            poseRef.getX(),
154            poseRef.getY(),
155            poseRef.getRotation().getRadians(),
156            leftVelocityRef,
157            rightVelocityRef);
158    var x =
159        VecBuilder.fill(
160            currentPose.getX(),
161            currentPose.getY(),
162            currentPose.getRotation().getRadians(),
163            leftVelocity,
164            rightVelocity);
165
166    m_error = r.minus(x);
167    m_error.set(2, 0, MathUtil.angleModulus(m_error.get(2, 0)));
168
169    // spotless:off
170    var A = MatBuilder.fill(Nat.N5(), Nat.N5(),
171        0.0, 0.0, 0.0, 0.5, 0.5,
172        0.0, 0.0, velocity, 0.0, 0.0,
173        0.0, 0.0, 0.0, -1.0 / m_trackwidth, 1.0 / m_trackwidth,
174        0.0, 0.0, 0.0, m_A.get(0, 0), m_A.get(0, 1),
175        0.0, 0.0, 0.0, m_A.get(1, 0), m_A.get(1, 1));
176    var B = MatBuilder.fill(Nat.N5(), Nat.N2(),
177        0.0, 0.0,
178        0.0, 0.0,
179        0.0, 0.0,
180        m_B.get(0, 0), m_B.get(0, 1),
181        m_B.get(1, 0), m_B.get(1, 1));
182    // spotless:on
183
184    var discABPair = Discretization.discretizeAB(A, B, m_dt);
185    var discA = discABPair.getFirst();
186    var discB = discABPair.getSecond();
187
188    var S = DARE.dareNoPrecond(discA, discB, m_Q, m_R);
189
190    // K = (BᵀSB + R)⁻¹BᵀSA
191    var K =
192        discB
193            .transpose()
194            .times(S)
195            .times(discB)
196            .plus(m_R)
197            .solve(discB.transpose().times(S).times(discA));
198
199    // spotless:off
200    var inRobotFrame = MatBuilder.fill(Nat.N5(), Nat.N5(),
201        Math.cos(x.get(2, 0)), Math.sin(x.get(2, 0)), 0.0, 0.0, 0.0,
202        -Math.sin(x.get(2, 0)), Math.cos(x.get(2, 0)), 0.0, 0.0, 0.0,
203        0.0, 0.0, 1.0, 0.0, 0.0,
204        0.0, 0.0, 0.0, 1.0, 0.0,
205        0.0, 0.0, 0.0, 0.0, 1.0);
206    // spotless:on
207
208    var u = K.times(inRobotFrame).times(m_error);
209
210    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
211  }
212
213  /**
214   * Returns the left and right output voltages of the LTV controller.
215   *
216   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
217   * trajectory.
218   *
219   * @param currentPose The current pose.
220   * @param leftVelocity The left velocity in meters per second.
221   * @param rightVelocity The right velocity in meters per second.
222   * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
223   * @return The left and right output voltages of the LTV controller.
224   */
225  public DifferentialDriveWheelVoltages calculate(
226      Pose2d currentPose,
227      double leftVelocity,
228      double rightVelocity,
229      Trajectory.State desiredState) {
230    // v = (v_r + v_l) / 2     (1)
231    // w = (v_r - v_l) / (2r)  (2)
232    // k = w / v               (3)
233    //
234    // v_l = v - wr
235    // v_l = v - (vk)r
236    // v_l = v(1 - kr)
237    //
238    // v_r = v + wr
239    // v_r = v + (vk)r
240    // v_r = v(1 + kr)
241    return calculate(
242        currentPose,
243        leftVelocity,
244        rightVelocity,
245        desiredState.pose,
246        desiredState.velocity * (1 - (desiredState.curvature * m_trackwidth / 2.0)),
247        desiredState.velocity * (1 + (desiredState.curvature * m_trackwidth / 2.0)));
248  }
249}