From 082dc8d05685653aec4ae7304528ec3df7781bf7 Mon Sep 17 00:00:00 2001 From: Nathan Brooks Date: Sun, 10 May 2026 20:36:59 -0600 Subject: [PATCH] feat(lab_sim): add Quest controller teleoperation objective MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds a single new "Quest Teleop" objective to lab_sim, plus the host-side infrastructure required to pair with the [meta_quest_teleoperation](https://github.com/PickNikRobotics/meta_quest_teleoperation) Unity app. Operators wearing a Quest can teleoperate the simulated UR through clutch-based pose tracking. How the disjoint TF trees are handled The Quest publishes controller pose in a `quest` frame anchored at the headset's spawn location. The robot's `world` frame is anchored at its base. We don't know the geometric relationship between them — it depends on where the operator is physically standing — and we don't try to measure it. The two TF trees are disjoint roots, and the design works by never crossing between them: * On grip press, snapshot both anchors: - controller pose at clutch time, expressed in `quest` - grasp_link pose at clutch time, expressed in `world` * While grip is held, each tick: - read the controller's current pose in `quest` - compute its pose relative to its clutch-time pose — a relative rigid-body transform with no inherent frame attachment - apply that same relative transform to the grasp_link's clutch-time pose; the result is a target pose in `world` - drive VFC at the target A basis change re-expresses the relative transform from Quest FLU (X=forward, Y=left, Z=up) into the IMarker EE convention used by `grasp_link` (X=left, Y=up, Z=forward) so the components line up when applied. The basis change is the conjugation R · X · R⁻¹, expressed with two existing core primitives — `TransformPoseWithPose` (pre-mul by R) followed by `TransformPose` (post-mul by R⁻¹). Specific to this EE — re-derive R for other conventions. Why no v1..v10 history? This is a clean cut. Earlier iterations explored in-app clutch logic, separate-subtree slot architectures, and various basis-change strategies. Only the final design ships: * App publishes raw pose + button state; host BT owns clutch + kinematics. * Disjoint quest/world TF roots are handled by snapshot + relative- transform composition, never by a frame crossing. * Basis-change is expressed via the two-call conjugation using only existing core primitives; no new behavior added. * Per-button slot actions are inline AlwaysSuccess in a single flat tree, so the objective is self-contained — no SubTree-files-to-copy. Files - src/lab_sim/objectives/quest_teleop.xml: the objective. Single flat BehaviorTree, runnable from the UI under the "User Input" subcategory. - src/lab_sim/launch/sim/robot_drivers_to_persist_sim.launch.py: drivers- to-persist launch override. Starts ros_tcp_endpoint (Unity bridge) and a debug-only world->quest static TF so RViz can render the in-quest-frame VisualizePose markers. The static TF is not used by any control path. Persists across agent_bridge restarts so the Quest's TCP socket stays up. - src/lab_sim/config/config.yaml: * simulated_robot_driver_persist_launch_file points at the new launch file (without this, the inherited empty launch wins and the Quest app never sees a peer). * experimental_behaviors loader added to behavior_loader_plugins, which provides the pose / vector / Odometry conversion, snapshot subscriber, and Bool publisher behaviors the objective uses. - .gitmodules: two new submodules under src/external_dependencies/: * moveit_pro_experimental_behaviors, pinned to feat/pose-vector-basis-snapshot-behaviors pending the PR landing on main; switch to `branch = main` once that merges. * ROS-TCP-Endpoint (Unity-Technologies), pinned to main-ros2. For end-to-end setup with the Quest app, see: https://docs.picknik.ai/hardware_guides/input_devices/setting_up_the_meta_quest_for_teleop/ Co-Authored-By: Claude Opus 4.7 (1M context) --- .gitmodules | 8 + src/external_dependencies/ROS-TCP-Endpoint | 1 + .../moveit_pro_experimental_behaviors | 1 + src/lab_sim/config/config.yaml | 10 + .../robot_drivers_to_persist_sim.launch.py | 49 ++ src/lab_sim/objectives/quest_teleop.xml | 444 ++++++++++++++++++ 6 files changed, 513 insertions(+) create mode 160000 src/external_dependencies/ROS-TCP-Endpoint create mode 160000 src/external_dependencies/moveit_pro_experimental_behaviors create mode 100644 src/lab_sim/launch/sim/robot_drivers_to_persist_sim.launch.py create mode 100644 src/lab_sim/objectives/quest_teleop.xml diff --git a/.gitmodules b/.gitmodules index 38091a9a1..18638b83b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -33,3 +33,11 @@ [submodule "src/moveit_pro_sam2"] path = src/moveit_pro_sam2 url = https://github.com/PickNikRobotics/moveit_pro_sam2.git +[submodule "src/external_dependencies/moveit_pro_experimental_behaviors"] + path = src/external_dependencies/moveit_pro_experimental_behaviors + url = git@github.com:PickNikRobotics/moveit_pro_experimental_behaviors.git + branch = feat/pose-vector-basis-snapshot-behaviors +[submodule "src/external_dependencies/ROS-TCP-Endpoint"] + path = src/external_dependencies/ROS-TCP-Endpoint + url = https://github.com/Unity-Technologies/ROS-TCP-Endpoint.git + branch = main-ros2 diff --git a/src/external_dependencies/ROS-TCP-Endpoint b/src/external_dependencies/ROS-TCP-Endpoint new file mode 160000 index 000000000..54c1a64b6 --- /dev/null +++ b/src/external_dependencies/ROS-TCP-Endpoint @@ -0,0 +1 @@ +Subproject commit 54c1a64b6d5ef6ffa0a0431570bb74329b79b15b diff --git a/src/external_dependencies/moveit_pro_experimental_behaviors b/src/external_dependencies/moveit_pro_experimental_behaviors new file mode 160000 index 000000000..c58ea6cd6 --- /dev/null +++ b/src/external_dependencies/moveit_pro_experimental_behaviors @@ -0,0 +1 @@ +Subproject commit c58ea6cd605be6024c6d739ced76dd4917c9d12d diff --git a/src/lab_sim/config/config.yaml b/src/lab_sim/config/config.yaml index e4873abf5..8798b6953 100644 --- a/src/lab_sim/config/config.yaml +++ b/src/lab_sim/config/config.yaml @@ -20,6 +20,14 @@ hardware: - mujoco_model: "description/scene.xml" - mujoco_viewer: false + # Override the inherited (empty) drivers-to-persist launch so the Meta Quest + # ros_tcp_endpoint node starts with the sim and persists across agent_bridge + # restarts. Without it, /right_controller_odom and the button-event topics + # never publish, and the "Quest Teleop" objective blocks forever. + simulated_robot_driver_persist_launch_file: + package: "lab_sim" + path: "launch/sim/robot_drivers_to_persist_sim.launch.py" + moveit_params: joint_limits: package: "lab_sim" @@ -51,6 +59,8 @@ objectives: - "moveit_pro::behaviors::MujocoBehaviorsLoader" lab_sim: - "lab_sim_behaviors::LabSimBehaviorsLoader" + experimental: + - "experimental_behaviors::ExperimentalBehaviorsLoader" # Specify source folder for objectives # [Required] objective_library_paths: diff --git a/src/lab_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/lab_sim/launch/sim/robot_drivers_to_persist_sim.launch.py new file mode 100644 index 000000000..7258d2967 --- /dev/null +++ b/src/lab_sim/launch/sim/robot_drivers_to_persist_sim.launch.py @@ -0,0 +1,49 @@ +# Persistent driver-side processes for lab_sim. +# +# Launched by MoveIt Pro via the simulated_robot_driver_persist_launch_file +# config field. Lives in the "drivers to persist" lifecycle so it survives +# agent_bridge restarts — that matters because the Meta Quest headset holds +# a TCP socket open against ros_tcp_endpoint, and tearing that down would +# force the user to reconnect from the headset. + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + ros_tcp_endpoint_node = Node( + package="ros_tcp_endpoint", + executable="default_server_endpoint", + name="ros_tcp_endpoint", + emulate_tty=True, + parameters=[ + {"ROS_IP": "192.168.1.34"}, + ], + output="screen", + ) + + # Debug-only static TF linking the disjoint quest TF root into the robot tree + # at the world frame. The numerical values are wrong — quest and world have + # no real geometric relationship — but the link satisfies the canTransform() + # check inside marker_utils::transformPoseToBaseFrame() so VisualizePose calls + # against quest-frame poses (controller markers in v11) can render. + # + # No control path uses this transform: the v11 kinematic math composes the + # controller delta in quest and applies it to the EE in world directly, + # bypassing TF entirely. The lie is contained to RViz markers. + # + # Before shipping v11, remove the quest-frame VisualizePose calls from the + # objective and delete this node — neither will be needed. + static_tf_world_to_quest = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_world_to_quest", + output="log", + arguments=[ + "0", "0", "0", # x y z (identity translation; the link is geometrically meaningless) + "0", "0", "0", # yaw pitch roll + "world", "quest", + ], + ) + + return LaunchDescription([ros_tcp_endpoint_node, static_tf_world_to_quest]) diff --git a/src/lab_sim/objectives/quest_teleop.xml b/src/lab_sim/objectives/quest_teleop.xml new file mode 100644 index 000000000..81bcf0b35 --- /dev/null +++ b/src/lab_sim/objectives/quest_teleop.xml @@ -0,0 +1,444 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +