From 596c17934eb3cb95be8d45d427a990507ea2f142 Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Tue, 7 Jul 2026 22:21:55 +0000 Subject: [PATCH 1/8] ROS: Publish hand joint poses in separate topics --- examples/teleop_ros2/AGENTS.md | 4 +- examples/teleop_ros2/CMakeLists.txt | 4 + examples/teleop_ros2/Dockerfile | 11 ++ examples/teleop_ros2/README.md | 17 ++- examples/teleop_ros2/python/constants.py | 10 +- .../teleop_ros2_topic_verifier.py | 48 ++++++++ examples/teleop_ros2/python/messages.py | 107 ++++++++++++------ .../teleop_ros2/python/teleop_ros2_node.py | 61 ++++++++-- .../teleop_ros2_interfaces/CMakeLists.txt | 21 ++++ .../msg/HandJointPose.msg | 7 ++ .../msg/HandJointPoses.msg | 6 + .../teleop_ros2_interfaces/package.xml | 24 ++++ 12 files changed, 269 insertions(+), 51 deletions(-) create mode 100644 examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt create mode 100644 examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPose.msg create mode 100644 examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg create mode 100644 examples/teleop_ros2/teleop_ros2_interfaces/package.xml diff --git a/examples/teleop_ros2/AGENTS.md b/examples/teleop_ros2/AGENTS.md index 8954adfa2..a25a5032a 100644 --- a/examples/teleop_ros2/AGENTS.md +++ b/examples/teleop_ros2/AGENTS.md @@ -12,7 +12,7 @@ SPDX-License-Identifier: Apache-2.0 - The image build disables some CI gates (`-DENABLE_CLANG_FORMAT_CHECK=OFF`, `-DBUILD_TESTING=OFF`), so a green Docker build does not mean C++ formatting or ctest pass. Validate those separately. - When creating temporary Docker images for `examples/teleop_ros2` validation, remove them before finishing the task unless the user explicitly asks to keep them. -## Python Node Layout +## Source Layout -- In `python/teleop_ros2_node.py`, preserve the existing grouped/sorted organization for global non-member helpers and `TeleopRos2Node` member functions: scan the surrounding order before inserting, and do not place helpers near call sites when the existing section is sorted. +- In source code files under this example, preserve the existing grouped/sorted organization for helpers, message builders, classes, and member functions: scan the surrounding order before inserting, and do not place helpers near call sites when the existing section is sorted. - In Python integration test verifier code, do not use bare `assert` for runtime validation; Python optimization can disable it, so raise explicit exceptions from validators. diff --git a/examples/teleop_ros2/CMakeLists.txt b/examples/teleop_ros2/CMakeLists.txt index 29dc30328..cdab1df63 100644 --- a/examples/teleop_ros2/CMakeLists.txt +++ b/examples/teleop_ros2/CMakeLists.txt @@ -18,3 +18,7 @@ install(FILES README.md Dockerfile install(DIRECTORY configs assets DESTINATION examples/teleop_ros2 ) + +install(DIRECTORY teleop_ros2_interfaces + DESTINATION examples/teleop_ros2 +) diff --git a/examples/teleop_ros2/Dockerfile b/examples/teleop_ros2/Dockerfile index 47921599f..b05963af0 100644 --- a/examples/teleop_ros2/Dockerfile +++ b/examples/teleop_ros2/Dockerfile @@ -46,6 +46,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libxxf86vm-dev \ swig \ ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ + ros-${ROS_DISTRO}-rosidl-default-runtime \ && rm -rf /var/lib/apt/lists/* # Install uv via standalone script (avoids PEP 668 "externally managed" pip on Ubuntu 24.04 / Jazzy) @@ -113,6 +114,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ git \ patchelf \ pkg-config \ + ros-${ROS_DISTRO}-rosidl-default-generators \ && rm -rf /var/lib/apt/lists/* WORKDIR /opt/isaacteleop @@ -143,10 +145,19 @@ RUN --mount=type=cache,target=/ccache,id=isaacteleop-teleop-ros2-ccache-${ROS_DI -DISAAC_TELEOP_PYTHON_VERSION=${PYTHON_VERSION} \ && cmake --build build --config Release -j$(nproc) \ && cmake --install build --config Release \ + && bash -lc "source /opt/ros/${ROS_DISTRO}/setup.bash \ + && cmake -B build/teleop_ros2_interfaces -S examples/teleop_ros2/teleop_ros2_interfaces \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/opt/isaacteleop/install \ + && cmake --build build/teleop_ros2_interfaces --config Release -j$(nproc) \ + && cmake --install build/teleop_ros2_interfaces --config Release" \ && ccache --show-stats COPY --chmod=644 < bool: @@ -69,6 +75,36 @@ def _assert_pose_array(msg: PoseArray, *, expected_count: int) -> None: raise ValueError("pose array positions are all zero") +def _assert_hand_joint_poses(msg: HandJointPoses) -> None: + if msg.header.frame_id != "world": + raise ValueError(f"unexpected frame_id {msg.header.frame_id!r}") + names = [joint.name for joint in msg.joints] + if names != _EXPECTED_HAND_POSE_NAMES: + raise ValueError("hand joint pose names do not match the expected order") + if len(set(names)) != len(names): + raise ValueError("hand joint pose names are not unique") + if not any(bool(joint.valid) for joint in msg.joints): + raise ValueError("all hand joint poses are invalid") + + positions = [] + for joint in msg.joints: + pose = joint.pose + position = (pose.position.x, pose.position.y, pose.position.z) + orientation = ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) + if not _is_finite_sequence(position): + raise ValueError("hand joint pose position contains non-finite values") + if not _is_finite_sequence(orientation): + raise ValueError("hand joint pose orientation contains non-finite values") + positions.extend(position) + if not any(abs(value) > 1e-6 for value in positions): + raise ValueError("hand joint pose positions are all zero") + + def _assert_pose_stamped( msg: PoseStamped, *, require_nonzero_position: bool = False ) -> None: @@ -268,6 +304,18 @@ def _expected_subscriptions( PoseArray, lambda msg: _assert_pose_array(msg, expected_count=50), ), + ( + "hand_left", + "xr_teleop/hand_left", + HandJointPoses, + _assert_hand_joint_poses, + ), + ( + "hand_right", + "xr_teleop/hand_right", + HandJointPoses, + _assert_hand_joint_poses, + ), ( "ee_pose_left", "xr_teleop/ee_pose_left", diff --git a/examples/teleop_ros2/python/messages.py b/examples/teleop_ros2/python/messages.py index 60037cfd1..3e76c4e73 100644 --- a/examples/teleop_ros2/python/messages.py +++ b/examples/teleop_ros2/python/messages.py @@ -11,6 +11,7 @@ from geometry_msgs.msg import Pose, PoseArray, PoseStamped from scipy.spatial.transform import Rotation from sensor_msgs.msg import JointState +from teleop_ros2_interfaces.msg import HandJointPose, HandJointPoses from isaacteleop.retargeting_engine.interface import OptionalTensorGroup from isaacteleop.retargeting_engine.tensor_types.indices import ( @@ -21,9 +22,8 @@ HeadPoseIndex, ) -from constants import BODY_JOINT_NAMES +from constants import BODY_JOINT_NAMES, HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES from geometry import ( - append_hand_poses, apply_manus_controller_to_hand_pose, apply_transform_to_pose, to_pose, @@ -36,6 +36,42 @@ ) +def _build_hand_joint_poses( + hand: OptionalTensorGroup, + transform_rot: Rotation | None = None, + transform_trans: Sequence[float] | None = None, +) -> list[HandJointPose]: + positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) + orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS]) + joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID]) + + joints: list[HandJointPose] = [] + for joint_idx, joint_name in zip(HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES): + joint_is_valid = bool(joint_valid[joint_idx]) + if joint_is_valid: + pose = to_pose(positions[joint_idx], orientations[joint_idx]) + if transform_rot is not None or transform_trans is not None: + pose = apply_transform_to_pose(pose, transform_rot, transform_trans) + else: + pose = to_pose([0.0, 0.0, 0.0]) + joints.append(HandJointPose(name=joint_name, pose=pose, valid=joint_is_valid)) + return joints + + +def _build_hand_poses( + hand: OptionalTensorGroup, + transform_rot: Rotation | None = None, + transform_trans: Sequence[float] | None = None, +) -> list[Pose]: + if hand.is_none: + return [to_pose([0.0, 0.0, 0.0]) for _ in HAND_POSE_JOINT_INDICES] + + return [ + joint.pose + for joint in _build_hand_joint_poses(hand, transform_rot, transform_trans) + ] + + def _to_pose_stamped(pose: Pose, now, frame_id: str) -> PoseStamped: msg = PoseStamped() msg.header.stamp = now @@ -214,7 +250,29 @@ def build_full_body_payload(full_body: OptionalTensorGroup) -> Dict: } -def build_hand_msg_from_hands( +def build_hand_msg_from_hand( + hand: OptionalTensorGroup, + now, + frame_id: str, + transform_rot: Rotation | None = None, + transform_trans: Sequence[float] | None = None, +) -> HandJointPoses | None: + """Build named hand joint poses for one hand.""" + if hand.is_none: + return None + + msg = HandJointPoses() + msg.header.stamp = now + msg.header.frame_id = frame_id + msg.joints = _build_hand_joint_poses( + hand, + transform_rot, + transform_trans, + ) + return msg + + +def build_hand_pose_array( left_hand: OptionalTensorGroup, right_hand: OptionalTensorGroup, now, @@ -226,38 +284,17 @@ def build_hand_msg_from_hands( msg = PoseArray() msg.header.stamp = now msg.header.frame_id = frame_id - - if not left_hand.is_none: - left_positions = np.asarray(left_hand[HandInputIndex.JOINT_POSITIONS]) - left_orientations = np.asarray(left_hand[HandInputIndex.JOINT_ORIENTATIONS]) - left_valid = np.asarray(left_hand[HandInputIndex.JOINT_VALID]) - append_hand_poses( - msg.poses, - left_positions, - left_orientations, - left_valid, - transform_rot, - transform_trans, - ) - else: - for _ in range(HandJointIndex.WRIST, HandJointIndex.LITTLE_TIP + 1): - msg.poses.append(to_pose([0.0, 0.0, 0.0])) - - if not right_hand.is_none: - right_positions = np.asarray(right_hand[HandInputIndex.JOINT_POSITIONS]) - right_orientations = np.asarray(right_hand[HandInputIndex.JOINT_ORIENTATIONS]) - right_valid = np.asarray(right_hand[HandInputIndex.JOINT_VALID]) - append_hand_poses( - msg.poses, - right_positions, - right_orientations, - right_valid, - transform_rot, - transform_trans, - ) - else: - for _ in range(HandJointIndex.WRIST, HandJointIndex.LITTLE_TIP + 1): - msg.poses.append(to_pose([0.0, 0.0, 0.0])) + left_poses = _build_hand_poses( + left_hand, + transform_rot, + transform_trans, + ) + right_poses = _build_hand_poses( + right_hand, + transform_rot, + transform_trans, + ) + msg.poses = left_poses + right_poses return msg diff --git a/examples/teleop_ros2/python/teleop_ros2_node.py b/examples/teleop_ros2/python/teleop_ros2_node.py index 928943f20..155535ac2 100755 --- a/examples/teleop_ros2/python/teleop_ros2_node.py +++ b/examples/teleop_ros2/python/teleop_ros2_node.py @@ -15,7 +15,8 @@ (retargeted TriHand angles), controller_data, head_pose, and TF transforms for left/right wrists and head - hand_teleop: ee_pose_left/ee_pose_right (from hand tracking wrist), hand - (finger joint poses), finger_joints (retargeted Sharpa joint + (raw finger joint pose array), hand_left/hand_right (named + per-hand joint poses), finger_joints (retargeted Sharpa joint angles), root_twist/root_pose (from foot pedal locomotion), head_pose, and TF transforms for left/right wrists and head - controller_raw: controller_data only @@ -23,6 +24,8 @@ Topic names (remappable via ROS 2 remapping): - xr_teleop/hand (PoseArray): [finger_joint_poses...] + - xr_teleop/hand_left (HandJointPoses): named left hand joint poses + - xr_teleop/hand_right (HandJointPoses): named right hand joint poses - xr_teleop/ee_pose_left (PoseStamped): left hand/controller EE pose - xr_teleop/ee_pose_right (PoseStamped): right hand/controller EE pose - xr_teleop/root_twist (TwistStamped): root velocity command @@ -55,6 +58,7 @@ from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import ByteMultiArray +from teleop_ros2_interfaces.msg import HandJointPoses from tf2_ros import TransformBroadcaster from isaacteleop.cloudxr import CloudXRLauncher @@ -66,7 +70,8 @@ build_ee_msg_from_hand, build_finger_joints_msg, build_full_body_payload, - build_hand_msg_from_hands, + build_hand_msg_from_hand, + build_hand_pose_array, build_head_msg, ) from node_parameters import ( @@ -133,6 +138,12 @@ def _get_orientation(pose: Pose) -> List[float]: def _create_publishers(self) -> None: self._pub_hand = self.create_publisher(PoseArray, "xr_teleop/hand", 10) + self._pub_hand_left = self.create_publisher( + HandJointPoses, "xr_teleop/hand_left", 10 + ) + self._pub_hand_right = self.create_publisher( + HandJointPoses, "xr_teleop/hand_right", 10 + ) self._pub_ee_pose_left = self.create_publisher( PoseStamped, "xr_teleop/ee_pose_left", 10 ) @@ -189,16 +200,33 @@ def _publish_controller_outputs(self, result: dict, now) -> None: if wrist_tfs: self._tf_broadcaster.sendTransform(wrist_tfs) if self._params.controller_uses_hands_source: - hand_msg = build_hand_msg_from_hands( + hand_msg = build_hand_pose_array( + result["hand_left"], + result["hand_right"], + now, + self._params.world_frame, + self._params.transform_rotation, + self._params.transform_translation, + ) + self._pub_hand.publish(hand_msg) + left_hand_msg = build_hand_msg_from_hand( result["hand_left"], + now, + self._params.world_frame, + self._params.transform_rotation, + self._params.transform_translation, + ) + right_hand_msg = build_hand_msg_from_hand( result["hand_right"], now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, ) - if hand_msg.poses: - self._pub_hand.publish(hand_msg) + if left_hand_msg is not None: + self._pub_hand_left.publish(left_hand_msg) + if right_hand_msg is not None: + self._pub_hand_right.publish(right_hand_msg) def _publish_controller_payload(self, result: dict) -> None: if self._params.mode not in ( @@ -249,7 +277,7 @@ def _publish_full_body_payload(self, result: dict) -> None: def _publish_hand_tracking_outputs(self, result: dict, now) -> None: left_hand = result["hand_left"] right_hand = result["hand_right"] - hand_msg = build_hand_msg_from_hands( + hand_msg = build_hand_pose_array( left_hand, right_hand, now, @@ -257,8 +285,25 @@ def _publish_hand_tracking_outputs(self, result: dict, now) -> None: self._params.transform_rotation, self._params.transform_translation, ) - if hand_msg.poses: - self._pub_hand.publish(hand_msg) + self._pub_hand.publish(hand_msg) + left_hand_msg = build_hand_msg_from_hand( + left_hand, + now, + self._params.world_frame, + self._params.transform_rotation, + self._params.transform_translation, + ) + right_hand_msg = build_hand_msg_from_hand( + right_hand, + now, + self._params.world_frame, + self._params.transform_rotation, + self._params.transform_translation, + ) + if left_hand_msg is not None: + self._pub_hand_left.publish(left_hand_msg) + if right_hand_msg is not None: + self._pub_hand_right.publish(right_hand_msg) left_ee_msg = build_ee_msg_from_hand( left_hand, diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt b/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt new file mode 100644 index 000000000..b920bd953 --- /dev/null +++ b/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt @@ -0,0 +1,21 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.8) +project(teleop_ros2_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/HandJointPose.msg" + "msg/HandJointPoses.msg" + DEPENDENCIES + geometry_msgs + std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPose.msg b/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPose.msg new file mode 100644 index 000000000..769792df9 --- /dev/null +++ b/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPose.msg @@ -0,0 +1,7 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. +# SPDX-License-Identifier: Apache-2.0 + +# One named OpenXR hand joint pose. +string name +geometry_msgs/Pose pose +bool valid diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg b/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg new file mode 100644 index 000000000..cd44db993 --- /dev/null +++ b/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. +# SPDX-License-Identifier: Apache-2.0 + +# Named hand joint poses in the same frame. +std_msgs/Header header +HandJointPose[] joints diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/package.xml b/examples/teleop_ros2/teleop_ros2_interfaces/package.xml new file mode 100644 index 000000000..abca4a30e --- /dev/null +++ b/examples/teleop_ros2/teleop_ros2_interfaces/package.xml @@ -0,0 +1,24 @@ + + + + teleop_ros2_interfaces + 0.1.0 + Custom ROS 2 interfaces for the Isaac Teleop ROS 2 example. + Isaac Teleop Maintainers + Apache-2.0 + + ament_cmake + + geometry_msgs + rosidl_default_generators + std_msgs + + geometry_msgs + rosidl_default_runtime + std_msgs + + rosidl_interface_packages + From 343ef5a3fd223f42110ebea13d2f5d9466184839 Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Tue, 7 Jul 2026 23:04:26 +0000 Subject: [PATCH 2/8] ROS: Remove legacy hand_poses topic --- examples/teleop_ros2/README.md | 16 ++-- .../teleop_ros2_topic_verifier.py | 31 +------ examples/teleop_ros2/python/messages.py | 83 ++++--------------- .../teleop_ros2/python/teleop_ros2_node.py | 32 ++----- 4 files changed, 28 insertions(+), 134 deletions(-) diff --git a/examples/teleop_ros2/README.md b/examples/teleop_ros2/README.md index 13bca4ff8..2f9b01808 100644 --- a/examples/teleop_ros2/README.md +++ b/examples/teleop_ros2/README.md @@ -44,7 +44,7 @@ parameter: In `controller_teleop`, explicitly setting `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik` keeps XR controllers responsible for EE poses, wrist TFs, locomotion, and `controller_data`, while Manus/OpenXR hand data drives -`xr_teleop/hand`, `xr_teleop/hand_left`, `xr_teleop/hand_right`, and Sharpa +`xr_teleop/hand_left`, `xr_teleop/hand_right`, and Sharpa `xr_teleop/finger_joints`. The Docker build fetches the pinned official Sharpa Wave URDFs and installs them @@ -59,8 +59,6 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime. ## Published Topics -- `xr_teleop/hand` (`geometry_msgs/PoseArray`) - - Raw hand joint poses (all joints except palm, left then right); published by `hand_teleop` and by `controller_teleop` when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik` - `xr_teleop/hand_left` (`teleop_ros2_interfaces/msg/HandJointPoses`) - Named left OpenXR hand joint poses; `joints` has 25 entries, each with `name`, `pose`, and `valid`; includes `WRIST`, omits `PALM` - `xr_teleop/hand_right` (`teleop_ros2_interfaces/msg/HandJointPoses`) @@ -72,7 +70,7 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime. - `xr_teleop/root_twist` (`geometry_msgs/TwistStamped`) - `xr_teleop/root_pose` (`geometry_msgs/PoseStamped`) - `xr_teleop/head_pose` (`geometry_msgs/PoseStamped`) - - Head pose; published in `controller_teleop` and `hand_teleop` modes when head tracking is valid + - Head pose - `xr_teleop/controller_data` (`std_msgs/ByteMultiArray`, msgpack-encoded dictionary) - `xr_teleop/finger_joints` (`sensor_msgs/JointState`) - Retargeted finger joint angles for the robot; contains joint names and position arrays corresponding to the robot finger joints (TriHand in default `controller_teleop`, selected Sharpa retargeter in `hand_teleop`, or explicit Sharpa retargeter in `controller_teleop`) @@ -125,7 +123,8 @@ docker run --rm --gpus all --net=host --ipc=host \ --name teleop_ros2_ref \ teleop_ros2_ref --ros-args -p cloudxr_accept_eula:=true \ -p world_frame:=odom -p rate_hz:=30.0 \ - -r xr_teleop/hand:=my_robot/hand \ + -r xr_teleop/hand_left:=my_robot/hand_left \ + -r xr_teleop/hand_right:=my_robot/hand_right \ -r xr_teleop/ee_pose_left:=my_robot/ee_pose_left \ -r xr_teleop/ee_pose_right:=my_robot/ee_pose_right ``` @@ -134,7 +133,7 @@ Available parameters: `rate_hz`, `mode`, `hand_retargeter`, `config_asset_root`, 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. -Available topics for remapping: `xr_teleop/hand`, `xr_teleop/hand_left`, `xr_teleop/hand_right`, `xr_teleop/ee_pose_left`, `xr_teleop/ee_pose_right`, `xr_teleop/root_twist`, `xr_teleop/root_pose`, `xr_teleop/head_pose`, `xr_teleop/controller_data`, `xr_teleop/full_body`, `xr_teleop/finger_joints`. Active remaps can be inspected with `ros2 node info /teleop_ros2_node`. +Available topics for remapping: `xr_teleop/hand_left`, `xr_teleop/hand_right`, `xr_teleop/ee_pose_left`, `xr_teleop/ee_pose_right`, `xr_teleop/root_twist`, `xr_teleop/root_pose`, `xr_teleop/head_pose`, `xr_teleop/controller_data`, `xr_teleop/full_body`, `xr_teleop/finger_joints`. Active remaps can be inspected with `ros2 node info /teleop_ros2_node`. ### Mode @@ -142,8 +141,8 @@ The `mode` parameter selects the teleoperation scenario and which topics are pub | Mode | Topics published | |------|------------------| -| `controller_teleop` (default) | `ee_pose_left`, `ee_pose_right` (from controller aim pose), `root_twist`, `root_pose`, `head_pose`, `finger_joints` (TriHand by default; Sharpa from Manus/OpenXR hands when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`), `controller_data`, `tf` (from controller aim pose and head pose), and `hand`/`hand_left`/`hand_right` only for the explicit Sharpa retargeter path | -| `hand_teleop` | `ee_pose_left`, `ee_pose_right` (from hand tracking wrist), `hand` (raw finger joint pose array), `hand_left`/`hand_right` (named per-hand joint poses), `finger_joints` (finger joints in joint space), `root_twist`, `root_pose`, `head_pose`, `tf` (from hand tracking wrist and head pose); locomotion comes from the configured foot pedal collection | +| `controller_teleop` (default) | `ee_pose_left`, `ee_pose_right` (from controller aim pose), `root_twist`, `root_pose`, `head_pose`, `finger_joints` (TriHand by default; Sharpa from Manus/OpenXR hands when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`), `controller_data`, `tf` (from controller aim pose and head pose), and `hand_left`/`hand_right` only for the explicit Sharpa retargeter path | +| `hand_teleop` | `ee_pose_left`, `ee_pose_right` (from hand tracking wrist), `hand_left`/`hand_right` (named per-hand joint poses), `finger_joints` (finger joints in joint space), `root_twist`, `root_pose`, `head_pose`, `tf` (from hand tracking wrist and head pose); locomotion comes from the configured foot pedal collection | | `controller_raw` | `controller_data` only | | `full_body` | `full_body` and `controller_data` | @@ -198,7 +197,6 @@ coverage. ```bash docker exec -it teleop_ros2_ref /bin/bash -ros2 topic echo /xr_teleop/hand geometry_msgs/msg/PoseArray ros2 topic echo /xr_teleop/hand_left teleop_ros2_interfaces/msg/HandJointPoses ros2 topic echo /xr_teleop/hand_right teleop_ros2_interfaces/msg/HandJointPoses ros2 topic echo /xr_teleop/ee_pose_left geometry_msgs/msg/PoseStamped diff --git a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py index 3a5102038..20f99a200 100755 --- a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py +++ b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py @@ -15,7 +15,7 @@ import msgpack import rclpy -from geometry_msgs.msg import PoseArray, PoseStamped, TwistStamped +from geometry_msgs.msg import PoseStamped, TwistStamped from isaacteleop.retargeting_engine.tensor_types.indices import HandJointIndex from rclpy.node import Node from sensor_msgs.msg import JointState @@ -52,29 +52,6 @@ def _unpack_msgpack(msg: ByteMultiArray) -> dict: ) -def _assert_pose_array(msg: PoseArray, *, expected_count: int) -> None: - if msg.header.frame_id != "world": - raise ValueError(f"unexpected frame_id {msg.header.frame_id!r}") - if len(msg.poses) != expected_count: - raise ValueError(f"expected {expected_count} poses, got {len(msg.poses)}") - positions = [] - for pose in msg.poses: - position = (pose.position.x, pose.position.y, pose.position.z) - orientation = ( - pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w, - ) - if not _is_finite_sequence(position): - raise ValueError("pose position contains non-finite values") - if not _is_finite_sequence(orientation): - raise ValueError("pose orientation contains non-finite values") - positions.extend(position) - if not any(abs(value) > 1e-6 for value in positions): - raise ValueError("pose array positions are all zero") - - def _assert_hand_joint_poses(msg: HandJointPoses) -> None: if msg.header.frame_id != "world": raise ValueError(f"unexpected frame_id {msg.header.frame_id!r}") @@ -298,12 +275,6 @@ def _expected_subscriptions( ] if mode == "hand_teleop": return [ - ( - "hand", - "xr_teleop/hand", - PoseArray, - lambda msg: _assert_pose_array(msg, expected_count=50), - ), ( "hand_left", "xr_teleop/hand_left", diff --git a/examples/teleop_ros2/python/messages.py b/examples/teleop_ros2/python/messages.py index 3e76c4e73..06afabc7b 100644 --- a/examples/teleop_ros2/python/messages.py +++ b/examples/teleop_ros2/python/messages.py @@ -8,7 +8,7 @@ from typing import Dict, Sequence import numpy as np -from geometry_msgs.msg import Pose, PoseArray, PoseStamped +from geometry_msgs.msg import Pose, PoseStamped from scipy.spatial.transform import Rotation from sensor_msgs.msg import JointState from teleop_ros2_interfaces.msg import HandJointPose, HandJointPoses @@ -36,42 +36,6 @@ ) -def _build_hand_joint_poses( - hand: OptionalTensorGroup, - transform_rot: Rotation | None = None, - transform_trans: Sequence[float] | None = None, -) -> list[HandJointPose]: - positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) - orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS]) - joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID]) - - joints: list[HandJointPose] = [] - for joint_idx, joint_name in zip(HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES): - joint_is_valid = bool(joint_valid[joint_idx]) - if joint_is_valid: - pose = to_pose(positions[joint_idx], orientations[joint_idx]) - if transform_rot is not None or transform_trans is not None: - pose = apply_transform_to_pose(pose, transform_rot, transform_trans) - else: - pose = to_pose([0.0, 0.0, 0.0]) - joints.append(HandJointPose(name=joint_name, pose=pose, valid=joint_is_valid)) - return joints - - -def _build_hand_poses( - hand: OptionalTensorGroup, - transform_rot: Rotation | None = None, - transform_trans: Sequence[float] | None = None, -) -> list[Pose]: - if hand.is_none: - return [to_pose([0.0, 0.0, 0.0]) for _ in HAND_POSE_JOINT_INDICES] - - return [ - joint.pose - for joint in _build_hand_joint_poses(hand, transform_rot, transform_trans) - ] - - def _to_pose_stamped(pose: Pose, now, frame_id: str) -> PoseStamped: msg = PoseStamped() msg.header.stamp = now @@ -264,38 +228,21 @@ def build_hand_msg_from_hand( msg = HandJointPoses() msg.header.stamp = now msg.header.frame_id = frame_id - msg.joints = _build_hand_joint_poses( - hand, - transform_rot, - transform_trans, - ) - return msg - - -def build_hand_pose_array( - left_hand: OptionalTensorGroup, - right_hand: OptionalTensorGroup, - now, - frame_id: str, - transform_rot: Rotation | None = None, - transform_trans: Sequence[float] | None = None, -) -> PoseArray: - """Build a PoseArray with finger joint poses, left hand then right hand.""" - msg = PoseArray() - msg.header.stamp = now - msg.header.frame_id = frame_id - left_poses = _build_hand_poses( - left_hand, - transform_rot, - transform_trans, - ) - right_poses = _build_hand_poses( - right_hand, - transform_rot, - transform_trans, - ) - msg.poses = left_poses + right_poses + positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) + orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS]) + joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID]) + for joint_idx, joint_name in zip(HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES): + joint_is_valid = bool(joint_valid[joint_idx]) + if joint_is_valid: + pose = to_pose(positions[joint_idx], orientations[joint_idx]) + if transform_rot is not None or transform_trans is not None: + pose = apply_transform_to_pose(pose, transform_rot, transform_trans) + else: + pose = to_pose([0.0, 0.0, 0.0]) + msg.joints.append( + HandJointPose(name=joint_name, pose=pose, valid=joint_is_valid) + ) return msg diff --git a/examples/teleop_ros2/python/teleop_ros2_node.py b/examples/teleop_ros2/python/teleop_ros2_node.py index 155535ac2..7bacc4cbb 100755 --- a/examples/teleop_ros2/python/teleop_ros2_node.py +++ b/examples/teleop_ros2/python/teleop_ros2_node.py @@ -14,16 +14,15 @@ aim pose), root_twist, root_pose, finger_joints (retargeted TriHand angles), controller_data, head_pose, and TF transforms for left/right wrists and head - - hand_teleop: ee_pose_left/ee_pose_right (from hand tracking wrist), hand - (raw finger joint pose array), hand_left/hand_right (named - per-hand joint poses), finger_joints (retargeted Sharpa joint - angles), root_twist/root_pose (from foot pedal locomotion), - head_pose, and TF transforms for left/right wrists and head + - hand_teleop: ee_pose_left/ee_pose_right (from hand tracking wrist), + hand_left/hand_right (named per-hand joint poses), + finger_joints (retargeted Sharpa joint angles), + root_twist/root_pose (from foot pedal locomotion), head_pose, + and TF transforms for left/right wrists and head - controller_raw: controller_data only - full_body: full_body and controller_data Topic names (remappable via ROS 2 remapping): - - xr_teleop/hand (PoseArray): [finger_joint_poses...] - xr_teleop/hand_left (HandJointPoses): named left hand joint poses - xr_teleop/hand_right (HandJointPoses): named right hand joint poses - xr_teleop/ee_pose_left (PoseStamped): left hand/controller EE pose @@ -50,7 +49,6 @@ import rclpy from geometry_msgs.msg import ( Pose, - PoseArray, PoseStamped, TransformStamped, TwistStamped, @@ -71,7 +69,6 @@ build_finger_joints_msg, build_full_body_payload, build_hand_msg_from_hand, - build_hand_pose_array, build_head_msg, ) from node_parameters import ( @@ -137,7 +134,6 @@ def _get_orientation(pose: Pose) -> List[float]: return tfs def _create_publishers(self) -> None: - self._pub_hand = self.create_publisher(PoseArray, "xr_teleop/hand", 10) self._pub_hand_left = self.create_publisher( HandJointPoses, "xr_teleop/hand_left", 10 ) @@ -200,15 +196,6 @@ def _publish_controller_outputs(self, result: dict, now) -> None: if wrist_tfs: self._tf_broadcaster.sendTransform(wrist_tfs) if self._params.controller_uses_hands_source: - hand_msg = build_hand_pose_array( - result["hand_left"], - result["hand_right"], - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - ) - self._pub_hand.publish(hand_msg) left_hand_msg = build_hand_msg_from_hand( result["hand_left"], now, @@ -277,15 +264,6 @@ def _publish_full_body_payload(self, result: dict) -> None: def _publish_hand_tracking_outputs(self, result: dict, now) -> None: left_hand = result["hand_left"] right_hand = result["hand_right"] - hand_msg = build_hand_pose_array( - left_hand, - right_hand, - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - ) - self._pub_hand.publish(hand_msg) left_hand_msg = build_hand_msg_from_hand( left_hand, now, From abd3a40fcebd0699ff0fb43be88b1a6950861831 Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Mon, 13 Jul 2026 22:30:08 +0000 Subject: [PATCH 3/8] Update messages to use index-aligned name, pose, and is_valid arrays --- examples/teleop_ros2/README.md | 4 ++-- .../integration_tests/teleop_ros2_topic_verifier.py | 9 +++++---- examples/teleop_ros2/python/messages.py | 8 ++++---- .../teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt | 1 - .../teleop_ros2_interfaces/msg/HandJointPose.msg | 7 ------- .../teleop_ros2_interfaces/msg/HandJointPoses.msg | 4 +++- 6 files changed, 14 insertions(+), 19 deletions(-) delete mode 100644 examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPose.msg diff --git a/examples/teleop_ros2/README.md b/examples/teleop_ros2/README.md index 2f9b01808..317b384f2 100644 --- a/examples/teleop_ros2/README.md +++ b/examples/teleop_ros2/README.md @@ -60,9 +60,9 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime. ## Published Topics - `xr_teleop/hand_left` (`teleop_ros2_interfaces/msg/HandJointPoses`) - - Named left OpenXR hand joint poses; `joints` has 25 entries, each with `name`, `pose`, and `valid`; includes `WRIST`, omits `PALM` + - Named left OpenXR hand joint poses; `name`, `pose`, and `is_valid` each have 25 index-aligned entries; includes `WRIST`, omits `PALM` - `xr_teleop/hand_right` (`teleop_ros2_interfaces/msg/HandJointPoses`) - - Named right OpenXR hand joint poses; `joints` has 25 entries, each with `name`, `pose`, and `valid`; includes `WRIST`, omits `PALM` + - Named right OpenXR hand joint poses; `name`, `pose`, and `is_valid` each have 25 index-aligned entries; includes `WRIST`, omits `PALM` - `xr_teleop/ee_pose_left` (`geometry_msgs/PoseStamped`) - Left hand/controller EE pose; published when the left EE source is active - `xr_teleop/ee_pose_right` (`geometry_msgs/PoseStamped`) diff --git a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py index 20f99a200..1c670b091 100755 --- a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py +++ b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py @@ -55,17 +55,18 @@ def _unpack_msgpack(msg: ByteMultiArray) -> dict: def _assert_hand_joint_poses(msg: HandJointPoses) -> None: if msg.header.frame_id != "world": raise ValueError(f"unexpected frame_id {msg.header.frame_id!r}") - names = [joint.name for joint in msg.joints] + names = list(msg.name) if names != _EXPECTED_HAND_POSE_NAMES: raise ValueError("hand joint pose names do not match the expected order") if len(set(names)) != len(names): raise ValueError("hand joint pose names are not unique") - if not any(bool(joint.valid) for joint in msg.joints): + if len(msg.pose) != len(names) or len(msg.is_valid) != len(names): + raise ValueError("hand joint pose arrays differ in length") + if not any(bool(is_valid) for is_valid in msg.is_valid): raise ValueError("all hand joint poses are invalid") positions = [] - for joint in msg.joints: - pose = joint.pose + for pose in msg.pose: position = (pose.position.x, pose.position.y, pose.position.z) orientation = ( pose.orientation.x, diff --git a/examples/teleop_ros2/python/messages.py b/examples/teleop_ros2/python/messages.py index 06afabc7b..40f2a910a 100644 --- a/examples/teleop_ros2/python/messages.py +++ b/examples/teleop_ros2/python/messages.py @@ -11,7 +11,7 @@ from geometry_msgs.msg import Pose, PoseStamped from scipy.spatial.transform import Rotation from sensor_msgs.msg import JointState -from teleop_ros2_interfaces.msg import HandJointPose, HandJointPoses +from teleop_ros2_interfaces.msg import HandJointPoses from isaacteleop.retargeting_engine.interface import OptionalTensorGroup from isaacteleop.retargeting_engine.tensor_types.indices import ( @@ -240,9 +240,9 @@ def build_hand_msg_from_hand( pose = apply_transform_to_pose(pose, transform_rot, transform_trans) else: pose = to_pose([0.0, 0.0, 0.0]) - msg.joints.append( - HandJointPose(name=joint_name, pose=pose, valid=joint_is_valid) - ) + msg.name.append(joint_name) + msg.pose.append(pose) + msg.is_valid.append(joint_is_valid) return msg diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt b/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt index b920bd953..1c195ae1b 100644 --- a/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt +++ b/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "msg/HandJointPose.msg" "msg/HandJointPoses.msg" DEPENDENCIES geometry_msgs diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPose.msg b/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPose.msg deleted file mode 100644 index 769792df9..000000000 --- a/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPose.msg +++ /dev/null @@ -1,7 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. -# SPDX-License-Identifier: Apache-2.0 - -# One named OpenXR hand joint pose. -string name -geometry_msgs/Pose pose -bool valid diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg b/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg index cd44db993..d47ba8cb3 100644 --- a/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg +++ b/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg @@ -3,4 +3,6 @@ # Named hand joint poses in the same frame. std_msgs/Header header -HandJointPose[] joints +string[] name +geometry_msgs/Pose[] pose +bool[] is_valid From b114991cf40d1d653ad27f6aacc9fc2baebfeb01 Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Mon, 13 Jul 2026 22:32:46 +0000 Subject: [PATCH 4/8] Consolidate left and right hand topics into a single topic --- examples/teleop_ros2/README.md | 21 ++++------ .../teleop_ros2_topic_verifier.py | 15 +++---- examples/teleop_ros2/python/messages.py | 40 ++++++++++-------- .../teleop_ros2/python/teleop_ros2_node.py | 42 +++++-------------- 4 files changed, 46 insertions(+), 72 deletions(-) diff --git a/examples/teleop_ros2/README.md b/examples/teleop_ros2/README.md index 317b384f2..f0b538c9d 100644 --- a/examples/teleop_ros2/README.md +++ b/examples/teleop_ros2/README.md @@ -44,8 +44,7 @@ parameter: In `controller_teleop`, explicitly setting `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik` keeps XR controllers responsible for EE poses, wrist TFs, locomotion, and `controller_data`, while Manus/OpenXR hand data drives -`xr_teleop/hand_left`, `xr_teleop/hand_right`, and Sharpa -`xr_teleop/finger_joints`. +`xr_teleop/hand` and Sharpa `xr_teleop/finger_joints`. The Docker build fetches the pinned official Sharpa Wave URDFs and installs them at `/opt/isaacteleop/install/examples/teleop_ros2/assets/urdf/sharpa_standalone/`. @@ -59,10 +58,8 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime. ## Published Topics -- `xr_teleop/hand_left` (`teleop_ros2_interfaces/msg/HandJointPoses`) - - Named left OpenXR hand joint poses; `name`, `pose`, and `is_valid` each have 25 index-aligned entries; includes `WRIST`, omits `PALM` -- `xr_teleop/hand_right` (`teleop_ros2_interfaces/msg/HandJointPoses`) - - Named right OpenXR hand joint poses; `name`, `pose`, and `is_valid` each have 25 index-aligned entries; includes `WRIST`, omits `PALM` +- `xr_teleop/hand` (`teleop_ros2_interfaces/msg/HandJointPoses`) + - Named OpenXR hand joint poses; `name`, `pose`, and `is_valid` are index-aligned, names use `left_`/`right_` prefixes, and each available hand contributes 25 entries including `WRIST` and omitting `PALM` - `xr_teleop/ee_pose_left` (`geometry_msgs/PoseStamped`) - Left hand/controller EE pose; published when the left EE source is active - `xr_teleop/ee_pose_right` (`geometry_msgs/PoseStamped`) @@ -123,8 +120,7 @@ docker run --rm --gpus all --net=host --ipc=host \ --name teleop_ros2_ref \ teleop_ros2_ref --ros-args -p cloudxr_accept_eula:=true \ -p world_frame:=odom -p rate_hz:=30.0 \ - -r xr_teleop/hand_left:=my_robot/hand_left \ - -r xr_teleop/hand_right:=my_robot/hand_right \ + -r xr_teleop/hand:=my_robot/hand \ -r xr_teleop/ee_pose_left:=my_robot/ee_pose_left \ -r xr_teleop/ee_pose_right:=my_robot/ee_pose_right ``` @@ -133,7 +129,7 @@ Available parameters: `rate_hz`, `mode`, `hand_retargeter`, `config_asset_root`, 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. -Available topics for remapping: `xr_teleop/hand_left`, `xr_teleop/hand_right`, `xr_teleop/ee_pose_left`, `xr_teleop/ee_pose_right`, `xr_teleop/root_twist`, `xr_teleop/root_pose`, `xr_teleop/head_pose`, `xr_teleop/controller_data`, `xr_teleop/full_body`, `xr_teleop/finger_joints`. Active remaps can be inspected with `ros2 node info /teleop_ros2_node`. +Available topics for remapping: `xr_teleop/hand`, `xr_teleop/ee_pose_left`, `xr_teleop/ee_pose_right`, `xr_teleop/root_twist`, `xr_teleop/root_pose`, `xr_teleop/head_pose`, `xr_teleop/controller_data`, `xr_teleop/full_body`, `xr_teleop/finger_joints`. Active remaps can be inspected with `ros2 node info /teleop_ros2_node`. ### Mode @@ -141,8 +137,8 @@ The `mode` parameter selects the teleoperation scenario and which topics are pub | Mode | Topics published | |------|------------------| -| `controller_teleop` (default) | `ee_pose_left`, `ee_pose_right` (from controller aim pose), `root_twist`, `root_pose`, `head_pose`, `finger_joints` (TriHand by default; Sharpa from Manus/OpenXR hands when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`), `controller_data`, `tf` (from controller aim pose and head pose), and `hand_left`/`hand_right` only for the explicit Sharpa retargeter path | -| `hand_teleop` | `ee_pose_left`, `ee_pose_right` (from hand tracking wrist), `hand_left`/`hand_right` (named per-hand joint poses), `finger_joints` (finger joints in joint space), `root_twist`, `root_pose`, `head_pose`, `tf` (from hand tracking wrist and head pose); locomotion comes from the configured foot pedal collection | +| `controller_teleop` (default) | `ee_pose_left`, `ee_pose_right` (from controller aim pose), `root_twist`, `root_pose`, `head_pose`, `finger_joints` (TriHand by default; Sharpa from Manus/OpenXR hands when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`), `controller_data`, `tf` (from controller aim pose and head pose), and `hand` only for the explicit Sharpa retargeter path | +| `hand_teleop` | `ee_pose_left`, `ee_pose_right` (from hand tracking wrist), `hand` (named left and right joint poses), `finger_joints` (finger joints in joint space), `root_twist`, `root_pose`, `head_pose`, `tf` (from hand tracking wrist and head pose); locomotion comes from the configured foot pedal collection | | `controller_raw` | `controller_data` only | | `full_body` | `full_body` and `controller_data` | @@ -197,8 +193,7 @@ coverage. ```bash docker exec -it teleop_ros2_ref /bin/bash -ros2 topic echo /xr_teleop/hand_left teleop_ros2_interfaces/msg/HandJointPoses -ros2 topic echo /xr_teleop/hand_right teleop_ros2_interfaces/msg/HandJointPoses +ros2 topic echo /xr_teleop/hand teleop_ros2_interfaces/msg/HandJointPoses ros2 topic echo /xr_teleop/ee_pose_left geometry_msgs/msg/PoseStamped ros2 topic echo /xr_teleop/ee_pose_right geometry_msgs/msg/PoseStamped ros2 topic echo /xr_teleop/root_twist geometry_msgs/msg/TwistStamped diff --git a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py index 1c670b091..ea52eaddb 100755 --- a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py +++ b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py @@ -26,10 +26,13 @@ _MODES = ("controller_teleop", "hand_teleop", "controller_raw", "full_body") _EXPECTED_TF_FRAMES = {"right_wrist", "left_wrist", "head"} -_EXPECTED_HAND_POSE_NAMES = [ +_HAND_POSE_NAMES = [ HandJointIndex(i).name for i in range(HandJointIndex.WRIST, HandJointIndex.LITTLE_TIP + 1) ] +_EXPECTED_HAND_POSE_NAMES = [ + f"{side}_{name}" for side in ("left", "right") for name in _HAND_POSE_NAMES +] def _is_finite_sequence(values) -> bool: @@ -277,14 +280,8 @@ def _expected_subscriptions( if mode == "hand_teleop": return [ ( - "hand_left", - "xr_teleop/hand_left", - HandJointPoses, - _assert_hand_joint_poses, - ), - ( - "hand_right", - "xr_teleop/hand_right", + "hand", + "xr_teleop/hand", HandJointPoses, _assert_hand_joint_poses, ), diff --git a/examples/teleop_ros2/python/messages.py b/examples/teleop_ros2/python/messages.py index 40f2a910a..4279626d9 100644 --- a/examples/teleop_ros2/python/messages.py +++ b/examples/teleop_ros2/python/messages.py @@ -214,35 +214,39 @@ def build_full_body_payload(full_body: OptionalTensorGroup) -> Dict: } -def build_hand_msg_from_hand( - hand: OptionalTensorGroup, +def build_hand_msg_from_hands( + left_hand: OptionalTensorGroup, + right_hand: OptionalTensorGroup, now, frame_id: str, transform_rot: Rotation | None = None, transform_trans: Sequence[float] | None = None, ) -> HandJointPoses | None: - """Build named hand joint poses for one hand.""" - if hand.is_none: + """Build named hand joint poses for available hands, left then right.""" + if left_hand.is_none and right_hand.is_none: return None msg = HandJointPoses() msg.header.stamp = now msg.header.frame_id = frame_id - positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) - orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS]) - joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID]) - for joint_idx, joint_name in zip(HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES): - joint_is_valid = bool(joint_valid[joint_idx]) - if joint_is_valid: - pose = to_pose(positions[joint_idx], orientations[joint_idx]) - if transform_rot is not None or transform_trans is not None: - pose = apply_transform_to_pose(pose, transform_rot, transform_trans) - else: - pose = to_pose([0.0, 0.0, 0.0]) - msg.name.append(joint_name) - msg.pose.append(pose) - msg.is_valid.append(joint_is_valid) + for side, hand in (("left", left_hand), ("right", right_hand)): + if hand.is_none: + continue + positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) + orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS]) + joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID]) + for joint_idx, joint_name in zip(HAND_POSE_JOINT_INDICES, HAND_POSE_NAMES): + joint_is_valid = bool(joint_valid[joint_idx]) + if joint_is_valid: + pose = to_pose(positions[joint_idx], orientations[joint_idx]) + if transform_rot is not None or transform_trans is not None: + pose = apply_transform_to_pose(pose, transform_rot, transform_trans) + else: + pose = to_pose([0.0, 0.0, 0.0]) + msg.name.append(f"{side}_{joint_name}") + msg.pose.append(pose) + msg.is_valid.append(joint_is_valid) return msg diff --git a/examples/teleop_ros2/python/teleop_ros2_node.py b/examples/teleop_ros2/python/teleop_ros2_node.py index 7bacc4cbb..4d2a82cd5 100755 --- a/examples/teleop_ros2/python/teleop_ros2_node.py +++ b/examples/teleop_ros2/python/teleop_ros2_node.py @@ -15,7 +15,7 @@ (retargeted TriHand angles), controller_data, head_pose, and TF transforms for left/right wrists and head - hand_teleop: ee_pose_left/ee_pose_right (from hand tracking wrist), - hand_left/hand_right (named per-hand joint poses), + hand (named left and right joint poses), finger_joints (retargeted Sharpa joint angles), root_twist/root_pose (from foot pedal locomotion), head_pose, and TF transforms for left/right wrists and head @@ -23,8 +23,7 @@ - full_body: full_body and controller_data Topic names (remappable via ROS 2 remapping): - - xr_teleop/hand_left (HandJointPoses): named left hand joint poses - - xr_teleop/hand_right (HandJointPoses): named right hand joint poses + - xr_teleop/hand (HandJointPoses): named left and right hand joint poses - xr_teleop/ee_pose_left (PoseStamped): left hand/controller EE pose - xr_teleop/ee_pose_right (PoseStamped): right hand/controller EE pose - xr_teleop/root_twist (TwistStamped): root velocity command @@ -68,7 +67,7 @@ build_ee_msg_from_hand, build_finger_joints_msg, build_full_body_payload, - build_hand_msg_from_hand, + build_hand_msg_from_hands, build_head_msg, ) from node_parameters import ( @@ -134,12 +133,7 @@ def _get_orientation(pose: Pose) -> List[float]: return tfs def _create_publishers(self) -> None: - self._pub_hand_left = self.create_publisher( - HandJointPoses, "xr_teleop/hand_left", 10 - ) - self._pub_hand_right = self.create_publisher( - HandJointPoses, "xr_teleop/hand_right", 10 - ) + self._pub_hand = self.create_publisher(HandJointPoses, "xr_teleop/hand", 10) self._pub_ee_pose_left = self.create_publisher( PoseStamped, "xr_teleop/ee_pose_left", 10 ) @@ -196,24 +190,16 @@ def _publish_controller_outputs(self, result: dict, now) -> None: if wrist_tfs: self._tf_broadcaster.sendTransform(wrist_tfs) if self._params.controller_uses_hands_source: - left_hand_msg = build_hand_msg_from_hand( + hand_msg = build_hand_msg_from_hands( result["hand_left"], - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - ) - right_hand_msg = build_hand_msg_from_hand( result["hand_right"], now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, ) - if left_hand_msg is not None: - self._pub_hand_left.publish(left_hand_msg) - if right_hand_msg is not None: - self._pub_hand_right.publish(right_hand_msg) + if hand_msg is not None: + self._pub_hand.publish(hand_msg) def _publish_controller_payload(self, result: dict) -> None: if self._params.mode not in ( @@ -264,24 +250,16 @@ def _publish_full_body_payload(self, result: dict) -> None: def _publish_hand_tracking_outputs(self, result: dict, now) -> None: left_hand = result["hand_left"] right_hand = result["hand_right"] - left_hand_msg = build_hand_msg_from_hand( + hand_msg = build_hand_msg_from_hands( left_hand, - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - ) - right_hand_msg = build_hand_msg_from_hand( right_hand, now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, ) - if left_hand_msg is not None: - self._pub_hand_left.publish(left_hand_msg) - if right_hand_msg is not None: - self._pub_hand_right.publish(right_hand_msg) + if hand_msg is not None: + self._pub_hand.publish(hand_msg) left_ee_msg = build_ee_msg_from_hand( left_hand, From 7ab32a5bc9fa567381238e1cf2ca74896cb84953 Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Mon, 13 Jul 2026 23:16:02 +0000 Subject: [PATCH 5/8] Use NamedPoseArray for hand and ee pose topics --- examples/teleop_ros2/README.md | 24 ++- .../teleop_ros2_topic_verifier.py | 85 +++++----- examples/teleop_ros2/python/messages.py | 148 +++++++++++++----- .../teleop_ros2/python/teleop_ros2_node.py | 142 ++++++----------- .../teleop_ros2_interfaces/CMakeLists.txt | 2 +- ...{HandJointPoses.msg => NamedPoseArray.msg} | 2 +- 6 files changed, 208 insertions(+), 195 deletions(-) rename examples/teleop_ros2/teleop_ros2_interfaces/msg/{HandJointPoses.msg => NamedPoseArray.msg} (81%) diff --git a/examples/teleop_ros2/README.md b/examples/teleop_ros2/README.md index f0b538c9d..99e694889 100644 --- a/examples/teleop_ros2/README.md +++ b/examples/teleop_ros2/README.md @@ -58,12 +58,10 @@ Robot assets are never downloaded by `teleop_ros2_node.py` at runtime. ## Published Topics -- `xr_teleop/hand` (`teleop_ros2_interfaces/msg/HandJointPoses`) - - Named OpenXR hand joint poses; `name`, `pose`, and `is_valid` are index-aligned, names use `left_`/`right_` prefixes, and each available hand contributes 25 entries including `WRIST` and omitting `PALM` -- `xr_teleop/ee_pose_left` (`geometry_msgs/PoseStamped`) - - Left hand/controller EE pose; published when the left EE source is active -- `xr_teleop/ee_pose_right` (`geometry_msgs/PoseStamped`) - - Right hand/controller EE pose; published when the right EE source is active +- `xr_teleop/hand` (`teleop_ros2_interfaces/msg/NamedPoseArray`) + - Named OpenXR hand joint poses; always contains 25 `left_` entries followed by 25 `right_` entries, including `WRIST` and omitting `PALM`; missing or invalid joints use zero poses with `is_valid=false` +- `xr_teleop/ee_pose` (`teleop_ros2_interfaces/msg/NamedPoseArray`) + - Named hand/controller EE poses; always contains `left` and `right` entries, with zero poses and `is_valid=false` for invalid sides - `xr_teleop/root_twist` (`geometry_msgs/TwistStamped`) - `xr_teleop/root_pose` (`geometry_msgs/PoseStamped`) - `xr_teleop/head_pose` (`geometry_msgs/PoseStamped`) @@ -121,15 +119,14 @@ docker run --rm --gpus all --net=host --ipc=host \ teleop_ros2_ref --ros-args -p cloudxr_accept_eula:=true \ -p world_frame:=odom -p rate_hz:=30.0 \ -r xr_teleop/hand:=my_robot/hand \ - -r xr_teleop/ee_pose_left:=my_robot/ee_pose_left \ - -r xr_teleop/ee_pose_right:=my_robot/ee_pose_right + -r xr_teleop/ee_pose:=my_robot/ee_pose ``` 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 ` (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. -Available topics for remapping: `xr_teleop/hand`, `xr_teleop/ee_pose_left`, `xr_teleop/ee_pose_right`, `xr_teleop/root_twist`, `xr_teleop/root_pose`, `xr_teleop/head_pose`, `xr_teleop/controller_data`, `xr_teleop/full_body`, `xr_teleop/finger_joints`. Active remaps can be inspected with `ros2 node info /teleop_ros2_node`. +Available topics for remapping: `xr_teleop/hand`, `xr_teleop/ee_pose`, `xr_teleop/root_twist`, `xr_teleop/root_pose`, `xr_teleop/head_pose`, `xr_teleop/controller_data`, `xr_teleop/full_body`, `xr_teleop/finger_joints`. Active remaps can be inspected with `ros2 node info /teleop_ros2_node`. ### Mode @@ -137,8 +134,8 @@ The `mode` parameter selects the teleoperation scenario and which topics are pub | Mode | Topics published | |------|------------------| -| `controller_teleop` (default) | `ee_pose_left`, `ee_pose_right` (from controller aim pose), `root_twist`, `root_pose`, `head_pose`, `finger_joints` (TriHand by default; Sharpa from Manus/OpenXR hands when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`), `controller_data`, `tf` (from controller aim pose and head pose), and `hand` only for the explicit Sharpa retargeter path | -| `hand_teleop` | `ee_pose_left`, `ee_pose_right` (from hand tracking wrist), `hand` (named left and right joint poses), `finger_joints` (finger joints in joint space), `root_twist`, `root_pose`, `head_pose`, `tf` (from hand tracking wrist and head pose); locomotion comes from the configured foot pedal collection | +| `controller_teleop` (default) | `ee_pose` (from controller aim poses), `root_twist`, `root_pose`, `head_pose`, `finger_joints` (TriHand by default; Sharpa from Manus/OpenXR hands when `hand_retargeter:=dexpilot` or `hand_retargeter:=pink_ik`), `controller_data`, `tf` (from controller aim poses and head pose), and `hand` only for the explicit Sharpa retargeter path | +| `hand_teleop` | `ee_pose` (from hand tracking wrists), `hand` (named left and right joint poses), `finger_joints` (finger joints in joint space), `root_twist`, `root_pose`, `head_pose`, `tf` (from hand tracking wrists and head pose); locomotion comes from the configured foot pedal collection | | `controller_raw` | `controller_data` only | | `full_body` | `full_body` and `controller_data` | @@ -193,9 +190,8 @@ coverage. ```bash docker exec -it teleop_ros2_ref /bin/bash -ros2 topic echo /xr_teleop/hand teleop_ros2_interfaces/msg/HandJointPoses -ros2 topic echo /xr_teleop/ee_pose_left geometry_msgs/msg/PoseStamped -ros2 topic echo /xr_teleop/ee_pose_right geometry_msgs/msg/PoseStamped +ros2 topic echo /xr_teleop/hand teleop_ros2_interfaces/msg/NamedPoseArray +ros2 topic echo /xr_teleop/ee_pose teleop_ros2_interfaces/msg/NamedPoseArray ros2 topic echo /xr_teleop/root_twist geometry_msgs/msg/TwistStamped ros2 topic echo /xr_teleop/root_pose geometry_msgs/msg/PoseStamped ros2 topic echo /xr_teleop/head_pose geometry_msgs/msg/PoseStamped diff --git a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py index ea52eaddb..e2ba06526 100755 --- a/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py +++ b/examples/teleop_ros2/python/integration_tests/teleop_ros2_topic_verifier.py @@ -20,7 +20,7 @@ from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import ByteMultiArray -from teleop_ros2_interfaces.msg import HandJointPoses +from teleop_ros2_interfaces.msg import NamedPoseArray from tf2_msgs.msg import TFMessage @@ -55,20 +55,17 @@ def _unpack_msgpack(msg: ByteMultiArray) -> dict: ) -def _assert_hand_joint_poses(msg: HandJointPoses) -> None: +def _assert_named_pose_array(msg: NamedPoseArray, expected_names: list[str]) -> None: if msg.header.frame_id != "world": raise ValueError(f"unexpected frame_id {msg.header.frame_id!r}") names = list(msg.name) - if names != _EXPECTED_HAND_POSE_NAMES: - raise ValueError("hand joint pose names do not match the expected order") + if names != expected_names: + raise ValueError("named pose entries do not match the expected order") if len(set(names)) != len(names): - raise ValueError("hand joint pose names are not unique") + raise ValueError("named pose entries are not unique") if len(msg.pose) != len(names) or len(msg.is_valid) != len(names): - raise ValueError("hand joint pose arrays differ in length") - if not any(bool(is_valid) for is_valid in msg.is_valid): - raise ValueError("all hand joint poses are invalid") + raise ValueError("named pose arrays differ in length") - positions = [] for pose in msg.pose: position = (pose.position.x, pose.position.y, pose.position.z) orientation = ( @@ -78,12 +75,32 @@ def _assert_hand_joint_poses(msg: HandJointPoses) -> None: pose.orientation.w, ) if not _is_finite_sequence(position): - raise ValueError("hand joint pose position contains non-finite values") + raise ValueError("named pose position contains non-finite values") if not _is_finite_sequence(orientation): - raise ValueError("hand joint pose orientation contains non-finite values") - positions.extend(position) - if not any(abs(value) > 1e-6 for value in positions): - raise ValueError("hand joint pose positions are all zero") + raise ValueError("named pose orientation contains non-finite values") + + +def _assert_nonzero_pose_positions(msg: NamedPoseArray, label: str) -> None: + if not any( + abs(value) > 1e-6 + for pose in msg.pose + for value in (pose.position.x, pose.position.y, pose.position.z) + ): + raise ValueError(f"{label} positions are all zero") + + +def _assert_ee_pose_array(msg: NamedPoseArray) -> None: + _assert_named_pose_array(msg, ["left", "right"]) + if not all(bool(is_valid) for is_valid in msg.is_valid): + raise ValueError("EE pose array contains an invalid entry") + _assert_nonzero_pose_positions(msg, "EE pose") + + +def _assert_hand_pose_array(msg: NamedPoseArray) -> None: + _assert_named_pose_array(msg, _EXPECTED_HAND_POSE_NAMES) + if not any(bool(is_valid) for is_valid in msg.is_valid): + raise ValueError("all hand joint poses are invalid") + _assert_nonzero_pose_positions(msg, "hand joint pose") def _assert_pose_stamped( @@ -246,20 +263,10 @@ def _expected_subscriptions( if mode == "controller_teleop": return [ ( - "ee_pose_left", - "xr_teleop/ee_pose_left", - PoseStamped, - lambda msg: _assert_pose_stamped( - msg, require_nonzero_position=True - ), - ), - ( - "ee_pose_right", - "xr_teleop/ee_pose_right", - PoseStamped, - lambda msg: _assert_pose_stamped( - msg, require_nonzero_position=True - ), + "ee_pose", + "xr_teleop/ee_pose", + NamedPoseArray, + _assert_ee_pose_array, ), ("root_twist", "xr_teleop/root_twist", TwistStamped, _assert_twist), ("root_pose", "xr_teleop/root_pose", PoseStamped, _assert_pose_stamped), @@ -282,24 +289,14 @@ def _expected_subscriptions( ( "hand", "xr_teleop/hand", - HandJointPoses, - _assert_hand_joint_poses, - ), - ( - "ee_pose_left", - "xr_teleop/ee_pose_left", - PoseStamped, - lambda msg: _assert_pose_stamped( - msg, require_nonzero_position=True - ), + NamedPoseArray, + _assert_hand_pose_array, ), ( - "ee_pose_right", - "xr_teleop/ee_pose_right", - PoseStamped, - lambda msg: _assert_pose_stamped( - msg, require_nonzero_position=True - ), + "ee_pose", + "xr_teleop/ee_pose", + NamedPoseArray, + _assert_ee_pose_array, ), ("root_twist", "xr_teleop/root_twist", TwistStamped, _assert_twist), ("root_pose", "xr_teleop/root_pose", PoseStamped, _assert_pose_stamped), diff --git a/examples/teleop_ros2/python/messages.py b/examples/teleop_ros2/python/messages.py index 4279626d9..d52b68b4b 100644 --- a/examples/teleop_ros2/python/messages.py +++ b/examples/teleop_ros2/python/messages.py @@ -11,7 +11,7 @@ from geometry_msgs.msg import Pose, PoseStamped from scipy.spatial.transform import Rotation from sensor_msgs.msg import JointState -from teleop_ros2_interfaces.msg import HandJointPoses +from teleop_ros2_interfaces.msg import NamedPoseArray from isaacteleop.retargeting_engine.interface import OptionalTensorGroup from isaacteleop.retargeting_engine.tensor_types.indices import ( @@ -36,6 +36,70 @@ ) +def _compose_ee_msg( + left_pose: Pose | None, + right_pose: Pose | None, + now, + frame_id: str, +) -> NamedPoseArray: + msg = NamedPoseArray() + msg.header.stamp = now + msg.header.frame_id = frame_id + for side, pose in (("left", left_pose), ("right", right_pose)): + # Set the pose and validity together so the parallel fields cannot diverge. + if pose is None: + pose = to_pose([0.0, 0.0, 0.0]) + pose_is_valid = False + else: + pose_is_valid = True + msg.name.append(side) + msg.pose.append(pose) + msg.is_valid.append(pose_is_valid) + return msg + + +def _compute_ee_pose_from_controller( + ctrl: OptionalTensorGroup, + side: str, + transform_rot: Rotation | None = None, + transform_trans: Sequence[float] | None = None, + controller_uses_hands_source: bool = False, +) -> Pose | None: + if not controller_aim_is_valid(ctrl): + return None + + pos = [float(x) for x in ctrl[ControllerInputIndex.AIM_POSITION]] + ori = [float(x) for x in ctrl[ControllerInputIndex.AIM_ORIENTATION]] + pose = to_pose(pos, ori) + + if transform_rot is not None or transform_trans is not None: + pose = apply_transform_to_pose(pose, transform_rot, transform_trans) + + if controller_uses_hands_source: + pose = apply_manus_controller_to_hand_pose(pose, side) + + return pose + + +def _compute_ee_pose_from_hand( + hand: OptionalTensorGroup, + transform_rot: Rotation | None = None, + transform_trans: Sequence[float] | None = None, +) -> Pose | None: + if not hand_wrist_is_valid(hand): + return None + + positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) + orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS]) + pose = to_pose( + positions[HandJointIndex.WRIST], + orientations[HandJointIndex.WRIST], + ) + if transform_rot is not None or transform_trans is not None: + pose = apply_transform_to_pose(pose, transform_rot, transform_trans) + return pose + + def _to_pose_stamped(pose: Pose, now, frame_id: str) -> PoseStamped: msg = PoseStamped() msg.header.stamp = now @@ -119,50 +183,53 @@ def _as_float(ctrl, index): } -def build_ee_msg_from_controller( - ctrl: OptionalTensorGroup, - side: str, +def build_ee_msg_from_controllers( + left_ctrl: OptionalTensorGroup, + right_ctrl: OptionalTensorGroup, now, frame_id: str, transform_rot: Rotation | None = None, transform_trans: Sequence[float] | None = None, controller_uses_hands_source: bool = False, -) -> PoseStamped | None: - if not controller_aim_is_valid(ctrl): - return None - - pos = [float(x) for x in ctrl[ControllerInputIndex.AIM_POSITION]] - ori = [float(x) for x in ctrl[ControllerInputIndex.AIM_ORIENTATION]] - pose = to_pose(pos, ori) - - if transform_rot is not None or transform_trans is not None: - pose = apply_transform_to_pose(pose, transform_rot, transform_trans) - - if controller_uses_hands_source: - pose = apply_manus_controller_to_hand_pose(pose, side) - - return _to_pose_stamped(pose, now, frame_id) +) -> NamedPoseArray: + """Build fixed left/right EE entries from controller aim poses.""" + left_pose = _compute_ee_pose_from_controller( + left_ctrl, + "left", + transform_rot, + transform_trans, + controller_uses_hands_source, + ) + right_pose = _compute_ee_pose_from_controller( + right_ctrl, + "right", + transform_rot, + transform_trans, + controller_uses_hands_source, + ) + return _compose_ee_msg(left_pose, right_pose, now, frame_id) -def build_ee_msg_from_hand( - hand: OptionalTensorGroup, +def build_ee_msg_from_hands( + left_hand: OptionalTensorGroup, + right_hand: OptionalTensorGroup, now, frame_id: str, transform_rot: Rotation | None = None, transform_trans: Sequence[float] | None = None, -) -> PoseStamped | None: - if not hand_wrist_is_valid(hand): - return None - - positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) - orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS]) - pose = to_pose( - positions[HandJointIndex.WRIST], - orientations[HandJointIndex.WRIST], +) -> NamedPoseArray: + """Build fixed left/right EE entries from hand wrist poses.""" + left_pose = _compute_ee_pose_from_hand( + left_hand, + transform_rot, + transform_trans, ) - if transform_rot is not None or transform_trans is not None: - pose = apply_transform_to_pose(pose, transform_rot, transform_trans) - return _to_pose_stamped(pose, now, frame_id) + right_pose = _compute_ee_pose_from_hand( + right_hand, + transform_rot, + transform_trans, + ) + return _compose_ee_msg(left_pose, right_pose, now, frame_id) def build_finger_joints_msg( @@ -214,24 +281,25 @@ def build_full_body_payload(full_body: OptionalTensorGroup) -> Dict: } -def build_hand_msg_from_hands( +def build_hand_msg( left_hand: OptionalTensorGroup, right_hand: OptionalTensorGroup, now, frame_id: str, transform_rot: Rotation | None = None, transform_trans: Sequence[float] | None = None, -) -> HandJointPoses | None: - """Build named hand joint poses for available hands, left then right.""" - if left_hand.is_none and right_hand.is_none: - return None - - msg = HandJointPoses() +) -> NamedPoseArray: + """Build fixed left/right hand joint entries with invalid placeholders.""" + msg = NamedPoseArray() msg.header.stamp = now msg.header.frame_id = frame_id for side, hand in (("left", left_hand), ("right", right_hand)): if hand.is_none: + for joint_name in HAND_POSE_NAMES: + msg.name.append(f"{side}_{joint_name}") + msg.pose.append(to_pose([0.0, 0.0, 0.0])) + msg.is_valid.append(False) continue positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) orientations = np.asarray(hand[HandInputIndex.JOINT_ORIENTATIONS]) diff --git a/examples/teleop_ros2/python/teleop_ros2_node.py b/examples/teleop_ros2/python/teleop_ros2_node.py index 4d2a82cd5..52aea971f 100755 --- a/examples/teleop_ros2/python/teleop_ros2_node.py +++ b/examples/teleop_ros2/python/teleop_ros2_node.py @@ -10,12 +10,12 @@ The `mode` parameter selects the teleoperation scenario and which topics are published: - - controller_teleop (default): ee_pose_left/ee_pose_right (from controller - aim pose), root_twist, root_pose, finger_joints + - controller_teleop (default): ee_pose (from controller aim poses), + root_twist, root_pose, finger_joints (retargeted TriHand angles), controller_data, head_pose, and TF transforms for left/right wrists and head - - hand_teleop: ee_pose_left/ee_pose_right (from hand tracking wrist), - hand (named left and right joint poses), + - hand_teleop: ee_pose (from hand tracking wrists), hand (named left and + right joint poses), finger_joints (retargeted Sharpa joint angles), root_twist/root_pose (from foot pedal locomotion), head_pose, and TF transforms for left/right wrists and head @@ -23,9 +23,8 @@ - full_body: full_body and controller_data Topic names (remappable via ROS 2 remapping): - - xr_teleop/hand (HandJointPoses): named left and right hand joint poses - - xr_teleop/ee_pose_left (PoseStamped): left hand/controller EE pose - - xr_teleop/ee_pose_right (PoseStamped): right hand/controller EE pose + - xr_teleop/hand (NamedPoseArray): named left and right hand joint poses + - xr_teleop/ee_pose (NamedPoseArray): named left and right EE poses - xr_teleop/root_twist (TwistStamped): root velocity command - xr_teleop/root_pose (PoseStamped): root pose command (height only) - xr_teleop/head_pose (PoseStamped): head pose @@ -55,7 +54,7 @@ from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import ByteMultiArray -from teleop_ros2_interfaces.msg import HandJointPoses +from teleop_ros2_interfaces.msg import NamedPoseArray from tf2_ros import TransformBroadcaster from isaacteleop.cloudxr import CloudXRLauncher @@ -63,11 +62,11 @@ from geometry import make_transform from messages import ( build_controller_payload, - build_ee_msg_from_controller, - build_ee_msg_from_hand, + build_ee_msg_from_controllers, + build_ee_msg_from_hands, build_finger_joints_msg, build_full_body_payload, - build_hand_msg_from_hands, + build_hand_msg, build_head_msg, ) from node_parameters import ( @@ -92,13 +91,14 @@ def __init__(self) -> None: def _build_wrist_tfs( self, - left_ee_msg: PoseStamped | None, - right_ee_msg: PoseStamped | None, - *, - now, + ee_msg: NamedPoseArray, ) -> List[TransformStamped]: - """Build wrist TF transforms from side-specific EE pose messages.""" + """Build wrist TF transforms from valid EE pose entries.""" tfs = [] + wrist_frames = { + "left": self._params.left_wrist_frame, + "right": self._params.right_wrist_frame, + } def _get_orientation(pose: Pose) -> List[float]: return [ @@ -108,24 +108,14 @@ def _get_orientation(pose: Pose) -> List[float]: pose.orientation.w, ] - if left_ee_msg is not None: - pose = left_ee_msg.pose + for side, pose, is_valid in zip(ee_msg.name, ee_msg.pose, ee_msg.is_valid): + if not is_valid: + continue tfs.append( make_transform( - now, - self._params.world_frame, - self._params.left_wrist_frame, - [pose.position.x, pose.position.y, pose.position.z], - _get_orientation(pose), - ) - ) - if right_ee_msg is not None: - pose = right_ee_msg.pose - tfs.append( - make_transform( - now, - self._params.world_frame, - self._params.right_wrist_frame, + ee_msg.header.stamp, + ee_msg.header.frame_id, + wrist_frames[side], [pose.position.x, pose.position.y, pose.position.z], _get_orientation(pose), ) @@ -133,12 +123,9 @@ def _get_orientation(pose: Pose) -> List[float]: return tfs def _create_publishers(self) -> None: - self._pub_hand = self.create_publisher(HandJointPoses, "xr_teleop/hand", 10) - self._pub_ee_pose_left = self.create_publisher( - PoseStamped, "xr_teleop/ee_pose_left", 10 - ) - self._pub_ee_pose_right = self.create_publisher( - PoseStamped, "xr_teleop/ee_pose_right", 10 + self._pub_hand = self.create_publisher(NamedPoseArray, "xr_teleop/hand", 10) + self._pub_ee_pose = self.create_publisher( + NamedPoseArray, "xr_teleop/ee_pose", 10 ) self._pub_root_twist = self.create_publisher( TwistStamped, "xr_teleop/root_twist", 10 @@ -157,49 +144,22 @@ def _create_publishers(self) -> None: ) self._pub_head = self.create_publisher(PoseStamped, "xr_teleop/head_pose", 10) - def _publish_controller_outputs(self, result: dict, now) -> None: + def _publish_ee_pose_from_controllers(self, result: dict, now) -> None: left_ctrl = result["controller_left"] right_ctrl = result["controller_right"] - left_ee_msg = build_ee_msg_from_controller( + ee_pose_msg = build_ee_msg_from_controllers( left_ctrl, - "left", - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - self._params.controller_uses_hands_source, - ) - right_ee_msg = build_ee_msg_from_controller( right_ctrl, - "right", now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, self._params.controller_uses_hands_source, ) - if left_ee_msg is not None: - self._pub_ee_pose_left.publish(left_ee_msg) - if right_ee_msg is not None: - self._pub_ee_pose_right.publish(right_ee_msg) - wrist_tfs = self._build_wrist_tfs( - left_ee_msg, - right_ee_msg, - now=now, - ) + self._pub_ee_pose.publish(ee_pose_msg) + wrist_tfs = self._build_wrist_tfs(ee_pose_msg) if wrist_tfs: self._tf_broadcaster.sendTransform(wrist_tfs) - if self._params.controller_uses_hands_source: - hand_msg = build_hand_msg_from_hands( - result["hand_left"], - result["hand_right"], - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - ) - if hand_msg is not None: - self._pub_hand.publish(hand_msg) def _publish_controller_payload(self, result: dict) -> None: if self._params.mode not in ( @@ -247,43 +207,30 @@ def _publish_full_body_payload(self, result: dict) -> None: body_msg.data = tuple(bytes([a]) for a in payload) self._pub_full_body.publish(body_msg) - def _publish_hand_tracking_outputs(self, result: dict, now) -> None: - left_hand = result["hand_left"] - right_hand = result["hand_right"] - hand_msg = build_hand_msg_from_hands( - left_hand, - right_hand, + def _publish_hand_poses(self, result: dict, now) -> None: + hand_msg = build_hand_msg( + result["hand_left"], + result["hand_right"], now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, ) - if hand_msg is not None: - self._pub_hand.publish(hand_msg) + self._pub_hand.publish(hand_msg) - left_ee_msg = build_ee_msg_from_hand( + def _publish_ee_pose_from_hands(self, result: dict, now) -> None: + left_hand = result["hand_left"] + right_hand = result["hand_right"] + ee_pose_msg = build_ee_msg_from_hands( left_hand, - now, - self._params.world_frame, - self._params.transform_rotation, - self._params.transform_translation, - ) - right_ee_msg = build_ee_msg_from_hand( right_hand, now, self._params.world_frame, self._params.transform_rotation, self._params.transform_translation, ) - if left_ee_msg is not None: - self._pub_ee_pose_left.publish(left_ee_msg) - if right_ee_msg is not None: - self._pub_ee_pose_right.publish(right_ee_msg) - wrist_tfs = self._build_wrist_tfs( - left_ee_msg, - right_ee_msg, - now=now, - ) + self._pub_ee_pose.publish(ee_pose_msg) + wrist_tfs = self._build_wrist_tfs(ee_pose_msg) if wrist_tfs: self._tf_broadcaster.sendTransform(wrist_tfs) @@ -374,9 +321,14 @@ def _run_session_loop(self, launcher: CloudXRLauncher | None = None) -> int: now = self.get_clock().now().to_msg() if self._params.mode == "hand_teleop": - self._publish_hand_tracking_outputs(result, now) + self._publish_ee_pose_from_hands(result, now) elif self._params.mode == "controller_teleop": - self._publish_controller_outputs(result, now) + self._publish_ee_pose_from_controllers(result, now) + if ( + self._params.mode == "hand_teleop" + or self._params.controller_uses_hands_source + ): + self._publish_hand_poses(result, now) self._publish_root_command(result, now) self._publish_finger_joints(result, now) diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt b/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt index 1c195ae1b..99c12f499 100644 --- a/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt +++ b/examples/teleop_ros2/teleop_ros2_interfaces/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "msg/HandJointPoses.msg" + "msg/NamedPoseArray.msg" DEPENDENCIES geometry_msgs std_msgs diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg b/examples/teleop_ros2/teleop_ros2_interfaces/msg/NamedPoseArray.msg similarity index 81% rename from examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg rename to examples/teleop_ros2/teleop_ros2_interfaces/msg/NamedPoseArray.msg index d47ba8cb3..609966abd 100644 --- a/examples/teleop_ros2/teleop_ros2_interfaces/msg/HandJointPoses.msg +++ b/examples/teleop_ros2/teleop_ros2_interfaces/msg/NamedPoseArray.msg @@ -1,7 +1,7 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. # SPDX-License-Identifier: Apache-2.0 -# Named hand joint poses in the same frame. +# Named poses in the same frame. std_msgs/Header header string[] name geometry_msgs/Pose[] pose From a3ca91b5189334558a54d279b263aaaba6bddfcd Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Mon, 13 Jul 2026 23:29:23 +0000 Subject: [PATCH 6/8] Update package metadata --- examples/teleop_ros2/teleop_ros2_interfaces/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/teleop_ros2/teleop_ros2_interfaces/package.xml b/examples/teleop_ros2/teleop_ros2_interfaces/package.xml index abca4a30e..03acb2356 100644 --- a/examples/teleop_ros2/teleop_ros2_interfaces/package.xml +++ b/examples/teleop_ros2/teleop_ros2_interfaces/package.xml @@ -6,8 +6,8 @@ SPDX-License-Identifier: Apache-2.0 teleop_ros2_interfaces 0.1.0 - Custom ROS 2 interfaces for the Isaac Teleop ROS 2 example. - Isaac Teleop Maintainers + Custom ROS 2 interfaces for Isaac Teleop. + Sergey Grizan Apache-2.0 ament_cmake From ca3360669c8204e0bfa61aabba7b88a419bc989d Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Mon, 13 Jul 2026 23:35:50 +0000 Subject: [PATCH 7/8] Optimize message package config --- examples/teleop_ros2/Dockerfile | 17 ++++++++++------- .../teleop_ros2_interfaces/CMakeLists.txt | 8 +++----- .../teleop_ros2_interfaces/package.xml | 8 +++----- 3 files changed, 16 insertions(+), 17 deletions(-) diff --git a/examples/teleop_ros2/Dockerfile b/examples/teleop_ros2/Dockerfile index b05963af0..138a418d7 100644 --- a/examples/teleop_ros2/Dockerfile +++ b/examples/teleop_ros2/Dockerfile @@ -114,6 +114,8 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ git \ patchelf \ pkg-config \ + python3-colcon-common-extensions \ + ros-${ROS_DISTRO}-ament-cmake-auto \ ros-${ROS_DISTRO}-rosidl-default-generators \ && rm -rf /var/lib/apt/lists/* @@ -146,17 +148,18 @@ RUN --mount=type=cache,target=/ccache,id=isaacteleop-teleop-ros2-ccache-${ROS_DI && cmake --build build --config Release -j$(nproc) \ && cmake --install build --config Release \ && bash -lc "source /opt/ros/${ROS_DISTRO}/setup.bash \ - && cmake -B build/teleop_ros2_interfaces -S examples/teleop_ros2/teleop_ros2_interfaces \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX=/opt/isaacteleop/install \ - && cmake --build build/teleop_ros2_interfaces --config Release -j$(nproc) \ - && cmake --install build/teleop_ros2_interfaces --config Release" \ + && colcon build \ + --base-paths examples/teleop_ros2/teleop_ros2_interfaces \ + --build-base build/teleop_ros2_interfaces \ + --install-base /opt/isaacteleop/install \ + --merge-install \ + --cmake-args -DCMAKE_BUILD_TYPE=Release" \ && ccache --show-stats COPY --chmod=644 <Sergey Grizan Apache-2.0 - ament_cmake + ament_cmake_auto - geometry_msgs + geometry_msgs rosidl_default_generators - std_msgs + std_msgs - geometry_msgs rosidl_default_runtime - std_msgs rosidl_interface_packages From bdf17d451ff83578e7d413e3015c4764d9e8b5dd Mon Sep 17 00:00:00 2001 From: Sergey Grizan Date: Tue, 14 Jul 2026 00:08:32 +0000 Subject: [PATCH 8/8] Fix ARM build --- examples/teleop_ros2/Dockerfile | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/examples/teleop_ros2/Dockerfile b/examples/teleop_ros2/Dockerfile index 138a418d7..3384863f9 100644 --- a/examples/teleop_ros2/Dockerfile +++ b/examples/teleop_ros2/Dockerfile @@ -128,7 +128,8 @@ COPY . /opt/isaacteleop # produces; a stale build/ would skip a clean CMake reconfigure. The build tree # is rebuilt from scratch every time -- ccache keeps recompiles cheap -- so each # build reconfigures against the current image rather than trusting cached -# configure results. +# configure results. Ament's Python tools come from apt, so the interface +# package build below pins the ROS distro's system Python instead of uv's. RUN --mount=type=cache,target=/ccache,id=isaacteleop-teleop-ros2-ccache-${ROS_DISTRO}-${TARGETARCH} \ rm -rf /opt/isaacteleop/install /opt/isaacteleop/build \ && cmake -B build -S . \ @@ -153,7 +154,9 @@ RUN --mount=type=cache,target=/ccache,id=isaacteleop-teleop-ros2-ccache-${ROS_DI --build-base build/teleop_ros2_interfaces \ --install-base /opt/isaacteleop/install \ --merge-install \ - --cmake-args -DCMAKE_BUILD_TYPE=Release" \ + --cmake-args \ + -DCMAKE_BUILD_TYPE=Release \ + -DPython3_EXECUTABLE=/usr/bin/python3" \ && ccache --show-stats COPY --chmod=644 <