Add history scrubbing mechanism to simulate.

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

PiperOrigin-RevId: 570676968
Change-Id: Id9a1fccc9418b851e007ddcf3fa89acd4edff3e9

python/mujoco/simulate.cc

@@ -210,7 +210,8 @@ PYBIND11_MODULE(_simulate, pymodule) {
            py::call_guard<py::gil_scoped_release>())
       .def("sync", CallIfNotNull(&mujoco::Simulate::Sync),
            py::call_guard<py::gil_scoped_release>())
-
+      .def("add_to_history", CallIfNotNull(&mujoco::Simulate::AddToHistory),
+           py::call_guard<py::gil_scoped_release>())
       .def("render_loop", CallIfNotNull(&mujoco::Simulate::RenderLoop),
            py::call_guard<py::gil_scoped_release>())
       .def("lock", GetIfNotNull(&mujoco::Simulate::mtx),

python/mujoco/viewer.py

@@ -237,6 +237,7 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
       if m is not None:
         assert d is not None
         if simulate.run:
+          stepped = False
           # Record CPU time at start of iteration.
           startcpu = time.time()
 
@@ -275,6 +276,7 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
 
             # Run single step, let next iteration deal with timing.
             mujoco.mj_step(m, d)
+            stepped = True
 
           # In-sync: step until ahead of cpu.
           else:
@@ -292,10 +294,16 @@ def _physics_loop(simulate: _Simulate, loader: Optional[_InternalLoaderType]):
 
               # Call mj_step.
               mujoco.mj_step(m, d)
+              stepped = True
 
               # Break if reset.
               if d.time < prevsim:
                 break
+
+          # save current state to history buffer
+          if (stepped):
+            simulate.add_to_history()
+
         else:  # simulate.run is False: GUI is paused.
 
           # Run mj_forward, to update rendering and joint sliders.