feat(vla_sim): collect demonstrations with the scripted oracle

docker-compose.yaml

@@ -16,6 +16,9 @@ services:
       # MOVEIT_INFERENCE_PORT that publishes the server's host port, so
       # changing the port is a one-variable edit. No other config reads it.
       - INFER_URL=http://127.0.0.1:${MOVEIT_INFERENCE_PORT:-8973}/infer
+      # What the Trainer records as `observation.state`; see
+      # src/vla_sim/script/joint_command_bridge.py.
+      - MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC=${MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC:-/observed_joint_states}
 
   # Starts the robot drivers.
   drivers:

src/vla_sim/CMakeLists.txt

@@ -18,6 +18,7 @@ install(
 
 install(PROGRAMS
   script/get_action_chunk_adapter.py
+  script/joint_command_bridge.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
@@ -33,6 +34,11 @@ if(BUILD_TESTING)
     TIMEOUT 60
   )
 
+  ament_add_pytest_test(test_joint_command_bridge
+    test/test_joint_command_bridge.py
+    TIMEOUT 60
+  )
+
   ament_lint_auto_find_test_dependencies()
 endif()
 

src/vla_sim/README.md

@@ -15,4 +15,19 @@ be too slow to run at all. A smaller model such as SmolVLA might be the better
 fit there. AMD GPUs are not passed through yet, so those machines run inference
 on the CPU as well.
 
+## Collecting demonstrations
+
+The same scene also records the demonstrations a replacement policy is trained
+on, one Pro dataset per prompt.
+
+`Run Cube-Stack Oracle` performs one stack from a scripted oracle, with no
+recording, which is the quickest way to see whether a change to the scene or the
+planner still produces a clean demonstration. `Collect Cube-Stack Demonstration`
+records one. The six `Record Cube-Stack <held> On <target>` objectives each
+sweep the 60 training layouts drawn for their prompt, producing one dataset per
+prompt.
+
+[`description/mujoco/keyframes.xml`](description/mujoco/keyframes.xml) holds the
+layouts: 360 `train_*` and 150 `eval_*`.
+
 For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

src/vla_sim/config/config.yaml

@@ -16,6 +16,11 @@ hardware:
   # [Optional, default=True]
   launch_robot_state_publisher: True
 
+  # Hosts the joint command bridge, on both the dev and runtime paths.
+  additional_agent_launch_file:
+    package: "vla_sim"
+    path: "launch/simulated_extras.launch.py"
+
   # Parameters used to configure the robot description through XACRO.
   # A URDF and SRDF are both required.
   # [Required]
@@ -115,6 +120,8 @@ objectives:
       - "moveit_pro::behaviors::VisionBehaviorsLoader"
       - "moveit_pro::behaviors::ConverterBehaviorsLoader"
       - "moveit_pro::behaviors::MujocoBehaviorsLoader"
+    vla_sim:
+      - "vla_sim_behaviors::VlaSimBehaviorsLoader"
   # Specify source folder for objectives
   # [Required]
   objective_library_paths:

src/vla_sim/description/mujoco/cube_stack_scene.xml

@@ -1,4 +1,4 @@
-<mujoco model="kinova color-cube stacking scene">
+<mujoco model="kinova cube stacking scene">
   <!-- ============================================================
        Kinova Gen3 (7-DoF) + Robotiq 2F-85 stacking colored cubes.
 
@@ -183,20 +183,14 @@
     </body>
   </worldbody>
 
-  <!-- Initial state. qpos layout (compile order):
-       robot [7 arm + 8 robotiq gripper] (15), then 3 cube freejoints (3x7=21).
-       The 7 arm values are the "Home" waypoint pose (arm tucked up off the
-       table), so the scene resets at home. The 8 robotiq values are the
-       gripper rest pose. Each cube freejoint is
-       pos(3)+quat(4): the three table-rest poses above, identity orientation.
-       ctrl = 8 actuators (7 arm + 1 gripper). The 7 arm position actuators must
-       be commanded to the SAME home angles as qpos (else they'd drive toward
-       zero on tick 1); the gripper actuator holds 0 (open). -->
-  <keyframe>
-    <key
-      name="default"
-      qpos="7.2854474427322627e-05 0.0060998872530424308 -3.1250605484909082 -2.5051932931762071 -2.7164243118882329e-05 0.95966324117024648 1.5699864595074029 0.00500747 0.000142046 0.0054543 -0.0057924 0.00500747 0.000142046 0.00545513 -0.00579407 0.44 -0.12 0.115 1 0 0 0 0.56 0.0 0.115 1 0 0 0 0.50 0.13 0.115 1 0 0 0"
-      ctrl="7.2854474427322627e-05 0.0060998872530424308 -3.1250605484909082 -2.5051932931762071 -2.7164243118882329e-05 0.95966324117024648 1.5699864595074029 0"
-    />
-  </keyframe>
+  <!-- Startup state (the `default` key) plus the randomized train/eval cube
+       layouts. qpos layout (compile order): robot [7 arm + 8 robotiq gripper]
+       (15), then 3 cube freejoints (3x7=21), in the red/green/blue declaration
+       order above.
+
+       The 7 arm values are the "Home" waypoint pose, so the scene starts and
+       resets with the arm tucked up off the table. ctrl = 8 actuators (7 arm +
+       1 gripper); the arm actuators carry the SAME home angles as qpos, so they
+       hold that pose from tick 1. The gripper holds 0 (open). -->
+  <include file="keyframes.xml" />
 </mujoco>