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