From a4d78a80e60311fe0a8ef1a8842fd83e4663411c Mon Sep 17 00:00:00 2001
From: Duncan Calvert <dcalvert@ihmc.us>
Date: Thu, 30 Oct 2025 14:41:47 -0500
Subject: [PATCH] Trying stopping between inferences with openpi.

---
 .../libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java | 5 +++++
 .../src/main/java/us/ihmc/lerobot/VLAUpdateThread.java      | 6 +++++-
 .../src/main/java/us/ihmc/sensors/ImageSensor.java          | 2 +-
 .../main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java | 6 ++++++
 4 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java
index ddf36500d355..93883de74abd 100644
--- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java
+++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/lerobot/RDXVLAOperation.java
@@ -273,4 +273,9 @@ private void getRenderables(Array<Renderable> renderables, Pool<Renderable> pool
             actionForearmPoseGraphics.get(side).getRenderables(renderables, pool);
          }
    }
+
+   public CRDTBidirectionalBoolean getRunning()
+   {
+      return running;
+   }
 }
diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/lerobot/VLAUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/lerobot/VLAUpdateThread.java
index c637026b1041..0b376478f0a4 100644
--- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/lerobot/VLAUpdateThread.java
+++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/lerobot/VLAUpdateThread.java
@@ -78,6 +78,7 @@ public class VLAUpdateThread extends VLAYoRegistry
    private final LatestTimestampModifiable latestTimestampModifiable;
    private long sequenceID = 0L;
    private final CRDTBidirectionalBoolean running;
+   private boolean keepRunning = true;
    private final CRDTBidirectionalBoolean controlRobot;
    private final TypedNotification<VLAOperationMessage> uiCommandSubscription;
    private final ROS2Publisher<VLAOperationMessage> uiStatusPublisher;
@@ -124,7 +125,7 @@ public void runTask()
    {
       handUISyncBefore();
 
-      if (running.getValue())
+      if (keepRunning && running.getValue())
       {
          if (openpiRequest == null)
          {
@@ -153,6 +154,7 @@ else if (openpiRequest.isDone())
          DoubleBuffer action = actionPlan.pollFirst();
          if (action != null)
          {
+            keepRunning = false;
             dispatchActionToKST(action);
          }
       }
@@ -396,6 +398,8 @@ private void handUISyncBefore()
             ToolboxStateMessage toolboxStateMessage = new ToolboxStateMessage();
             toolboxStateMessage.setRequestedToolboxState(ToolboxState.WAKE_UP.toByte());
             kstStatePublisher.publish(toolboxStateMessage);
+
+            keepRunning = true;
          }
          controlRobot.fromMessage(uiCommand.getControlRobot());
       }
diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java
index 4d4c2fec7428..b5fcd127fa96 100644
--- a/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java
+++ b/ihmc-perception/src/main/java/us/ihmc/sensors/ImageSensor.java
@@ -171,7 +171,7 @@ public RepeatingTaskThread getGrabThread()
       return grabThread;
    }
 
-   private void grabAndNotify()
+   public void grabAndNotify()
    {
       // If the sensor is not running, try to start the sensor
       if (!isSensorRunning() && !startSensor())
diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java b/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java
index fdf8bb40342d..547e228f94be 100644
--- a/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java
+++ b/ihmc-perception/src/main/java/us/ihmc/sensors/zed/ZEDSVOPlaybackSensor.java
@@ -28,6 +28,12 @@ public boolean startSensor()
       return super.startSensor();
    }
 
+   @Override
+   public boolean grab()
+   {
+      return super.grab();
+   }
+
    public void play()
    {
       run(true);