Merge pull request #7 from ROBOTIS-GIT/feature-vr-offset

Feature vr offset

Author: nhw-robotis

docker/docker-compose.yml

@@ -1,7 +1,7 @@
 services:
   robotis_applications:
     container_name: robotis-applications
-    image: robotis/robotis-applications:0.0.3
+    image: robotis/robotis-applications:0.0.4
     tty: true
     build:
       context: .

robotis_applications/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package robotis_applications
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.0.4 (2026-04-17)
+------------------
+* Updated VR z offsets
+* Contributors: Hyunwoo Nam
+
 0.0.3 (2026-04-13)
 ------------------
 * Add VR support for HX5

robotis_applications/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>robotis_applications</name>
-  <version>0.0.3</version>
+  <version>0.0.4</version>
   <description>Meta package for Robotis Applications</description>
   <maintainer email="pyo@robotis.com">Pyo</maintainer>
   <license>Apache License 2.0</license>

robotis_vuer/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package robotis_vuer
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.0.4 (2026-04-17)
+------------------
+* Updated VR z offsets
+* Contributors: Hyunwoo Nam
+
 0.0.3 (2026-04-13)
 ------------------
 * Add VR support for HX5

robotis_vuer/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>robotis_vuer</name>
-  <version>0.0.3</version>
+  <version>0.0.4</version>
   <description>VR Publisher for Robotis</description>
   <maintainer email="pyo@robotis.com">Pyo</maintainer>
   <license>Apache License 2.0</license>

robotis_vuer/robotis_vuer/vr_publisher_hx5.py

@@ -22,7 +22,7 @@
 import threading
 import traceback
 
-from geometry_msgs.msg import Point32, Quaternion
+from geometry_msgs.msg import Point32
 import nest_asyncio
 import numpy as np
 import rclpy
@@ -121,13 +121,6 @@ def __init__(self):
         self.prev_poses_left = np.zeros((21, 3))
         self.start_poses_left = False
 
-        # Cached transform constants for hot paths
-        self.vr_to_ros_matrix = np.array([
-            [0.0, 0.0, -1.0],
-            [-1.0, 0.0, 0.0],
-            [0.0, 1.0, 0.0],
-        ], dtype=np.float64)
-
         # VR data storage
         self.left_hand_data = None
         self.right_hand_data = None
@@ -139,7 +132,6 @@ def __init__(self):
 
         # Low-pass filter settings
         self.low_pass_filter_alpha = 0.9
-        self.pose_filters = {}
 
         self.hand_log_counter = 0
         self.wrist_debug_log_counter = 0
@@ -181,38 +173,12 @@ def _set_vr_publishing_enabled(self, new_state):
             self.start_poses_right = False
             self.prev_poses_left.fill(0.0)
             self.prev_poses_right.fill(0.0)
-            self.pose_filters.clear()
 
     def log_status(self):
         """Log current system status for debugging."""
         vr_status = 'ENABLED' if self.vr_publishing_enabled else 'DISABLED'
         self.get_logger().info(f'Status: VR={vr_status}')
 
-    def is_valid_float(self, value):
-        """Check if value is valid float (excluding NaN, inf)."""
-        return isinstance(value, (int, float)) and np.isfinite(value)
-
-    # Body head-relative frame from head_inverse @ world:
-    # +Y=forward, +Z=right, +X=down. Convert to ROS (+X forward, +Y left, +Z up).
-    BODY_HEAD_TO_ROS_POSITION = np.array([
-        [0, 1, 0],    # ROS X = head +Y (forward)
-        [0, 0, -1],   # ROS Y = -head Z (left)
-        [-1, 0, 0]    # ROS Z = -head X (up)
-    ])
-
-    def quat_multiply(self, q1, q2):
-        """Multiply two quaternions."""
-        w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z
-        x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y
-        y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x
-        z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w
-        msg = Quaternion()
-        msg.x = x
-        msg.y = y
-        msg.z = z
-        msg.w = w
-        return msg
-
     def process_hand_joints(self, hand_data, side='left'):
         """Process VR hand data (retarget) and publish HandJoints."""
         if hand_data is None or len(hand_data) != 400:

robotis_vuer/robotis_vuer/vr_publisher_sg2.py

@@ -56,10 +56,10 @@ def __init__(self):
         self.get_logger().set_level(rclpy.logging.LoggingSeverity.INFO)
         self.declare_parameter('left_wrist_offset_x', 0.0)
         self.declare_parameter('left_wrist_offset_y', 0.0)
-        self.declare_parameter('left_wrist_offset_z', 0.0)
+        self.declare_parameter('left_wrist_offset_z', -0.3)
         self.declare_parameter('right_wrist_offset_x', 0.0)
         self.declare_parameter('right_wrist_offset_y', 0.0)
-        self.declare_parameter('right_wrist_offset_z', 0.0)
+        self.declare_parameter('right_wrist_offset_z', -0.3)
         self.declare_parameter('left_wrist_roll_offset_deg', 90.0)
         self.declare_parameter('left_wrist_pitch_offset_deg', 0.0)
         self.declare_parameter('left_wrist_yaw_offset_deg', 0.0)
@@ -79,10 +79,10 @@ def __init__(self):
         self.declare_parameter('lift_to_arm_z_scale', 1.0)
         self.declare_parameter('left_elbow_offset_x', 0.0)
         self.declare_parameter('left_elbow_offset_y', 0.0)
-        self.declare_parameter('left_elbow_offset_z', 0.0)
+        self.declare_parameter('left_elbow_offset_z', -0.3)
         self.declare_parameter('right_elbow_offset_x', 0.0)
         self.declare_parameter('right_elbow_offset_y', 0.0)
-        self.declare_parameter('right_elbow_offset_z', 0.0)
+        self.declare_parameter('right_elbow_offset_z', -0.3)
         self.declare_parameter('goal_pose_squeeze_threshold', 0.8)
 
         # VR publishing control flag

robotis_vuer/robotis_vuer/vr_publisher_sh5.py

@@ -225,10 +225,10 @@ def __init__(self):
         # Wrist/elbow position offsets (head-relative, ROS frame: X forward, Y left, Z up)
         self.declare_parameter('wrist_offset_x', 0.0)
         self.declare_parameter('wrist_offset_y', 0.0)
-        self.declare_parameter('wrist_offset_z', 0.0)
+        self.declare_parameter('wrist_offset_z', -0.3)
         self.declare_parameter('elbow_offset_x', 0.0)
         self.declare_parameter('elbow_offset_y', 0.0)
-        self.declare_parameter('elbow_offset_z', 0.0)
+        self.declare_parameter('elbow_offset_z', -0.3)
 
         self.enable_vr_image = (
             self.get_parameter('enable_vr_image')

robotis_vuer/setup.py

@@ -12,7 +12,7 @@
 
 setup(
     name=package_name,
-    version='0.0.3',
+    version='0.0.4',
     packages=find_packages(exclude=['test']),
     data_files=[
         ('share/ament_index/resource_index/packages',