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