Add history scrubbing mechanism to simulate.

[screen capture](https://www.youtube.com/watch?v=YSvWn_poqWs)

PiperOrigin-RevId: 570676968
Change-Id: Id9a1fccc9418b851e007ddcf3fa89acd4edff3e9

simulate/main.cc

@@ -279,8 +279,7 @@ void PhysicsLoop(mj::Simulate& sim) {
         free(ctrlnoise);
         ctrlnoise = (mjtNum*) malloc(sizeof(mjtNum)*m->nu);
         mju_zero(ctrlnoise, m->nu);
-      }
-      else {
+      } else {
         sim.LoadMessageClear();
       }
     }
@@ -305,8 +304,7 @@ void PhysicsLoop(mj::Simulate& sim) {
         free(ctrlnoise);
         ctrlnoise = static_cast<mjtNum*>(malloc(sizeof(mjtNum)*m->nu));
         mju_zero(ctrlnoise, m->nu);
-      }
-      else {
+      } else {
         sim.LoadMessageClear();
       }
     }
@@ -327,6 +325,8 @@ void PhysicsLoop(mj::Simulate& sim) {
       if (m) {
         // running
         if (sim.run) {
+          bool stepped = false;
+
           // record cpu time at start of iteration
           const auto startCPU = mj::Simulate::Clock::now();
 
@@ -366,6 +366,7 @@ void PhysicsLoop(mj::Simulate& sim) {
 
             // run single step, let next iteration deal with timing
             mj_step(m, d);
+            stepped = true;
           }
 
           // in-sync: step until ahead of cpu
@@ -387,6 +388,7 @@ void PhysicsLoop(mj::Simulate& sim) {
 
               // call mj_step
               mj_step(m, d);
+              stepped = true;
 
               // break if reset
               if (d->time < prevSim) {
@@ -394,6 +396,11 @@ void PhysicsLoop(mj::Simulate& sim) {
               }
             }
           }
+
+          // save current state to history buffer
+          if (stepped) {
+            sim.AddToHistory();
+          }
         }
 
         // paused