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