From c1b778c28e805e48a7dcc79bcc3f85517fe415ea Mon Sep 17 00:00:00 2001
From: Griswald Brooks <griswald.brooks@gmail.com>
Date: Thu, 16 Jul 2026 00:38:07 -0400
Subject: [PATCH] feat(lab_sim): add Run pi0.5 Policy objective wiring
 ExecutePolicyPi05Torch

Wires the new ExecutePolicyPi05Torch behavior (moveit_pro PR pending) into a
lab_sim Objective. Wiring demo only: the pi0.5 base checkpoint's declared
state_dim (32) does not match lab_sim's real joint count, so this needs a
matching-dims fine-tuned checkpoint export before it can run end-to-end.
Added to skip_objectives in the integration test since no such checkpoint
ships in CI.
---
 src/lab_sim/objectives/run_pi05_policy.xml    | 50 +++++++++++++++++++
 .../test/objectives_integration_test.py       |  1 +
 2 files changed, 51 insertions(+)
 create mode 100644 src/lab_sim/objectives/run_pi05_policy.xml

diff --git a/src/lab_sim/objectives/run_pi05_policy.xml b/src/lab_sim/objectives/run_pi05_policy.xml
new file mode 100644
index 000000000..91fcc9140
--- /dev/null
+++ b/src/lab_sim/objectives/run_pi05_policy.xml
@@ -0,0 +1,50 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Run pi0.5 Policy">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Run pi0.5 Policy"
+    _description="Runs the in-process libtorch pi0.5 vision-language-action policy on the lab_sim arm. Wiring demo: runnable end-to-end once a matching-dims checkpoint (real lab_sim joint count, not the base checkpoint's 32-dim placeholder) is exported -- see moveit_pro PR that adds ExecutePolicyPi05Torch."
+    _favorite="false"
+  >
+    <Control ID="Sequence">
+      <!-- Move to a known start pose so the policy's first observation is reproducible. -->
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        waypoint_name="Look at Table"
+      />
+      <Action
+        ID="ExecutePolicyPi05Torch"
+        prefix_pt_path="/models/pi05/pi05_prefix.pt"
+        expert_pt_path="/models/pi05/pi05_expert.pt"
+        tokenizer_path=""
+        stats_path="/models/pi05/pi05_stats.json"
+        prompt="pick up the block"
+        joint_group_name="manipulator"
+        total_action_steps="200"
+        committed_action_steps="32"
+        dt="0.05"
+        num_interpolation_points="100"
+        goal_path_tolerance="0.05"
+        policy_tracking_weight="300.0"
+        policy_uses_real_time_chunking="false"
+        guidance_horizon="0"
+        controller_action_name="/joint_trajectory_admittance_controller/follow_joint_trajectory"
+        absolute_force_torque_threshold="45;45;45;10;10;10"
+        policy_call_timeout="30.0"
+        image_names="observation.images.base_0_rgb;observation.images.left_wrist_0_rgb;observation.images.right_wrist_0_rgb"
+        image_topics="/scene_camera/color;/wrist_camera/color;/wrist_camera/color"
+        joint_states_topic="/joint_states"
+        policy_service_name="unused"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Run pi0.5 Policy">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - ML (GPU Recommended)" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/test/objectives_integration_test.py b/src/lab_sim/test/objectives_integration_test.py
index d33ae4a3f..a381c011b 100644
--- a/src/lab_sim/test/objectives_integration_test.py
+++ b/src/lab_sim/test/objectives_integration_test.py
@@ -295,6 +295,7 @@ def _controller_safe_mujoco_reset(
     # rate even with the 0.003 s scene timestep and CPU pinning. See PR #615.
     "Push Button With a Trajectory",
     "Record Square Trajectory",
+    "Run pi0.5 Policy",  # Wiring demo only: needs an external ~15 GB libtorch checkpoint export not present in CI.
     "Scan Scene - Multiple Point Clouds",
     "Stack Objects with ICP",  # Skipped because there is no primary ui to switch to in ci
     "Stitch Multiple Point Clouds Together",