feat(examples/teleop_ros2): Add head-relative EE pose frame to teleop_ros2 example

examples/teleop_ros2/README.md

@@ -73,6 +73,7 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime.
   - `world_frame` → `right_wrist_frame`: Right wrist transform (published in `controller_teleop` and `hand_teleop` modes)
   - `world_frame` → `left_wrist_frame`: Left wrist transform (published in `controller_teleop` and `hand_teleop` modes)
   - `world_frame` → `head_frame`: Head transform (published in `controller_teleop` and `hand_teleop` modes)
+  - With `ee_poses_frame:=head` the wrist transforms are parented to `head_frame` instead of `world_frame`
 
 ## Run in Docker
 
@@ -125,7 +126,7 @@ docker run --rm --gpus all --net=host --ipc=host \
   -r xr_teleop/ee_poses:=my_robot/ee_poses
 ```
 
-Available parameters: `rate_hz`, `mode`, `hand_retargeter`, `config_asset_root`, `cloudxr_install_dir`, `cloudxr_env_config`, `cloudxr_accept_eula`, `cloudxr_setup_oob`, `cloudxr_usb_local`, `pedal_collection_id`, `world_frame`, `right_wrist_frame`, `left_wrist_frame`, `head_frame`, `left_finger_joint_names`, `right_finger_joint_names`. Use `ros2 param list /teleop_ros2_node` and `ros2 param describe /teleop_ros2_node <param>` (with the node running) for the full set.
+Available parameters: `rate_hz`, `mode`, `hand_retargeter`, `config_asset_root`, `cloudxr_install_dir`, `cloudxr_env_config`, `cloudxr_accept_eula`, `cloudxr_setup_oob`, `cloudxr_usb_local`, `pedal_collection_id`, `world_frame`, `right_wrist_frame`, `left_wrist_frame`, `head_frame`, `ee_poses_frame`, `left_finger_joint_names`, `right_finger_joint_names`. Use `ros2 param list /teleop_ros2_node` and `ros2 param describe /teleop_ros2_node <param>` (with the node running) for the full set.
 
 By default, `left_finger_joint_names` and `right_finger_joint_names` use the selected mode's retargeter joint names. They can be overridden to publish robot-specific names on `xr_teleop/finger_joints`, but each override must provide the same number of names as the joints emitted by that mode's retargeter.
 
@@ -144,6 +145,29 @@ The `mode` parameter selects the teleoperation scenario and which topics are pub
 
 Example: `--ros-args -p mode:=controller_raw`
 
+### EE Poses Frame
+
+The `ee_poses_frame` parameter selects the reference frame of `xr_teleop/ee_poses`:
+
+| Value | Behavior |
+|-------|----------|
+| `world` (default) | Absolute poses in `world_frame`; wrist TFs parented to `world_frame` |
+| `head` | Poses relative to the headset, stamped `head_frame`; wrist TFs parented to `head_frame` |
+
+Head-relative poses stay fixed when the operator walks or turns, so they track the
+hands as the robot's end effectors see them rather than as the session origin does.
+When no head pose is available the node warns and publishes in `world_frame`.
+
+```bash
+docker run --rm --gpus all --net=host --ipc=host \
+  -e NVIDIA_VISIBLE_DEVICES=all -e NVIDIA_DRIVER_CAPABILITIES=all \
+  -e ROS_LOCALHOST_ONLY=1 \
+  -v $HOME/.cloudxr:/root/.cloudxr \
+  --name teleop_ros2_ref \
+  teleop_ros2_ref --ros-args -p cloudxr_accept_eula:=true \
+  -p ee_poses_frame:=head
+```
+
 ### OOB Teleop Control
 
 For live sessions, the node can enable the out-of-band (OOB) teleop control hub

examples/teleop_ros2/python/constants.py

@@ -26,6 +26,11 @@ class TeleopMode(StrEnum):
     FULL_BODY = "full_body"
 
 
+class EePoseFrame(StrEnum):
+    WORLD = "world"
+    HEAD = "head"
+
+
 BODY_JOINT_NAMES = [e.name for e in BodyJointIndex]
 HAND_POSE_JOINT_INDICES = tuple(
     HandJointIndex(i)
@@ -35,6 +40,7 @@ class TeleopMode(StrEnum):
 HAND_RETARGETERS = tuple(retargeter.value for retargeter in HandRetargeter)
 SHARPA_HAND_RETARGETERS = (HandRetargeter.PINK_IK, HandRetargeter.DEXPILOT)
 TELEOP_MODES = tuple(mode.value for mode in TeleopMode)
+EE_POSE_FRAMES = tuple(f.value for f in EePoseFrame)
 
 TRIHAND_JOINT_NAMES = [
     "thumb_rotation",

examples/teleop_ros2/python/geometry.py

@@ -11,6 +11,47 @@
 from scipy.spatial.transform import Rotation
 
 
+def apply_relative_pose(reference: Pose, pose: Pose) -> Pose:
+    """
+    Transform ``pose`` in ``reference``'s frame, both poses needs to be
+    initially in the same frame.
+
+    This is equivalent to:
+
+        T_reference_pose = inv(T_world_reference) @ T_world_pose
+    """
+    reference_pos = np.array(
+        [reference.position.x, reference.position.y, reference.position.z],
+        dtype=float,
+    )
+    reference_rot = Rotation.from_quat(
+        [
+            reference.orientation.x,
+            reference.orientation.y,
+            reference.orientation.z,
+            reference.orientation.w,
+        ]
+    )
+    pos = np.array(
+        [pose.position.x, pose.position.y, pose.position.z],
+        dtype=float,
+    )
+    rot = Rotation.from_quat(
+        [
+            pose.orientation.x,
+            pose.orientation.y,
+            pose.orientation.z,
+            pose.orientation.w,
+        ]
+    )
+
+    reference_rot_inv = reference_rot.inv()
+    return to_pose(
+        reference_rot_inv.apply(pos - reference_pos),
+        (reference_rot_inv * rot).as_quat(),
+    )
+
+
 def apply_manus_controller_to_hand_pose(pose: Pose, side: str) -> Pose:
     """
     Apply MANUS controller-to-hand calibration in the pose's current frame.

examples/teleop_ros2/python/messages.py

@@ -28,6 +28,7 @@
 from constants import BODY_JOINT_NAMES, HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES
 from geometry import (
     apply_manus_controller_to_hand_pose,
+    apply_relative_pose,
     apply_transform_to_pose,
     make_transform,
     to_pose,
@@ -235,6 +236,33 @@ def _as_float(ctrl, index):
     )
 
 
+def rebase_ee_poses_relative_to_head(
+    ee_poses_msg: NamedPoseArray,
+    head_msg: PoseStamped | None,
+    left_wrist_frame: str,
+    right_wrist_frame: str,
+) -> tuple[NamedPoseArray, list[TransformStamped]]:
+    relative_poses: list[Pose | None] = []
+    for pose, is_valid in zip(ee_poses_msg.pose, ee_poses_msg.is_valid):
+        if is_valid:
+            relative_poses.append(apply_relative_pose(head_msg.pose, pose))
+        else:
+            relative_poses.append(None)
+
+    rebased_msg = _compose_ee_msg(
+        relative_poses[0],
+        relative_poses[1],
+        ee_poses_msg.header.stamp,
+        "head",
+    )
+
+    return rebased_msg, _wrist_tfs_from_ee_msg(
+        rebased_msg,
+        left_wrist_frame,
+        right_wrist_frame,
+    )
+
+
 def build_ee_output_from_controllers(
     left_ctrl: OptionalTensorGroup,
     right_ctrl: OptionalTensorGroup,
@@ -278,6 +306,7 @@ def build_ee_output_from_hands(
     right_wrist_frame: str,
     transform_rot: Rotation | None = None,
     transform_trans: Sequence[float] | None = None,
+    reference_pose: Pose | None = None,
 ) -> tuple[NamedPoseArray, list[TransformStamped]]:
     """Build the hand-derived EE message and its valid wrist TFs."""
     left_pose = _compute_ee_pose_from_hand(

examples/teleop_ros2/python/node_parameters.py

@@ -32,8 +32,10 @@
 from constants import (
     HAND_RETARGETERS,
     TELEOP_MODES,
+    EE_POSE_FRAMES,
     HandRetargeter,
     TeleopMode,
+    EePoseFrame,
     resolve_hand_retargeter,
     uses_hands_source_for_controller,
 )
@@ -76,6 +78,7 @@ class NodeParameters:
     transform_rotation: Rotation | None
     left_finger_joint_name_aliases: list[str] | None
     right_finger_joint_name_aliases: list[str] | None
+    ee_poses_frame: EePoseFrame
 
 
 def _load_cloudxr(node: Node) -> CloudXRParams:
@@ -363,6 +366,30 @@ def _load_mode(node: Node) -> TeleopMode:
     return mode
 
 
+def _load_ee_poses_frame(node: Node) -> EePoseFrame:
+    node.declare_parameter(
+        "ee_poses_frame",
+        EePoseFrame.WORLD.value,
+        ParameterDescriptor(
+            description=(
+                "Reference frame of the published end effector poses. "
+                "'world' (default) publishes absolute poses in world_frame; "
+                "'head' publishes poses relative to head_frame."
+            )
+        ),
+    )
+    raw_frame = node.get_parameter("ee_poses_frame").get_parameter_value().string_value
+    try:
+        ee_poses_frame = EePoseFrame(raw_frame)
+    except ValueError as exc:
+        raise ValueError(
+            f"Parameter 'ee_poses_frame' must be one of {EE_POSE_FRAMES}, "
+            f"got {raw_frame!r}"
+        ) from exc
+    node.get_logger().info(f"EE poses frame: {ee_poses_frame}")
+    return ee_poses_frame
+
+
 def _load_pedal_collection_id(node: Node) -> str:
     node.declare_parameter(
         "pedal_collection_id",
@@ -477,6 +504,7 @@ def create_node_parameters(node: Node) -> NodeParameters:
     transform_rotation = _load_transform_rotation(node)
     left_finger_joint_name_aliases = _load_finger_joint_name_aliases(node, "left")
     right_finger_joint_name_aliases = _load_finger_joint_name_aliases(node, "right")
+    ee_poses_frame = _load_ee_poses_frame(node)
 
     return NodeParameters(
         mode=mode,
@@ -497,4 +525,5 @@ def create_node_parameters(node: Node) -> NodeParameters:
         transform_rotation=transform_rotation,
         left_finger_joint_name_aliases=left_finger_joint_name_aliases,
         right_finger_joint_name_aliases=right_finger_joint_name_aliases,
+        ee_poses_frame=ee_poses_frame,
     )

examples/teleop_ros2/python/teleop_ros2_node.py

@@ -36,6 +36,7 @@
   - world_frame -> right_wrist_frame
   - world_frame -> left_wrist_frame
   - world_frame -> head_frame
+  - (With ee_poses_frame=head, world_frame -> head_frame -> left/right_wrist_frame)
 """
 
 import time
@@ -62,6 +63,7 @@
     build_hand_msg,
     build_head_output,
     build_root_command_output,
+    rebase_ee_poses_relative_to_head,
 )
 from teleop_profiles import (
     PublishType,
@@ -111,7 +113,9 @@ def _create_publishers(self) -> None:
         )
         self._pub_head = self.create_publisher(PoseStamped, "xr_teleop/head_pose", 10)
 
-    def _publish_ee_poses_from_controllers(self, result: SessionResult, now) -> None:
+    def _publish_ee_poses_from_controllers(
+        self, result: SessionResult, now, head_msg: PoseStamped | None
+    ) -> None:
         ee_poses_msg, wrist_tfs = build_ee_output_from_controllers(
             result["controller_left"],
             result["controller_right"],
@@ -123,6 +127,20 @@ def _publish_ee_poses_from_controllers(self, result: SessionResult, now) -> None
             self._params.transform_translation,
             self._params.controller_uses_hands_source,
         )
+        if self._params.ee_poses_frame.value == "head":
+            if head_msg is None:
+                self.get_logger().warn(
+                    "ee_poses_frame is 'head' but no head pose is available; "
+                    "publishing ee_poses_frame in default frame 'world'.",
+                    throttle_duration_sec=5.0,
+                )
+            else:
+                ee_poses_msg, wrist_tfs = rebase_ee_poses_relative_to_head(
+                    ee_poses_msg,
+                    head_msg,
+                    self._params.left_wrist_frame,
+                    self._params.right_wrist_frame,
+                )
         self._pub_ee_poses.publish(ee_poses_msg)
         if wrist_tfs:
             self._tf_broadcaster.sendTransform(wrist_tfs)
@@ -161,7 +179,9 @@ def _publish_hand_poses(self, result: SessionResult, now) -> None:
         )
         self._pub_hand.publish(hand_msg)
 
-    def _publish_ee_poses_from_hands(self, result: SessionResult, now) -> None:
+    def _publish_ee_poses_from_hands(
+        self, result: SessionResult, now, head_msg: PoseStamped | None
+    ) -> None:
         ee_poses_msg, wrist_tfs = build_ee_output_from_hands(
             result["hand_left"],
             result["hand_right"],
@@ -172,11 +192,25 @@ def _publish_ee_poses_from_hands(self, result: SessionResult, now) -> None:
             self._params.transform_rotation,
             self._params.transform_translation,
         )
+        if self._params.ee_poses_frame.value == "head":
+            if head_msg is None:
+                self.get_logger().warn(
+                    "ee_poses_frame is 'head' but no head pose is available;"
+                    "publishing ee_poses_frame in default frame 'world'.",
+                    throttle_duration_sec=5.0,
+                )
+            else:
+                ee_poses_msg, wrist_tfs = rebase_ee_poses_relative_to_head(
+                    ee_poses_msg,
+                    head_msg,
+                    self._params.left_wrist_frame,
+                    self._params.right_wrist_frame,
+                )
         self._pub_ee_poses.publish(ee_poses_msg)
         if wrist_tfs:
             self._tf_broadcaster.sendTransform(wrist_tfs)
 
-    def _publish_head(self, result: SessionResult, now) -> None:
+    def _publish_head(self, result: SessionResult, now) -> PoseStamped | None:
         maybe_head_output = build_head_output(
             result["head"],
             now,
@@ -192,6 +226,8 @@ def _publish_head(self, result: SessionResult, now) -> None:
         self._pub_head.publish(head_msg)
         self._tf_broadcaster.sendTransform(head_tf)
 
+        return head_msg
+
     def _publish_root_command(self, result: SessionResult, now) -> None:
         maybe_root_output = build_root_command_output(
             result["root_command"],
@@ -235,16 +271,22 @@ def _run_session_loop(self, launcher: CloudXRLauncher | None = None) -> int:
 
                         now = self.get_clock().now().to_msg()
 
+                        if PublishType.HEAD in self._profile_spec.publish_types:
+                            head_messages = self._publish_head(result, now)
                         if (
                             PublishType.EE_FROM_HANDS
                             in self._profile_spec.publish_types
                         ):
-                            self._publish_ee_poses_from_hands(result, now)
+                            self._publish_ee_poses_from_hands(
+                                result, now, head_messages
+                            )
                         if (
                             PublishType.EE_FROM_CONTROLLERS
                             in self._profile_spec.publish_types
                         ):
-                            self._publish_ee_poses_from_controllers(result, now)
+                            self._publish_ee_poses_from_controllers(
+                                result, now, head_messages
+                            )
                         if PublishType.HAND_POSES in self._profile_spec.publish_types:
                             self._publish_hand_poses(result, now)
                         if PublishType.ROOT_COMMAND in self._profile_spec.publish_types:
@@ -254,8 +296,6 @@ def _run_session_loop(self, launcher: CloudXRLauncher | None = None) -> int:
                             in self._profile_spec.publish_types
                         ):
                             self._publish_finger_joints(result, now)
-                        if PublishType.HEAD in self._profile_spec.publish_types:
-                            self._publish_head(result, now)
                         if (
                             PublishType.CONTROLLER_PAYLOAD
                             in self._profile_spec.publish_types