ROS: Refactor node

examples/teleop_ros2/python/assets.py

@@ -0,0 +1,72 @@
+# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES.
+# All rights reserved.
+# SPDX-License-Identifier: Apache-2.0
+
+"""Asset and configuration path resolution for teleop_ros2_node."""
+
+from pathlib import Path
+
+
+def resolve_config_asset_root(raw_root: str, default_root: Path) -> Path:
+    raw_root = raw_root.strip()
+    if not raw_root:
+        return default_root
+
+    root = Path(raw_root).expanduser().resolve()
+    if not root.is_dir():
+        raise FileNotFoundError(f"config_asset_root directory not found: {root}")
+    return root
+
+
+def resolve_dex_sharpa_config(root: Path, filename: str) -> str:
+    return resolve_teleop_ros2_file(
+        root,
+        "DexPilot Sharpa Wave retargeting config",
+        "configs",
+        filename,
+    )
+
+
+def resolve_dex_sharpa_urdf(root: Path, filename: str) -> str:
+    try:
+        return resolve_teleop_ros2_file(
+            root,
+            "Standalone Sharpa Wave URDF",
+            "assets",
+            "urdf",
+            "sharpa_standalone",
+            filename,
+        )
+    except FileNotFoundError as exc:
+        raise FileNotFoundError(
+            f"{exc}. Populate the official Sharpa Wave URDFs with "
+            "examples/teleop_ros2/scripts/fetch_sharpa_wave_urdfs.py before using "
+            "hand_retargeter:=dexpilot, or use a Docker image built from "
+            "examples/teleop_ros2/Dockerfile."
+        ) from exc
+
+
+def resolve_sharpa_mjcf(filename: str) -> str:
+    try:
+        from importlib.resources import files
+    except ImportError as exc:  # pragma: no cover - Python 3.10+ has this.
+        raise ModuleNotFoundError(
+            "Sharpa hand retargeting requires importlib.resources support"
+        ) from exc
+
+    try:
+        return str(
+            files("robotic_grounding") / "assets" / "xmls" / "sharpawave" / filename
+        )
+    except ModuleNotFoundError as exc:
+        raise ModuleNotFoundError(
+            "Sharpa hand retargeting requires robotic_grounding assets. "
+            "Install/use a build with isaacteleop[grounding] and bundled robotic_grounding."
+        ) from exc
+
+
+def resolve_teleop_ros2_file(root: Path, description: str, *parts: str) -> str:
+    path = root.joinpath(*parts)
+    if not path.is_file():
+        raise FileNotFoundError(f"{description} not found at: {path}")
+    return str(path)

examples/teleop_ros2/python/constants.py

@@ -0,0 +1,97 @@
+# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES.
+# All rights reserved.
+# SPDX-License-Identifier: Apache-2.0
+
+"""Constants and enum values shared by the Teleop ROS 2 node."""
+
+from enum import Enum
+
+from isaacteleop.retargeting_engine.tensor_types.indices import BodyJointPicoIndex
+
+try:
+    from enum import StrEnum
+except ImportError:  # pragma: no cover - Python 3.10 compatibility for ROS Humble.
+
+    class StrEnum(str, Enum):
+        def __str__(self) -> str:
+            return self.value
+
+
+class HandRetargeter(StrEnum):
+    MODE_DEFAULT = "mode_default"
+    TRIHAND = "trihand"
+    PINK_IK = "pink_ik"
+    DEXPILOT = "dexpilot"
+
+
+BODY_JOINT_NAMES = [e.name for e in BodyJointPicoIndex]
+HAND_RETARGETERS = tuple(retargeter.value for retargeter in HandRetargeter)
+SHARPA_HAND_RETARGETERS = (HandRetargeter.PINK_IK, HandRetargeter.DEXPILOT)
+TELEOP_MODES = ("controller_teleop", "hand_teleop", "controller_raw", "full_body")
+
+TRIHAND_JOINT_NAMES = [
+    "thumb_rotation",
+    "thumb_proximal",
+    "thumb_distal",
+    "index_proximal",
+    "index_distal",
+    "middle_proximal",
+    "middle_distal",
+]
+LEFT_FINGER_JOINT_NAMES = [f"left_{n}" for n in TRIHAND_JOINT_NAMES]
+RIGHT_FINGER_JOINT_NAMES = [f"right_{n}" for n in TRIHAND_JOINT_NAMES]
+
+SHARPA_WAVE_JOINT_NAMES = [
+    "thumb_CMC_FE",
+    "thumb_CMC_AA",
+    "thumb_MCP_FE",
+    "thumb_MCP_AA",
+    "thumb_IP",
+    "index_MCP_FE",
+    "index_MCP_AA",
+    "index_PIP",
+    "index_DIP",
+    "middle_MCP_FE",
+    "middle_MCP_AA",
+    "middle_PIP",
+    "middle_DIP",
+    "ring_MCP_FE",
+    "ring_MCP_AA",
+    "ring_PIP",
+    "ring_DIP",
+    "pinky_CMC",
+    "pinky_MCP_FE",
+    "pinky_MCP_AA",
+    "pinky_PIP",
+    "pinky_DIP",
+]
+SHARPA_FINGER_JOINT_COUNT = len(SHARPA_WAVE_JOINT_NAMES)
+LEFT_SHARPA_WAVE_JOINT_NAMES = [f"left_{n}" for n in SHARPA_WAVE_JOINT_NAMES]
+RIGHT_SHARPA_WAVE_JOINT_NAMES = [f"right_{n}" for n in SHARPA_WAVE_JOINT_NAMES]
+
+DEX_HANDTRACKING_TO_BASELINK_FRAME_TRANSFORM = (0, -1, 0, -1, 0, 0, 0, 0, -1)
+
+
+def resolve_hand_retargeter(
+    mode: str, hand_retargeter: HandRetargeter
+) -> HandRetargeter:
+    if hand_retargeter == HandRetargeter.MODE_DEFAULT:
+        if mode == "controller_teleop":
+            return HandRetargeter.TRIHAND
+        if mode == "hand_teleop":
+            return HandRetargeter.DEXPILOT
+        return hand_retargeter
+
+    if mode == "hand_teleop" and hand_retargeter == HandRetargeter.TRIHAND:
+        raise ValueError(
+            "Parameter 'hand_retargeter:=trihand' is only valid with "
+            "mode:=controller_teleop"
+        )
+
+    return hand_retargeter
+
+
+def uses_hands_source_for_controller(
+    mode: str, hand_retargeter: HandRetargeter
+) -> bool:
+    return mode == "controller_teleop" and hand_retargeter in SHARPA_HAND_RETARGETERS

examples/teleop_ros2/python/geometry.py

@@ -0,0 +1,167 @@
+# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES.
+# All rights reserved.
+# SPDX-License-Identifier: Apache-2.0
+
+"""Geometry helpers for ROS message construction."""
+
+from typing import Sequence, Union
+
+import numpy as np
+from geometry_msgs.msg import Pose, TransformStamped
+from scipy.spatial.transform import Rotation
+
+from isaacteleop.retargeting_engine.tensor_types.indices import HandJointIndex
+
+
+def append_hand_poses(
+    poses: list[Pose],
+    joint_positions: np.ndarray,
+    joint_orientations: np.ndarray,
+    joint_valid: np.ndarray,
+    transform_rot: Rotation | None = None,
+    transform_trans: Sequence[float] | None = None,
+) -> None:
+    for joint_idx in range(
+        HandJointIndex.THUMB_METACARPAL, HandJointIndex.LITTLE_TIP + 1
+    ):
+        if joint_valid[joint_idx]:
+            pose = to_pose(joint_positions[joint_idx], joint_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])
+        poses.append(pose)
+
+
+def apply_manus_controller_to_hand_pose(pose: Pose, side: str) -> Pose:
+    """
+    Apply MANUS controller-to-hand calibration in the pose's current frame.
+
+    This is equivalent to:
+
+        T_world_hand = T_world_controller @ T_controller_hand
+    """
+    if side not in ("left", "right"):
+        raise ValueError(f"side must be 'left' or 'right', got {side!r}")
+
+    # All MANUS calibration data is intentionally kept in this one function.
+    hand_left_pico_rotation = np.array(
+        [
+            [-0.91777945, -0.18672461, -0.35044942],
+            [0.37550315, -0.69513369, -0.61301431],
+            [-0.12914434, -0.6942068, 0.70809509],
+        ],
+        dtype=float,
+    )
+    hand_pico_translation = np.array([0.0, 0.0, 0.08], dtype=float)
+
+    if side == "left":
+        controller_to_hand_rot_mat = hand_left_pico_rotation.T
+    else:
+        mirror_y = np.diag([1.0, -1.0, 1.0])
+        hand_right_pico_rotation = mirror_y @ hand_left_pico_rotation @ mirror_y
+        controller_to_hand_rot_mat = hand_right_pico_rotation.T
+
+    controller_to_hand_trans = -controller_to_hand_rot_mat @ hand_pico_translation
+
+    world_controller_pos = np.array(
+        [pose.position.x, pose.position.y, pose.position.z],
+        dtype=float,
+    )
+    world_controller_rot = Rotation.from_quat(
+        [
+            pose.orientation.x,
+            pose.orientation.y,
+            pose.orientation.z,
+            pose.orientation.w,
+        ]
+    )
+
+    controller_to_hand_rot = Rotation.from_matrix(controller_to_hand_rot_mat)
+
+    world_hand_pos = world_controller_pos + world_controller_rot.apply(
+        controller_to_hand_trans
+    )
+    world_hand_rot = world_controller_rot * controller_to_hand_rot
+
+    return to_pose(world_hand_pos, world_hand_rot.as_quat())
+
+
+def apply_transform_to_pose(
+    pose: Pose,
+    rotation: Rotation | None = None,
+    translation: Sequence[float] | None = None,
+) -> Pose:
+    """
+    Return a new Pose with world-frame position transform and orientation
+    basis change applied.
+    """
+    p = [pose.position.x, pose.position.y, pose.position.z]
+    orientation = Rotation.from_quat(
+        [
+            pose.orientation.x,
+            pose.orientation.y,
+            pose.orientation.z,
+            pose.orientation.w,
+        ]
+    )
+
+    if rotation is not None:
+        p = rotation.apply(p)
+        # Conjugation keeps the same physical orientation while expressing it
+        # in the rotated basis used for published EE and hand poses.
+        orientation = rotation * orientation * rotation.inv()
+
+    q = orientation.as_quat()
+
+    result = Pose()
+    if translation is not None:
+        result.position.x = float(p[0]) + translation[0]
+        result.position.y = float(p[1]) + translation[1]
+        result.position.z = float(p[2]) + translation[2]
+    else:
+        result.position.x = float(p[0])
+        result.position.y = float(p[1])
+        result.position.z = float(p[2])
+
+    result.orientation.x = float(q[0])
+    result.orientation.y = float(q[1])
+    result.orientation.z = float(q[2])
+    result.orientation.w = float(q[3])
+    return result
+
+
+def make_transform(
+    stamp,
+    parent_frame: str,
+    child_frame: str,
+    position: Union[np.ndarray, Sequence[float]],
+    orientation: Union[np.ndarray, Sequence[float]],
+) -> TransformStamped:
+    tf = TransformStamped()
+    tf.header.stamp = stamp
+    tf.header.frame_id = parent_frame
+    tf.child_frame_id = child_frame
+    tf.transform.translation.x = float(position[0])
+    tf.transform.translation.y = float(position[1])
+    tf.transform.translation.z = float(position[2])
+    tf.transform.rotation.x = float(orientation[0])
+    tf.transform.rotation.y = float(orientation[1])
+    tf.transform.rotation.z = float(orientation[2])
+    tf.transform.rotation.w = float(orientation[3])
+    return tf
+
+
+def to_pose(position, orientation=None) -> Pose:
+    pose = Pose()
+    pose.position.x = float(position[0])
+    pose.position.y = float(position[1])
+    pose.position.z = float(position[2])
+    if orientation is None:
+        pose.orientation.w = 1.0
+    else:
+        pose.orientation.x = float(orientation[0])
+        pose.orientation.y = float(orientation[1])
+        pose.orientation.z = float(orientation[2])
+        pose.orientation.w = float(orientation[3])
+    return pose