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.system.plant; 006 007import edu.wpi.first.math.MatBuilder; 008import edu.wpi.first.math.Matrix; 009import edu.wpi.first.math.Nat; 010import edu.wpi.first.math.VecBuilder; 011import edu.wpi.first.math.numbers.N1; 012import edu.wpi.first.math.numbers.N2; 013import edu.wpi.first.math.system.LinearSystem; 014 015/** Linear system ID utility functions. */ 016public final class LinearSystemId { 017 private LinearSystemId() { 018 // Utility class 019 } 020 021 /** 022 * Create a state-space model of an elevator system. The states of the system are [position, 023 * velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ. 024 * 025 * @param motor The motor (or gearbox) attached to the carriage. 026 * @param mass The mass of the elevator carriage, in kilograms. 027 * @param radius The radius of the elevator's driving drum, in meters. 028 * @param gearing The reduction between motor and drum, as a ratio of output to input. 029 * @return A LinearSystem representing the given characterized constants. 030 * @throws IllegalArgumentException if mass <= 0, radius <= 0, or gearing <= 0. 031 */ 032 public static LinearSystem<N2, N1, N2> createElevatorSystem( 033 DCMotor motor, double mass, double radius, double gearing) { 034 if (mass <= 0.0) { 035 throw new IllegalArgumentException("mass must be greater than zero."); 036 } 037 if (radius <= 0.0) { 038 throw new IllegalArgumentException("radius must be greater than zero."); 039 } 040 if (gearing <= 0) { 041 throw new IllegalArgumentException("gearing must be greater than zero."); 042 } 043 044 return new LinearSystem<>( 045 MatBuilder.fill( 046 Nat.N2(), 047 Nat.N2(), 048 0, 049 1, 050 0, 051 -Math.pow(gearing, 2) * motor.Kt / (motor.R * radius * radius * mass * motor.Kv)), 052 VecBuilder.fill(0, gearing * motor.Kt / (motor.R * radius * mass)), 053 Matrix.eye(Nat.N2()), 054 new Matrix<>(Nat.N2(), Nat.N1())); 055 } 056 057 /** 058 * Create a state-space model of a flywheel system. The states of the system are [angular 059 * velocity], inputs are [voltage], and outputs are [angular velocity]. 060 * 061 * @param motor The motor (or gearbox) attached to the flywheel. 062 * @param J The moment of inertia J of the flywheel in kg-m². 063 * @param gearing The reduction between motor and drum, as a ratio of output to input. 064 * @return A LinearSystem representing the given characterized constants. 065 * @throws IllegalArgumentException if J <= 0 or gearing <= 0. 066 */ 067 public static LinearSystem<N1, N1, N1> createFlywheelSystem( 068 DCMotor motor, double J, double gearing) { 069 if (J <= 0.0) { 070 throw new IllegalArgumentException("J must be greater than zero."); 071 } 072 if (gearing <= 0.0) { 073 throw new IllegalArgumentException("gearing must be greater than zero."); 074 } 075 076 return new LinearSystem<>( 077 VecBuilder.fill(-gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)), 078 VecBuilder.fill(gearing * motor.Kt / (motor.R * J)), 079 Matrix.eye(Nat.N1()), 080 new Matrix<>(Nat.N1(), Nat.N1())); 081 } 082 083 /** 084 * Create a state-space model of a DC motor system. The states of the system are [angular 085 * position, angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position, angular 086 * velocity]ᵀ. 087 * 088 * @param motor The motor (or gearbox) attached to system. 089 * @param J The moment of inertia J of the DC motor in kg-m². 090 * @param gearing The reduction between motor and drum, as a ratio of output to input. 091 * @return A LinearSystem representing the given characterized constants. 092 * @throws IllegalArgumentException if J <= 0 or gearing <= 0. 093 */ 094 public static LinearSystem<N2, N1, N2> createDCMotorSystem( 095 DCMotor motor, double J, double gearing) { 096 if (J <= 0.0) { 097 throw new IllegalArgumentException("J must be greater than zero."); 098 } 099 if (gearing <= 0.0) { 100 throw new IllegalArgumentException("gearing must be greater than zero."); 101 } 102 103 return new LinearSystem<>( 104 MatBuilder.fill( 105 Nat.N2(), Nat.N2(), 0, 1, 0, -gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)), 106 VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)), 107 Matrix.eye(Nat.N2()), 108 new Matrix<>(Nat.N2(), Nat.N1())); 109 } 110 111 /** 112 * Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA 113 * (volts/(unit/sec²)). These constants can be found using SysId. the states of the system are 114 * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position]. 115 * 116 * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the 117 * {@link edu.wpi.first.math.util.Units} class for converting between unit types. 118 * 119 * <p>The parameters provided by the user are from this feedforward model: 120 * 121 * <p>u = K_v v + K_a a 122 * 123 * @param kV The velocity gain, in volts/(unit/sec) 124 * @param kA The acceleration gain, in volts/(unit/sec²) 125 * @return A LinearSystem representing the given characterized constants. 126 * @throws IllegalArgumentException if kV < 0 or kA <= 0. 127 * @see <a 128 * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a> 129 */ 130 public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) { 131 if (kV < 0.0) { 132 throw new IllegalArgumentException("Kv must be greater than or equal to zero."); 133 } 134 if (kA <= 0.0) { 135 throw new IllegalArgumentException("Ka must be greater than zero."); 136 } 137 138 return new LinearSystem<>( 139 MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA), 140 VecBuilder.fill(0, 1 / kA), 141 Matrix.eye(Nat.N2()), 142 new Matrix<>(Nat.N2(), Nat.N1())); 143 } 144 145 /** 146 * Create a state-space model of a differential drive drivetrain. In this model, the states are 147 * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are 148 * [left velocity, right velocity]ᵀ. 149 * 150 * @param motor The motor (or gearbox) driving the drivetrain. 151 * @param mass The mass of the robot in kilograms. 152 * @param r The radius of the wheels in meters. 153 * @param rb The radius of the base (half the trackwidth) in meters. 154 * @param J The moment of inertia of the robot in kg-m². 155 * @param gearing The gearing reduction as output over input. 156 * @return A LinearSystem representing a differential drivetrain. 157 * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or gearing 158 * <= 0. 159 */ 160 public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem( 161 DCMotor motor, double mass, double r, double rb, double J, double gearing) { 162 if (mass <= 0.0) { 163 throw new IllegalArgumentException("mass must be greater than zero."); 164 } 165 if (r <= 0.0) { 166 throw new IllegalArgumentException("r must be greater than zero."); 167 } 168 if (rb <= 0.0) { 169 throw new IllegalArgumentException("rb must be greater than zero."); 170 } 171 if (J <= 0.0) { 172 throw new IllegalArgumentException("J must be greater than zero."); 173 } 174 if (gearing <= 0.0) { 175 throw new IllegalArgumentException("gearing must be greater than zero."); 176 } 177 178 var C1 = -(gearing * gearing) * motor.Kt / (motor.Kv * motor.R * r * r); 179 var C2 = gearing * motor.Kt / (motor.R * r); 180 181 final double C3 = 1 / mass + rb * rb / J; 182 final double C4 = 1 / mass - rb * rb / J; 183 var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1); 184 var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2); 185 var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); 186 var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0); 187 188 return new LinearSystem<>(A, B, C, D); 189 } 190 191 /** 192 * Create a state-space model of a single jointed arm system. The states of the system are [angle, 193 * angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ. 194 * 195 * @param motor The motor (or gearbox) attached to the arm. 196 * @param J The moment of inertia J of the arm in kg-m². 197 * @param gearing The gearing between the motor and arm, in output over input. Most of the time 198 * this will be greater than 1. 199 * @return A LinearSystem representing the given characterized constants. 200 */ 201 public static LinearSystem<N2, N1, N2> createSingleJointedArmSystem( 202 DCMotor motor, double J, double gearing) { 203 if (J <= 0.0) { 204 throw new IllegalArgumentException("J must be greater than zero."); 205 } 206 if (gearing <= 0.0) { 207 throw new IllegalArgumentException("gearing must be greater than zero."); 208 } 209 210 return new LinearSystem<>( 211 MatBuilder.fill( 212 Nat.N2(), 213 Nat.N2(), 214 0, 215 1, 216 0, 217 -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)), 218 VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)), 219 Matrix.eye(Nat.N2()), 220 new Matrix<>(Nat.N2(), Nat.N1())); 221 } 222 223 /** 224 * Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA 225 * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are 226 * [velocity], inputs are [voltage], and outputs are [velocity]. 227 * 228 * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the 229 * {@link edu.wpi.first.math.util.Units} class for converting between unit types. 230 * 231 * <p>The parameters provided by the user are from this feedforward model: 232 * 233 * <p>u = K_v v + K_a a 234 * 235 * @param kV The velocity gain, in volts/(unit/sec) 236 * @param kA The acceleration gain, in volts/(unit/sec²) 237 * @return A LinearSystem representing the given characterized constants. 238 * @throws IllegalArgumentException if kV < 0 or kA <= 0. 239 * @see <a 240 * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a> 241 */ 242 public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) { 243 if (kV < 0.0) { 244 throw new IllegalArgumentException("Kv must be greater than or equal to zero."); 245 } 246 if (kA <= 0.0) { 247 throw new IllegalArgumentException("Ka must be greater than zero."); 248 } 249 250 return new LinearSystem<>( 251 VecBuilder.fill(-kV / kA), 252 VecBuilder.fill(1.0 / kA), 253 VecBuilder.fill(1.0), 254 VecBuilder.fill(0.0)); 255 } 256 257 /** 258 * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA 259 * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are 260 * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ. 261 * 262 * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the 263 * {@link edu.wpi.first.math.util.Units} class for converting between unit types. 264 * 265 * <p>The parameters provided by the user are from this feedforward model: 266 * 267 * <p>u = K_v v + K_a a 268 * 269 * @param kV The velocity gain, in volts/(unit/sec) 270 * @param kA The acceleration gain, in volts/(unit/sec²) 271 * @return A LinearSystem representing the given characterized constants. 272 * @throws IllegalArgumentException if kV < 0 or kA <= 0. 273 * @see <a 274 * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a> 275 */ 276 public static LinearSystem<N2, N1, N2> identifyPositionSystem(double kV, double kA) { 277 if (kV < 0.0) { 278 throw new IllegalArgumentException("Kv must be greater than or equal to zero."); 279 } 280 if (kA <= 0.0) { 281 throw new IllegalArgumentException("Ka must be greater than zero."); 282 } 283 284 return new LinearSystem<>( 285 MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA), 286 VecBuilder.fill(0.0, 1.0 / kA), 287 Matrix.eye(Nat.N2()), 288 new Matrix<>(Nat.N2(), Nat.N1())); 289 } 290 291 /** 292 * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear 293 * {(volts/(meter/sec), (volts/(meter/sec²))} and angular {(volts/(radian/sec)), 294 * (volts/(radian/sec²))} cases. These constants can be found using SysId. 295 * 296 * <p>States: [[left velocity], [right velocity]]<br> 297 * Inputs: [[left voltage], [right voltage]]<br> 298 * Outputs: [[left velocity], [right velocity]] 299 * 300 * @param kVLinear The linear velocity gain in volts per (meters per second). 301 * @param kALinear The linear acceleration gain in volts per (meters per second squared). 302 * @param kVAngular The angular velocity gain in volts per (meters per second). 303 * @param kAAngular The angular acceleration gain in volts per (meters per second squared). 304 * @return A LinearSystem representing the given characterized constants. 305 * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or 306 * kAAngular <= 0. 307 * @see <a 308 * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a> 309 */ 310 public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem( 311 double kVLinear, double kALinear, double kVAngular, double kAAngular) { 312 if (kVLinear <= 0.0) { 313 throw new IllegalArgumentException("Kv,linear must be greater than zero."); 314 } 315 if (kALinear <= 0.0) { 316 throw new IllegalArgumentException("Ka,linear must be greater than zero."); 317 } 318 if (kVAngular <= 0.0) { 319 throw new IllegalArgumentException("Kv,angular must be greater than zero."); 320 } 321 if (kAAngular <= 0.0) { 322 throw new IllegalArgumentException("Ka,angular must be greater than zero."); 323 } 324 325 final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular); 326 final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular); 327 final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular); 328 final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular); 329 330 return new LinearSystem<>( 331 MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1), 332 MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1), 333 MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1), 334 MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0)); 335 } 336 337 /** 338 * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear 339 * {(volts/(meter/sec)), (volts/(meter/sec²))} and angular {(volts/(radian/sec)), 340 * (volts/(radian/sec²))} cases. This can be found using SysId. 341 * 342 * <p>States: [[left velocity], [right velocity]]<br> 343 * Inputs: [[left voltage], [right voltage]]<br> 344 * Outputs: [[left velocity], [right velocity]] 345 * 346 * @param kVLinear The linear velocity gain in volts per (meters per second). 347 * @param kALinear The linear acceleration gain in volts per (meters per second squared). 348 * @param kVAngular The angular velocity gain in volts per (radians per second). 349 * @param kAAngular The angular acceleration gain in volts per (radians per second squared). 350 * @param trackwidth The distance between the differential drive's left and right wheels, in 351 * meters. 352 * @return A LinearSystem representing the given characterized constants. 353 * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, 354 * kAAngular <= 0, or trackwidth <= 0. 355 * @see <a 356 * href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a> 357 */ 358 public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem( 359 double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { 360 if (kVLinear <= 0.0) { 361 throw new IllegalArgumentException("Kv,linear must be greater than zero."); 362 } 363 if (kALinear <= 0.0) { 364 throw new IllegalArgumentException("Ka,linear must be greater than zero."); 365 } 366 if (kVAngular <= 0.0) { 367 throw new IllegalArgumentException("Kv,angular must be greater than zero."); 368 } 369 if (kAAngular <= 0.0) { 370 throw new IllegalArgumentException("Ka,angular must be greater than zero."); 371 } 372 if (trackwidth <= 0.0) { 373 throw new IllegalArgumentException("trackwidth must be greater than zero."); 374 } 375 376 // We want to find a factor to include in Kv,angular that will convert 377 // `u = Kv,angular omega` to `u = Kv,angular v`. 378 // 379 // v = omega r 380 // omega = v/r 381 // omega = 1/r v 382 // omega = 1/(trackwidth/2) v 383 // omega = 2/trackwidth v 384 // 385 // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) 386 // to V/(m/s). 387 return identifyDrivetrainSystem( 388 kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth); 389 } 390}