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