Skip to content

Add retargeting of the finger joints #224

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
life1ess merged 6 commits into from
Mar 10, 2026
Merged

Add retargeting of the finger joints #224

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e5145a4
added type hints
kajananchinniahNV Mar 5, 2026
0591cb0
add retargeting of finger joints and publish to joint states topic
kajananchinniahNV Mar 5, 2026
5d6c7d9
only publish retargeted if using control teleop mode
kajananchinniahNV Mar 9, 2026
37a8de1
update readme
kajananchinniahNV Mar 9, 2026
254e384
add guard using is_none
kajananchinniahNV Mar 10, 2026
0b18225
made publishing one side possible if the other isn't available
kajananchinniahNV Mar 10, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 6 additions & 3 deletions examples/teleop_ros2/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@ NV_CXR_ENABLE_PUSH_DEVICES=0
- `xr_teleop/root_twist` (`geometry_msgs/TwistStamped`)
- `xr_teleop/root_pose` (`geometry_msgs/PoseStamped`)
- `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 (published in `controller_teleop` mode only)
- `/tf` (`tf2_msgs/TFMessage`)
- `world_frame` → `right_wrist_frame`: Right wrist transform (published in `controller_teleop` and `hand_teleop` modes)
- `world_frame` → `left_wrist_frame`: Left wrist transform (published in `controller_teleop` and `hand_teleop` modes)
Expand Down Expand Up @@ -83,16 +85,16 @@ docker run --rm --net=host --ipc=host \
teleop_ros2_ref --ros-args -p world_frame:=odom -p rate_hz:=30.0
```

Available parameters: `hand_topic`, `ee_pose_topic`, `root_twist_topic`, `root_pose_topic`, `controller_topic`, `full_body_topic`, `rate_hz`, `mode`, `world_frame`, `right_wrist_frame`, `left_wrist_frame`. Use `ros2 param list /teleop_ros2_publisher` and `ros2 param describe /teleop_ros2_publisher <param>` (with the node running) for the full set.
Available parameters: `hand_topic`, `ee_pose_topic`, `root_twist_topic`, `root_pose_topic`, `controller_topic`, `full_body_topic`, `finger_joints_topic`, `rate_hz`, `mode`, `world_frame`, `right_wrist_frame`, `left_wrist_frame`. Use `ros2 param list /teleop_ros2_publisher` and `ros2 param describe /teleop_ros2_publisher <param>` (with the node running) for the full set.

### Mode

The `mode` parameter selects the teleoperation scenario and which topics are published:

| Mode | Topics published |
|------|------------------|
| `controller_teleop` (default) | `ee_poses` (from controller aim pose), `root_twist`, `root_pose`, `tf` (from controller aim pose) |
| `hand_teleop` | `ee_poses` (from hand tracking wrist), `hand` (finger joints only), `root_twist`, `root_pose`, `tf` (from hand tracking wrist) |
| `controller_teleop` (default) | `ee_poses` (from controller aim pose), `root_twist`, `root_pose`, `finger_joints` (finger joints in joint space), `tf` (from controller aim pose) |
| `hand_teleop` | `ee_poses` (from hand tracking wrist), `hand` (finger joints only in pose space), `root_twist`, `root_pose`, `tf` (from hand tracking wrist) |
| `controller_raw` | `controller_data` only |
| `full_body` | `full_body` only |

Expand All @@ -109,6 +111,7 @@ 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/controller_data std_msgs/msg/ByteMultiArray
ros2 topic echo /xr_teleop/full_body std_msgs/msg/ByteMultiArray
ros2 topic echo /xr_teleop/finger_joints sensor_msgs/msg/JointState
ros2 topic echo /tf tf2_msgs/msg/TFMessage
```

Expand Down
86 changes: 84 additions & 2 deletions examples/teleop_ros2/python/teleop_ros2_publisher.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,9 @@
published:

- controller_teleop (default): ee_poses (from controller aim pose), root_twist, root_pose,
and TF transforms for left/right wrists
- hand_teleop: ee_poses (from hand tracking wrist), hand (finger joints only),
finger_joints (retargeted TriHand angles), and TF transforms for
left/right wrists
- hand_teleop: ee_poses (from hand tracking wrist), hand (raw finger poses),
root_twist, root_pose, and TF transforms for left/right wrists
- controller_raw: controller_data only
- full_body: full_body only
Expand All @@ -24,6 +25,7 @@
- xr_teleop/root_pose (PoseStamped): root pose command (height only)
- xr_teleop/controller_data (ByteMultiArray): msgpack-encoded controller data
- xr_teleop/full_body (ByteMultiArray): msgpack-encoded full body tracking data
- xr_teleop/finger_joints (JointState): retargeted TriHand finger joint angles (controller_teleop only)

TF frames published in hand_teleop and controller_teleop modes (configurable via parameters):
- world_frame -> right_wrist_frame
Expand All @@ -47,6 +49,7 @@
)
from rcl_interfaces.msg import ParameterDescriptor
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import ByteMultiArray
from tf2_ros import TransformBroadcaster

Expand All @@ -59,6 +62,8 @@
from isaacteleop.retargeters import (
LocomotionRootCmdRetargeter,
LocomotionRootCmdRetargeterConfig,
TriHandMotionControllerRetargeter,
TriHandMotionControllerConfig,
)
from isaacteleop.retargeting_engine.tensor_types.indices import (
BodyJointPicoIndex,
Expand Down Expand Up @@ -346,6 +351,20 @@ def _as_float(ctrl, index):
}


_TRIHAND_JOINT_NAMES = [
"thumb_rotation",
"thumb_proximal",
"thumb_distal",
"index_proximal",
"index_distal",
"middle_proximal",
"middle_distal",
]
_FINGER_JOINT_NAMES = [f"left_{n}" for n in _TRIHAND_JOINT_NAMES] + [
f"right_{n}" for n in _TRIHAND_JOINT_NAMES
]


def _build_full_body_payload(full_body: OptionalTensorGroup) -> Dict:
positions = np.asarray(full_body[FullBodyInputIndex.JOINT_POSITIONS])
orientations = np.asarray(full_body[FullBodyInputIndex.JOINT_ORIENTATIONS])
Expand All @@ -372,6 +391,7 @@ def __init__(self) -> None:
self.declare_parameter("root_pose_topic", "xr_teleop/root_pose")
self.declare_parameter("controller_topic", "xr_teleop/controller_data")
self.declare_parameter("full_body_topic", "xr_teleop/full_body")
self.declare_parameter("finger_joints_topic", "xr_teleop/finger_joints")
self.declare_parameter("rate_hz", 60.0)
self.declare_parameter("mode", "controller_teleop")
self.declare_parameter(
Expand Down Expand Up @@ -413,6 +433,9 @@ def __init__(self) -> None:
self._full_body_topic = (
self.get_parameter("full_body_topic").get_parameter_value().string_value
)
self._finger_joints_topic = (
self.get_parameter("finger_joints_topic").get_parameter_value().string_value
)
rate_hz = self.get_parameter("rate_hz").get_parameter_value().double_value
if rate_hz <= 0 or not math.isfinite(rate_hz):
raise ValueError("Parameter 'rate_hz' must be > 0")
Expand Down Expand Up @@ -467,6 +490,9 @@ def __init__(self) -> None:
self._pub_full_body = self.create_publisher(
ByteMultiArray, self._full_body_topic, 10
)
self._pub_finger_joints = self.create_publisher(
JointState, self._finger_joints_topic, 10
)

hands = HandsSource(name="hands")
controllers = ControllersSource(name="controllers")
Expand All @@ -481,6 +507,25 @@ def __init__(self) -> None:
}
)

left_hand_retargeter = TriHandMotionControllerRetargeter(
TriHandMotionControllerConfig(
hand_joint_names=_TRIHAND_JOINT_NAMES, controller_side="left"
),
name="trihand_left",
)
right_hand_retargeter = TriHandMotionControllerRetargeter(
TriHandMotionControllerConfig(
hand_joint_names=_TRIHAND_JOINT_NAMES, controller_side="right"
),
name="trihand_right",
)
left_hand_connected = left_hand_retargeter.connect(
{ControllersSource.LEFT: controllers.output(ControllersSource.LEFT)}
)
right_hand_connected = right_hand_retargeter.connect(
{ControllersSource.RIGHT: controllers.output(ControllersSource.RIGHT)}
)
Comment thread
life1ess marked this conversation as resolved.

pipeline = OutputCombiner(
{
"hand_left": hands.output(HandsSource.LEFT),
Expand All @@ -489,6 +534,8 @@ def __init__(self) -> None:
"controller_right": controllers.output(ControllersSource.RIGHT),
"root_command": locomotion_connected.output("root_command"),
"full_body": full_body.output(FullBodySource.FULL_BODY),
"finger_joints_left": left_hand_connected.output("hand_joints"),
"finger_joints_right": right_hand_connected.output("hand_joints"),
}
)

Expand Down Expand Up @@ -573,6 +620,41 @@ def run(self) -> int:
pose_msg.pose.orientation.w = 1.0
self._pub_root_pose.publish(pose_msg)

if self._mode == "controller_teleop":
left_joints = result["finger_joints_left"]
right_joints = result["finger_joints_right"]
if not left_joints.is_none or not right_joints.is_none:
finger_joints_msg = JointState()
finger_joints_msg.header.stamp = now
finger_joints_msg.header.frame_id = self._world_frame
left_arr = (
np.asarray(list(left_joints), dtype=np.float32)
if not left_joints.is_none
else np.array([], dtype=np.float32)
)
right_arr = (
np.asarray(list(right_joints), dtype=np.float32)
if not right_joints.is_none
else np.array([], dtype=np.float32)
)
finger_joints_msg.name = (
list(
_FINGER_JOINT_NAMES[: len(_TRIHAND_JOINT_NAMES)]
)
if not left_joints.is_none
else []
) + (
list(
_FINGER_JOINT_NAMES[len(_TRIHAND_JOINT_NAMES) :]
)
if not right_joints.is_none
else []
)
finger_joints_msg.position = np.concatenate(
[left_arr, right_arr]
).tolist()
self._pub_finger_joints.publish(finger_joints_msg)
Comment thread
kajananchinniahNV marked this conversation as resolved.

if self._mode == "controller_raw":
if not left_ctrl.is_none or not right_ctrl.is_none:
controller_payload = _build_controller_payload(
Expand Down