From b3b8c7600632f5fb77f7ee460c400f1a3e3d0ac9 Mon Sep 17 00:00:00 2001 From: ppastabtw <130695819+ppastabtw@users.noreply.github.com> Date: Wed, 25 Mar 2026 00:01:23 -0400 Subject: [PATCH 1/2] Fix import order in hand_test.py for Isaac Sim Python 3.11 compatibility Move all Isaac Lab and rclpy imports to after AppLauncher initialization to use Isaac Sim's internal Python 3.11 rclpy instead of system Python 3.10 rclpy. Co-Authored-By: Claude Sonnet 4.6 --- .../ros2_bridge/hand_test.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py index 59d60b1..5df22f6 100644 --- a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py +++ b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py @@ -1,14 +1,3 @@ -from test_subscriber import HandPoseSubscriber -from test_publisher import TestFloatPublisher -from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR -from HumanoidRL.HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG -from isaaclab.utils import configclass -from isaaclab.scene import InteractiveScene, InteractiveSceneCfg -from isaaclab.managers import SceneEntityCfg -from isaaclab.assets import AssetBaseCfg -import isaaclab.sim as sim_utils -import rclpy -import torch import argparse from isaaclab.app import AppLauncher @@ -19,6 +8,17 @@ app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app +import rclpy +import torch +from isaaclab.utils import configclass +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.managers import SceneEntityCfg +from isaaclab.assets import AssetBaseCfg +import isaaclab.sim as sim_utils +from HumanoidRL.HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG +from test_subscriber import HandPoseSubscriber +from test_publisher import TestFloatPublisher + @configclass class HandSceneCfg(InteractiveSceneCfg): From e3fe60244a23f30bc24e9ef2b4de2115bb7e3a2b Mon Sep 17 00:00:00 2001 From: ppastabtw <130695819+ppastabtw@users.noreply.github.com> Date: Wed, 25 Mar 2026 00:39:30 -0400 Subject: [PATCH 2/2] Update ROS2 subscriber/publisher to use VRHandState message format --- .../ros2_bridge/hand_test.py | 4 +- .../ros2_bridge/test_publisher.py | 38 +++++++++----- .../ros2_bridge/test_subscriber.py | 49 +++++++++++++++---- 3 files changed, 68 insertions(+), 23 deletions(-) diff --git a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py index 5df22f6..e07540e 100644 --- a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py +++ b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/hand_test.py @@ -17,7 +17,7 @@ import isaaclab.sim as sim_utils from HumanoidRL.HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG from test_subscriber import HandPoseSubscriber -from test_publisher import TestFloatPublisher +from test_publisher import TestVRHandPublisher @configclass @@ -97,7 +97,7 @@ def main(): # Initialize ROS2 rclpy.init() node = HandPoseSubscriber() - test_node = TestFloatPublisher() + test_node = TestVRHandPublisher() # Initialize simulation sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) diff --git a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_publisher.py b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_publisher.py index 34fb679..8071d8f 100644 --- a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_publisher.py +++ b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_publisher.py @@ -1,27 +1,43 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import Float32MultiArray +from common_msgs.msg import VRHandState, VRFingerFeatures -class TestFloatPublisher(Node): +# TODO: confirm topic name with Yichen +TOPIC_NAME = '/vr_hand_state' + + +def make_finger(curl, flexion=0.0, abduction=0.0, opposition=0.0): + f = VRFingerFeatures() + f.curl = curl + f.flexion = flexion + f.abduction = abduction + f.opposition = opposition + return f + + +class TestVRHandPublisher(Node): def __init__(self): - super().__init__('test_float_publisher') - self.publisher_ = self.create_publisher( - Float32MultiArray, '/hand_joint_positions', 10) + super().__init__('test_vr_hand_publisher') + self.publisher_ = self.create_publisher(VRHandState, TOPIC_NAME, 10) self.timer = self.create_timer(0.1, self.timer_callback) - self.get_logger().info('Test Float Publisher initialized') + self.get_logger().info('Test VR Hand Publisher initialized') def timer_callback(self): - msg = Float32MultiArray() - # Publish a list of 15 floats (e.g., all 1.0 for testing) - msg.data = [2.0] * 15 + msg = VRHandState() + # Publish test values: all fingers curled at 1.0 + msg.thumb = make_finger(curl=1.0) + msg.index = make_finger(curl=1.0, flexion=1.0, abduction=0.5) + msg.middle = make_finger(curl=1.0, flexion=1.0, abduction=0.5) + msg.ring = make_finger(curl=1.0, flexion=1.0, abduction=0.5) + msg.pinky = make_finger(curl=1.0, flexion=1.0) self.publisher_.publish(msg) - self.get_logger().info(f'Published joint positions: {msg.data}') + self.get_logger().info('Published VR hand state') def main(args=None): rclpy.init(args=args) - node = TestFloatPublisher() + node = TestVRHandPublisher() try: rclpy.spin(node) except KeyboardInterrupt: diff --git a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_subscriber.py b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_subscriber.py index ee2e6ee..a530180 100644 --- a/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_subscriber.py +++ b/autonomy/simulation/Wato_additional_scripts/ros2_bridge/test_subscriber.py @@ -1,27 +1,56 @@ import rclpy from rclpy.node import Node -from std_msgs.msg import Float32MultiArray +from common_msgs.msg import VRHandState + + +# TODO: confirm topic name with Yichen +TOPIC_NAME = '/vr_hand_state' class HandPoseSubscriber(Node): def __init__(self): super().__init__('hand_pose_subscriber') self.subscription = self.create_subscription( - Float32MultiArray, - '/hand_joint_positions', + VRHandState, + TOPIC_NAME, self.listener_callback, 10) self.joint_positions = None self.get_logger().info('Hand Pose Subscriber initialized') def listener_callback(self, msg): - if len(msg.data) == 15: - self.joint_positions = list(msg.data) - self.get_logger().info( - f'Received joint positions: {self.joint_positions}') - else: - self.get_logger().warn( - f'Received invalid joint positions: expected 15 floats, got {len(msg.data)}') + # Map VRHandState fields to 15 joint positions. + # Joint order matches HAND_CFG (Revolute_1 to Revolute_15) as loaded + # from hand.usd. TODO: verify exact ordering with simulation. + # + # Mapping: + # index: mcp=curl, pip=flexion, dip=flexion + # middle: mcp=curl, pip=flexion, dip=flexion + # ring: mcp=curl, pip=flexion, dip=flexion + # pinky: mcp=curl, pip=flexion, dip=flexion + # thumb: cmc=curl, mcp=curl, ip=curl + # + # abduction and opposition are not used (no corresponding joints in + # the 15-DOF hand model) + self.joint_positions = [ + msg.index.curl, + msg.index.flexion, + msg.index.flexion, + msg.middle.curl, + msg.middle.flexion, + msg.middle.flexion, + msg.ring.curl, + msg.ring.flexion, + msg.ring.flexion, + msg.pinky.curl, + msg.pinky.flexion, + msg.pinky.flexion, + msg.thumb.curl, + msg.thumb.curl, + msg.thumb.curl, + ] + self.get_logger().info( + f'Received joint positions: {self.joint_positions}') def get_latest_joint_positions(self): return self.joint_positions