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}