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