Openpi experiments

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;
+   }
 }

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());
       }

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())

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);