feat(vla_sim): scripted red-block pick and its recording wrapper

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/config/config.yaml

@@ -20,6 +20,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]

src/vla_sim/launch/simulated_extras.launch.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+
+# Copyright 2026 PickNik Inc.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the PickNik Inc. nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """Host the joint command bridge, on both the dev and runtime paths."""
+    return LaunchDescription(
+        [
+            Node(
+                package="vla_sim",
+                executable="joint_command_bridge.py",
+                name="joint_command_bridge",
+                output="both",
+            ),
+        ]
+    )

src/vla_sim/objectives/close_gripper.xml

@@ -6,6 +6,21 @@
     _favorite="false"
   >
     <Control ID="Sequence" name="root">
+      <!--
+        The recorded `action` gets its gripper channel from here: a
+        GripperCommand goal is not observable on any topic. A failed update
+        would label the episode with the previous jaw command while the gripper
+        moved, so it fails the Objective rather than recording the wrong value.
+      -->
+      <Action
+        ID="SetRos2Parameter"
+        node_name="/joint_command_bridge"
+        parameter_name="gripper_command_position"
+        parameter_value="0.7"
+        parameter_type="double"
+        response_timeout="1.000000"
+        wait_for_server_available_timeout="0.500000"
+      />
       <Action
         ID="MoveGripperAction"
         gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"

src/vla_sim/objectives/collect_the_red_block_pick.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Collect the Red Block Pick">
+  <BehaviorTree
+    ID="Collect the Red Block Pick"
+    _description="Record one scripted demonstration of picking up the red block and convert it to a LeRobot dataset."
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="root">
+      <SubTree ID="Reset MuJoCo Sim" _collapsed="true" />
+      <Control ID="Fallback" name="RecordWithCleanup">
+        <Control ID="Sequence" name="RecordAndConvert">
+          <Action
+            ID="RecordEpisode"
+            name="Start Trainer recording"
+            dataset_name="red_block_pick"
+            task="Pick up the red block."
+            config_name="vla_sim"
+            num_episodes="1"
+          />
+          <!--Let the recording settle before motion begins.-->
+          <Action
+            ID="WaitForDuration"
+            name="Let recording settle"
+            delay_duration="2.0"
+          />
+          <SubTree ID="Pick the Red Block" _collapsed="true" />
+          <Action ID="SaveEpisode" name="Keep successful demonstration" />
+          <!--Let the saved episode finish updating in Trainer before the
+               session closes.-->
+          <Action
+            ID="WaitForDuration"
+            name="Let episode finish saving"
+            delay_duration="1.0"
+          />
+          <Action ID="StopRecording" name="Finish recording session" />
+          <Action
+            ID="ConvertDataset"
+            name="Convert to LeRobot dataset"
+            dataset_name="red_block_pick"
+          />
+        </Control>
+        <!--Cleanup, not a forced success: StopRecording discards the unsaved
+             attempt and AlwaysFailure preserves the failed result.-->
+        <Control ID="Sequence" name="DiscardFailedAttempt">
+          <Action ID="StopRecording" name="Discard unsaved attempt" />
+          <Action ID="AlwaysFailure" />
+        </Control>
+      </Control>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Collect the Red Block Pick">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Data Recording" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/vla_sim/objectives/open_gripper.xml

@@ -6,6 +6,21 @@
     _favorite="false"
   >
     <Control ID="Sequence" name="root">
+      <!--
+        The recorded `action` gets its gripper channel from here: a
+        GripperCommand goal is not observable on any topic. A failed update
+        would label the episode with the previous jaw command while the gripper
+        moved, so it fails the Objective rather than recording the wrong value.
+      -->
+      <Action
+        ID="SetRos2Parameter"
+        node_name="/joint_command_bridge"
+        parameter_name="gripper_command_position"
+        parameter_value="0.0"
+        parameter_type="double"
+        response_timeout="1.000000"
+        wait_for_server_available_timeout="0.500000"
+      />
       <Action
         ID="MoveGripperAction"
         gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"

src/vla_sim/objectives/pick_from_pose.xml

@@ -0,0 +1,119 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Pick from Pose">
+  <!--
+    `approach_distance` runs along `grasp_link`'s own axis, so a top-down grasp
+    pose makes both the approach and the retract vertical.
+
+    Joints 1, 3, 5 and 7 of a Gen3 are continuous, so a narrow `max_ik_solutions`
+    can leave only solutions a full turn from the arm's current pose, which the
+    controller rejects.
+  -->
+  <BehaviorTree
+    ID="Pick from Pose"
+    _description="Takes a grasp pose as input and picks up the object at that pose"
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="InitializeMTCTask"
+        task="{mtc_task}"
+        task_id="pick_object"
+        timeout="-1.000000"
+      />
+      <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+      <Action
+        ID="SetupMTCConnectWithProRRT"
+        acceleration_scale_factor="1.000000"
+        link_padding="0.000000"
+        max_iterations="5000"
+        planning_group_name="manipulator"
+        seed="0"
+        task="{mtc_task}"
+        trajectory_sampling_rate="100"
+        velocity_scale_factor="1.000000"
+      />
+      <Action
+        ID="SetupMTCMoveAlongFrameAxis"
+        acceleration_scale="1.000000"
+        axis_frame="grasp_link"
+        axis_x="0.000000"
+        axis_y="0.000000"
+        axis_z="1.000000"
+        hand_frame="grasp_link"
+        max_distance="{approach_distance}"
+        min_distance="{approach_distance}"
+        planning_group_name="manipulator"
+        task="{mtc_task}"
+        velocity_scale="1.000000"
+        ignore_environment_collisions="false"
+      />
+      <Action ID="ResetPoseStampedVector" vector="{grasp_pose_vector}" />
+      <Action
+        ID="PushBackVector"
+        input_vector="{grasp_pose_vector}"
+        element="{grasp_pose}"
+        output_vector="{grasp_pose_vector}"
+      />
+      <Action
+        ID="SetupMTCBatchPoseIK"
+        end_effector_group="gripper"
+        end_effector_link="grasp_link"
+        ik_group="manipulator"
+        ik_timeout_s="0.01"
+        max_ik_solutions="16"
+        monitored_stage="current state"
+        target_poses="{grasp_pose_vector}"
+        task="{mtc_task}"
+      />
+      <Action ID="PlanMTCTask" solution="{mtc_solution}" task="{mtc_task}" />
+      <SubTree ID="Execute MTC Solution" solution="{mtc_solution}" />
+      <!--
+        ExecuteMTCSolution returns on trajectory time, with the admittance
+        controller still catching up the last few centimetres. Closing on that
+        lag grasps air, and retracting on it pulls away mid-close.
+      -->
+      <Action ID="WaitForDuration" delay_duration="{settle_duration}" />
+      <SubTree ID="Close Gripper" _collapsed="true" />
+      <Action ID="WaitForDuration" delay_duration="{settle_duration}" />
+      <Action
+        ID="CreatePoseStamped"
+        reference_frame="grasp_link"
+        pose_stamped="{retract_pose}"
+        position_xyz="{retract_xyz}"
+      />
+      <Action ID="ResetPoseStampedVector" vector="{retract_path}" />
+      <Action
+        ID="PushBackVector"
+        input_vector="{retract_path}"
+        element="{retract_pose}"
+        output_vector="{retract_path}"
+      />
+      <Action
+        ID="PlanCartesianPath"
+        blending_radius="0.020000"
+        debug_solution="{debug_solution}"
+        ik_cartesian_space_density="0.010000"
+        ik_joint_space_density="0.100000"
+        joint_trajectory_msg="{joint_trajectory_msg}"
+        path="{retract_path}"
+        planning_group_name="manipulator"
+        cartesian_constraint="[{constraint_type: position_and_orientation}]"
+        tip_links="grasp_link"
+        trajectory_timing="[{mode: time_optimal}]"
+      />
+      <Action ID="ExecuteTrajectory" />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Pick from Pose">
+      <MetadataFields>
+        <Metadata subcategory="Manipulation" />
+        <Metadata runnable="false" />
+      </MetadataFields>
+      <inout_port name="approach_distance" default="0.1" />
+      <inout_port name="grasp_pose" default="{grasp_pose}" />
+      <inout_port name="retract_xyz" default="0;0;-0.1" />
+      <inout_port name="settle_duration" default="1.0" />
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/vla_sim/objectives/pick_the_red_block.xml

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Pick the Red Block">
+  <!--
+    `cube_red_tf` is a MuJoCo site on the cube's centroid, which picknik_mujoco_ros
+    publishes to TF whenever `tf_publish_rate` is set. It carries the cube's own
+    orientation, which is not a grasp, so a top-down one replaces it.
+
+    Nothing here resets the scene: run **Reset MuJoCo Sim** between attempts.
+    **Collect the Red Block Pick** resets on its own.
+  -->
+  <BehaviorTree
+    ID="Pick the Red Block"
+    _description="Pick up the red block from its simulated pose, without recording."
+    _favorite="true"
+  >
+    <Control ID="Sequence" name="root">
+      <SubTree ID="Open Gripper" _collapsed="true" />
+      <Action ID="SwitchUIPrimaryView" primary_view_name="Visualization" />
+      <Action
+        ID="GetLatestTransform"
+        target_frame_id="world"
+        source_frame_id="cube_red_tf"
+        transform="{cube_transform}"
+      />
+      <Action
+        ID="ConvertTransformStampedToPoseStamped"
+        transform_stamped="{cube_transform}"
+        pose_stamped="{cube_pose}"
+      />
+      <!--Straight down at the cube, yawed a quarter turn about world Z.-->
+      <Action
+        ID="OverridePoseOrientation"
+        input_pose="{cube_pose}"
+        orientation_xyzw="0.7071068;0.7071068;0;0"
+        output_pose="{grasp_pose}"
+      />
+      <Action
+        ID="VisualizePose"
+        marker_lifetime="0.000000"
+        marker_name="Red Block Grasp"
+        marker_size="0.100000"
+        pose="{grasp_pose}"
+      />
+      <SubTree
+        ID="Pick from Pose"
+        _collapsed="true"
+        grasp_pose="{grasp_pose}"
+        approach_distance="0.12"
+        retract_xyz="0;0;-0.12"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Pick the Red Block">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - VLA Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/vla_sim/package.xml

@@ -23,6 +23,7 @@
   <exec_depend>robotiq_description</exec_depend>
   <exec_depend>velocity_force_controller</exec_depend>
 
+  <exec_depend>control_msgs</exec_depend>
   <exec_depend>moveit_pro_ml_msgs</exec_depend>
   <exec_depend>python3-numpy</exec_depend>
   <exec_depend>python3-opencv</exec_depend>