Physics Simulation with WPILib
Because state-space notation allows us to compactly represent the dynamics of systems, we can leverage it to provide a backend for simulating physical systems on robots. The goal of these simulators is to simulate the motion of robot mechanisms without modifying existing non-simulation user code. The basic flow of such simulators is as follows:
In normal user code:
PID or similar control algorithms generate voltage commands from encoder (or other sensor) readings
Motor outputs are set
In simulation periodic code:
WPILib’s Simulation Classes
The following physics simulation classes are available in WPILib:
LinearSystemSim, for modeling systems with linear dynamics
FlywheelSim
DifferentialDrivetrainSim
ElevatorSim, which models gravity
SingleJointedArmSim, which models gravity
BatterySim, which simply estimates battery voltage sag based on drawn currents
All simulation classes (with the exception of the differential drive simulator) inherit from the LinearSystemSim
class. By default, the dynamics are the linear system dynamics \(\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k\). Subclasses override the UpdateX(x, u, dt)
method to provide custom, nonlinear dynamics, such as modeling gravity.
Note
Swerve support for simulation is in the works, but we cannot provide an ETA. For updates on progress, please follow this pull request.
Usage in User Code
The following is available from the WPILib elevatorsimulation
example project.
In addition to standard objects such as motors and encoders, we instantiate our elevator simulator using known constants such as carriage mass and gearing reduction. We also instantiate an EncoderSim
, which sets the distance and rate read by our our Encoder
.
In the following example, we simulate an elevator given the mass of the moving carriage (in kilograms), the radius of the drum driving the elevator (in meters), the gearing reduction between motor and drum as output over input (so usually greater than one), the minimum and maximum height of the elevator (in meters), and some random noise to add to our position estimate.
Note
The elevator and arm simulators will prevent the simulated position from exceeding given minimum or maximum heights or angles. If you wish to simulate a mechanism with infinite rotation or motion, LinearSystemSim
may be a better option.
53 // Simulation classes help us simulate what's going on, including gravity.
54 private final ElevatorSim m_elevatorSim =
55 new ElevatorSim(
56 m_elevatorGearbox,
57 kElevatorGearing,
58 kCarriageMass,
59 kElevatorDrumRadius,
60 kMinElevatorHeight,
61 kMaxElevatorHeight,
62 VecBuilder.fill(0.01));
63 private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
60 // Simulation classes help us simulate what's going on, including gravity.
61 frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
62 kElevatorGearing,
63 kCarriageMass,
64 kElevatorDrumRadius,
65 kMinElevatorHeight,
66 kMaxElevatorHeight,
67 {0.01}};
68 frc::sim::EncoderSim m_encoderSim{m_encoder};
Next, teleopPeriodic
/TeleopPeriodic
(Java/C++) uses a simple PID control loop to drive our elevator to a setpoint 30 inches off the ground.
100 @Override
101 public void teleopPeriodic() {
102 if (m_joystick.getTrigger()) {
103 // Here, we run PID control like normal, with a constant setpoint of 30in.
104 double pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.inchesToMeters(30));
105 m_motor.setVoltage(pidOutput);
106 } else {
107 // Otherwise, we disable the motor.
108 m_motor.set(0.0);
109 }
110 }
100 frc::sim::RoboRioSim::SetVInVoltage(
101 frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
102
103 // Update the Elevator length based on the simulated elevator height
104 m_elevatorMech2d->SetLength(
105 units::inch_t(m_elevatorSim.GetPosition()).value());
106 }
107
108 void TeleopPeriodic() override {
Next, simulationPeriodic
/SimulationPeriodic
(Java/C++) uses the voltage applied to the motor to update the simulated position of the elevator. We use SimulationPeriodic
because it runs periodically only for simulated robots. This means that our simulation code will not be run on a real robot.
Finally, the simulated encoder’s distance reading is set using the simulated elevator’s position, and the robot’s battery voltage is set using the estimated current drawn by the elevator.
81 @Override
82 public void simulationPeriodic() {
83 // In this method, we update our simulation of what our elevator is doing
84 // First, we set our "inputs" (voltages)
85 m_elevatorSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
86
87 // Next, we update it. The standard loop time is 20ms.
88 m_elevatorSim.update(0.020);
89
90 // Finally, we set our simulated encoder's readings and simulated battery voltage
91 m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
92 // SimBattery estimates loaded battery voltages
93 RoboRioSim.setVInVoltage(
94 BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
95
96 // Update elevator visualization with simulated position
97 m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSim.getPositionMeters()));
98 }
87 void SimulationPeriodic() override {
88 // In this method, we update our simulation of what our elevator is doing
89 // First, we set our "inputs" (voltages)
90 m_elevatorSim.SetInput(Eigen::Vector<double, 1>{
91 m_motor.Get() * frc::RobotController::GetInputVoltage()});
92
93 // Next, we update it. The standard loop time is 20ms.
94 m_elevatorSim.Update(20_ms);
95
96 // Finally, we set our simulated encoder's readings and simulated battery
97 // voltage
98 m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
99 // SimBattery estimates loaded battery voltages
100 frc::sim::RoboRioSim::SetVInVoltage(
101 frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
102
103 // Update the Elevator length based on the simulated elevator height
104 m_elevatorMech2d->SetLength(
105 units::inch_t(m_elevatorSim.GetPosition()).value());
106 }