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.InterpolatingMatrixTreeMap;
008import edu.wpi.first.math.MatBuilder;
009import edu.wpi.first.math.Matrix;
010import edu.wpi.first.math.Nat;
011import edu.wpi.first.math.StateSpaceUtil;
012import edu.wpi.first.math.VecBuilder;
013import edu.wpi.first.math.Vector;
014import edu.wpi.first.math.geometry.Pose2d;
015import edu.wpi.first.math.kinematics.ChassisSpeeds;
016import edu.wpi.first.math.numbers.N2;
017import edu.wpi.first.math.numbers.N3;
018import edu.wpi.first.math.trajectory.Trajectory;
019
020/**
021 * The linear time-varying unicycle controller has a similar form to the LQR, but the model used to
022 * compute the controller gain is the nonlinear model linearized around the drivetrain's current
023 * state.
024 *
025 * <p>See section 8.9 in Controls Engineering in FRC for a derivation of the control law we used
026 * shown in theorem 8.9.1.
027 */
028public class LTVUnicycleController {
029  // LUT from drivetrain linear velocity to LQR gain
030  private final InterpolatingMatrixTreeMap<Double, N2, N3> m_table =
031      new InterpolatingMatrixTreeMap<>();
032
033  private Pose2d m_poseError;
034  private Pose2d m_poseTolerance;
035  private boolean m_enabled = true;
036
037  /** States of the drivetrain system. */
038  private enum State {
039    kX(0),
040    kY(1),
041    kHeading(2);
042
043    public final int value;
044
045    State(int i) {
046      this.value = i;
047    }
048  }
049
050  /**
051   * Constructs a linear time-varying unicycle controller with default maximum desired error
052   * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s,
053   * 2 rad/s).
054   *
055   * @param dt Discretization timestep in seconds.
056   */
057  public LTVUnicycleController(double dt) {
058    this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, 9.0);
059  }
060
061  /**
062   * Constructs a linear time-varying unicycle controller with default maximum desired error
063   * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s,
064   * 2 rad/s).
065   *
066   * @param dt Discretization timestep in seconds.
067   * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
068   *     table. The default is 9 m/s.
069   * @throws IllegalArgumentException if maxVelocity &lt;= 0.
070   */
071  public LTVUnicycleController(double dt, double maxVelocity) {
072    this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
073  }
074
075  /**
076   * Constructs a linear time-varying unicycle controller.
077   *
078   * @param qelems The maximum desired error tolerance for each state.
079   * @param relems The maximum desired control effort for each input.
080   * @param dt Discretization timestep in seconds.
081   */
082  public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
083    this(qelems, relems, dt, 9.0);
084  }
085
086  /**
087   * Constructs a linear time-varying unicycle controller.
088   *
089   * @param qelems The maximum desired error tolerance for each state.
090   * @param relems The maximum desired control effort for each input.
091   * @param dt Discretization timestep in seconds.
092   * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup
093   *     table. The default is 9 m/s.
094   * @throws IllegalArgumentException if maxVelocity &lt;= 0.
095   */
096  public LTVUnicycleController(
097      Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
098    if (maxVelocity <= 0.0) {
099      throw new IllegalArgumentException("Max velocity must be greater than zero.");
100    }
101
102    // The change in global pose for a unicycle is defined by the following
103    // three equations.
104    //
105    // ẋ = v cosθ
106    // ẏ = v sinθ
107    // θ̇ = ω
108    //
109    // Here's the model as a vector function where x = [x  y  θ]ᵀ and
110    // u = [v  ω]ᵀ.
111    //
112    //           [v cosθ]
113    // f(x, u) = [v sinθ]
114    //           [  ω   ]
115    //
116    // To create an LQR, we need to linearize this.
117    //
118    //               [0  0  −v sinθ]                  [cosθ  0]
119    // ∂f(x, u)/∂x = [0  0   v cosθ]    ∂f(x, u)/∂u = [sinθ  0]
120    //               [0  0     0   ]                  [ 0    1]
121    //
122    // We're going to make a cross-track error controller, so we'll apply a
123    // clockwise rotation matrix to the global tracking error to transform it
124    // into the robot's coordinate frame. Since the cross-track error is always
125    // measured from the robot's coordinate frame, the model used to compute the
126    // LQR should be linearized around θ = 0 at all times.
127    //
128    //     [0  0  −v sin0]        [cos0  0]
129    // A = [0  0   v cos0]    B = [sin0  0]
130    //     [0  0     0   ]        [ 0    1]
131    //
132    //     [0  0  0]              [1  0]
133    // A = [0  0  v]          B = [0  0]
134    //     [0  0  0]              [0  1]
135    var A = new Matrix<>(Nat.N3(), Nat.N3());
136    var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
137    var Q = StateSpaceUtil.makeCostMatrix(qelems);
138    var R = StateSpaceUtil.makeCostMatrix(relems);
139
140    for (double velocity = -maxVelocity; velocity < maxVelocity; velocity += 0.01) {
141      // The DARE is ill-conditioned if the velocity is close to zero, so don't
142      // let the system stop.
143      if (Math.abs(velocity) < 1e-4) {
144        m_table.put(velocity, new Matrix<>(Nat.N2(), Nat.N3()));
145      } else {
146        A.set(State.kY.value, State.kHeading.value, velocity);
147        m_table.put(velocity, new LinearQuadraticRegulator<N3, N2, N3>(A, B, Q, R, dt).getK());
148      }
149    }
150  }
151
152  /**
153   * Returns true if the pose error is within tolerance of the reference.
154   *
155   * @return True if the pose error is within tolerance of the reference.
156   */
157  public boolean atReference() {
158    final var eTranslate = m_poseError.getTranslation();
159    final var eRotate = m_poseError.getRotation();
160    final var tolTranslate = m_poseTolerance.getTranslation();
161    final var tolRotate = m_poseTolerance.getRotation();
162    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
163        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
164        && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
165  }
166
167  /**
168   * Sets the pose error which is considered tolerable for use with atReference().
169   *
170   * @param poseTolerance Pose error which is tolerable.
171   */
172  public void setTolerance(Pose2d poseTolerance) {
173    m_poseTolerance = poseTolerance;
174  }
175
176  /**
177   * Returns the linear and angular velocity outputs of the LTV controller.
178   *
179   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
180   * trajectory.
181   *
182   * @param currentPose The current pose.
183   * @param poseRef The desired pose.
184   * @param linearVelocityRef The desired linear velocity in meters per second.
185   * @param angularVelocityRef The desired angular velocity in radians per second.
186   * @return The linear and angular velocity outputs of the LTV controller.
187   */
188  public ChassisSpeeds calculate(
189      Pose2d currentPose, Pose2d poseRef, double linearVelocityRef, double angularVelocityRef) {
190    if (!m_enabled) {
191      return new ChassisSpeeds(linearVelocityRef, 0.0, angularVelocityRef);
192    }
193
194    m_poseError = poseRef.relativeTo(currentPose);
195
196    var K = m_table.get(linearVelocityRef);
197    var e =
198        new MatBuilder<>(Nat.N3(), Nat.N1())
199            .fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians());
200    var u = K.times(e);
201
202    return new ChassisSpeeds(
203        linearVelocityRef + u.get(0, 0), 0.0, angularVelocityRef + u.get(1, 0));
204  }
205
206  /**
207   * Returns the next output of the LTV controller.
208   *
209   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
210   * trajectory.
211   *
212   * @param currentPose The current pose.
213   * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
214   * @return The linear and angular velocity outputs of the LTV controller.
215   */
216  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
217    return calculate(
218        currentPose,
219        desiredState.poseMeters,
220        desiredState.velocityMetersPerSecond,
221        desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
222  }
223
224  /**
225   * Enables and disables the controller for troubleshooting purposes.
226   *
227   * @param enabled If the controller is enabled or not.
228   */
229  public void setEnabled(boolean enabled) {
230    m_enabled = enabled;
231  }
232}