Physics-First Development

Waiting for a physical robot to be built is the #1 cause of losing FRC competitions. MARSLib use simulation as a platform for Innovation and Discovery. Using Dyn4j, we create a high-fidelity virtual world where you can develop 100% of your code before the metal is even cut. This ensures Inclusion by allowing every student to contribute and iterate—making the engineering process both productive and Fun regardless of hardware availability.

1 The Physics World

The MARSPhysicsWorld is a singleton that manages all rigid bodies on the field natively. It handles collisions, friction, and even battery voltage sag natively alongside standard WPILib simulation classes.

// In simulationPeriodic()
MARSPhysicsWorld.getInstance().update(0.020);

2 Spawning Game Pieces

To test an intake or the complex Ladder kinematics, you need game pieces in the simulation. You can spawn Fuel Balls dynamically into the world. They will bounce off walls and react to your robot’s bumpers.

Body fuelBall = new Body();
fuelBall.addFixture(Geometry.createCircle(0.12)); // 24cm diameter
fuelBall.setMass(MassType.NORMAL);
fuelBall.translate(2.0, 2.0); // Meters x,y
MARSPhysicsWorld.getInstance().getDyn4jWorld().addBody(fuelBall);

3 Subsystem IO-Sim

As covered in the IO Layer tutorial, your subsystems should use IOSim implementations. These classes connect WPILib simulators (like ElevatorSim) to the physics world. The simulator provides the mathematical state, while Dyn4j provides the collision bounds.

Caution

Realistic Power Modeling

MARSLib doesn’t just sim motion; it sims electrical draw. If your elevator hits a mechanical limit and stalls, the MARSPowerManager will calculate the voltage drop, which might cause your “virtual” Rio to brown out! This allows you to tune current limits safely in offline code.

4 Visualizing with AdvantageScope

You can’t “see” physics math, so we export all body positions to AdvantageScope. By dragging the PhysicsWorld/GamePieces field into the 3D Field view, you can watch your robot interact with Fuel Balls and the Hub in real-time.

Note

Student Pro-Tip

Run your PathPlanner autonomous paths in simulation with Efficiency Mode OFF to test how physical collisions with obstacle walls might knock your robot off path. If your pathing breaks here, it will definitely break on the real field.

5 Offline Log Replay and Auto-Tuning

Because MARSLib strictly abstracts hardware out with the AdvantageKit IO layer, simulation isn’t just about the physics engine—it natively supports Log Replay. You can pull an active .wpilog file from a real match and feed it straight back into the robot code on your laptop.

• Automatically Figure Out Constants: Run a match log through the replay tool, and MARSLib’s Continuous SysId will passively process the historical velocity and voltage telemetry.

• Iterative Vision Tuning: Modify your Megatag thresholds in code, then run replay on an old match log to see improvements in AdvantageScope.

• Post-Mortem Debugging: If the superstructure gets stuck, replay the log and step through the state machine transitions line by line.

// To run Replay offline, pass the wpilog path to the Robot configuration
Robot.setUseReplayMode(true, "C:/logs/einstein_finals_m1.wpilog");

---

6 Physics-Backed Testing

Most FRC unit tests simply “mock” the hardware. They verify that a motor was told to run, but they never verify if the robot actually moves. In MARSLib, we run Physics-Backed Unit Tests that integrate tightly with Dyn4j, ensuring that our software actually solves real-world physics problems during Continuous Integration.

6.1 The MARSTestHarness

To run physics-backed tests, your JUnit 5 test class must initialize the MARSTestHarness. This harness overrides the internal HAL clock and resets static singletons, guaranteeing that every test starts with a clean slate.

@BeforeEach
public void setup() {
  MARSTestHarness.setupTestEnvironment();
  // Initialize subsystems...
}

6.2 Advancing Time

Because tests run instantly on your CPU, you must explicitly command the simulation clock to tick forward to watch physics happen.

// Command the elevator to position 1.5 meters
elevator.setGoal(1.5);

// Tick the physics world forward by 2.0 seconds
for (int i = 0; i < 100; i++) {
  elevator.periodic();
  MARSPhysicsWorld.getInstance().update(0.02);
}

// Assert the physical rigibody actually reached the goal
assertEquals(1.5, elevator.getPosition().in(Meters), 0.05);

Caution

Beware the 20ms Loop

Never advance the clock by more than 0.02 seconds (20ms) at a time. The PID controllers assume a 50Hz execution cycle. Bypassing this will cause your integrators to explode.

6.3 Testing Autonomous Paths

Because we have IOSim and Dyn4j, you can actually test a full PathPlanner macro in JUnit!

Command autoRoutine = AutoBuilder.buildAuto("Four Fuel Ball Auto");
autoRoutine.initialize();

// Run the auto for 15 seconds
for (int i = 0; i < 750; i++) {
  autoRoutine.execute();
  swerve.periodic();
  MARSPhysicsWorld.getInstance().update(0.02);
}

assertTrue(autoRoutine.isFinished());
// Assert that we scored exactly 4 times in the physics engine
assertEquals(4, GameField.getScoredHubCount());

6.4 CI/CD Validation

Every time you push to GitHub, a GitHub Actions workflow executes ./gradlew test. If a math commit accidentally breaks the chassis kinematics, the trajectory tests will fail, and the PR will be blocked. This is true Einstein-level safety.

---

7 Continuous SysId Auto-Tuning

Thanks to MARSLib’s tight integration with AdvantageKit’s logging structure, we never have to run traditional SysId Quasistatic/Dynamic sequences on real hardware if we don’t want to.

By utilizing the OnlineFeedforwardEstimator injected into our core subsystems proactively, you can perform Continuous SysId Auto-Tuning under the hood of normal operations. This algorithm acts as a Zero-GC, realtime Feedforward extractor. It continuously monitors mechanism telemetry (Voltage, Velocity, and Acceleration) during normal simulated or teleop driving and computes empirical kV and kA values on the fly.

To view these automatically generated mathematical constants, simply open AdvantageScope and graph:

• <MechanismName>/AutoTune/Estimated_kV
• <MechanismName>/AutoTune/Estimated_kA

You can plug these values right into your PID/Feedforward constructors and achieve near-perfect trajectory tracking.

---

📖 Further Reading & External Resources

• Dyn4j Physics Engine - The core library powering MARSLib simulation.

• WPILib: Robot Simulation - Official guide to physics-backed development.

• AdvantageKit: Recording and Replay - Detailed breakdown of the replay engine.