WPILibC++ 2024.3.2
DifferentialDrivetrainSim.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <frc/EigenCore.h>
11
12#include <units/length.h>
14#include <units/time.h>
15#include <units/voltage.h>
16
17namespace frc::sim {
18
20 public:
21 /**
22 * Creates a simulated differential drivetrain.
23 *
24 * @param plant The LinearSystem representing the robot's drivetrain. This
25 * system can be created with
26 * LinearSystemId::DrivetrainVelocitySystem() or
27 * LinearSystemId::IdentifyDrivetrainSystem().
28 * @param trackWidth The robot's track width.
29 * @param driveMotor A DCMotor representing the left side of the drivetrain.
30 * @param gearingRatio The gearingRatio ratio of the left side, as output over
31 * input. This must be the same ratio as the ratio used to
32 * identify or create the plant.
33 * @param wheelRadius The radius of the wheels on the drivetrain, in meters.
34 * @param measurementStdDevs Standard deviations for measurements, in the form
35 * [x, y, heading, left velocity, right velocity,
36 * left distance, right distance]ᵀ. Can be omitted
37 * if no noise is desired. Gyro standard deviations
38 * of 0.0001 radians, velocity standard deviations
39 * of 0.05 m/s, and position measurement standard
40 * deviations of 0.005 meters are a reasonable
41 * starting point.
42 */
44 LinearSystem<2, 2, 2> plant, units::meter_t trackWidth,
45 DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius,
46 const std::array<double, 7>& measurementStdDevs = {});
47
48 /**
49 * Creates a simulated differential drivetrain.
50 *
51 * @param driveMotor A DCMotor representing the left side of the drivetrain.
52 * @param gearing The gearing on the drive between motor and wheel, as
53 * output over input. This must be the same ratio as the
54 * ratio used to identify or create the plant.
55 * @param J The moment of inertia of the drivetrain about its
56 * center.
57 * @param mass The mass of the drivebase.
58 * @param wheelRadius The radius of the wheels on the drivetrain.
59 * @param trackWidth The robot's track width, or distance between left and
60 * right wheels.
61 * @param measurementStdDevs Standard deviations for measurements, in the form
62 * [x, y, heading, left velocity, right velocity,
63 * left distance, right distance]ᵀ. Can be omitted
64 * if no noise is desired. Gyro standard deviations
65 * of 0.0001 radians, velocity standard deviations
66 * of 0.05 m/s, and position measurement standard
67 * deviations of 0.005 meters are a reasonable
68 * starting point.
69 */
71 frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
72 units::kilogram_t mass, units::meter_t wheelRadius,
73 units::meter_t trackWidth,
74 const std::array<double, 7>& measurementStdDevs = {});
75
76 /**
77 * Clamp the input vector such that no element exceeds the battery voltage.
78 * If any does, the relative magnitudes of the input will be maintained.
79 *
80 * @param u The input vector.
81 * @return The normalized input.
82 */
83 Eigen::Vector2d ClampInput(const Eigen::Vector2d& u);
84
85 /**
86 * Sets the applied voltage to the drivetrain. Note that positive voltage must
87 * make that side of the drivetrain travel forward (+X).
88 *
89 * @param leftVoltage The left voltage.
90 * @param rightVoltage The right voltage.
91 */
92 void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage);
93
94 /**
95 * Sets the gearing reduction on the drivetrain. This is commonly used for
96 * shifting drivetrains.
97 *
98 * @param newGearing The new gear ratio, as output over input.
99 */
100 void SetGearing(double newGearing);
101
102 /**
103 * Updates the simulation.
104 *
105 * @param dt The time that's passed since the last Update(units::second_t)
106 * call.
107 */
108 void Update(units::second_t dt);
109
110 /**
111 * Returns the current gearing reduction of the drivetrain, as output over
112 * input.
113 */
114 double GetGearing() const;
115
116 /**
117 * Returns the direction the robot is pointing.
118 *
119 * Note that this angle is counterclockwise-positive, while most gyros are
120 * clockwise positive.
121 */
123
124 /**
125 * Returns the current pose.
126 */
128
129 /**
130 * Get the right encoder position in meters.
131 * @return The encoder position.
132 */
133 units::meter_t GetRightPosition() const {
134 return units::meter_t{GetOutput(State::kRightPosition)};
135 }
136
137 /**
138 * Get the right encoder velocity in meters per second.
139 * @return The encoder velocity.
140 */
141 units::meters_per_second_t GetRightVelocity() const {
142 return units::meters_per_second_t{GetOutput(State::kRightVelocity)};
143 }
144
145 /**
146 * Get the left encoder position in meters.
147 * @return The encoder position.
148 */
149 units::meter_t GetLeftPosition() const {
150 return units::meter_t{GetOutput(State::kLeftPosition)};
151 }
152
153 /**
154 * Get the left encoder velocity in meters per second.
155 * @return The encoder velocity.
156 */
157 units::meters_per_second_t GetLeftVelocity() const {
158 return units::meters_per_second_t{GetOutput(State::kLeftVelocity)};
159 }
160
161 /**
162 * Returns the currently drawn current for the right side.
163 */
164 units::ampere_t GetRightCurrentDraw() const;
165
166 /**
167 * Returns the currently drawn current for the left side.
168 */
169 units::ampere_t GetLeftCurrentDraw() const;
170
171 /**
172 * Returns the currently drawn current.
173 */
174 units::ampere_t GetCurrentDraw() const;
175
176 /**
177 * Sets the system state.
178 *
179 * @param state The state.
180 */
182
183 /**
184 * Sets the system pose.
185 *
186 * @param pose The pose.
187 */
188 void SetPose(const frc::Pose2d& pose);
189
190 /**
191 * The differential drive dynamics function.
192 *
193 * @param x The state.
194 * @param u The input.
195 * @return The state derivative with respect to time.
196 */
197 Vectord<7> Dynamics(const Vectord<7>& x, const Eigen::Vector2d& u);
198
199 class State {
200 public:
201 static constexpr int kX = 0;
202 static constexpr int kY = 1;
203 static constexpr int kHeading = 2;
204 static constexpr int kLeftVelocity = 3;
205 static constexpr int kRightVelocity = 4;
206 static constexpr int kLeftPosition = 5;
207 static constexpr int kRightPosition = 6;
208 };
209
210 /**
211 * Represents a gearing option of the Toughbox mini.
212 * 12.75:1 -- 14:50 and 14:50
213 * 10.71:1 -- 14:50 and 16:48
214 * 8.45:1 -- 14:50 and 19:45
215 * 7.31:1 -- 14:50 and 21:43
216 * 5.95:1 -- 14:50 and 24:40
217 */
219 public:
220 /// Gear ratio of 12.75:1.
221 static constexpr double k12p75 = 12.75;
222 /// Gear ratio of 10.71:1.
223 static constexpr double k10p71 = 10.71;
224 /// Gear ratio of 8.45:1.
225 static constexpr double k8p45 = 8.45;
226 /// Gear ratio of 7.31:1.
227 static constexpr double k7p31 = 7.31;
228 /// Gear ratio of 5.95:1.
229 static constexpr double k5p95 = 5.95;
230 };
231
232 /**
233 * Represents common motor layouts of the kit drivetrain.
234 */
236 public:
237 /// One CIM motor per drive side.
239 /// Two CIM motors per drive side.
241 /// One Mini CIM motor per drive side.
244 /// Two Mini CIM motors per drive side.
246 /// One Falcon 500 motor per drive side.
249 /// Two Falcon 500 motors per drive side.
252 /// One NEO motor per drive side.
254 /// Two NEO motors per drive side.
256 };
257
258 /**
259 * Represents common wheel sizes of the kit drivetrain.
260 */
262 public:
263 /// Six inch diameter wheels.
264 static constexpr units::meter_t kSixInch = 6_in;
265 /// Eight inch diameter wheels.
266 static constexpr units::meter_t kEightInch = 8_in;
267 /// Ten inch diameter wheels.
268 static constexpr units::meter_t kTenInch = 10_in;
269 };
270
271 /**
272 * Create a sim for the standard FRC kitbot.
273 *
274 * @param motor The motors installed in the bot.
275 * @param gearing The gearing reduction used.
276 * @param wheelSize The wheel size.
277 * @param measurementStdDevs Standard deviations for measurements, in the form
278 * [x, y, heading, left velocity, right velocity, left distance, right
279 * distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
280 * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
281 * position measurement standard deviations of 0.005 meters are a reasonable
282 * starting point.
283 */
285 frc::DCMotor motor, double gearing, units::meter_t wheelSize,
286 const std::array<double, 7>& measurementStdDevs = {}) {
287 // MOI estimation -- note that I = mr² for point masses
288 units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in;
289 units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) *
290 2 // CIM plus toughbox per side
291 * (26_in / 2) * (26_in / 2);
292
294 motor, gearing, batteryMoi + gearboxMoi, 60_lb,
295 wheelSize / 2.0, 26_in, measurementStdDevs};
296 }
297
298 /**
299 * Create a sim for the standard FRC kitbot.
300 *
301 * @param motor The motors installed in the bot.
302 * @param gearing The gearing reduction used.
303 * @param wheelSize The wheel size.
304 * @param J The moment of inertia of the drivebase. This can be
305 * calculated using SysId.
306 * @param measurementStdDevs Standard deviations for measurements, in the form
307 * [x, y, heading, left velocity, right velocity, left distance, right
308 * distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
309 * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
310 * position measurement standard deviations of 0.005 meters are a reasonable
311 * starting point.
312 */
314 frc::DCMotor motor, double gearing, units::meter_t wheelSize,
315 units::kilogram_square_meter_t J,
316 const std::array<double, 7>& measurementStdDevs = {}) {
318 motor, gearing, J, 60_lb, wheelSize / 2.0, 26_in, measurementStdDevs};
319 }
320
321 private:
322 /**
323 * Returns an element of the state vector.
324 *
325 * @param output The row of the output vector.
326 */
327 double GetOutput(int output) const;
328
329 /**
330 * Returns the current output vector y.
331 */
332 Vectord<7> GetOutput() const;
333
334 /**
335 * Returns an element of the state vector. Note that this will not include
336 * noise!
337 *
338 * @param output The row of the output vector.
339 */
340 double GetState(int state) const;
341
342 /**
343 * Returns the current state vector x. Note that this will not include noise!
344 */
345 Vectord<7> GetState() const;
346
347 LinearSystem<2, 2, 2> m_plant;
348 units::meter_t m_rb;
349 units::meter_t m_wheelRadius;
350
351 DCMotor m_motor;
352
353 double m_originalGearing;
354 double m_currentGearing;
355
356 Vectord<7> m_x;
357 Eigen::Vector2d m_u;
358 Vectord<7> m_y;
359 std::array<double, 7> m_measurementStdDevs;
360};
361} // namespace frc::sim
Holds the constants for a DC motor.
Definition: DCMotor.h:20
static constexpr DCMotor Falcon500(int numMotors=1)
Returns a gearbox of Falcon 500 brushless motors.
Definition: DCMotor.h:203
static constexpr DCMotor CIM(int numMotors=1)
Returns a gearbox of CIM motors.
Definition: DCMotor.h:133
static constexpr DCMotor NEO(int numMotors=1)
Returns a gearbox of NEO brushless motors.
Definition: DCMotor.h:189
static constexpr DCMotor MiniCIM(int numMotors=1)
Returns a gearbox of MiniCIM motors.
Definition: DCMotor.h:140
Represents a 2D pose containing translational and rotational elements.
Definition: Pose2d.h:23
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:23
Represents a gearing option of the Toughbox mini.
Definition: DifferentialDrivetrainSim.h:218
static constexpr double k10p71
Gear ratio of 10.71:1.
Definition: DifferentialDrivetrainSim.h:223
static constexpr double k5p95
Gear ratio of 5.95:1.
Definition: DifferentialDrivetrainSim.h:229
static constexpr double k8p45
Gear ratio of 8.45:1.
Definition: DifferentialDrivetrainSim.h:225
static constexpr double k7p31
Gear ratio of 7.31:1.
Definition: DifferentialDrivetrainSim.h:227
static constexpr double k12p75
Gear ratio of 12.75:1.
Definition: DifferentialDrivetrainSim.h:221
Represents common motor layouts of the kit drivetrain.
Definition: DifferentialDrivetrainSim.h:235
static constexpr frc::DCMotor DualNEOPerSide
Two NEO motors per drive side.
Definition: DifferentialDrivetrainSim.h:255
static constexpr frc::DCMotor DualFalcon500PerSide
Two Falcon 500 motors per drive side.
Definition: DifferentialDrivetrainSim.h:250
static constexpr frc::DCMotor SingleNEOPerSide
One NEO motor per drive side.
Definition: DifferentialDrivetrainSim.h:253
static constexpr frc::DCMotor SingleMiniCIMPerSide
One Mini CIM motor per drive side.
Definition: DifferentialDrivetrainSim.h:242
static constexpr frc::DCMotor SingleCIMPerSide
One CIM motor per drive side.
Definition: DifferentialDrivetrainSim.h:238
static constexpr frc::DCMotor DualCIMPerSide
Two CIM motors per drive side.
Definition: DifferentialDrivetrainSim.h:240
static constexpr frc::DCMotor DualMiniCIMPerSide
Two Mini CIM motors per drive side.
Definition: DifferentialDrivetrainSim.h:245
static constexpr frc::DCMotor SingleFalcon500PerSide
One Falcon 500 motor per drive side.
Definition: DifferentialDrivetrainSim.h:247
Represents common wheel sizes of the kit drivetrain.
Definition: DifferentialDrivetrainSim.h:261
static constexpr units::meter_t kTenInch
Ten inch diameter wheels.
Definition: DifferentialDrivetrainSim.h:268
static constexpr units::meter_t kSixInch
Six inch diameter wheels.
Definition: DifferentialDrivetrainSim.h:264
static constexpr units::meter_t kEightInch
Eight inch diameter wheels.
Definition: DifferentialDrivetrainSim.h:266
Definition: DifferentialDrivetrainSim.h:199
static constexpr int kLeftVelocity
Definition: DifferentialDrivetrainSim.h:204
static constexpr int kX
Definition: DifferentialDrivetrainSim.h:201
static constexpr int kY
Definition: DifferentialDrivetrainSim.h:202
static constexpr int kLeftPosition
Definition: DifferentialDrivetrainSim.h:206
static constexpr int kHeading
Definition: DifferentialDrivetrainSim.h:203
static constexpr int kRightPosition
Definition: DifferentialDrivetrainSim.h:207
static constexpr int kRightVelocity
Definition: DifferentialDrivetrainSim.h:205
Definition: DifferentialDrivetrainSim.h:19
units::ampere_t GetCurrentDraw() const
Returns the currently drawn current.
DifferentialDrivetrainSim(frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J, units::kilogram_t mass, units::meter_t wheelRadius, units::meter_t trackWidth, const std::array< double, 7 > &measurementStdDevs={})
Creates a simulated differential drivetrain.
Eigen::Vector2d ClampInput(const Eigen::Vector2d &u)
Clamp the input vector such that no element exceeds the battery voltage.
void Update(units::second_t dt)
Updates the simulation.
units::meter_t GetRightPosition() const
Get the right encoder position in meters.
Definition: DifferentialDrivetrainSim.h:133
void SetGearing(double newGearing)
Sets the gearing reduction on the drivetrain.
Pose2d GetPose() const
Returns the current pose.
units::meter_t GetLeftPosition() const
Get the left encoder position in meters.
Definition: DifferentialDrivetrainSim.h:149
static DifferentialDrivetrainSim CreateKitbotSim(frc::DCMotor motor, double gearing, units::meter_t wheelSize, const std::array< double, 7 > &measurementStdDevs={})
Create a sim for the standard FRC kitbot.
Definition: DifferentialDrivetrainSim.h:284
units::meters_per_second_t GetLeftVelocity() const
Get the left encoder velocity in meters per second.
Definition: DifferentialDrivetrainSim.h:157
units::ampere_t GetLeftCurrentDraw() const
Returns the currently drawn current for the left side.
void SetState(const Vectord< 7 > &state)
Sets the system state.
units::meters_per_second_t GetRightVelocity() const
Get the right encoder velocity in meters per second.
Definition: DifferentialDrivetrainSim.h:141
void SetPose(const frc::Pose2d &pose)
Sets the system pose.
units::ampere_t GetRightCurrentDraw() const
Returns the currently drawn current for the right side.
Vectord< 7 > Dynamics(const Vectord< 7 > &x, const Eigen::Vector2d &u)
The differential drive dynamics function.
double GetGearing() const
Returns the current gearing reduction of the drivetrain, as output over input.
void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage)
Sets the applied voltage to the drivetrain.
static DifferentialDrivetrainSim CreateKitbotSim(frc::DCMotor motor, double gearing, units::meter_t wheelSize, units::kilogram_square_meter_t J, const std::array< double, 7 > &measurementStdDevs={})
Create a sim for the standard FRC kitbot.
Definition: DifferentialDrivetrainSim.h:313
DifferentialDrivetrainSim(LinearSystem< 2, 2, 2 > plant, units::meter_t trackWidth, DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius, const std::array< double, 7 > &measurementStdDevs={})
Creates a simulated differential drivetrain.
Rotation2d GetHeading() const
Returns the direction the robot is pointing.
state
Definition: core.h:2271
Definition: XboxControllerSim.h:13
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12