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