From 0e9918f937ed56a3da44bfbbe98b68af349542a4 Mon Sep 17 00:00:00 2001 From: silencht Date: Wed, 14 May 2025 14:36:56 +0800 Subject: [PATCH 01/32] [update] support new vuer version --- requirements.txt | 8 - teleop/open_television/__init__.py | 4 + teleop/open_television/_test_television.py | 77 ++++ teleop/open_television/_test_tv_wrapper.py | 80 ++++ teleop/open_television/constants.py | 53 --- teleop/open_television/setup.py | 20 + teleop/open_television/television.py | 476 ++++++++++++++++++--- teleop/open_television/tv_wrapper.py | 470 +++++++++++++++----- teleop/robot_control/robot_arm_ik.py | 5 +- teleop/robot_control/robot_hand_inspire.py | 6 +- teleop/robot_control/robot_hand_unitree.py | 70 +-- teleop/teleop_hand_and_arm.py | 90 ++-- teleop/utils/mat_tool.py | 14 - 13 files changed, 1050 insertions(+), 323 deletions(-) create mode 100644 teleop/open_television/__init__.py create mode 100644 teleop/open_television/_test_television.py create mode 100644 teleop/open_television/_test_tv_wrapper.py delete mode 100644 teleop/open_television/constants.py create mode 100644 teleop/open_television/setup.py delete mode 100644 teleop/utils/mat_tool.py diff --git a/requirements.txt b/requirements.txt index 178d506..281492b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,14 +1,7 @@ -aiohttp==3.9.5 -aiohttp_cors==0.7.0 -aiortc==1.8.0 -av==11.0.0 einops==0.8.0 h5py==3.11.0 ipython==8.12.3 matplotlib==3.7.5 -numpy==1.23.0 -opencv_contrib_python==4.10.0.82 -opencv_python==4.9.0.80 packaging==24.1 pandas==2.0.3 params_proto==2.12.1 @@ -20,7 +13,6 @@ seaborn==0.13.2 setuptools==69.5.1 torch==2.3.0 torchvision==0.18.0 -vuer==0.0.32rc7 rerun-sdk==0.20.1 nlopt>=2.6.1,<2.8.0 trimesh>=4.4.0 diff --git a/teleop/open_television/__init__.py b/teleop/open_television/__init__.py new file mode 100644 index 0000000..200cce1 --- /dev/null +++ b/teleop/open_television/__init__.py @@ -0,0 +1,4 @@ +# open_television/__init__.py +# openssl req -x509 -nodes -days 365 -newkey rsa:2048 -keyout key.pem -out cert.pem +from .television import TeleVision +from .tv_wrapper import TeleVisionWrapper, TeleData, TeleStateData \ No newline at end of file diff --git a/teleop/open_television/_test_television.py b/teleop/open_television/_test_television.py new file mode 100644 index 0000000..f311101 --- /dev/null +++ b/teleop/open_television/_test_television.py @@ -0,0 +1,77 @@ +import os, sys +this_file = os.path.abspath(__file__) +project_root = os.path.abspath(os.path.join(os.path.dirname(this_file), '..')) +if project_root not in sys.path: + sys.path.insert(0, project_root) + +import time +import numpy as np +from multiprocessing import shared_memory +from open_television import TeleVision + +def run_test_television(): + # image + image_shape = (480, 640 * 2, 3) + image_shm = shared_memory.SharedMemory(create=True, size=np.prod(image_shape) * np.uint8().itemsize) + image_array = np.ndarray(image_shape, dtype=np.uint8, buffer=image_shm.buf) + + # television + use_hand_track = False + tv = TeleVision(binocular = True, use_hand_tracking = use_hand_track, img_shape = image_shape, img_shm_name = image_shm.name, webrtc=True) + + try: + input("Press Enter to start television test...") + running = True + while running: + print("=" * 80) + print("Common Data (always available):") + print(f"head_pose shape: {tv.head_pose.shape}\n{tv.head_pose}\n") + print(f"left_arm_pose shape: {tv.left_arm_pose.shape}\n{tv.left_arm_pose}\n") + print(f"right_arm_pose shape: {tv.right_arm_pose.shape}\n{tv.right_arm_pose}\n") + print("=" * 80) + + if use_hand_track: + print("Hand Tracking Data:") + print(f"left_hand_positions shape: {tv.left_hand_positions.shape}\n{tv.left_hand_positions}\n") + print(f"right_hand_positions shape: {tv.right_hand_positions.shape}\n{tv.right_hand_positions}\n") + print(f"left_hand_orientations shape: {tv.left_hand_orientations.shape}\n{tv.left_hand_orientations}\n") + print(f"right_hand_orientations shape: {tv.right_hand_orientations.shape}\n{tv.right_hand_orientations}\n") + print(f"left_hand_pinch_state: {tv.left_hand_pinch_state}") + print(f"left_hand_pinch_value: {tv.left_hand_pinch_value}") + print(f"left_hand_squeeze_state: {tv.left_hand_squeeze_state}") + print(f"left_hand_squeeze_value: {tv.left_hand_squeeze_value}") + print(f"right_hand_pinch_state: {tv.right_hand_pinch_state}") + print(f"right_hand_pinch_value: {tv.right_hand_pinch_value}") + print(f"right_hand_squeeze_state: {tv.right_hand_squeeze_state}") + print(f"right_hand_squeeze_value: {tv.right_hand_squeeze_value}") + else: + print("Controller Data:") + print(f"left_controller_trigger_state: {tv.left_controller_trigger_state}") + print(f"left_controller_trigger_value: {tv.left_controller_trigger_value}") + print(f"left_controller_squeeze_state: {tv.left_controller_squeeze_state}") + print(f"left_controller_squeeze_value: {tv.left_controller_squeeze_value}") + print(f"left_controller_thumbstick_state: {tv.left_controller_thumbstick_state}") + print(f"left_controller_thumbstick_value: {tv.left_controller_thumbstick_value}") + print(f"left_controller_aButton: {tv.left_controller_aButton}") + print(f"left_controller_bButton: {tv.left_controller_bButton}") + print(f"right_controller_trigger_state: {tv.right_controller_trigger_state}") + print(f"right_controller_trigger_value: {tv.right_controller_trigger_value}") + print(f"right_controller_squeeze_state: {tv.right_controller_squeeze_state}") + print(f"right_controller_squeeze_value: {tv.right_controller_squeeze_value}") + print(f"right_controller_thumbstick_state: {tv.right_controller_thumbstick_state}") + print(f"right_controller_thumbstick_value: {tv.right_controller_thumbstick_value}") + print(f"right_controller_aButton: {tv.right_controller_aButton}") + print(f"right_controller_bButton: {tv.right_controller_bButton}") + print("=" * 80) + time.sleep(0.03) + except KeyboardInterrupt: + running = False + print("KeyboardInterrupt, exiting program...") + finally: + image_shm.unlink() + image_shm.close() + print("Finally, exiting program...") + exit(0) + +if __name__ == '__main__': + run_test_television() \ No newline at end of file diff --git a/teleop/open_television/_test_tv_wrapper.py b/teleop/open_television/_test_tv_wrapper.py new file mode 100644 index 0000000..938c0d3 --- /dev/null +++ b/teleop/open_television/_test_tv_wrapper.py @@ -0,0 +1,80 @@ +import os, sys +this_file = os.path.abspath(__file__) +project_root = os.path.abspath(os.path.join(os.path.dirname(this_file), '..')) +if project_root not in sys.path: + sys.path.insert(0, project_root) + +import numpy as np +import time +from multiprocessing import shared_memory +from open_television import TeleVisionWrapper + +def run_test_tv_wrapper(): + image_shape = (480, 640 * 2, 3) + image_shm = shared_memory.SharedMemory(create=True, size=np.prod(image_shape) * np.uint8().itemsize) + image_array = np.ndarray(image_shape, dtype=np.uint8, buffer=image_shm.buf) + use_hand_track=False + tv_wrapper = TeleVisionWrapper(binocular=True, use_hand_tracking=use_hand_track, img_shape=image_shape, img_shm_name=image_shm.name, + return_state_data=True, return_hand_rot_data = True) + try: + input("Press Enter to start tv_wrapper test...") + running = True + while running: + start_time = time.time() + teleData = tv_wrapper.get_motion_state_data() + + print("=== TeleData Snapshot ===") + print("[Head Rotation Matrix]:\n", teleData.head_rotation) + print("[Left EE Pose]:\n", teleData.left_arm_pose) + print("[Right EE Pose]:\n", teleData.right_arm_pose) + + if use_hand_track: + print("[Left Hand Position] shape {}:\n{}".format(teleData.left_hand_pos.shape, teleData.left_hand_pos)) + print("[Right Hand Position] shape {}:\n{}".format(teleData.right_hand_pos.shape, teleData.right_hand_pos)) + if teleData.left_hand_rot is not None: + print("[Left Hand Rotation] shape {}:\n{}".format(teleData.left_hand_rot.shape, teleData.left_hand_rot)) + if teleData.right_hand_rot is not None: + print("[Right Hand Rotation] shape {}:\n{}".format(teleData.right_hand_rot.shape, teleData.right_hand_rot)) + if teleData.left_pinch_value is not None: + print("[Left Pinch Value]: {:.2f}".format(teleData.left_pinch_value)) + if teleData.right_pinch_value is not None: + print("[Right Pinch Value]: {:.2f}".format(teleData.right_pinch_value)) + if teleData.tele_state: + state = teleData.tele_state + print("[Hand State]:") + print(f" Left Pinch state: {state.left_pinch_state}") + print(f" Left Squeeze: {state.left_squeeze_state} ({state.left_squeeze_value:.2f})") + print(f" Right Pinch state: {state.right_pinch_state}") + print(f" Right Squeeze: {state.right_squeeze_state} ({state.right_squeeze_value:.2f})") + else: + print(f"[Left Trigger Value]: {teleData.left_trigger_value:.2f}") + print(f"[Right Trigger Value]: {teleData.right_trigger_value:.2f}") + if teleData.tele_state: + state = teleData.tele_state + print("[Controller State]:") + print(f" Left Trigger: {state.left_trigger_state}") + print(f" Left Squeeze: {state.left_squeeze_ctrl_state} ({state.left_squeeze_ctrl_value:.2f})") + print(f" Left Thumbstick: {state.left_thumbstick_state} ({state.left_thumbstick_value})") + print(f" Left A/B Buttons: A={state.left_aButton}, B={state.left_bButton}") + print(f" Right Trigger: {state.right_trigger_state}") + print(f" Right Squeeze: {state.right_squeeze_ctrl_state} ({state.right_squeeze_ctrl_value:.2f})") + print(f" Right Thumbstick: {state.right_thumbstick_state} ({state.right_thumbstick_value})") + print(f" Right A/B Buttons: A={state.right_aButton}, B={state.right_bButton}") + + current_time = time.time() + time_elapsed = current_time - start_time + sleep_time = max(0, 0.033 - time_elapsed) + time.sleep(sleep_time) + # print(f"main process sleep: {sleep_time}") + + except KeyboardInterrupt: + running = False + print("KeyboardInterrupt, exiting program...") + finally: + image_shm.unlink() + image_shm.close() + print("Finally, exiting program...") + exit(0) + +if __name__ == '__main__': + run_test_tv_wrapper() \ No newline at end of file diff --git a/teleop/open_television/constants.py b/teleop/open_television/constants.py deleted file mode 100644 index 4b13257..0000000 --- a/teleop/open_television/constants.py +++ /dev/null @@ -1,53 +0,0 @@ -import numpy as np - - -T_to_unitree_left_wrist = np.array([[1, 0, 0, 0], - [0, 0, -1, 0], - [0, 1, 0, 0], - [0, 0, 0, 1]]) - -T_to_unitree_right_wrist = np.array([[1, 0, 0, 0], - [0, 0, 1, 0], - [0, -1, 0, 0], - [0, 0, 0, 1]]) - -T_to_unitree_hand = np.array([[0, 0, 1, 0], - [-1,0, 0, 0], - [0, -1,0, 0], - [0, 0, 0, 1]]) - -T_robot_openxr = np.array([[0, 0, -1, 0], - [-1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 0, 1]]) - - - -const_head_vuer_mat = np.array([[1, 0, 0, 0], - [0, 1, 0, 1.5], - [0, 0, 1, -0.2], - [0, 0, 0, 1]]) - - -# For G1 initial position -const_right_wrist_vuer_mat = np.array([[1, 0, 0, 0.15], - [0, 1, 0, 1.13], - [0, 0, 1, -0.3], - [0, 0, 0, 1]]) - -# For G1 initial position -const_left_wrist_vuer_mat = np.array([[1, 0, 0, -0.15], - [0, 1, 0, 1.13], - [0, 0, 1, -0.3], - [0, 0, 0, 1]]) - -# legacy -# const_right_wrist_vuer_mat = np.array([[1, 0, 0, 0.5], -# [0, 1, 0, 1], -# [0, 0, 1, -0.5], -# [0, 0, 0, 1]]) - -# const_left_wrist_vuer_mat = np.array([[1, 0, 0, -0.5], -# [0, 1, 0, 1], -# [0, 0, 1, -0.5], -# [0, 0, 0, 1]]) diff --git a/teleop/open_television/setup.py b/teleop/open_television/setup.py new file mode 100644 index 0000000..7407009 --- /dev/null +++ b/teleop/open_television/setup.py @@ -0,0 +1,20 @@ +from setuptools import setup, find_packages + +setup( + name='open_television', + version='0.0.1', + description='XR vision and hand/controller interface for unitree robotics', + author='silencht', + packages=find_packages(), + install_requires=[ + 'numpy==1.23.0', + 'opencv_contrib_python==4.10.0.82', + 'opencv_python==4.9.0.80', + 'aiohttp==3.9.5', + 'aiohttp_cors==0.7.0', + 'aiortc==1.8.0', + 'av==11.0.0', + 'vuer==0.0.42rc16', + ], + python_requires='>=3.8', +) \ No newline at end of file diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index fbcc7c0..c0be8fc 100644 --- a/teleop/open_television/television.py +++ b/teleop/open_television/television.py @@ -1,74 +1,220 @@ -import time from vuer import Vuer -from vuer.schemas import ImageBackground, Hands -from multiprocessing import Array, Process, shared_memory +from vuer.schemas import ImageBackground, Hands, MotionControllers, WebRTCVideoPlane, WebRTCStereoVideoPlane +from multiprocessing import Value, Array, Process, shared_memory import numpy as np import asyncio import cv2 - -from multiprocessing import context -Value = context._default_context.Value +import os class TeleVision: - def __init__(self, binocular, img_shape, img_shm_name, cert_file="./cert.pem", key_file="./key.pem", ngrok=False): + def __init__(self, binocular: bool, use_hand_tracking: bool, img_shape, img_shm_name, cert_file=None, key_file=None, ngrok=False, webrtc=False): + """ + TeleVision class for OpenXR-based VR/AR applications. + This class handles the communication with the Vuer server and manages the shared memory for image and pose data. + + :param binocular: bool, whether the application is binocular (stereoscopic) or monocular. + :param use_hand_tracking: bool, whether to use hand tracking or controller tracking. + :param img_shape: tuple, shape of the image (height, width, channels). + :param img_shm_name: str, name of the shared memory for the image. + :param cert_file: str, path to the SSL certificate file. + :param key_file: str, path to the SSL key file. + :param ngrok: bool, whether to use ngrok for tunneling. + """ self.binocular = binocular + self.use_hand_tracking = use_hand_tracking self.img_height = img_shape[0] - if binocular: + if self.binocular: self.img_width = img_shape[1] // 2 else: self.img_width = img_shape[1] + + current_module_dir = os.path.dirname(os.path.abspath(__file__)) + print(f"current_module_dir: {current_module_dir}") + if cert_file is None: + cert_file = os.path.join(current_module_dir, "cert.pem") + if key_file is None: + key_file = os.path.join(current_module_dir, "key.pem") if ngrok: self.vuer = Vuer(host='0.0.0.0', queries=dict(grid=False), queue_len=3) else: self.vuer = Vuer(host='0.0.0.0', cert=cert_file, key=key_file, queries=dict(grid=False), queue_len=3) - self.vuer.add_handler("HAND_MOVE")(self.on_hand_move) self.vuer.add_handler("CAMERA_MOVE")(self.on_cam_move) + if self.use_hand_tracking: + self.vuer.add_handler("HAND_MOVE")(self.on_hand_move) + else: + self.vuer.add_handler("CONTROLLER_MOVE")(self.on_controller_move) existing_shm = shared_memory.SharedMemory(name=img_shm_name) self.img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=existing_shm.buf) - if binocular: + self.webrtc = webrtc + if self.binocular and not self.webrtc: self.vuer.spawn(start=False)(self.main_image_binocular) - else: + elif not self.binocular and not self.webrtc: self.vuer.spawn(start=False)(self.main_image_monocular) + elif self.webrtc: + self.vuer.spawn(start=False)(self.main_image_webrtc) - self.left_hand_shared = Array('d', 16, lock=True) - self.right_hand_shared = Array('d', 16, lock=True) - self.left_landmarks_shared = Array('d', 75, lock=True) - self.right_landmarks_shared = Array('d', 75, lock=True) - - self.head_matrix_shared = Array('d', 16, lock=True) - self.aspect_shared = Value('d', 1.0, lock=True) + self.head_pose_shared = Array('d', 16, lock=True) + self.left_arm_pose_shared = Array('d', 16, lock=True) + self.right_arm_pose_shared = Array('d', 16, lock=True) + if self.use_hand_tracking: + self.left_hand_position_shared = Array('d', 75, lock=True) + self.right_hand_position_shared = Array('d', 75, lock=True) + self.left_hand_orientation_shared = Array('d', 25 * 9, lock=True) + self.right_hand_orientation_shared = Array('d', 25 * 9, lock=True) + + self.left_pinch_state_shared = Value('b', False, lock=True) + self.left_pinch_value_shared = Value('d', 0.0, lock=True) + self.left_squeeze_state_shared = Value('b', False, lock=True) + self.left_squeeze_value_shared = Value('d', 0.0, lock=True) + + self.right_pinch_state_shared = Value('b', False, lock=True) + self.right_pinch_value_shared = Value('d', 0.0, lock=True) + self.right_squeeze_state_shared = Value('b', False, lock=True) + self.right_squeeze_value_shared = Value('d', 0.0, lock=True) + else: + self.left_trigger_state_shared = Value('b', False, lock=True) + self.left_trigger_value_shared = Value('d', 0.0, lock=True) + self.left_squeeze_state_shared = Value('b', False, lock=True) + self.left_squeeze_value_shared = Value('d', 0.0, lock=True) + self.left_thumbstick_state_shared = Value('b', False, lock=True) + self.left_thumbstick_value_shared = Array('d', 2, lock=True) + self.left_aButton_shared = Value('b', False, lock=True) + self.left_bButton_shared = Value('b', False, lock=True) + + self.right_trigger_state_shared = Value('b', False, lock=True) + self.right_trigger_value_shared = Value('d', 0.0, lock=True) + self.right_squeeze_state_shared = Value('b', False, lock=True) + self.right_squeeze_value_shared = Value('d', 0.0, lock=True) + self.right_thumbstick_state_shared = Value('b', False, lock=True) + self.right_thumbstick_value_shared = Array('d', 2, lock=True) + self.right_aButton_shared = Value('b', False, lock=True) + self.right_bButton_shared = Value('b', False, lock=True) self.process = Process(target=self.vuer_run) self.process.daemon = True self.process.start() - def vuer_run(self): self.vuer.run() async def on_cam_move(self, event, session, fps=60): try: - self.head_matrix_shared[:] = event.value["camera"]["matrix"] - self.aspect_shared.value = event.value['camera']['aspect'] + with self.head_pose_shared.get_lock(): + self.head_pose_shared[:] = event.value["camera"]["matrix"] + except: + pass + + async def on_controller_move(self, event, session, fps=60): + try: + with self.left_arm_pose_shared.get_lock(): + self.left_arm_pose_shared[:] = event.value["left"] + with self.right_arm_pose_shared.get_lock(): + self.right_arm_pose_shared[:] = event.value["right"] + + left_controller_state = event.value["leftState"] + right_controller_state = event.value["rightState"] + + def extract_controller_states(state_dict, prefix): + # trigger + with getattr(self, f"{prefix}_trigger_state_shared").get_lock(): + getattr(self, f"{prefix}_trigger_state_shared").value = bool(state_dict.get("trigger", False)) + with getattr(self, f"{prefix}_trigger_value_shared").get_lock(): + getattr(self, f"{prefix}_trigger_value_shared").value = float(state_dict.get("triggerValue", 0.0)) + # squeeze + with getattr(self, f"{prefix}_squeeze_state_shared").get_lock(): + getattr(self, f"{prefix}_squeeze_state_shared").value = bool(state_dict.get("squeeze", False)) + with getattr(self, f"{prefix}_squeeze_value_shared").get_lock(): + getattr(self, f"{prefix}_squeeze_value_shared").value = float(state_dict.get("squeezeValue", 0.0)) + # thumbstick + with getattr(self, f"{prefix}_thumbstick_state_shared").get_lock(): + getattr(self, f"{prefix}_thumbstick_state_shared").value = bool(state_dict.get("thumbstick", False)) + with getattr(self, f"{prefix}_thumbstick_value_shared").get_lock(): + getattr(self, f"{prefix}_thumbstick_value_shared")[:] = state_dict.get("thumbstickValue", [0.0, 0.0]) + # buttons + with getattr(self, f"{prefix}_aButton_shared").get_lock(): + getattr(self, f"{prefix}_aButton_shared").value = bool(state_dict.get("aButton", False)) + with getattr(self, f"{prefix}_bButton_shared").get_lock(): + getattr(self, f"{prefix}_bButton_shared").value = bool(state_dict.get("bButton", False)) + + extract_controller_states(left_controller_state, "left") + extract_controller_states(right_controller_state, "right") except: pass async def on_hand_move(self, event, session, fps=60): try: - self.left_hand_shared[:] = event.value["leftHand"] - self.right_hand_shared[:] = event.value["rightHand"] - self.left_landmarks_shared[:] = np.array(event.value["leftLandmarks"]).flatten() - self.right_landmarks_shared[:] = np.array(event.value["rightLandmarks"]).flatten() - except: + left_hand_data = event.value["left"] + right_hand_data = event.value["right"] + left_hand_state = event.value["leftState"] + right_hand_state = event.value["rightState"] + + def extract_hand_poses(hand_data, arm_pose_shared, hand_position_shared, hand_orientation_shared): + with arm_pose_shared.get_lock(): + arm_pose_shared[:] = hand_data[0:16] + + with hand_position_shared.get_lock(): + for i in range(25): + base = i * 16 + hand_position_shared[i * 3: i * 3 + 3] = [hand_data[base + 12], hand_data[base + 13], hand_data[base + 14]] + + with hand_orientation_shared.get_lock(): + for i in range(25): + base = i * 16 + hand_orientation_shared[i * 9: i * 9 + 9] = [ + hand_data[base + 0], hand_data[base + 1], hand_data[base + 2], + hand_data[base + 4], hand_data[base + 5], hand_data[base + 6], + hand_data[base + 8], hand_data[base + 9], hand_data[base + 10], + ] + + def extract_hand_states(state_dict, prefix): + # pinch + with getattr(self, f"{prefix}_pinch_state_shared").get_lock(): + getattr(self, f"{prefix}_pinch_state_shared").value = bool(state_dict.get("pinch", False)) + with getattr(self, f"{prefix}_pinch_value_shared").get_lock(): + getattr(self, f"{prefix}_pinch_value_shared").value = float(state_dict.get("pinchValue", 0.0)) + # squeeze + with getattr(self, f"{prefix}_squeeze_state_shared").get_lock(): + getattr(self, f"{prefix}_squeeze_state_shared").value = bool(state_dict.get("squeeze", False)) + with getattr(self, f"{prefix}_squeeze_value_shared").get_lock(): + getattr(self, f"{prefix}_squeeze_value_shared").value = float(state_dict.get("squeezeValue", 0.0)) + + extract_hand_poses(left_hand_data, self.left_arm_pose_shared, self.left_hand_position_shared, self.left_hand_orientation_shared) + extract_hand_poses(right_hand_data, self.right_arm_pose_shared, self.right_hand_position_shared, self.right_hand_orientation_shared) + extract_hand_states(left_hand_state, "left") + extract_hand_states(right_hand_state, "right") + + except: pass async def main_image_binocular(self, session, fps=60): - session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False) + if self.use_hand_tracking: + session.upsert( + Hands( + stream=True, + key="hands", + showLeft=False, + showRight=False + ), + # we place this into the background children list, so that it is + # not affected by the global rotation + to="bgChildren", + ) + else: + session.upsert( + MotionControllers( + stream=True, + key="motion-controller", + showLeft=False, + showRight=False, + ), + to="bgChildren", + ) + while True: display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB) # aspect_ratio = self.img_width / self.img_height @@ -106,7 +252,26 @@ class TeleVision: await asyncio.sleep(0.016 * 2) async def main_image_monocular(self, session, fps=60): - session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False) + if self.use_hand_tracking: + session.upsert( + Hands( + stream=True, + key="hands", + showLeft=False, + showRight=False + ), + to="bgChildren", + ) + else: + session.upsert( + MotionControllers( + stream=True, + key="motion-controller", + showLeft=False, + showRight=False, + ), + to="bgChildren", + ) while True: display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB) # aspect_ratio = self.img_width / self.img_height @@ -127,52 +292,225 @@ class TeleVision: ) await asyncio.sleep(0.016) - @property - def left_hand(self): - return np.array(self.left_hand_shared[:]).reshape(4, 4, order="F") - + async def main_image_webrtc(self, session, fps=60): + if self.use_hand_tracking: + session.upsert( + Hands( + stream=True, + key="hands", + showLeft=False, + showRight=False + ), + to="bgChildren", + ) + else: + session.upsert( + MotionControllers( + stream=True, + key="motion-controller", + showLeft=False, + showRight=False, + ) + ) + session.upsert( + WebRTCVideoPlane( + # WebRTCStereoVideoPlane( + src="https://10.0.7.49:8080/offer", + iceServer={}, + key="webrtc", + aspect=1.778, + height = 7, + ), + to="bgChildren", + ) + while True: + await asyncio.sleep(1) + # ==================== common data ==================== @property - def right_hand(self): - return np.array(self.right_hand_shared[:]).reshape(4, 4, order="F") - - + def head_pose(self): + """np.ndarray, shape (4, 4), head SE(3) pose matrix from Vuer (basis OpenXR Convention).""" + with self.head_pose_shared.get_lock(): + return np.array(self.head_pose_shared[:]).reshape(4, 4, order="F") + @property - def left_landmarks(self): - return np.array(self.left_landmarks_shared[:]).reshape(25, 3) - + def left_arm_pose(self): + """np.ndarray, shape (4, 4), left arm SE(3) pose matrix from Vuer (basis OpenXR Convention).""" + with self.left_arm_pose_shared.get_lock(): + return np.array(self.left_arm_pose_shared[:]).reshape(4, 4, order="F") + @property - def right_landmarks(self): - return np.array(self.right_landmarks_shared[:]).reshape(25, 3) + def right_arm_pose(self): + """np.ndarray, shape (4, 4), right arm SE(3) pose matrix from Vuer (basis OpenXR Convention).""" + with self.right_arm_pose_shared.get_lock(): + return np.array(self.right_arm_pose_shared[:]).reshape(4, 4, order="F") + # ==================== Hand Tracking Data ==================== @property - def head_matrix(self): - return np.array(self.head_matrix_shared[:]).reshape(4, 4, order="F") + def left_hand_positions(self): + """np.ndarray, shape (25, 3), left hand 25 landmarks' 3D positions.""" + with self.left_hand_position_shared.get_lock(): + return np.array(self.left_hand_position_shared[:]).reshape(25, 3) @property - def aspect(self): - return float(self.aspect_shared.value) - -if __name__ == '__main__': - import os - import sys - current_dir = os.path.dirname(os.path.abspath(__file__)) - parent_dir = os.path.dirname(current_dir) - sys.path.append(parent_dir) - import threading - from image_server.image_client import ImageClient - - # image - img_shape = (480, 640 * 2, 3) - img_shm = shared_memory.SharedMemory(create=True, size=np.prod(img_shape) * np.uint8().itemsize) - img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=img_shm.buf) - img_client = ImageClient(tv_img_shape = img_shape, tv_img_shm_name = img_shm.name) - image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True) - image_receive_thread.start() - - # television - tv = TeleVision(True, img_shape, img_shm.name) - print("vuer unit test program running...") - print("you can press ^C to interrupt program.") - while True: - time.sleep(0.03) \ No newline at end of file + def right_hand_positions(self): + """np.ndarray, shape (25, 3), right hand 25 landmarks' 3D positions.""" + with self.right_hand_position_shared.get_lock(): + return np.array(self.right_hand_position_shared[:]).reshape(25, 3) + + @property + def left_hand_orientations(self): + """np.ndarray, shape (25, 3, 3), left hand 25 landmarks' orientations (flattened 3x3 matrices, column-major).""" + with self.left_hand_orientation_shared.get_lock(): + return np.array(self.left_hand_orientation_shared[:]).reshape(25, 9).reshape(25, 3, 3, order="F") + + @property + def right_hand_orientations(self): + """np.ndarray, shape (25, 3, 3), right hand 25 landmarks' orientations (flattened 3x3 matrices, column-major).""" + with self.right_hand_orientation_shared.get_lock(): + return np.array(self.right_hand_orientation_shared[:]).reshape(25, 9).reshape(25, 3, 3, order="F") + + @property + def left_hand_pinch_state(self): + """bool, whether left hand is pinching.""" + with self.left_pinch_state_shared.get_lock(): + return self.left_pinch_state_shared.value + + @property + def left_hand_pinch_value(self): + """float, pinch strength of left hand.""" + with self.left_pinch_value_shared.get_lock(): + return self.left_pinch_value_shared.value + + @property + def left_hand_squeeze_state(self): + """bool, whether left hand is squeezing.""" + with self.left_squeeze_state_shared.get_lock(): + return self.left_squeeze_state_shared.value + + @property + def left_hand_squeeze_value(self): + """float, squeeze strength of left hand.""" + with self.left_squeeze_value_shared.get_lock(): + return self.left_squeeze_value_shared.value + + @property + def right_hand_pinch_state(self): + """bool, whether right hand is pinching.""" + with self.right_pinch_state_shared.get_lock(): + return self.right_pinch_state_shared.value + + @property + def right_hand_pinch_value(self): + """float, pinch strength of right hand.""" + with self.right_pinch_value_shared.get_lock(): + return self.right_pinch_value_shared.value + + @property + def right_hand_squeeze_state(self): + """bool, whether right hand is squeezing.""" + with self.right_squeeze_state_shared.get_lock(): + return self.right_squeeze_state_shared.value + + @property + def right_hand_squeeze_value(self): + """float, squeeze strength of right hand.""" + with self.right_squeeze_value_shared.get_lock(): + return self.right_squeeze_value_shared.value + + # ==================== Controller Data ==================== + @property + def left_controller_trigger_state(self): + """bool, left controller trigger pressed or not.""" + with self.left_trigger_state_shared.get_lock(): + return self.left_trigger_state_shared.value + + @property + def left_controller_trigger_value(self): + """float, left controller trigger analog value (0.0 ~ 1.0).""" + with self.left_trigger_value_shared.get_lock(): + return self.left_trigger_value_shared.value + + @property + def left_controller_squeeze_state(self): + """bool, left controller squeeze pressed or not.""" + with self.left_squeeze_state_shared.get_lock(): + return self.left_squeeze_state_shared.value + + @property + def left_controller_squeeze_value(self): + """float, left controller squeeze analog value (0.0 ~ 1.0).""" + with self.left_squeeze_value_shared.get_lock(): + return self.left_squeeze_value_shared.value + + @property + def left_controller_thumbstick_state(self): + """bool, whether left thumbstick is touched or clicked.""" + with self.left_thumbstick_state_shared.get_lock(): + return self.left_thumbstick_state_shared.value + + @property + def left_controller_thumbstick_value(self): + """np.ndarray, shape (2,), left thumbstick 2D axis values (x, y).""" + with self.left_thumbstick_value_shared.get_lock(): + return np.array(self.left_thumbstick_value_shared[:]) + + @property + def left_controller_aButton(self): + """bool, left controller 'A' button pressed.""" + with self.left_aButton_shared.get_lock(): + return self.left_aButton_shared.value + + @property + def left_controller_bButton(self): + """bool, left controller 'B' button pressed.""" + with self.left_bButton_shared.get_lock(): + return self.left_bButton_shared.value + + @property + def right_controller_trigger_state(self): + """bool, right controller trigger pressed or not.""" + with self.right_trigger_state_shared.get_lock(): + return self.right_trigger_state_shared.value + + @property + def right_controller_trigger_value(self): + """float, right controller trigger analog value (0.0 ~ 1.0).""" + with self.right_trigger_value_shared.get_lock(): + return self.right_trigger_value_shared.value + + @property + def right_controller_squeeze_state(self): + """bool, right controller squeeze pressed or not.""" + with self.right_squeeze_state_shared.get_lock(): + return self.right_squeeze_state_shared.value + + @property + def right_controller_squeeze_value(self): + """float, right controller squeeze analog value (0.0 ~ 1.0).""" + with self.right_squeeze_value_shared.get_lock(): + return self.right_squeeze_value_shared.value + + @property + def right_controller_thumbstick_state(self): + """bool, whether right thumbstick is touched or clicked.""" + with self.right_thumbstick_state_shared.get_lock(): + return self.right_thumbstick_state_shared.value + + @property + def right_controller_thumbstick_value(self): + """np.ndarray, shape (2,), right thumbstick 2D axis values (x, y).""" + with self.right_thumbstick_value_shared.get_lock(): + return np.array(self.right_thumbstick_value_shared[:]) + + @property + def right_controller_aButton(self): + """bool, right controller 'A' button pressed.""" + with self.right_aButton_shared.get_lock(): + return self.right_aButton_shared.value + + @property + def right_controller_bButton(self): + """bool, right controller 'B' button pressed.""" + with self.right_bButton_shared.get_lock(): + return self.right_bButton_shared.value \ No newline at end of file diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py index 1363ba5..e4abfac 100644 --- a/teleop/open_television/tv_wrapper.py +++ b/teleop/open_television/tv_wrapper.py @@ -1,147 +1,411 @@ import numpy as np -from teleop.open_television.television import TeleVision -from teleop.open_television.constants import * -from teleop.utils.mat_tool import mat_update, fast_mat_inv +from open_television.television import TeleVision +from dataclasses import dataclass, field """ (basis) OpenXR Convention : y up, z back, x right. (basis) Robot Convention : z up, y left, x front. -p.s. Vuer's all raw data follows OpenXR Convention, WORLD coordinate. -under (basis) Robot Convention, wrist's initial pose convention: +under (basis) Robot Convention, humanoid arm's initial pose convention: - # (Left Wrist) XR/AppleVisionPro Convention: + # (initial pose) OpenXR Left Arm Pose Convention (hand tracking): - the x-axis pointing from wrist toward middle. - the y-axis pointing from index toward pinky. - the z-axis pointing from palm toward back of the hand. - # (Right Wrist) XR/AppleVisionPro Convention: + # (initial pose) OpenXR Right Arm Pose Convention (hand tracking): - the x-axis pointing from wrist toward middle. - the y-axis pointing from pinky toward index. - the z-axis pointing from palm toward back of the hand. - # (Left Wrist URDF) Unitree Convention: + # (initial pose) Unitree Humanoid Left Arm URDF Convention: - the x-axis pointing from wrist toward middle. - the y-axis pointing from palm toward back of the hand. - the z-axis pointing from pinky toward index. - # (Right Wrist URDF) Unitree Convention: + # (initial pose) Unitree Humanoid Right Arm URDF Convention: - the x-axis pointing from wrist toward middle. - the y-axis pointing from back of the hand toward palm. - the z-axis pointing from pinky toward index. -under (basis) Robot Convention, hand's initial pose convention: +under (basis) Robot Convention, humanoid hand's initial pose convention: - # (Left Hand) XR/AppleVisionPro Convention: + # (initial pose) OpenXR Left Hand Pose Convention (hand tracking): - the x-axis pointing from wrist toward middle. - the y-axis pointing from index toward pinky. - the z-axis pointing from palm toward back of the hand. - # (Right Hand) XR/AppleVisionPro Convention: + # (initial pose) OpenXR Right Hand Pose Convention (hand tracking): - the x-axis pointing from wrist toward middle. - the y-axis pointing from pinky toward index. - the z-axis pointing from palm toward back of the hand. - # (Left Hand URDF) Unitree Convention: + # (initial pose) Unitree Humanoid Left Hand URDF Convention: - The x-axis pointing from palm toward back of the hand. - The y-axis pointing from middle toward wrist. - The z-axis pointing from pinky toward index. - # (Right Hand URDF) Unitree Convention: + # (initial pose) Unitree Humanoid Right Hand URDF Convention: - The x-axis pointing from palm toward back of the hand. - The y-axis pointing from middle toward wrist. - The z-axis pointing from index toward pinky. + +p.s. TeleVision (Vuer) obtains all raw data under the (basis) OpenXR Convention. + In addition, arm pose data (hand tracking) follows the (initial pose) OpenXR Arm Pose Convention, + while arm pose data (controller tracking) directly follows the (initial pose) Unitree Humanoid Arm URDF Convention (thus no transform is needed). + Meanwhile, all raw data is in the WORLD frame defined by XR device odometry. - p.s. From website: https://registry.khronos.org/OpenXR/specs/1.1/man/html/openxr.html. - You can find **(Left/Right Wrist) XR/AppleVisionPro Convention** related information like this below: - "The wrist joint is located at the pivot point of the wrist, which is location invariant when twisting the hand without moving the forearm. - The backward (+Z) direction is parallel to the line from wrist joint to middle finger metacarpal joint, and points away from the finger tips. - The up (+Y) direction points out towards back of the hand and perpendicular to the skin at wrist. - The X direction is perpendicular to the Y and Z directions and follows the right hand rule." - Note: The above context is of course under **(basis) OpenXR Convention**. +p.s. From website: https://registry.khronos.org/OpenXR/specs/1.1/man/html/openxr.html. + You can find **(initial pose) OpenXR Left/Right Arm Pose Convention** related information like this below: + "The wrist joint is located at the pivot point of the wrist, which is location invariant when twisting the hand without moving the forearm. + The backward (+Z) direction is parallel to the line from wrist joint to middle finger metacarpal joint, and points away from the finger tips. + The up (+Y) direction points out towards back of the hand and perpendicular to the skin at wrist. + The X direction is perpendicular to the Y and Z directions and follows the right hand rule." + Note: The above context is of course under **(basis) OpenXR Convention**. - p.s. **(Wrist/Hand URDF) Unitree Convention** information come from URDF files. +p.s. **Unitree Arm/Hand URDF initial pose Convention** information come from URDF files. """ + +def safe_mat_update(prev_mat, mat): + # Return previous matrix and False flag if the new matrix is non-singular (determinant ≠ 0). + det = np.linalg.det(mat) + if not np.isfinite(det) or np.isclose(det, 0.0, atol=1e-6): + return prev_mat, False + return mat, True + +def fast_mat_inv(mat): + ret = np.eye(4) + ret[:3, :3] = mat[:3, :3].T + ret[:3, 3] = -mat[:3, :3].T @ mat[:3, 3] + return ret + +def safe_rot_update(prev_rot_array, rot_array): + dets = np.linalg.det(rot_array) + if not np.all(np.isfinite(dets)) or np.any(np.isclose(dets, 0.0, atol=1e-6)): + return prev_rot_array, False + return rot_array, True + +# constants variable +T_TO_UNITREE_HUMANOID_LEFT_ARM = np.array([[1, 0, 0, 0], + [0, 0,-1, 0], + [0, 1, 0, 0], + [0, 0, 0, 1]]) + +T_TO_UNITREE_HUMANOID_RIGHT_ARM = np.array([[1, 0, 0, 0], + [0, 0, 1, 0], + [0,-1, 0, 0], + [0, 0, 0, 1]]) + +T_TO_UNITREE_HAND = np.array([[0, 0, 1, 0], + [-1, 0, 0, 0], + [0, -1, 0, 0], + [0, 0, 0, 1]]) + +T_ROBOT_OPENXR = np.array([[ 0, 0,-1, 0], + [-1, 0, 0, 0], + [ 0, 1, 0, 0], + [ 0, 0, 0, 1]]) + +T_OPENXR_ROBOT = np.array([[ 0,-1, 0, 0], + [ 0, 0, 1, 0], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + +R_ROBOT_OPENXR = np.array([[ 0, 0,-1], + [-1, 0, 0], + [ 0, 1, 0]]) + +R_OPENXR_ROBOT = np.array([[ 0,-1, 0], + [ 0, 0, 1], + [-1, 0, 0]]) + +CONST_HEAD_POSE = np.array([[1, 0, 0, 0], + [0, 1, 0, 1.5], + [0, 0, 1, -0.2], + [0, 0, 0, 1]]) + +# For Robot initial position +CONST_RIGHT_ARM_POSE = np.array([[1, 0, 0, 0.15], + [0, 1, 0, 1.13], + [0, 0, 1, -0.3], + [0, 0, 0, 1]]) + +CONST_LEFT_ARM_POSE = np.array([[1, 0, 0, -0.15], + [0, 1, 0, 1.13], + [0, 0, 1, -0.3], + [0, 0, 0, 1]]) + +CONST_HAND_ROT = np.tile(np.eye(3)[None, :, :], (25, 1, 1)) + +@dataclass +class TeleStateData: + # hand tracking + left_pinch_state: bool = False # True if index and thumb are pinching + left_squeeze_state: bool = False # True if hand is making a fist + left_squeeze_value: float = 0.0 # (0.0 ~ 1.0) degree of hand squeeze + right_pinch_state: bool = False # True if index and thumb are pinching + right_squeeze_state: bool = False # True if hand is making a fist + right_squeeze_value: float = 0.0 # (0.0 ~ 1.0) degree of hand squeeze + + # controller tracking + left_trigger_state: bool = False # True if trigger is actively pressed + left_squeeze_ctrl_state: bool = False # True if grip button is pressed + left_squeeze_ctrl_value: float = 0.0 # (0.0 ~ 1.0) grip pull depth + left_thumbstick_state: bool = False # True if thumbstick button is pressed + left_thumbstick_value: np.ndarray = field(default_factory=lambda: np.zeros(2)) # 2D vector (x, y), normalized + left_aButton: bool = False # True if A button is pressed + left_bButton: bool = False # True if B button is pressed + right_trigger_state: bool = False # True if trigger is actively pressed + right_squeeze_ctrl_state: bool = False # True if grip button is pressed + right_squeeze_ctrl_value: float = 0.0 # (0.0 ~ 1.0) grip pull depth + right_thumbstick_state: bool = False # True if thumbstick button is pressed + right_thumbstick_value: np.ndarray = field(default_factory=lambda: np.zeros(2)) # 2D vector (x, y), normalized + right_aButton: bool = False # True if A button is pressed + right_bButton: bool = False # True if B button is pressed + +@dataclass +class TeleData: + head_rotation: np.ndarray # (3,3) Head orientation matrix + left_arm_pose: np.ndarray # (4,4) SE(3) pose of left arm + right_arm_pose: np.ndarray # (4,4) SE(3) pose of right arm + left_hand_pos: np.ndarray = None # (25,3) 3D positions of left hand joints + right_hand_pos: np.ndarray = None # (25,3) 3D positions of right hand joints + left_hand_rot: np.ndarray = None # (25,3,3) rotation matrices of left hand joints + right_hand_rot: np.ndarray = None # (25,3,3) rotation matrices of right hand joints + left_pinch_value: float = None # float (1x.0 ~ 0.0) pinch distance between index and thumb + right_pinch_value: float = None # float (1x.0 ~ 0.0) pinch distance between index and thumb + left_trigger_value: float = None # float (10.0 ~ 0.0) trigger pull depth + right_trigger_value: float = None # float (10.0 ~ 0.0) trigger pull depth + tele_state: TeleStateData = field(default_factory=TeleStateData) + + class TeleVisionWrapper: - def __init__(self, binocular, img_shape, img_shm_name): - self.tv = TeleVision(binocular, img_shape, img_shm_name) - - def get_data(self): - - # --------------------------------wrist------------------------------------- - - # TeleVision obtains a basis coordinate that is OpenXR Convention - head_vuer_mat, head_flag = mat_update(const_head_vuer_mat, self.tv.head_matrix.copy()) - left_wrist_vuer_mat, left_wrist_flag = mat_update(const_left_wrist_vuer_mat, self.tv.left_hand.copy()) - right_wrist_vuer_mat, right_wrist_flag = mat_update(const_right_wrist_vuer_mat, self.tv.right_hand.copy()) - - # Change basis convention: VuerMat ((basis) OpenXR Convention) to WristMat ((basis) Robot Convention) - # p.s. WristMat = T_{robot}_{openxr} * VuerMat * T_{robot}_{openxr}^T - # Reason for right multiply fast_mat_inv(T_robot_openxr): - # This is similarity transformation: B = PAP^{-1}, that is B ~ A - # For example: - # - For a pose data T_r under the Robot Convention, left-multiplying WristMat means: - # - WristMat * T_r ==> T_{robot}_{openxr} * VuerMat * T_{openxr}_{robot} * T_r - # - First, transform to the OpenXR Convention (The function of T_{openxr}_{robot}) - # - then, apply the rotation VuerMat in the OpenXR Convention (The function of VuerMat) - # - finally, transform back to the Robot Convention (The function of T_{robot}_{openxr}) - # This results in the same rotation effect under the Robot Convention as in the OpenXR Convention. - head_mat = T_robot_openxr @ head_vuer_mat @ fast_mat_inv(T_robot_openxr) - left_wrist_mat = T_robot_openxr @ left_wrist_vuer_mat @ fast_mat_inv(T_robot_openxr) - right_wrist_mat = T_robot_openxr @ right_wrist_vuer_mat @ fast_mat_inv(T_robot_openxr) - - # Change wrist convention: WristMat ((Left Wrist) XR/AppleVisionPro Convention) to UnitreeWristMat((Left Wrist URDF) Unitree Convention) - # Reason for right multiply (T_to_unitree_left_wrist) : Rotate 90 degrees counterclockwise about its own x-axis. - # Reason for right multiply (T_to_unitree_right_wrist): Rotate 90 degrees clockwise about its own x-axis. - unitree_left_wrist = left_wrist_mat @ (T_to_unitree_left_wrist if left_wrist_flag else np.eye(4)) - unitree_right_wrist = right_wrist_mat @ (T_to_unitree_right_wrist if right_wrist_flag else np.eye(4)) - - # Transfer from WORLD to HEAD coordinate (translation only). - unitree_left_wrist[0:3, 3] = unitree_left_wrist[0:3, 3] - head_mat[0:3, 3] - unitree_right_wrist[0:3, 3] = unitree_right_wrist[0:3, 3] - head_mat[0:3, 3] - - # --------------------------------hand------------------------------------- - - # Homogeneous, [xyz] to [xyz1] - # p.s. np.concatenate([25,3]^T,(1,25)) ==> hand_vuer_mat.shape is (4,25) - # Now under (basis) OpenXR Convention, mat shape like this: - # x0 x1 x2 ··· x23 x24 - # y0 y1 y1 ··· y23 y24 - # z0 z1 z2 ··· z23 z24 - # 1 1 1 ··· 1 1 - left_hand_vuer_mat = np.concatenate([self.tv.left_landmarks.copy().T, np.ones((1, self.tv.left_landmarks.shape[0]))]) - right_hand_vuer_mat = np.concatenate([self.tv.right_landmarks.copy().T, np.ones((1, self.tv.right_landmarks.shape[0]))]) - - # Change basis convention: from (basis) OpenXR Convention to (basis) Robot Convention - # Just a change of basis for 3D points. No rotation, only translation. No need to right-multiply fast_mat_inv(T_robot_openxr). - left_hand_mat = T_robot_openxr @ left_hand_vuer_mat - right_hand_mat = T_robot_openxr @ right_hand_vuer_mat - - # Transfer from WORLD to WRIST coordinate. (this process under (basis) Robot Convention) - # p.s. HandMat_WristBased = WristMat_{wrold}_{wrist}^T * HandMat_{wrold} - # HandMat_WristBased = WristMat_{wrist}_{wrold} * HandMat_{wrold}, that is HandMat_{wrist} - left_hand_mat_wb = fast_mat_inv(left_wrist_mat) @ left_hand_mat - right_hand_mat_wb = fast_mat_inv(right_wrist_mat) @ right_hand_mat - # Change hand convention: HandMat ((Left Hand) XR/AppleVisionPro Convention) to UnitreeHandMat((Left Hand URDF) Unitree Convention) - # Reason for left multiply : T_to_unitree_hand @ left_hand_mat_wb ==> (4,4) @ (4,25) ==> (4,25), (4,25)[0:3, :] ==> (3,25), (3,25).T ==> (25,3) - # Now under (Left Hand URDF) Unitree Convention, mat shape like this: - # [x0, y0, z0] - # [x1, y1, z1] - # ··· - # [x23,y23,z23] - # [x24,y24,z24] - unitree_left_hand = (T_to_unitree_hand @ left_hand_mat_wb)[0:3, :].T - unitree_right_hand = (T_to_unitree_hand @ right_hand_mat_wb)[0:3, :].T - - # --------------------------------offset------------------------------------- - - head_rmat = head_mat[:3, :3] - # The origin of the coordinate for IK Solve is the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to check it. - # The origin of the coordinate of unitree_left_wrist is HEAD. So it is necessary to translate the origin of unitree_left_wrist from HEAD to WAIST. - unitree_left_wrist[0, 3] +=0.15 - unitree_right_wrist[0,3] +=0.15 - unitree_left_wrist[2, 3] +=0.45 - unitree_right_wrist[2,3] +=0.45 - - return head_rmat, unitree_left_wrist, unitree_right_wrist, unitree_left_hand, unitree_right_hand \ No newline at end of file + def __init__(self, binocular: bool, use_hand_tracking: bool, img_shape, img_shm_name, return_state_data: bool = True, return_hand_rot_data: bool = False, + cert_file = None, key_file = None): + """ + TeleVisionWrapper is a wrapper for the TeleVision class, which handles XR device's data suit for robot control. + It initializes the TeleVision instance with the specified parameters and provides a method to get motion state data. + + :param binocular: A boolean indicating whether the head camera device is binocular or not. + :param use_hand_tracking: A boolean indicating whether to use hand tracking or use controller tracking. + :param img_shape: The shape of the image to be processed. + :param img_shm_name: The name of the shared memory for the image. + :param return_state: A boolean indicating whether to return the state of the hand or controller. + :param return_hand_rot: A boolean indicating whether to return the hand rotation data. + :param cert_file: The path to the certificate file for secure connection. + :param key_file: The path to the key file for secure connection. + """ + self.use_hand_tracking = use_hand_tracking + self.return_state_data = return_state_data + self.return_hand_rot_data = return_hand_rot_data + self.tvuer = TeleVision(binocular, use_hand_tracking, img_shape, img_shm_name, cert_file=cert_file, key_file=key_file) + + def get_motion_state_data(self): + """ + Get processed motion state data from the TeleVision instance. + + All returned data are transformed from the OpenXR Convention to the (Robot & Unitree) Convention. + """ + # Variable Naming Convention below, + # ┌────────────┬───────────────────────────┬──────────────────────────────────┬────────────────────────────────────┬────────────────────────────────────┐ + # │left / right│ Bxr │ Brobot │ IPxr │ IPunitree │ + # │────────────│───────────────────────────│──────────────────────────────────│────────────────────────────────────│────────────────────────────────────│ + # │ side │ (basis) OpenXR Convention │ (basis) Robot Convention │ (initial pose) OpenXR Convention │ (initial pose) Unitree Convention │ + # └────────────┴───────────────────────────┴──────────────────────────────────┴────────────────────────────────────┴────────────────────────────────────┘ + # ┌───────────────────────────────────┬─────────────────────┐ + # │ world / arm / head / waist │ arm / head / hand │ + # │───────────────────────────────────│─────────────────────│ + # │ source frame │ target frame │ + # └───────────────────────────────────┴─────────────────────┘ + + # TeleVision (Vuer) obtains all raw data under the (basis) OpenXR Convention. + Bxr_world_head, head_pose_is_valid = safe_mat_update(CONST_HEAD_POSE, self.tvuer.head_pose) + + if self.use_hand_tracking: + # 'Arm' pose data follows (basis) OpenXR Convention and (initial pose) OpenXR Arm Convention. + left_IPxr_Bxr_world_arm, left_arm_is_valid = safe_mat_update(CONST_LEFT_ARM_POSE, self.tvuer.left_arm_pose) + right_IPxr_Bxr_world_arm, right_arm_is_valid = safe_mat_update(CONST_RIGHT_ARM_POSE, self.tvuer.right_arm_pose) + + # Change basis convention + # From (basis) OpenXR Convention to (basis) Robot Convention: + # Brobot_Pose = T_{robot}_{openxr} * Bxr_Pose * T_{robot}_{openxr}^T ==> + # Brobot_Pose = T_{robot}_{openxr} * Bxr_Pose * T_{openxr}_{robot} + # Reason for right multiply T_OPENXR_ROBOT = fast_mat_inv(T_ROBOT_OPENXR): + # This is similarity transformation: B = PAP^{-1}, that is B ~ A + # For example: + # - For a pose data T_r under the (basis) Robot Convention, left-multiplying Brobot_Pose means: + # Brobot_Pose * T_r ==> T_{robot}_{openxr} * PoseMatrix_openxr * T_{openxr}_{robot} * T_r + # - First, transform T_r to the (basis) OpenXR Convention (The function of T_{openxr}_{robot}) + # - Then, apply the rotation PoseMatrix_openxr in the OpenXR Convention (The function of PoseMatrix_openxr) + # - Finally, transform back to the Robot Convention (The function of T_{robot}_{openxr}) + # - This results in the same rotation effect under the Robot Convention as in the OpenXR Convention. + Brobot_world_head = T_ROBOT_OPENXR @ Bxr_world_head @ T_OPENXR_ROBOT + left_IPxr_Brobot_world_arm = T_ROBOT_OPENXR @ left_IPxr_Bxr_world_arm @ T_OPENXR_ROBOT + right_IPxr_Brobot_world_arm = T_ROBOT_OPENXR @ right_IPxr_Bxr_world_arm @ T_OPENXR_ROBOT + + # Change initial pose convention + # From (initial pose) OpenXR Arm Convention to (initial pose) Unitree Humanoid Arm URDF Convention + # Reason for right multiply (T_TO_UNITREE_HUMANOID_LEFT_ARM) : Rotate 90 degrees counterclockwise about its own x-axis. + # Reason for right multiply (T_TO_UNITREE_HUMANOID_RIGHT_ARM): Rotate 90 degrees clockwise about its own x-axis. + left_IPunitree_Brobot_world_arm = left_IPxr_Brobot_world_arm @ (T_TO_UNITREE_HUMANOID_LEFT_ARM if left_arm_is_valid else np.eye(4)) + right_IPunitree_Brobot_world_arm = right_IPxr_Brobot_world_arm @ (T_TO_UNITREE_HUMANOID_RIGHT_ARM if right_arm_is_valid else np.eye(4)) + + # Transfer from WORLD to HEAD coordinate (translation adjustment only) + left_IPunitree_Brobot_head_arm = left_IPunitree_Brobot_world_arm.copy() + right_IPunitree_Brobot_head_arm = right_IPunitree_Brobot_world_arm.copy() + left_IPunitree_Brobot_head_arm[0:3, 3] = left_IPunitree_Brobot_head_arm[0:3, 3] - Brobot_world_head[0:3, 3] + right_IPunitree_Brobot_head_arm[0:3, 3] = right_IPunitree_Brobot_world_arm[0:3, 3] - Brobot_world_head[0:3, 3] + + # =====coordinate origin offset===== + Brobot_world_head_rot = Brobot_world_head[:3, :3] + # The origin of the coordinate for IK Solve is near the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to visualize it. + # The origin of the coordinate of IPunitree_Brobot_head_arm is HEAD. + # So it is necessary to translate the origin of IPunitree_Brobot_head_arm from HEAD to WAIST. + left_IPunitree_Brobot_waist_arm = left_IPunitree_Brobot_head_arm.copy() + right_IPunitree_Brobot_waist_arm = right_IPunitree_Brobot_head_arm.copy() + left_IPunitree_Brobot_waist_arm[0, 3] +=0.15 # x + right_IPunitree_Brobot_waist_arm[0,3] +=0.15 + left_IPunitree_Brobot_waist_arm[2, 3] +=0.45 # z + right_IPunitree_Brobot_waist_arm[2,3] +=0.45 + + # -----------------------------------hand position---------------------------------------- + if left_arm_is_valid and right_arm_is_valid: + # Homogeneous, [xyz] to [xyz1] + # np.concatenate([25,3]^T,(1,25)) ==> Bxr_world_hand_pos.shape is (4,25) + # Now under (basis) OpenXR Convention, Bxr_world_hand_pos data like this: + # [x0 x1 x2 ··· x23 x24] + # [y0 y1 y1 ··· y23 y24] + # [z0 z1 z2 ··· z23 z24] + # [ 1 1 1 ··· 1 1] + left_IPxr_Bxr_world_hand_pos = np.concatenate([self.tvuer.left_hand_positions.T, np.ones((1, self.tvuer.left_hand_positions.shape[0]))]) + right_IPxr_Bxr_world_hand_pos = np.concatenate([self.tvuer.right_hand_positions.T, np.ones((1, self.tvuer.right_hand_positions.shape[0]))]) + + # Change basis convention + # From (basis) OpenXR Convention to (basis) Robot Convention + # Just a change of basis for 3D points. No rotation, only translation. So, no need to right-multiply fast_mat_inv(T_ROBOT_OPENXR). + left_IPxr_Brobot_world_hand_pos = T_ROBOT_OPENXR @ left_IPxr_Bxr_world_hand_pos + right_IPxr_Brobot_world_hand_pos = T_ROBOT_OPENXR @ right_IPxr_Bxr_world_hand_pos + + # Transfer from WORLD to ARM frame under (basis) Robot Convention: + # Brobot_{world}_{arm}^T * Brobot_{world}_pos ==> Brobot_{arm}_{world} * Brobot_{world}_pos ==> Brobot_arm_hand_pos, Now it's based on the arm frame. + left_IPxr_Brobot_arm_hand_pos = fast_mat_inv(left_IPxr_Brobot_world_arm) @ left_IPxr_Brobot_world_hand_pos + right_IPxr_Brobot_arm_hand_pos = fast_mat_inv(right_IPxr_Brobot_world_arm) @ right_IPxr_Brobot_world_hand_pos + + # Change initial pose convention + # From (initial pose) XR Hand Convention to (initial pose) Unitree Humanoid Hand URDF Convention: + # T_TO_UNITREE_HAND @ IPxr_Brobot_arm_hand_pos ==> IPunitree_Brobot_arm_hand_pos + # ((4,4) @ (4,25))[0:3, :].T ==> (4,25)[0:3, :].T ==> (3,25).T ==> (25,3) + # Now under (initial pose) Unitree Humanoid Hand URDF Convention, matrix shape like this: + # [x0, y0, z0] + # [x1, y1, z1] + # ··· + # [x23,y23,z23] + # [x24,y24,z24] + left_IPunitree_Brobot_arm_hand_pos = (T_TO_UNITREE_HAND @ left_IPxr_Brobot_arm_hand_pos)[0:3, :].T + right_IPunitree_Brobot_arm_hand_pos = (T_TO_UNITREE_HAND @ right_IPxr_Brobot_arm_hand_pos)[0:3, :].T + else: + left_IPunitree_Brobot_arm_hand_pos = np.zeros((25, 3)) + right_IPunitree_Brobot_arm_hand_pos = np.zeros((25, 3)) + + # -----------------------------------hand rotation---------------------------------------- + if self.return_hand_rot_data: + left_Bxr_world_hand_rot, left_hand_rot_is_valid = safe_rot_update(CONST_HAND_ROT, self.tvuer.left_hand_orientations) # [25, 3, 3] + right_Bxr_world_hand_rot, right_hand_rot_is_valid = safe_rot_update(CONST_HAND_ROT, self.tvuer.right_hand_orientations) + + if left_hand_rot_is_valid and right_hand_rot_is_valid: + left_Bxr_arm_hand_rot = np.einsum('ij,njk->nik', left_IPxr_Bxr_world_arm[:3, :3].T, left_Bxr_world_hand_rot) + right_Bxr_arm_hand_rot = np.einsum('ij,njk->nik', right_IPxr_Bxr_world_arm[:3, :3].T, right_Bxr_world_hand_rot) + # Change basis convention + left_Brobot_arm_hand_rot = np.einsum('ij,njk,kl->nil', R_ROBOT_OPENXR, left_Bxr_arm_hand_rot, R_OPENXR_ROBOT) + right_Brobot_arm_hand_rot = np.einsum('ij,njk,kl->nil', R_ROBOT_OPENXR, right_Bxr_arm_hand_rot, R_OPENXR_ROBOT) + else: + left_Brobot_arm_hand_rot = left_Bxr_world_hand_rot + right_Brobot_arm_hand_rot = right_Bxr_world_hand_rot + else: + left_Brobot_arm_hand_rot = None + right_Brobot_arm_hand_rot = None + + if self.return_state_data: + hand_state = TeleStateData( + left_pinch_state=self.tvuer.left_hand_pinch_state, + left_squeeze_state=self.tvuer.left_hand_squeeze_state, + left_squeeze_value=self.tvuer.left_hand_squeeze_value, + right_pinch_state=self.tvuer.right_hand_pinch_state, + right_squeeze_state=self.tvuer.right_hand_squeeze_state, + right_squeeze_value=self.tvuer.right_hand_squeeze_value, + ) + else: + hand_state = None + + return TeleData( + head_rotation=Brobot_world_head_rot, + left_arm_pose=left_IPunitree_Brobot_waist_arm, + right_arm_pose=right_IPunitree_Brobot_waist_arm, + left_hand_pos=left_IPunitree_Brobot_arm_hand_pos, + right_hand_pos=right_IPunitree_Brobot_arm_hand_pos, + left_hand_rot=left_Brobot_arm_hand_rot, + right_hand_rot=right_Brobot_arm_hand_rot, + left_pinch_value=self.tvuer.left_hand_pinch_value * 100.0, + right_pinch_value=self.tvuer.right_hand_pinch_value * 100.0, + tele_state=hand_state + ) + else: + # Controller pose data directly follows the (initial pose) Unitree Humanoid Arm URDF Convention (thus no transform is needed). + left_IPunitree_Bxr_world_arm, left_arm_is_valid = safe_mat_update(CONST_LEFT_ARM_POSE, self.tvuer.left_arm_pose) + right_IPunitree_Bxr_world_arm, right_arm_is_valid = safe_mat_update(CONST_RIGHT_ARM_POSE, self.tvuer.right_arm_pose) + + # Change basis convention + Brobot_world_head = T_ROBOT_OPENXR @ Bxr_world_head @ T_OPENXR_ROBOT + left_IPunitree_Brobot_world_arm = T_ROBOT_OPENXR @ left_IPunitree_Bxr_world_arm @ T_OPENXR_ROBOT + right_IPunitree_Brobot_world_arm = T_ROBOT_OPENXR @ right_IPunitree_Bxr_world_arm @ T_OPENXR_ROBOT + + # Transfer from WORLD to HEAD coordinate (translation adjustment only) + left_IPunitree_Brobot_head_arm = left_IPunitree_Brobot_world_arm.copy() + right_IPunitree_Brobot_head_arm = right_IPunitree_Brobot_world_arm.copy() + left_IPunitree_Brobot_head_arm[0:3, 3] = left_IPunitree_Brobot_head_arm[0:3, 3] - Brobot_world_head[0:3, 3] + right_IPunitree_Brobot_head_arm[0:3, 3] = right_IPunitree_Brobot_head_arm[0:3, 3] - Brobot_world_head[0:3, 3] + + # =====coordinate origin offset===== + Brobot_world_head_rot = Brobot_world_head[:3, :3] + # The origin of the coordinate for IK Solve is near the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to check it. + # The origin of the coordinate of IPunitree_Brobot_head_arm is HEAD. + # So it is necessary to translate the origin of IPunitree_Brobot_head_arm from HEAD to WAIST. + left_IPunitree_Brobot_waist_arm = left_IPunitree_Brobot_head_arm.copy() + right_IPunitree_Brobot_waist_arm = right_IPunitree_Brobot_head_arm.copy() + left_IPunitree_Brobot_waist_arm[0, 3] +=0.15 # x + right_IPunitree_Brobot_waist_arm[0,3] +=0.15 + left_IPunitree_Brobot_waist_arm[2, 3] +=0.45 # z + right_IPunitree_Brobot_waist_arm[2,3] +=0.45 + # left_IPunitree_Brobot_waist_arm[1, 3] +=0.02 # y + # right_IPunitree_Brobot_waist_arm[1,3] +=0.02 + + # return data + if self.return_state_data: + controller_state = TeleStateData( + left_trigger_state=self.tvuer.left_controller_trigger_state, + left_squeeze_ctrl_state=self.tvuer.left_controller_squeeze_state, + left_squeeze_ctrl_value=self.tvuer.left_controller_squeeze_value, + left_thumbstick_state=self.tvuer.left_controller_thumbstick_state, + left_thumbstick_value=self.tvuer.left_controller_thumbstick_value, + left_aButton=self.tvuer.left_controller_aButton, + left_bButton=self.tvuer.left_controller_bButton, + right_trigger_state=self.tvuer.right_controller_trigger_state, + right_squeeze_ctrl_state=self.tvuer.right_controller_squeeze_state, + right_squeeze_ctrl_value=self.tvuer.right_controller_squeeze_value, + right_thumbstick_state=self.tvuer.right_controller_thumbstick_state, + right_thumbstick_value=self.tvuer.right_controller_thumbstick_value, + right_aButton=self.tvuer.right_controller_aButton, + right_bButton=self.tvuer.right_controller_bButton, + ) + else: + controller_state = None + + return TeleData( + head_rotation=Brobot_world_head_rot, + left_arm_pose=left_IPunitree_Brobot_waist_arm, + right_arm_pose=right_IPunitree_Brobot_waist_arm, + left_trigger_value=10.0 - self.tvuer.left_controller_trigger_value * 10, + right_trigger_value=10.0 - self.tvuer.right_controller_trigger_value * 10, + tele_state=controller_state + ) \ No newline at end of file diff --git a/teleop/robot_control/robot_arm_ik.py b/teleop/robot_control/robot_arm_ik.py index 1ef1787..57c92e0 100644 --- a/teleop/robot_control/robot_arm_ik.py +++ b/teleop/robot_control/robot_arm_ik.py @@ -84,8 +84,9 @@ class G1_29_ArmIK: # frame = self.reduced_robot.model.frames[i] # frame_id = self.reduced_robot.model.getFrameId(frame.name) # print(f"Frame ID: {frame_id}, Name: {frame.name}") - for idx, name in enumerate(self.reduced_robot.model.names): - print(f"{idx}: {name}") + # for idx, name in enumerate(self.reduced_robot.model.names): + # print(f"{idx}: {name}") + # Creating Casadi models and data for symbolic computing self.cmodel = cpin.Model(self.reduced_robot.model) self.cdata = self.cmodel.createData() diff --git a/teleop/robot_control/robot_hand_inspire.py b/teleop/robot_control/robot_hand_inspire.py index ac57397..3c907b9 100644 --- a/teleop/robot_control/robot_hand_inspire.py +++ b/teleop/robot_control/robot_hand_inspire.py @@ -98,8 +98,10 @@ class Inspire_Controller: while self.running: start_time = time.time() # get dual hand state - left_hand_mat = np.array(left_hand_array[:]).reshape(25, 3).copy() - right_hand_mat = np.array(right_hand_array[:]).reshape(25, 3).copy() + with left_hand_array.get_lock(): + left_hand_mat = np.array(left_hand_array[:]).reshape(25, 3).copy() + with right_hand_array.get_lock(): + right_hand_mat = np.array(right_hand_array[:]).reshape(25, 3).copy() # Read left and right q_state from shared arrays state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index 039b759..9142ea6 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -30,20 +30,20 @@ kTopicDex3RightState = "rt/dex3/right/state" class Dex3_1_Controller: - def __init__(self, left_hand_array, right_hand_array, dual_hand_data_lock = None, dual_hand_state_array = None, - dual_hand_action_array = None, fps = 100.0, Unit_Test = False): + def __init__(self, left_hand_array_in, right_hand_array_in, dual_hand_data_lock = None, dual_hand_state_array_out = None, + dual_hand_action_array_out = None, fps = 100.0, Unit_Test = False): """ [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process - left_hand_array: [input] Left hand skeleton data (required from XR device) to hand_ctrl.control_process + left_hand_array_in: [input] Left hand skeleton data (required from XR device) to hand_ctrl.control_process - right_hand_array: [input] Right hand skeleton data (required from XR device) to hand_ctrl.control_process + right_hand_array_in: [input] Right hand skeleton data (required from XR device) to hand_ctrl.control_process dual_hand_data_lock: Data synchronization lock for dual_hand_state_array and dual_hand_action_array - dual_hand_state_array: [output] Return left(7), right(7) hand motor state + dual_hand_state_array_out: [output] Return left(7), right(7) hand motor state - dual_hand_action_array: [output] Return left(7), right(7) hand motor action + dual_hand_action_array_out: [output] Return left(7), right(7) hand motor action fps: Control frequency @@ -85,8 +85,8 @@ class Dex3_1_Controller: time.sleep(0.01) print("[Dex3_1_Controller] Waiting to subscribe dds...") - hand_control_process = Process(target=self.control_process, args=(left_hand_array, right_hand_array, self.left_hand_state_array, self.right_hand_state_array, - dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)) + hand_control_process = Process(target=self.control_process, args=(left_hand_array_in, right_hand_array_in, self.left_hand_state_array, self.right_hand_state_array, + dual_hand_data_lock, dual_hand_state_array_out, dual_hand_action_array_out)) hand_control_process.daemon = True hand_control_process.start() @@ -129,8 +129,8 @@ class Dex3_1_Controller: self.RightHandCmb_publisher.Write(self.right_msg) # print("hand ctrl publish ok.") - def control_process(self, left_hand_array, right_hand_array, left_hand_state_array, right_hand_state_array, - dual_hand_data_lock = None, dual_hand_state_array = None, dual_hand_action_array = None): + def control_process(self, left_hand_array_in, right_hand_array_in, left_hand_state_array, right_hand_state_array, + dual_hand_data_lock = None, dual_hand_state_array_out = None, dual_hand_action_array_out = None): self.running = True left_q_target = np.full(Dex3_Num_Motors, 0) @@ -170,15 +170,17 @@ class Dex3_1_Controller: while self.running: start_time = time.time() # get dual hand state - left_hand_mat = np.array(left_hand_array[:]).reshape(25, 3).copy() - right_hand_mat = np.array(right_hand_array[:]).reshape(25, 3).copy() + with left_hand_array_in.get_lock(): + left_hand_data = np.array(left_hand_array_in[:]).reshape(25, 3).copy() + with right_hand_array_in.get_lock(): + right_hand_data = np.array(right_hand_array_in[:]).reshape(25, 3).copy() # Read left and right q_state from shared arrays state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) - if not np.all(right_hand_mat == 0.0) and not np.all(left_hand_mat[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. - ref_left_value = left_hand_mat[unitree_tip_indices] - ref_right_value = right_hand_mat[unitree_tip_indices] + if not np.all(right_hand_data == 0.0) and not np.all(left_hand_data[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. + ref_left_value = left_hand_data[unitree_tip_indices] + ref_right_value = right_hand_data[unitree_tip_indices] ref_left_value[0] = ref_left_value[0] * 1.15 ref_left_value[1] = ref_left_value[1] * 1.05 ref_left_value[2] = ref_left_value[2] * 0.95 @@ -191,10 +193,10 @@ class Dex3_1_Controller: # get dual hand action action_data = np.concatenate((left_q_target, right_q_target)) - if dual_hand_state_array and dual_hand_action_array: + if dual_hand_state_array_out and dual_hand_action_array_out: with dual_hand_data_lock: - dual_hand_state_array[:] = state_data - dual_hand_action_array[:] = action_data + dual_hand_state_array_out[:] = state_data + dual_hand_action_array_out[:] = action_data self.ctrl_dual_hand(left_q_target, right_q_target) current_time = time.time() @@ -229,20 +231,20 @@ kTopicGripperCommand = "rt/unitree_actuator/cmd" kTopicGripperState = "rt/unitree_actuator/state" class Gripper_Controller: - def __init__(self, left_hand_array, right_hand_array, dual_gripper_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None, + def __init__(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None, filter = True, fps = 200.0, Unit_Test = False): """ [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process - left_hand_array: [input] Left hand skeleton data (required from XR device) to control_thread + left_gripper_value_in: [input] Left ctrl data (required from XR device) to control_thread - right_hand_array: [input] Right hand skeleton data (required from XR device) to control_thread + right_gripper_value_in: [input] Right ctrl data (required from XR device) to control_thread dual_gripper_data_lock: Data synchronization lock for dual_gripper_state_array and dual_gripper_action_array - dual_gripper_state: [output] Return left(1), right(1) gripper motor state + dual_gripper_state_out: [output] Return left(1), right(1) gripper motor state - dual_gripper_action: [output] Return left(1), right(1) gripper motor action + dual_gripper_action_out: [output] Return left(1), right(1) gripper motor action fps: Control frequency @@ -281,7 +283,7 @@ class Gripper_Controller: time.sleep(0.01) print("[Gripper_Controller] Waiting to subscribe dds...") - self.gripper_control_thread = threading.Thread(target=self.control_thread, args=(left_hand_array, right_hand_array, self.dual_gripper_state, + self.gripper_control_thread = threading.Thread(target=self.control_thread, args=(left_gripper_value_in, right_gripper_value_in, self.dual_gripper_state, dual_gripper_data_lock, dual_gripper_state_out, dual_gripper_action_out)) self.gripper_control_thread.daemon = True self.gripper_control_thread.start() @@ -304,13 +306,13 @@ class Gripper_Controller: self.GripperCmb_publisher.Write(self.gripper_msg) # print("gripper ctrl publish ok.") - def control_thread(self, left_hand_array, right_hand_array, dual_gripper_state_in, dual_hand_data_lock = None, + def control_thread(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_state_in, dual_hand_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None): self.running = True DELTA_GRIPPER_CMD = 0.18 # The motor rotates 5.4 radians, the clamping jaw slide open 9 cm, so 0.6 rad <==> 1 cm, 0.18 rad <==> 3 mm - THUMB_INDEX_DISTANCE_MIN = 0.05 # Assuming a minimum Euclidean distance is 5 cm between thumb and index. - THUMB_INDEX_DISTANCE_MAX = 0.07 # Assuming a maximum Euclidean distance is 9 cm between thumb and index. + THUMB_INDEX_DISTANCE_MIN = 5.0 + THUMB_INDEX_DISTANCE_MAX = 7.0 LEFT_MAPPED_MIN = 0.0 # The minimum initial motor position when the gripper closes at startup. RIGHT_MAPPED_MIN = 0.0 # The minimum initial motor position when the gripper closes at startup. # The maximum initial motor position when the gripper closes before calibration (with the rail stroke calculated as 0.6 cm/rad * 9 rad = 5.4 cm). @@ -336,15 +338,15 @@ class Gripper_Controller: while self.running: start_time = time.time() # get dual hand skeletal point state from XR device - left_hand_mat = np.array(left_hand_array[:]).reshape(25, 3).copy() - right_hand_mat = np.array(right_hand_array[:]).reshape(25, 3).copy() + with left_gripper_value_in.get_lock(): + left_gripper_value = left_gripper_value_in + with right_gripper_value_in.get_lock(): + right_gripper_value = right_gripper_value_in - if not np.all(right_hand_mat == 0.0) and not np.all(left_hand_mat[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. - left_euclidean_distance = np.linalg.norm(left_hand_mat[unitree_gripper_indices[1]] - left_hand_mat[unitree_gripper_indices[0]]) - right_euclidean_distance = np.linalg.norm(right_hand_mat[unitree_gripper_indices[1]] - right_hand_mat[unitree_gripper_indices[0]]) + if left_gripper_value != 0.0 or right_gripper_value != 0.0: # if hand data has been initialized. # Linear mapping from [0, THUMB_INDEX_DISTANCE_MAX] to gripper action range - left_target_action = np.interp(left_euclidean_distance, [THUMB_INDEX_DISTANCE_MIN, THUMB_INDEX_DISTANCE_MAX], [LEFT_MAPPED_MIN, LEFT_MAPPED_MAX]) - right_target_action = np.interp(right_euclidean_distance, [THUMB_INDEX_DISTANCE_MIN, THUMB_INDEX_DISTANCE_MAX], [RIGHT_MAPPED_MIN, RIGHT_MAPPED_MAX]) + left_target_action = np.interp(left_gripper_value, [THUMB_INDEX_DISTANCE_MIN, THUMB_INDEX_DISTANCE_MAX], [LEFT_MAPPED_MIN, LEFT_MAPPED_MAX]) + right_target_action = np.interp(right_gripper_value, [THUMB_INDEX_DISTANCE_MIN, THUMB_INDEX_DISTANCE_MAX], [RIGHT_MAPPED_MIN, RIGHT_MAPPED_MAX]) # else: # TEST WITHOUT XR DEVICE # current_time = time.time() # period = 2.5 diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 0ede616..b6bb76c 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -2,7 +2,7 @@ import numpy as np import time import argparse import cv2 -from multiprocessing import shared_memory, Array, Lock +from multiprocessing import shared_memory, Value, Array, Lock import threading import os @@ -11,7 +11,7 @@ current_dir = os.path.dirname(os.path.abspath(__file__)) parent_dir = os.path.dirname(current_dir) sys.path.append(parent_dir) -from teleop.open_television.tv_wrapper import TeleVisionWrapper +from teleop.open_television import TeleVisionWrapper from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, H1_ArmController from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK, H1_ArmIK from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_Controller @@ -23,14 +23,15 @@ from teleop.utils.episode_writer import EpisodeWriter if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--task_dir', type = str, default = './utils/data', help = 'path to save data') - parser.add_argument('--frequency', type = int, default = 30.0, help = 'save data\'s frequency') + parser.add_argument('--frequency', type = float, default = 30.0, help = 'save data\'s frequency') parser.add_argument('--record', action = 'store_true', help = 'Save data or not') parser.add_argument('--no-record', dest = 'record', action = 'store_false', help = 'Do not save data') parser.set_defaults(record = False) + parser.add_argument('--xr_mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') - parser.add_argument('--hand', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select hand controller') + parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') args = parser.parse_args() print(f"args:{args}\n") @@ -77,7 +78,7 @@ if __name__ == '__main__': image_receive_thread.start() # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. - tv_wrapper = TeleVisionWrapper(BINOCULAR, tv_img_shape, tv_img_shm.name) + tv_wrapper = TeleVisionWrapper(BINOCULAR, args.xr_mode == 'hand', tv_img_shape, tv_img_shm.name) # arm if args.arm == 'G1_29': @@ -93,28 +94,28 @@ if __name__ == '__main__': arm_ctrl = H1_ArmController() arm_ik = H1_ArmIK() - # hand - if args.hand == "dex3": - left_hand_array = Array('d', 75, lock = True) # [input] - right_hand_array = Array('d', 75, lock = True) # [input] + # end-effector + if args.ee == "dex3": + left_hand_pos_array = Array('d', 75, lock = True) # [input] + right_hand_pos_array = Array('d', 75, lock = True) # [input] dual_hand_data_lock = Lock() - dual_hand_state_array = Array('d', 14, lock = False) # [output] current left, right hand state(14) data. - dual_hand_action_array = Array('d', 14, lock = False) # [output] current left, right hand action(14) data. - hand_ctrl = Dex3_1_Controller(left_hand_array, right_hand_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) - elif args.hand == "gripper": - left_hand_array = Array('d', 75, lock=True) - right_hand_array = Array('d', 75, lock=True) + dual_hand_state_array = Array('d', 14, lock = False) # [output] current left, right hand state(14) data. + dual_hand_action_array = Array('d', 14, lock = False) # [output] current left, right hand action(14) data. + hand_ctrl = Dex3_1_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) + elif args.ee == "gripper": + left_gripper_value = Value('d', 0.0, lock=True) # [input] + right_gripper_value = Value('d', 0.0, lock=True) # [input] dual_gripper_data_lock = Lock() dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. - gripper_ctrl = Gripper_Controller(left_hand_array, right_hand_array, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array) - elif args.hand == "inspire1": - left_hand_array = Array('d', 75, lock = True) # [input] - right_hand_array = Array('d', 75, lock = True) # [input] + gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array) + elif args.ee == "inspire1": + left_hand_pos_array = Array('d', 75, lock = True) # [input] + right_hand_pos_array = Array('d', 75, lock = True) # [input] dual_hand_data_lock = Lock() dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. - hand_ctrl = Inspire_Controller(left_hand_array, right_hand_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) + hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) else: pass @@ -126,16 +127,28 @@ if __name__ == '__main__': user_input = input("Please enter the start signal (enter 'r' to start the subsequent program):\n") if user_input.lower() == 'r': arm_ctrl.speed_gradual_max() - running = True while running: start_time = time.time() - head_rmat, left_wrist, right_wrist, left_hand, right_hand = tv_wrapper.get_data() + tele_data = tv_wrapper.get_motion_state_data() - # send hand skeleton data to hand_ctrl.control_process - if args.hand: - left_hand_array[:] = left_hand.flatten() - right_hand_array[:] = right_hand.flatten() + if (args.ee == 'dex3' or args.ee == 'inspire1') and args.xr_mode == 'hand': + with left_hand_pos_array.get_lock(): + left_hand_pos_array[:] = tele_data.left_hand_pos.flatten() + with right_hand_pos_array.get_lock(): + right_hand_pos_array[:] = tele_data.right_hand_pos.flatten() + elif args.ee == 'gripper' and args.xr_mode == 'controller': + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_trigger_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_trigger_value + elif args.ee == 'gripper' and args.xr_mode == 'hand': + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_pinch_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_pinch_value + else: + pass # get current state data. current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() @@ -143,7 +156,7 @@ if __name__ == '__main__': # solve ik using motor data and wrist pose, then use ik results to control arms. time_ik_start = time.time() - sol_q, sol_tauff = arm_ik.solve_ik(left_wrist, right_wrist, current_lr_arm_q, current_lr_arm_dq) + sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) time_ik_end = time.time() # print(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) @@ -164,27 +177,29 @@ if __name__ == '__main__': # record data if args.record: # dex hand or gripper - if args.hand == "dex3": + if args.ee == "dex3": with dual_hand_data_lock: left_hand_state = dual_hand_state_array[:7] right_hand_state = dual_hand_state_array[-7:] left_hand_action = dual_hand_action_array[:7] right_hand_action = dual_hand_action_array[-7:] - elif args.hand == "gripper": + elif args.ee == "gripper": with dual_gripper_data_lock: left_hand_state = [dual_gripper_state_array[1]] right_hand_state = [dual_gripper_state_array[0]] left_hand_action = [dual_gripper_action_array[1]] right_hand_action = [dual_gripper_action_array[0]] - elif args.hand == "inspire1": + elif args.ee == "inspire1": with dual_hand_data_lock: left_hand_state = dual_hand_state_array[:6] right_hand_state = dual_hand_state_array[-6:] left_hand_action = dual_hand_action_array[:6] right_hand_action = dual_hand_action_array[-6:] else: - print("No dexterous hand set.") - pass + left_hand_state = [] + right_hand_state = [] + left_hand_action = [] + right_hand_action = [] # head image current_tv_image = tv_img_array.copy() # wrist image @@ -195,7 +210,6 @@ if __name__ == '__main__': right_arm_state = current_lr_arm_q[-7:] left_arm_action = sol_q[:7] right_arm_action = sol_q[-7:] - if recording: colors = {} depths = {} @@ -221,12 +235,12 @@ if __name__ == '__main__': "qvel": [], "torque": [], }, - "left_hand": { + "left_hand_pos": { "qpos": left_hand_state, "qvel": [], "torque": [], }, - "right_hand": { + "right_hand_pos": { "qpos": right_hand_state, "qvel": [], "torque": [], @@ -244,12 +258,12 @@ if __name__ == '__main__': "qvel": [], "torque": [], }, - "left_hand": { + "left_hand_pos": { "qpos": left_hand_action, "qvel": [], "torque": [], }, - "right_hand": { + "right_hand_pos": { "qpos": right_hand_action, "qvel": [], "torque": [], @@ -260,7 +274,7 @@ if __name__ == '__main__': current_time = time.time() time_elapsed = current_time - start_time - sleep_time = max(0, (1 / float(args.frequency)) - time_elapsed) + sleep_time = max(0, (1 / args.frequency) - time_elapsed) time.sleep(sleep_time) # print(f"main process sleep: {sleep_time}") diff --git a/teleop/utils/mat_tool.py b/teleop/utils/mat_tool.py deleted file mode 100644 index 349489f..0000000 --- a/teleop/utils/mat_tool.py +++ /dev/null @@ -1,14 +0,0 @@ -import numpy as np - -def mat_update(prev_mat, mat): - if np.linalg.det(mat) == 0: - return prev_mat, False # Return previous matrix and False flag if the new matrix is non-singular (determinant ≠ 0). - else: - return mat, True - - -def fast_mat_inv(mat): - ret = np.eye(4) - ret[:3, :3] = mat[:3, :3].T - ret[:3, 3] = -mat[:3, :3].T @ mat[:3, 3] - return ret \ No newline at end of file From 7c3850451e4f9c1b85eedfaaea53e3603fa9b13e Mon Sep 17 00:00:00 2001 From: silencht Date: Wed, 14 May 2025 17:50:42 +0800 Subject: [PATCH 02/32] [fix] type bug. --- teleop/robot_control/robot_hand_unitree.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index 9142ea6..4787a59 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -339,9 +339,9 @@ class Gripper_Controller: start_time = time.time() # get dual hand skeletal point state from XR device with left_gripper_value_in.get_lock(): - left_gripper_value = left_gripper_value_in + left_gripper_value = left_gripper_value_in.value with right_gripper_value_in.get_lock(): - right_gripper_value = right_gripper_value_in + right_gripper_value = right_gripper_value_in.value if left_gripper_value != 0.0 or right_gripper_value != 0.0: # if hand data has been initialized. # Linear mapping from [0, THUMB_INDEX_DISTANCE_MAX] to gripper action range From 275ebdeacf2de9df3bfb5792f37bca07a1730876 Mon Sep 17 00:00:00 2001 From: silencht Date: Mon, 19 May 2025 12:10:29 +0800 Subject: [PATCH 03/32] [update] under main operation control, teleoperate and record. --- teleop/open_television/television.py | 47 +++++----------------------- teleop/open_television/tv_wrapper.py | 2 +- teleop/robot_control/robot_arm.py | 9 +++++- teleop/teleop_hand_and_arm.py | 44 ++++++++++++++++++++++---- 4 files changed, 54 insertions(+), 48 deletions(-) diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index c0be8fc..9087f81 100644 --- a/teleop/open_television/television.py +++ b/teleop/open_television/television.py @@ -193,27 +193,9 @@ class TeleVision: async def main_image_binocular(self, session, fps=60): if self.use_hand_tracking: - session.upsert( - Hands( - stream=True, - key="hands", - showLeft=False, - showRight=False - ), - # we place this into the background children list, so that it is - # not affected by the global rotation - to="bgChildren", - ) + session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False) else: - session.upsert( - MotionControllers( - stream=True, - key="motion-controller", - showLeft=False, - showRight=False, - ), - to="bgChildren", - ) + session.upsert @ MotionControllers(fps=fps, stream=True, key="motion-controller", showLeft=False, showRight=False) while True: display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB) @@ -230,7 +212,7 @@ class TeleVision: # Note that these two masks are associated with left eye’s camera and the right eye’s camera. layers=1, format="jpeg", - quality=50, + quality=90, key="background-left", interpolate=True, ), @@ -241,7 +223,7 @@ class TeleVision: distanceToCamera=1, layers=2, format="jpeg", - quality=50, + quality=90, key="background-right", interpolate=True, ), @@ -253,25 +235,10 @@ class TeleVision: async def main_image_monocular(self, session, fps=60): if self.use_hand_tracking: - session.upsert( - Hands( - stream=True, - key="hands", - showLeft=False, - showRight=False - ), - to="bgChildren", - ) + session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False) else: - session.upsert( - MotionControllers( - stream=True, - key="motion-controller", - showLeft=False, - showRight=False, - ), - to="bgChildren", - ) + session.upsert @ MotionControllers(fps=fps, stream=True, key="motion-controller", showLeft=False, showRight=False) + while True: display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB) # aspect_ratio = self.img_width / self.img_height diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py index e4abfac..e399a84 100644 --- a/teleop/open_television/tv_wrapper.py +++ b/teleop/open_television/tv_wrapper.py @@ -1,5 +1,5 @@ import numpy as np -from open_television.television import TeleVision +from television import TeleVision from dataclasses import dataclass, field """ diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index fa040f7..1e64c7b 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -11,8 +11,10 @@ from unitree_sdk2py.utils.crc import CRC from unitree_sdk2py.idl.unitree_go.msg.dds_ import ( LowCmd_ as go_LowCmd, LowState_ as go_LowState) # idl for h1 from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ -kTopicLowCommand = "rt/lowcmd" +# kTopicLowCommand = "rt/lowcmd" kTopicLowState = "rt/lowstate" +kTopicLowCommand = "rt/arm_sdk" + G1_29_Num_Motors = 35 G1_23_Num_Motors = 35 H1_2_Num_Motors = 35 @@ -149,6 +151,8 @@ class G1_29_ArmController: return cliped_arm_q_target def _ctrl_motor_state(self): + self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0; + while True: start_time = time.time() @@ -209,6 +213,9 @@ class G1_29_ArmController: while True: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): + for weight in np.arange(1, 0, -0.01): + self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; + time.sleep(0.02) print("[G1_29_ArmController] both arms have reached the home position.") break time.sleep(0.05) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index b6bb76c..dc8bc8c 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -29,7 +29,7 @@ if __name__ == '__main__': parser.add_argument('--no-record', dest = 'record', action = 'store_false', help = 'Do not save data') parser.set_defaults(record = False) - parser.add_argument('--xr_mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') + parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') @@ -118,6 +118,13 @@ if __name__ == '__main__': hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) else: pass + + # xr mode + if args.xr_mode == 'controller': + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + sport_client = LocoClient() + sport_client.SetTimeout(0.0001) + sport_client.Init() if args.record: recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = True) @@ -142,6 +149,15 @@ if __name__ == '__main__': left_gripper_value.value = tele_data.left_trigger_value with right_gripper_value.get_lock(): right_gripper_value.value = tele_data.right_trigger_value + # quit or damp + if tele_data.tele_state.right_aButton: + running = False + if tele_data.tele_state.right_thumbstick_state: + sport_client.Damp() + # control + sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3) elif args.ee == 'gripper' and args.xr_mode == 'hand': with left_gripper_value.get_lock(): left_gripper_value.value = tele_data.left_pinch_value @@ -177,19 +193,29 @@ if __name__ == '__main__': # record data if args.record: # dex hand or gripper - if args.ee == "dex3": + if args.ee == "dex3" and args.xr_mode == 'hand': with dual_hand_data_lock: left_hand_state = dual_hand_state_array[:7] right_hand_state = dual_hand_state_array[-7:] left_hand_action = dual_hand_action_array[:7] right_hand_action = dual_hand_action_array[-7:] - elif args.ee == "gripper": + elif args.ee == "gripper" and args.xr_mode == 'hand': + with dual_gripper_data_lock: + left_hand_state = [dual_gripper_state_array[1]] + right_hand_state = [dual_gripper_state_array[0]] + left_hand_action = [dual_gripper_action_array[1]] + right_hand_action = [dual_gripper_action_array[0]] + elif args.ee == "gripper" and args.xr_mode == 'controller': with dual_gripper_data_lock: left_hand_state = [dual_gripper_state_array[1]] right_hand_state = [dual_gripper_state_array[0]] left_hand_action = [dual_gripper_action_array[1]] right_hand_action = [dual_gripper_action_array[0]] - elif args.ee == "inspire1": + current_body_state = arm_ctrl.get_current_motor_q().tolist() + current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3] + elif args.ee == "inspire1" and args.xr_mode == 'hand': with dual_hand_data_lock: left_hand_state = dual_hand_state_array[:6] right_hand_state = dual_hand_state_array[-6:] @@ -200,6 +226,8 @@ if __name__ == '__main__': right_hand_state = [] left_hand_action = [] right_hand_action = [] + current_body_state = [] + current_body_action = [] # head image current_tv_image = tv_img_array.copy() # wrist image @@ -245,7 +273,9 @@ if __name__ == '__main__': "qvel": [], "torque": [], }, - "body": None, + "body": { + "qpos": current_body_state, + }, } actions = { "left_arm": { @@ -268,7 +298,9 @@ if __name__ == '__main__': "qvel": [], "torque": [], }, - "body": None, + "body": { + "qpos": current_body_action, + }, } recorder.add_item(colors=colors, depths=depths, states=states, actions=actions) From 85ffd6b8de4e3a60dba97be2a301ec5e34e411fd Mon Sep 17 00:00:00 2001 From: silencht Date: Wed, 21 May 2025 15:16:12 +0800 Subject: [PATCH 04/32] [update] optimize interface --- teleop/open_television/_test_television.py | 11 ++++- teleop/open_television/_test_tv_wrapper.py | 10 ++++- teleop/open_television/setup.py | 24 +++++++++- teleop/open_television/television.py | 47 +++++++++++++++---- teleop/open_television/tv_wrapper.py | 5 ++- teleop/robot_control/robot_arm.py | 30 ++++++++----- teleop/teleop_hand_and_arm.py | 52 ++++++++++++---------- 7 files changed, 129 insertions(+), 50 deletions(-) diff --git a/teleop/open_television/_test_television.py b/teleop/open_television/_test_television.py index f311101..79e1902 100644 --- a/teleop/open_television/_test_television.py +++ b/teleop/open_television/_test_television.py @@ -15,9 +15,16 @@ def run_test_television(): image_shm = shared_memory.SharedMemory(create=True, size=np.prod(image_shape) * np.uint8().itemsize) image_array = np.ndarray(image_shape, dtype=np.uint8, buffer=image_shm.buf) + # from image_server.image_client import ImageClient + # import threading + # image_client = ImageClient(tv_img_shape = image_shape, tv_img_shm_name = image_shm.name, image_show=True, server_address="127.0.0.1") + # image_receive_thread = threading.Thread(target = image_client.receive_process, daemon = True) + # image_receive_thread.daemon = True + # image_receive_thread.start() + # television - use_hand_track = False - tv = TeleVision(binocular = True, use_hand_tracking = use_hand_track, img_shape = image_shape, img_shm_name = image_shm.name, webrtc=True) + use_hand_track = True + tv = TeleVision(binocular = True, use_hand_tracking = use_hand_track, img_shape = image_shape, img_shm_name = image_shm.name, webrtc=False) try: input("Press Enter to start television test...") diff --git a/teleop/open_television/_test_tv_wrapper.py b/teleop/open_television/_test_tv_wrapper.py index 938c0d3..00f8330 100644 --- a/teleop/open_television/_test_tv_wrapper.py +++ b/teleop/open_television/_test_tv_wrapper.py @@ -13,7 +13,15 @@ def run_test_tv_wrapper(): image_shape = (480, 640 * 2, 3) image_shm = shared_memory.SharedMemory(create=True, size=np.prod(image_shape) * np.uint8().itemsize) image_array = np.ndarray(image_shape, dtype=np.uint8, buffer=image_shm.buf) - use_hand_track=False + + # from image_server.image_client import ImageClient + # import threading + # image_client = ImageClient(tv_img_shape = image_shape, tv_img_shm_name = image_shm.name, image_show=True, server_address="127.0.0.1") + # image_receive_thread = threading.Thread(target = image_client.receive_process, daemon = True) + # image_receive_thread.daemon = True + # image_receive_thread.start() + + use_hand_track=True tv_wrapper = TeleVisionWrapper(binocular=True, use_hand_tracking=use_hand_track, img_shape=image_shape, img_shm_name=image_shm.name, return_state_data=True, return_hand_rot_data = True) try: diff --git a/teleop/open_television/setup.py b/teleop/open_television/setup.py index 7407009..6799080 100644 --- a/teleop/open_television/setup.py +++ b/teleop/open_television/setup.py @@ -14,7 +14,29 @@ setup( 'aiohttp_cors==0.7.0', 'aiortc==1.8.0', 'av==11.0.0', - 'vuer==0.0.42rc16', + # 'vuer==0.0.32rc7', # avp \ pico, hand tracking all work fine. + + # 'vuer==0.0.35', # avp hand tracking works fine. pico is fine too, but the right eye occasionally goes black for a short time at start. + + # from 'vuer==0.0.36rc1', # avp hand tracking only shows a flat RGB image — there's no stereo view. Pico (hand / controller tracking) is the same, + # and sometimes the right eye occasionally goes black for a short time at start. + # Both avp / pico can display the hand or controller marker, which looks like a black box. + + # to 'vuer==0.0.42rc16', # avp hand tracking only shows a flat RGB image — there's no stereo view. Pico (hand / controller tracking) is the same, + # and sometimes the right eye occasionally goes black for a short time at start. + # Both avp / pico can display the hand or controller marker, which looks like RGB three-axis color coordinates. + + # from 'vuer==0.0.42', # avp hand tracking only shows a flat RGB image — there's no stereo view. + # to 'vuer==0.0.45', pico hand tracking also only shows a flat RGB image — there's no stereo view. Unable to retrieve hand tracking data. + # pico controller tracking also only shows a flat RGB image — there's no stereo view. Controller data can be retrieved. + + # from 'vuer==0.0.46' # avp hand tracking work fine. + # to + 'vuer==0.0.56', # In pico hand tracking mode, using a hand gesture to click the "Virtual Reality" button + # causes the screen to stay black and stuck loading. But if the button is clicked with the controller instead, everything works fine. + # In pico controller tracking mode, using controller to click the "Virtual Reality" button + # causes the screen to stay black and stuck loading. But if the button is clicked with a hand gesture instead, everything works fine. + # avp \ pico all have hand marker visualization (RGB three-axis color coordinates). ], python_requires='>=3.8', ) \ No newline at end of file diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index 9087f81..3f6787a 100644 --- a/teleop/open_television/television.py +++ b/teleop/open_television/television.py @@ -30,7 +30,6 @@ class TeleVision: self.img_width = img_shape[1] current_module_dir = os.path.dirname(os.path.abspath(__file__)) - print(f"current_module_dir: {current_module_dir}") if cert_file is None: cert_file = os.path.join(current_module_dir, "cert.pem") if key_file is None: @@ -193,9 +192,25 @@ class TeleVision: async def main_image_binocular(self, session, fps=60): if self.use_hand_tracking: - session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False) + session.upsert( + Hands( + stream=True, + key="hands", + showLeft=False, + showRight=False + ), + to="bgChildren", + ) else: - session.upsert @ MotionControllers(fps=fps, stream=True, key="motion-controller", showLeft=False, showRight=False) + session.upsert( + MotionControllers( + stream=True, + key="motionControllers", + showLeft=False, + showRight=False, + ), + to="bgChildren", + ) while True: display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB) @@ -212,7 +227,7 @@ class TeleVision: # Note that these two masks are associated with left eye’s camera and the right eye’s camera. layers=1, format="jpeg", - quality=90, + quality=100, key="background-left", interpolate=True, ), @@ -223,7 +238,7 @@ class TeleVision: distanceToCamera=1, layers=2, format="jpeg", - quality=90, + quality=100, key="background-right", interpolate=True, ), @@ -235,9 +250,25 @@ class TeleVision: async def main_image_monocular(self, session, fps=60): if self.use_hand_tracking: - session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False) + session.upsert( + Hands( + stream=True, + key="hands", + showLeft=False, + showRight=False + ), + to="bgChildren", + ) else: - session.upsert @ MotionControllers(fps=fps, stream=True, key="motion-controller", showLeft=False, showRight=False) + session.upsert( + MotionControllers( + stream=True, + key="motionControllers", + showLeft=False, + showRight=False, + ), + to="bgChildren", + ) while True: display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB) @@ -274,7 +305,7 @@ class TeleVision: session.upsert( MotionControllers( stream=True, - key="motion-controller", + key="motionControllers", showLeft=False, showRight=False, ) diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py index e399a84..cc2e1f4 100644 --- a/teleop/open_television/tv_wrapper.py +++ b/teleop/open_television/tv_wrapper.py @@ -182,7 +182,7 @@ class TeleData: class TeleVisionWrapper: def __init__(self, binocular: bool, use_hand_tracking: bool, img_shape, img_shm_name, return_state_data: bool = True, return_hand_rot_data: bool = False, - cert_file = None, key_file = None): + cert_file = None, key_file = None, ngrok = False, webrtc = False): """ TeleVisionWrapper is a wrapper for the TeleVision class, which handles XR device's data suit for robot control. It initializes the TeleVision instance with the specified parameters and provides a method to get motion state data. @@ -199,7 +199,8 @@ class TeleVisionWrapper: self.use_hand_tracking = use_hand_tracking self.return_state_data = return_state_data self.return_hand_rot_data = return_hand_rot_data - self.tvuer = TeleVision(binocular, use_hand_tracking, img_shape, img_shm_name, cert_file=cert_file, key_file=key_file) + self.tvuer = TeleVision(binocular, use_hand_tracking, img_shape, img_shm_name, cert_file=cert_file, key_file=key_file, + ngrok=ngrok, webrtc=webrtc) def get_motion_state_data(self): """ diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index 1e64c7b..6194cd4 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -11,9 +11,9 @@ from unitree_sdk2py.utils.crc import CRC from unitree_sdk2py.idl.unitree_go.msg.dds_ import ( LowCmd_ as go_LowCmd, LowState_ as go_LowState) # idl for h1 from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ -# kTopicLowCommand = "rt/lowcmd" +kTopicLowCommand_Debug = "rt/lowcmd" +kTopicLowCommand_Motion = "rt/arm_sdk" kTopicLowState = "rt/lowstate" -kTopicLowCommand = "rt/arm_sdk" G1_29_Num_Motors = 35 G1_23_Num_Motors = 35 @@ -56,10 +56,11 @@ class DataBuffer: self.data = data class G1_29_ArmController: - def __init__(self): + def __init__(self, debug_mode = True): print("Initialize G1_29_ArmController...") self.q_target = np.zeros(14) self.tauff_target = np.zeros(14) + self.debug_mode = debug_mode self.kp_high = 300.0 self.kd_high = 3.0 @@ -78,7 +79,10 @@ class G1_29_ArmController: # initialize lowcmd publisher and lowstate subscriber ChannelFactoryInitialize(0) - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand, hg_LowCmd) + if self.debug_mode: + self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) + else: + self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Motion, hg_LowCmd) self.lowcmd_publisher.Init() self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_LowState) self.lowstate_subscriber.Init() @@ -151,7 +155,8 @@ class G1_29_ArmController: return cliped_arm_q_target def _ctrl_motor_state(self): - self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0; + if self.debug_mode: + self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0; while True: start_time = time.time() @@ -165,7 +170,7 @@ class G1_29_ArmController: for idx, id in enumerate(G1_29_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] self.msg.motor_cmd[id].dq = 0 - self.msg.motor_cmd[id].tau = arm_tauff_target[idx] + self.msg.motor_cmd[id].tau = arm_tauff_target[idx] self.msg.crc = self.crc.Crc(self.msg) self.lowcmd_publisher.Write(self.msg) @@ -213,9 +218,10 @@ class G1_29_ArmController: while True: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): - for weight in np.arange(1, 0, -0.01): - self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; - time.sleep(0.02) + if self.debug_mode: + for weight in np.arange(1, 0, -0.01): + self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; + time.sleep(0.02) print("[G1_29_ArmController] both arms have reached the home position.") break time.sleep(0.05) @@ -347,7 +353,7 @@ class G1_23_ArmController: # initialize lowcmd publisher and lowstate subscriber ChannelFactoryInitialize(0) - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand, hg_LowCmd) + self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) self.lowcmd_publisher.Init() self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_LowState) self.lowstate_subscriber.Init() @@ -603,7 +609,7 @@ class H1_2_ArmController: # initialize lowcmd publisher and lowstate subscriber ChannelFactoryInitialize(0) - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand, hg_LowCmd) + self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) self.lowcmd_publisher.Init() self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_LowState) self.lowstate_subscriber.Init() @@ -864,7 +870,7 @@ class H1_ArmController: # initialize lowcmd publisher and lowstate subscriber ChannelFactoryInitialize(0) - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand, go_LowCmd) + self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, go_LowCmd) self.lowcmd_publisher.Init() self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, go_LowState) self.lowstate_subscriber.Init() diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index dc8bc8c..be3f67c 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -78,7 +78,8 @@ if __name__ == '__main__': image_receive_thread.start() # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. - tv_wrapper = TeleVisionWrapper(BINOCULAR, args.xr_mode == 'hand', tv_img_shape, tv_img_shm.name) + tv_wrapper = TeleVisionWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == 'hand', img_shape=tv_img_shape, img_shm_name=tv_img_shm.name, + return_state_data=True, return_hand_rot_data = False) # arm if args.arm == 'G1_29': @@ -96,8 +97,8 @@ if __name__ == '__main__': # end-effector if args.ee == "dex3": - left_hand_pos_array = Array('d', 75, lock = True) # [input] - right_hand_pos_array = Array('d', 75, lock = True) # [input] + left_hand_pos_array = Array('d', 75, lock = True) # [input] + right_hand_pos_array = Array('d', 75, lock = True) # [input] dual_hand_data_lock = Lock() dual_hand_state_array = Array('d', 14, lock = False) # [output] current left, right hand state(14) data. dual_hand_action_array = Array('d', 14, lock = False) # [output] current left, right hand action(14) data. @@ -110,8 +111,8 @@ if __name__ == '__main__': dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array) elif args.ee == "inspire1": - left_hand_pos_array = Array('d', 75, lock = True) # [input] - right_hand_pos_array = Array('d', 75, lock = True) # [input] + left_hand_pos_array = Array('d', 75, lock = True) # [input] + right_hand_pos_array = Array('d', 75, lock = True) # [input] dual_hand_data_lock = Lock() dual_hand_state_array = Array('d', 12, lock = False) # [output] current left, right hand state(12) data. dual_hand_action_array = Array('d', 12, lock = False) # [output] current left, right hand action(12) data. @@ -122,7 +123,7 @@ if __name__ == '__main__': # xr mode if args.xr_mode == 'controller': from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient - sport_client = LocoClient() + sport_client = LocoClient() sport_client.SetTimeout(0.0001) sport_client.Init() @@ -137,8 +138,23 @@ if __name__ == '__main__': running = True while running: start_time = time.time() - tele_data = tv_wrapper.get_motion_state_data() + # opencv image + tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) + cv2.imshow("record image", tv_resized_image) + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + running = False + elif key == ord('s') and args.record: + recording = not recording # state flipping + if recording: + if not recorder.create_episode(): + recording = False + else: + recorder.save_episode() + + # get input data + tele_data = tv_wrapper.get_motion_state_data() if (args.ee == 'dex3' or args.ee == 'inspire1') and args.xr_mode == 'hand': with left_hand_pos_array.get_lock(): left_hand_pos_array[:] = tele_data.left_hand_pos.flatten() @@ -149,12 +165,13 @@ if __name__ == '__main__': left_gripper_value.value = tele_data.left_trigger_value with right_gripper_value.get_lock(): right_gripper_value.value = tele_data.right_trigger_value - # quit or damp + # quit teleoperate if tele_data.tele_state.right_aButton: running = False - if tele_data.tele_state.right_thumbstick_state: + # command robot to enter damping mode. soft emergency stop function + if tele_data.tele_state.left_thumbstick_state and tele_data.tele_state.right_thumbstick_state: sport_client.Damp() - # control + # high level control, limit velocity to within 0.3 sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, -tele_data.tele_state.left_thumbstick_value[0] * 0.3, -tele_data.tele_state.right_thumbstick_value[0] * 0.3) @@ -166,7 +183,7 @@ if __name__ == '__main__': else: pass - # get current state data. + # get current robot state data. current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() @@ -177,19 +194,6 @@ if __name__ == '__main__': # print(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) - tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) - cv2.imshow("record image", tv_resized_image) - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - running = False - elif key == ord('s') and args.record: - recording = not recording # state flipping - if recording: - if not recorder.create_episode(): - recording = False - else: - recorder.save_episode() - # record data if args.record: # dex hand or gripper From e62af5a152ff7a233175e4d1ed96fff83b41445b Mon Sep 17 00:00:00 2001 From: silencht Date: Wed, 21 May 2025 17:16:52 +0800 Subject: [PATCH 05/32] [fix] hide hand marker visualization --- teleop/open_television/television.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index 3f6787a..d891fb9 100644 --- a/teleop/open_television/television.py +++ b/teleop/open_television/television.py @@ -196,8 +196,8 @@ class TeleVision: Hands( stream=True, key="hands", - showLeft=False, - showRight=False + hideLeft=True, + hideRight=True ), to="bgChildren", ) @@ -254,8 +254,8 @@ class TeleVision: Hands( stream=True, key="hands", - showLeft=False, - showRight=False + hideLeft=True, + hideRight=True ), to="bgChildren", ) From 7d65da46345ddb90f1f9c1fad0ec22da7bf215ce Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 22 May 2025 18:05:39 +0800 Subject: [PATCH 06/32] [fix] bug. --- teleop/robot_control/robot_hand_inspire.py | 2 +- teleop/teleop_hand_and_arm.py | 18 ++++++++++++------ teleop/utils/rerun_visualizer.py | 4 ++-- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/teleop/robot_control/robot_hand_inspire.py b/teleop/robot_control/robot_hand_inspire.py index 3c907b9..5c69477 100644 --- a/teleop/robot_control/robot_hand_inspire.py +++ b/teleop/robot_control/robot_hand_inspire.py @@ -147,7 +147,7 @@ class Inspire_Controller: sleep_time = max(0, (1 / self.fps) - time_elapsed) time.sleep(sleep_time) finally: - print("Dex3_1_Controller has been closed.") + print("Inspire_Controller has been closed.") # Update hand state, according to the official documentation, https://support.unitree.com/home/en/G1_developer/inspire_dfx_dexterous_hand # the state sequence is as shown in the table below diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index be3f67c..1051fc4 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -23,7 +23,7 @@ from teleop.utils.episode_writer import EpisodeWriter if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--task_dir', type = str, default = './utils/data', help = 'path to save data') - parser.add_argument('--frequency', type = float, default = 30.0, help = 'save data\'s frequency') + parser.add_argument('--frequency', type = float, default = 90.0, help = 'save data\'s frequency') parser.add_argument('--record', action = 'store_true', help = 'Save data or not') parser.add_argument('--no-record', dest = 'record', action = 'store_false', help = 'Do not save data') @@ -83,7 +83,7 @@ if __name__ == '__main__': # arm if args.arm == 'G1_29': - arm_ctrl = G1_29_ArmController() + arm_ctrl = G1_29_ArmController(debug_mode=True) arm_ik = G1_29_ArmIK() elif args.arm == 'G1_23': arm_ctrl = G1_23_ArmController() @@ -203,12 +203,16 @@ if __name__ == '__main__': right_hand_state = dual_hand_state_array[-7:] left_hand_action = dual_hand_action_array[:7] right_hand_action = dual_hand_action_array[-7:] + current_body_state = [] + current_body_action = [] elif args.ee == "gripper" and args.xr_mode == 'hand': with dual_gripper_data_lock: left_hand_state = [dual_gripper_state_array[1]] right_hand_state = [dual_gripper_state_array[0]] left_hand_action = [dual_gripper_action_array[1]] right_hand_action = [dual_gripper_action_array[0]] + current_body_state = [] + current_body_action = [] elif args.ee == "gripper" and args.xr_mode == 'controller': with dual_gripper_data_lock: left_hand_state = [dual_gripper_state_array[1]] @@ -225,6 +229,8 @@ if __name__ == '__main__': right_hand_state = dual_hand_state_array[-6:] left_hand_action = dual_hand_action_array[:6] right_hand_action = dual_hand_action_array[-6:] + current_body_state = [] + current_body_action = [] else: left_hand_state = [] right_hand_state = [] @@ -267,12 +273,12 @@ if __name__ == '__main__': "qvel": [], "torque": [], }, - "left_hand_pos": { + "left_hand": { "qpos": left_hand_state, "qvel": [], "torque": [], }, - "right_hand_pos": { + "right_hand": { "qpos": right_hand_state, "qvel": [], "torque": [], @@ -292,12 +298,12 @@ if __name__ == '__main__': "qvel": [], "torque": [], }, - "left_hand_pos": { + "left_hand": { "qpos": left_hand_action, "qvel": [], "torque": [], }, - "right_hand_pos": { + "right_hand": { "qpos": right_hand_action, "qvel": [], "torque": [], diff --git a/teleop/utils/rerun_visualizer.py b/teleop/utils/rerun_visualizer.py index c11a10e..9b2f057 100644 --- a/teleop/utils/rerun_visualizer.py +++ b/teleop/utils/rerun_visualizer.py @@ -141,7 +141,7 @@ class RerunLogger: # Log states states = item_data.get('states', {}) or {} for part, state_info in states.items(): - if state_info: + if part != "body" and state_info: values = state_info.get('qpos', []) for idx, val in enumerate(values): rr.log(f"{self.prefix}{part}/states/qpos/{idx}", rr.Scalar(val)) @@ -149,7 +149,7 @@ class RerunLogger: # Log actions actions = item_data.get('actions', {}) or {} for part, action_info in actions.items(): - if action_info: + if part != "body" and action_info: values = action_info.get('qpos', []) for idx, val in enumerate(values): rr.log(f"{self.prefix}{part}/actions/qpos/{idx}", rr.Scalar(val)) From c0cc780ba9dc870d8ced39bb009469726a07ce91 Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 22 May 2025 20:55:25 +0800 Subject: [PATCH 07/32] [fix] bug --- teleop/robot_control/robot_arm.py | 4 ++-- teleop/teleop_hand_and_arm.py | 27 +++++++++++++++++---------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index 6194cd4..d728024 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -155,7 +155,7 @@ class G1_29_ArmController: return cliped_arm_q_target def _ctrl_motor_state(self): - if self.debug_mode: + if not self.debug_mode: self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0; while True: @@ -218,7 +218,7 @@ class G1_29_ArmController: while True: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): - if self.debug_mode: + if not self.debug_mode: for weight in np.arange(1, 0, -0.01): self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; time.sleep(0.02) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 1051fc4..9b0d216 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -33,6 +33,10 @@ if __name__ == '__main__': parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') + parser.add_argument('--debug', action = 'store_true', help = 'debug mode') + parser.add_argument('--no-debug', dest = 'debug', action = 'store_false', help = 'motion mode') + parser.set_defaults(debug = True) + args = parser.parse_args() print(f"args:{args}\n") @@ -83,7 +87,7 @@ if __name__ == '__main__': # arm if args.arm == 'G1_29': - arm_ctrl = G1_29_ArmController(debug_mode=True) + arm_ctrl = G1_29_ArmController(debug_mode=args.debug) arm_ik = G1_29_ArmIK() elif args.arm == 'G1_23': arm_ctrl = G1_23_ArmController() @@ -121,7 +125,7 @@ if __name__ == '__main__': pass # xr mode - if args.xr_mode == 'controller': + if args.xr_mode == 'controller' and not args.debug: from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient sport_client = LocoClient() sport_client.SetTimeout(0.0001) @@ -165,23 +169,26 @@ if __name__ == '__main__': left_gripper_value.value = tele_data.left_trigger_value with right_gripper_value.get_lock(): right_gripper_value.value = tele_data.right_trigger_value + elif args.ee == 'gripper' and args.xr_mode == 'hand': + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_pinch_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_pinch_value + else: + pass + + # high level control + if args.xr_mode == 'controller' and not args.debug: # quit teleoperate if tele_data.tele_state.right_aButton: running = False # command robot to enter damping mode. soft emergency stop function if tele_data.tele_state.left_thumbstick_state and tele_data.tele_state.right_thumbstick_state: sport_client.Damp() - # high level control, limit velocity to within 0.3 + # control, limit velocity to within 0.3 sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, -tele_data.tele_state.left_thumbstick_value[0] * 0.3, -tele_data.tele_state.right_thumbstick_value[0] * 0.3) - elif args.ee == 'gripper' and args.xr_mode == 'hand': - with left_gripper_value.get_lock(): - left_gripper_value.value = tele_data.left_pinch_value - with right_gripper_value.get_lock(): - right_gripper_value.value = tele_data.right_pinch_value - else: - pass # get current robot state data. current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() From 13b384f86197c0d49d5ced376bb02286cc7882ec Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 27 May 2025 10:09:50 +0800 Subject: [PATCH 08/32] [fix] make var names consistent --- teleop/teleop_hand_and_arm.py | 32 ++++++++++++++++---------------- teleop/utils/episode_writer.py | 8 ++++---- teleop/utils/rerun_visualizer.py | 4 ++-- 3 files changed, 22 insertions(+), 22 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 9b0d216..7a1b353 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -206,24 +206,24 @@ if __name__ == '__main__': # dex hand or gripper if args.ee == "dex3" and args.xr_mode == 'hand': with dual_hand_data_lock: - left_hand_state = dual_hand_state_array[:7] - right_hand_state = dual_hand_state_array[-7:] + left_ee_state = dual_hand_state_array[:7] + right_ee_state = dual_hand_state_array[-7:] left_hand_action = dual_hand_action_array[:7] right_hand_action = dual_hand_action_array[-7:] current_body_state = [] current_body_action = [] elif args.ee == "gripper" and args.xr_mode == 'hand': with dual_gripper_data_lock: - left_hand_state = [dual_gripper_state_array[1]] - right_hand_state = [dual_gripper_state_array[0]] + left_ee_state = [dual_gripper_state_array[1]] + right_ee_state = [dual_gripper_state_array[0]] left_hand_action = [dual_gripper_action_array[1]] right_hand_action = [dual_gripper_action_array[0]] current_body_state = [] current_body_action = [] elif args.ee == "gripper" and args.xr_mode == 'controller': with dual_gripper_data_lock: - left_hand_state = [dual_gripper_state_array[1]] - right_hand_state = [dual_gripper_state_array[0]] + left_ee_state = [dual_gripper_state_array[1]] + right_ee_state = [dual_gripper_state_array[0]] left_hand_action = [dual_gripper_action_array[1]] right_hand_action = [dual_gripper_action_array[0]] current_body_state = arm_ctrl.get_current_motor_q().tolist() @@ -232,15 +232,15 @@ if __name__ == '__main__': -tele_data.tele_state.right_thumbstick_value[0] * 0.3] elif args.ee == "inspire1" and args.xr_mode == 'hand': with dual_hand_data_lock: - left_hand_state = dual_hand_state_array[:6] - right_hand_state = dual_hand_state_array[-6:] + left_ee_state = dual_hand_state_array[:6] + right_ee_state = dual_hand_state_array[-6:] left_hand_action = dual_hand_action_array[:6] right_hand_action = dual_hand_action_array[-6:] current_body_state = [] current_body_action = [] else: - left_hand_state = [] - right_hand_state = [] + left_ee_state = [] + right_ee_state = [] left_hand_action = [] right_hand_action = [] current_body_state = [] @@ -280,13 +280,13 @@ if __name__ == '__main__': "qvel": [], "torque": [], }, - "left_hand": { - "qpos": left_hand_state, + "left_ee": { + "qpos": left_ee_state, "qvel": [], "torque": [], }, - "right_hand": { - "qpos": right_hand_state, + "right_ee": { + "qpos": right_ee_state, "qvel": [], "torque": [], }, @@ -305,12 +305,12 @@ if __name__ == '__main__': "qvel": [], "torque": [], }, - "left_hand": { + "left_ee": { "qpos": left_hand_action, "qvel": [], "torque": [], }, - "right_hand": { + "right_ee": { "qpos": right_hand_action, "qvel": [], "torque": [], diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index 019e576..ec05ba1 100644 --- a/teleop/utils/episode_writer.py +++ b/teleop/utils/episode_writer.py @@ -59,15 +59,15 @@ class EpisodeWriter(): "audio": {"sample_rate": 16000, "channels": 1, "format":"PCM", "bits":16}, # PCM_S16 "joint_names":{ "left_arm": ['kLeftShoulderPitch' ,'kLeftShoulderRoll', 'kLeftShoulderYaw', 'kLeftElbow', 'kLeftWristRoll', 'kLeftWristPitch', 'kLeftWristyaw'], - "left_hand": [], + "left_ee": [], "right_arm": [], - "right_hand": [], + "right_ee": [], "body": [], }, "tactile_names": { - "left_hand": [], - "right_hand": [], + "left_ee": [], + "right_ee": [], }, } def text_desc(self): diff --git a/teleop/utils/rerun_visualizer.py b/teleop/utils/rerun_visualizer.py index 9b2f057..f5c8a87 100644 --- a/teleop/utils/rerun_visualizer.py +++ b/teleop/utils/rerun_visualizer.py @@ -89,8 +89,8 @@ class RerunLogger: data_plot_paths = [ f"{self.prefix}left_arm", f"{self.prefix}right_arm", - f"{self.prefix}left_hand", - f"{self.prefix}right_hand" + f"{self.prefix}left_ee", + f"{self.prefix}right_ee" ] for plot_path in data_plot_paths: view = rrb.TimeSeriesView( From 4c554b0cd3605c2e69afcacbdb0741ca1aa54f66 Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 10 Jun 2025 17:46:20 +0800 Subject: [PATCH 09/32] [update] print to logging_mp. --- teleop/image_server/image_client.py | 20 ++--- teleop/image_server/image_server.py | 42 +++++----- teleop/open_television/_test_television.py | 80 +++++++++---------- teleop/open_television/_test_tv_wrapper.py | 61 ++++++++------- teleop/robot_control/hand_retargeting.py | 12 +-- teleop/robot_control/robot_arm.py | 91 +++++++++++----------- teleop/robot_control/robot_arm_ik.py | 29 +++---- teleop/robot_control/robot_hand_inspire.py | 15 ++-- teleop/robot_control/robot_hand_unitree.py | 31 ++++---- teleop/teleop_hand_and_arm.py | 38 +++++---- teleop/utils/episode_writer.py | 30 +++---- teleop/utils/rerun_visualizer.py | 23 +++--- 12 files changed, 251 insertions(+), 221 deletions(-) diff --git a/teleop/image_server/image_client.py b/teleop/image_server/image_client.py index 3806f4b..1ff75dd 100644 --- a/teleop/image_server/image_client.py +++ b/teleop/image_server/image_client.py @@ -5,6 +5,8 @@ import time import struct from collections import deque from multiprocessing import shared_memory +import logging_mp +logger_mp = logging_mp.get_logger(__name__) class ImageClient: def __init__(self, tv_img_shape = None, tv_img_shm_name = None, wrist_img_shape = None, wrist_img_shm_name = None, @@ -85,10 +87,10 @@ class ImageClient: if frame_id != expected_frame_id: lost = frame_id - expected_frame_id if lost < 0: - print(f"[Image Client] Received out-of-order frame ID: {frame_id}") + logger_mp.info(f"[Image Client] Received out-of-order frame ID: {frame_id}") else: self._lost_frames += lost - print(f"[Image Client] Detected lost frames: {lost}, Expected frame ID: {expected_frame_id}, Received frame ID: {frame_id}") + logger_mp.warning(f"[Image Client] Detected lost frames: {lost}, Expected frame ID: {expected_frame_id}, Received frame ID: {frame_id}") self._last_frame_id = frame_id self._total_frames = frame_id + 1 @@ -111,7 +113,7 @@ class ImageClient: # Calculate lost frame rate lost_frame_rate = (self._lost_frames / self._total_frames) * 100 if self._total_frames > 0 else 0 - print(f"[Image Client] Real-time FPS: {real_time_fps:.2f}, Avg Latency: {avg_latency*1000:.2f} ms, Max Latency: {max_latency*1000:.2f} ms, \ + logger_mp.info(f"[Image Client] Real-time FPS: {real_time_fps:.2f}, Avg Latency: {avg_latency*1000:.2f} ms, Max Latency: {max_latency*1000:.2f} ms, \ Min Latency: {min_latency*1000:.2f} ms, Jitter: {jitter*1000:.2f} ms, Lost Frame Rate: {lost_frame_rate:.2f}%") def _close(self): @@ -119,7 +121,7 @@ class ImageClient: self._context.term() if self._image_show: cv2.destroyAllWindows() - print("Image client has been closed.") + logger_mp.info("Image client has been closed.") def receive_process(self): @@ -129,7 +131,7 @@ class ImageClient: self._socket.connect(f"tcp://{self._server_address}:{self._port}") self._socket.setsockopt_string(zmq.SUBSCRIBE, "") - print("\nImage client has started, waiting to receive data...") + logger_mp.info("\nImage client has started, waiting to receive data...") try: while self.running: # Receive message @@ -144,7 +146,7 @@ class ImageClient: jpg_bytes = message[header_size:] timestamp, frame_id = struct.unpack('dI', header) except struct.error as e: - print(f"[Image Client] Error unpacking header: {e}, discarding message.") + logger_mp.warning(f"[Image Client] Error unpacking header: {e}, discarding message.") continue else: # No header, entire message is image data @@ -153,7 +155,7 @@ class ImageClient: np_img = np.frombuffer(jpg_bytes, dtype=np.uint8) current_image = cv2.imdecode(np_img, cv2.IMREAD_COLOR) if current_image is None: - print("[Image Client] Failed to decode image.") + logger_mp.warning("[Image Client] Failed to decode image.") continue if self.tv_enable_shm: @@ -174,9 +176,9 @@ class ImageClient: self._print_performance_metrics(receive_time) except KeyboardInterrupt: - print("Image client interrupted by user.") + logger_mp.info("Image client interrupted by user.") except Exception as e: - print(f"[Image Client] An error occurred while receiving data: {e}") + logger_mp.warning(f"[Image Client] An error occurred while receiving data: {e}") finally: self._close() diff --git a/teleop/image_server/image_server.py b/teleop/image_server/image_server.py index fa542ff..19799fb 100644 --- a/teleop/image_server/image_server.py +++ b/teleop/image_server/image_server.py @@ -5,6 +5,8 @@ import struct from collections import deque import numpy as np import pyrealsense2 as rs +import logging_mp +logger_mp = logging_mp.get_logger(__name__, level=logging_mp.DEBUG) class RealSenseCamera(object): @@ -37,7 +39,7 @@ class RealSenseCamera(object): profile = self.pipeline.start(config) self._device = profile.get_device() if self._device is None: - print('[Image Server] pipe_profile.get_device() is None .') + logger_mp.error('[Image Server] pipe_profile.get_device() is None .') if self.enable_depth: assert self._device is not None depth_sensor = self._device.first_depth_sensor() @@ -82,7 +84,7 @@ class OpenCVCamera(): # Test if the camera can read frames if not self._can_read_frame(): - print(f"[Image Server] Camera {self.id} Error: Failed to initialize the camera or read frames. Exiting...") + logger_mp.error(f"[Image Server] Camera {self.id} Error: Failed to initialize the camera or read frames. Exiting...") self.release() def _can_read_frame(self): @@ -136,7 +138,7 @@ class ImageServer: #'wrist_camera_id_numbers': ["218622271789", "241222076627"], # serial number (realsense) } """ - print(config) + logger_mp.info(config) self.fps = config.get('fps', 30) self.head_camera_type = config.get('head_camera_type', 'opencv') self.head_image_shape = config.get('head_camera_image_shape', [480, 640]) # (height, width) @@ -161,7 +163,7 @@ class ImageServer: camera = RealSenseCamera(img_shape=self.head_image_shape, fps=self.fps, serial_number=serial_number) self.head_cameras.append(camera) else: - print(f"[Image Server] Unsupported head_camera_type: {self.head_camera_type}") + logger_mp.warning(f"[Image Server] Unsupported head_camera_type: {self.head_camera_type}") # Initialize wrist cameras if provided self.wrist_cameras = [] @@ -175,7 +177,7 @@ class ImageServer: camera = RealSenseCamera(img_shape=self.wrist_image_shape, fps=self.fps, serial_number=serial_number) self.wrist_cameras.append(camera) else: - print(f"[Image Server] Unsupported wrist_camera_type: {self.wrist_camera_type}") + logger_mp.warning(f"[Image Server] Unsupported wrist_camera_type: {self.wrist_camera_type}") # Set ZeroMQ context and socket self.context = zmq.Context() @@ -187,21 +189,21 @@ class ImageServer: for cam in self.head_cameras: if isinstance(cam, OpenCVCamera): - print(f"[Image Server] Head camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") + logger_mp.info(f"[Image Server] Head camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") elif isinstance(cam, RealSenseCamera): - print(f"[Image Server] Head camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") + logger_mp.info(f"[Image Server] Head camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") else: - print("[Image Server] Unknown camera type in head_cameras.") + logger_mp.warning("[Image Server] Unknown camera type in head_cameras.") for cam in self.wrist_cameras: if isinstance(cam, OpenCVCamera): - print(f"[Image Server] Wrist camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") + logger_mp.info(f"[Image Server] Wrist camera {cam.id} resolution: {cam.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)} x {cam.cap.get(cv2.CAP_PROP_FRAME_WIDTH)}") elif isinstance(cam, RealSenseCamera): - print(f"[Image Server] Wrist camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") + logger_mp.info(f"[Image Server] Wrist camera {cam.serial_number} resolution: {cam.img_shape[0]} x {cam.img_shape[1]}") else: - print("[Image Server] Unknown camera type in wrist_cameras.") + logger_mp.warning("[Image Server] Unknown camera type in wrist_cameras.") - print("[Image Server] Image server has started, waiting for client connections...") + logger_mp.info("[Image Server] Image server has started, waiting for client connections...") @@ -224,7 +226,7 @@ class ImageServer: if self.frame_count % 30 == 0: elapsed_time = current_time - self.start_time real_time_fps = len(self.frame_times) / self.time_window - print(f"[Image Server] Real-time FPS: {real_time_fps:.2f}, Total frames sent: {self.frame_count}, Elapsed time: {elapsed_time:.2f} sec") + logger_mp.info(f"[Image Server] Real-time FPS: {real_time_fps:.2f}, Total frames sent: {self.frame_count}, Elapsed time: {elapsed_time:.2f} sec") def _close(self): for cam in self.head_cameras: @@ -233,7 +235,7 @@ class ImageServer: cam.release() self.socket.close() self.context.term() - print("[Image Server] The server has been closed.") + logger_mp.info("[Image Server] The server has been closed.") def send_process(self): try: @@ -243,12 +245,12 @@ class ImageServer: if self.head_camera_type == 'opencv': color_image = cam.get_frame() if color_image is None: - print("[Image Server] Head camera frame read is error.") + logger_mp.error("[Image Server] Head camera frame read is error.") break elif self.head_camera_type == 'realsense': color_image, depth_iamge = cam.get_frame() if color_image is None: - print("[Image Server] Head camera frame read is error.") + logger_mp.error("[Image Server] Head camera frame read is error.") break head_frames.append(color_image) if len(head_frames) != len(self.head_cameras): @@ -261,12 +263,12 @@ class ImageServer: if self.wrist_camera_type == 'opencv': color_image = cam.get_frame() if color_image is None: - print("[Image Server] Wrist camera frame read is error.") + logger_mp.error("[Image Server] Wrist camera frame read is error.") break elif self.wrist_camera_type == 'realsense': color_image, depth_iamge = cam.get_frame() if color_image is None: - print("[Image Server] Wrist camera frame read is error.") + logger_mp.error("[Image Server] Wrist camera frame read is error.") break wrist_frames.append(color_image) wrist_color = cv2.hconcat(wrist_frames) @@ -278,7 +280,7 @@ class ImageServer: ret, buffer = cv2.imencode('.jpg', full_color) if not ret: - print("[Image Server] Frame imencode is failed.") + logger_mp.error("[Image Server] Frame imencode is failed.") continue jpg_bytes = buffer.tobytes() @@ -299,7 +301,7 @@ class ImageServer: self._print_performance_metrics(current_time) except KeyboardInterrupt: - print("[Image Server] Interrupted by user.") + logger_mp.warning("[Image Server] Interrupted by user.") finally: self._close() diff --git a/teleop/open_television/_test_television.py b/teleop/open_television/_test_television.py index 79e1902..ccb2005 100644 --- a/teleop/open_television/_test_television.py +++ b/teleop/open_television/_test_television.py @@ -8,6 +8,8 @@ import time import numpy as np from multiprocessing import shared_memory from open_television import TeleVision +import logging_mp +logger_mp = logging_mp.get_logger(__name__, level=logging_mp.INFO) def run_test_television(): # image @@ -30,54 +32,54 @@ def run_test_television(): input("Press Enter to start television test...") running = True while running: - print("=" * 80) - print("Common Data (always available):") - print(f"head_pose shape: {tv.head_pose.shape}\n{tv.head_pose}\n") - print(f"left_arm_pose shape: {tv.left_arm_pose.shape}\n{tv.left_arm_pose}\n") - print(f"right_arm_pose shape: {tv.right_arm_pose.shape}\n{tv.right_arm_pose}\n") - print("=" * 80) + logger_mp.info("=" * 80) + logger_mp.info("Common Data (always available):") + logger_mp.info(f"head_pose shape: {tv.head_pose.shape}\n{tv.head_pose}\n") + logger_mp.info(f"left_arm_pose shape: {tv.left_arm_pose.shape}\n{tv.left_arm_pose}\n") + logger_mp.info(f"right_arm_pose shape: {tv.right_arm_pose.shape}\n{tv.right_arm_pose}\n") + logger_mp.info("=" * 80) if use_hand_track: - print("Hand Tracking Data:") - print(f"left_hand_positions shape: {tv.left_hand_positions.shape}\n{tv.left_hand_positions}\n") - print(f"right_hand_positions shape: {tv.right_hand_positions.shape}\n{tv.right_hand_positions}\n") - print(f"left_hand_orientations shape: {tv.left_hand_orientations.shape}\n{tv.left_hand_orientations}\n") - print(f"right_hand_orientations shape: {tv.right_hand_orientations.shape}\n{tv.right_hand_orientations}\n") - print(f"left_hand_pinch_state: {tv.left_hand_pinch_state}") - print(f"left_hand_pinch_value: {tv.left_hand_pinch_value}") - print(f"left_hand_squeeze_state: {tv.left_hand_squeeze_state}") - print(f"left_hand_squeeze_value: {tv.left_hand_squeeze_value}") - print(f"right_hand_pinch_state: {tv.right_hand_pinch_state}") - print(f"right_hand_pinch_value: {tv.right_hand_pinch_value}") - print(f"right_hand_squeeze_state: {tv.right_hand_squeeze_state}") - print(f"right_hand_squeeze_value: {tv.right_hand_squeeze_value}") + logger_mp.info("Hand Tracking Data:") + logger_mp.info(f"left_hand_positions shape: {tv.left_hand_positions.shape}\n{tv.left_hand_positions}\n") + logger_mp.info(f"right_hand_positions shape: {tv.right_hand_positions.shape}\n{tv.right_hand_positions}\n") + logger_mp.info(f"left_hand_orientations shape: {tv.left_hand_orientations.shape}\n{tv.left_hand_orientations}\n") + logger_mp.info(f"right_hand_orientations shape: {tv.right_hand_orientations.shape}\n{tv.right_hand_orientations}\n") + logger_mp.info(f"left_hand_pinch_state: {tv.left_hand_pinch_state}") + logger_mp.info(f"left_hand_pinch_value: {tv.left_hand_pinch_value}") + logger_mp.info(f"left_hand_squeeze_state: {tv.left_hand_squeeze_state}") + logger_mp.info(f"left_hand_squeeze_value: {tv.left_hand_squeeze_value}") + logger_mp.info(f"right_hand_pinch_state: {tv.right_hand_pinch_state}") + logger_mp.info(f"right_hand_pinch_value: {tv.right_hand_pinch_value}") + logger_mp.info(f"right_hand_squeeze_state: {tv.right_hand_squeeze_state}") + logger_mp.info(f"right_hand_squeeze_value: {tv.right_hand_squeeze_value}") else: - print("Controller Data:") - print(f"left_controller_trigger_state: {tv.left_controller_trigger_state}") - print(f"left_controller_trigger_value: {tv.left_controller_trigger_value}") - print(f"left_controller_squeeze_state: {tv.left_controller_squeeze_state}") - print(f"left_controller_squeeze_value: {tv.left_controller_squeeze_value}") - print(f"left_controller_thumbstick_state: {tv.left_controller_thumbstick_state}") - print(f"left_controller_thumbstick_value: {tv.left_controller_thumbstick_value}") - print(f"left_controller_aButton: {tv.left_controller_aButton}") - print(f"left_controller_bButton: {tv.left_controller_bButton}") - print(f"right_controller_trigger_state: {tv.right_controller_trigger_state}") - print(f"right_controller_trigger_value: {tv.right_controller_trigger_value}") - print(f"right_controller_squeeze_state: {tv.right_controller_squeeze_state}") - print(f"right_controller_squeeze_value: {tv.right_controller_squeeze_value}") - print(f"right_controller_thumbstick_state: {tv.right_controller_thumbstick_state}") - print(f"right_controller_thumbstick_value: {tv.right_controller_thumbstick_value}") - print(f"right_controller_aButton: {tv.right_controller_aButton}") - print(f"right_controller_bButton: {tv.right_controller_bButton}") - print("=" * 80) + logger_mp.info("Controller Data:") + logger_mp.info(f"left_controller_trigger_state: {tv.left_controller_trigger_state}") + logger_mp.info(f"left_controller_trigger_value: {tv.left_controller_trigger_value}") + logger_mp.info(f"left_controller_squeeze_state: {tv.left_controller_squeeze_state}") + logger_mp.info(f"left_controller_squeeze_value: {tv.left_controller_squeeze_value}") + logger_mp.info(f"left_controller_thumbstick_state: {tv.left_controller_thumbstick_state}") + logger_mp.info(f"left_controller_thumbstick_value: {tv.left_controller_thumbstick_value}") + logger_mp.info(f"left_controller_aButton: {tv.left_controller_aButton}") + logger_mp.info(f"left_controller_bButton: {tv.left_controller_bButton}") + logger_mp.info(f"right_controller_trigger_state: {tv.right_controller_trigger_state}") + logger_mp.info(f"right_controller_trigger_value: {tv.right_controller_trigger_value}") + logger_mp.info(f"right_controller_squeeze_state: {tv.right_controller_squeeze_state}") + logger_mp.info(f"right_controller_squeeze_value: {tv.right_controller_squeeze_value}") + logger_mp.info(f"right_controller_thumbstick_state: {tv.right_controller_thumbstick_state}") + logger_mp.info(f"right_controller_thumbstick_value: {tv.right_controller_thumbstick_value}") + logger_mp.info(f"right_controller_aButton: {tv.right_controller_aButton}") + logger_mp.info(f"right_controller_bButton: {tv.right_controller_bButton}") + logger_mp.info("=" * 80) time.sleep(0.03) except KeyboardInterrupt: running = False - print("KeyboardInterrupt, exiting program...") + logger_mp.info("KeyboardInterrupt, exiting program...") finally: image_shm.unlink() image_shm.close() - print("Finally, exiting program...") + logger_mp.info("Finally, exiting program...") exit(0) if __name__ == '__main__': diff --git a/teleop/open_television/_test_tv_wrapper.py b/teleop/open_television/_test_tv_wrapper.py index 00f8330..47163b0 100644 --- a/teleop/open_television/_test_tv_wrapper.py +++ b/teleop/open_television/_test_tv_wrapper.py @@ -8,6 +8,9 @@ import numpy as np import time from multiprocessing import shared_memory from open_television import TeleVisionWrapper +import logging_mp +logger_mp = logging_mp.get_logger(__name__, level=logging_mp.INFO) + def run_test_tv_wrapper(): image_shape = (480, 640 * 2, 3) @@ -31,57 +34,57 @@ def run_test_tv_wrapper(): start_time = time.time() teleData = tv_wrapper.get_motion_state_data() - print("=== TeleData Snapshot ===") - print("[Head Rotation Matrix]:\n", teleData.head_rotation) - print("[Left EE Pose]:\n", teleData.left_arm_pose) - print("[Right EE Pose]:\n", teleData.right_arm_pose) + logger_mp.info("=== TeleData Snapshot ===") + logger_mp.info("[Head Rotation Matrix]:\n", teleData.head_rotation) + logger_mp.info("[Left EE Pose]:\n", teleData.left_arm_pose) + logger_mp.info("[Right EE Pose]:\n", teleData.right_arm_pose) if use_hand_track: - print("[Left Hand Position] shape {}:\n{}".format(teleData.left_hand_pos.shape, teleData.left_hand_pos)) - print("[Right Hand Position] shape {}:\n{}".format(teleData.right_hand_pos.shape, teleData.right_hand_pos)) + logger_mp.info("[Left Hand Position] shape {}:\n{}".format(teleData.left_hand_pos.shape, teleData.left_hand_pos)) + logger_mp.info("[Right Hand Position] shape {}:\n{}".format(teleData.right_hand_pos.shape, teleData.right_hand_pos)) if teleData.left_hand_rot is not None: - print("[Left Hand Rotation] shape {}:\n{}".format(teleData.left_hand_rot.shape, teleData.left_hand_rot)) + logger_mp.info("[Left Hand Rotation] shape {}:\n{}".format(teleData.left_hand_rot.shape, teleData.left_hand_rot)) if teleData.right_hand_rot is not None: - print("[Right Hand Rotation] shape {}:\n{}".format(teleData.right_hand_rot.shape, teleData.right_hand_rot)) + logger_mp.info("[Right Hand Rotation] shape {}:\n{}".format(teleData.right_hand_rot.shape, teleData.right_hand_rot)) if teleData.left_pinch_value is not None: - print("[Left Pinch Value]: {:.2f}".format(teleData.left_pinch_value)) + logger_mp.info("[Left Pinch Value]: {:.2f}".format(teleData.left_pinch_value)) if teleData.right_pinch_value is not None: - print("[Right Pinch Value]: {:.2f}".format(teleData.right_pinch_value)) + logger_mp.info("[Right Pinch Value]: {:.2f}".format(teleData.right_pinch_value)) if teleData.tele_state: state = teleData.tele_state - print("[Hand State]:") - print(f" Left Pinch state: {state.left_pinch_state}") - print(f" Left Squeeze: {state.left_squeeze_state} ({state.left_squeeze_value:.2f})") - print(f" Right Pinch state: {state.right_pinch_state}") - print(f" Right Squeeze: {state.right_squeeze_state} ({state.right_squeeze_value:.2f})") + logger_mp.info("[Hand State]:") + logger_mp.info(f" Left Pinch state: {state.left_pinch_state}") + logger_mp.info(f" Left Squeeze: {state.left_squeeze_state} ({state.left_squeeze_value:.2f})") + logger_mp.info(f" Right Pinch state: {state.right_pinch_state}") + logger_mp.info(f" Right Squeeze: {state.right_squeeze_state} ({state.right_squeeze_value:.2f})") else: - print(f"[Left Trigger Value]: {teleData.left_trigger_value:.2f}") - print(f"[Right Trigger Value]: {teleData.right_trigger_value:.2f}") + logger_mp.info(f"[Left Trigger Value]: {teleData.left_trigger_value:.2f}") + logger_mp.info(f"[Right Trigger Value]: {teleData.right_trigger_value:.2f}") if teleData.tele_state: state = teleData.tele_state - print("[Controller State]:") - print(f" Left Trigger: {state.left_trigger_state}") - print(f" Left Squeeze: {state.left_squeeze_ctrl_state} ({state.left_squeeze_ctrl_value:.2f})") - print(f" Left Thumbstick: {state.left_thumbstick_state} ({state.left_thumbstick_value})") - print(f" Left A/B Buttons: A={state.left_aButton}, B={state.left_bButton}") - print(f" Right Trigger: {state.right_trigger_state}") - print(f" Right Squeeze: {state.right_squeeze_ctrl_state} ({state.right_squeeze_ctrl_value:.2f})") - print(f" Right Thumbstick: {state.right_thumbstick_state} ({state.right_thumbstick_value})") - print(f" Right A/B Buttons: A={state.right_aButton}, B={state.right_bButton}") + logger_mp.info("[Controller State]:") + logger_mp.info(f" Left Trigger: {state.left_trigger_state}") + logger_mp.info(f" Left Squeeze: {state.left_squeeze_ctrl_state} ({state.left_squeeze_ctrl_value:.2f})") + logger_mp.info(f" Left Thumbstick: {state.left_thumbstick_state} ({state.left_thumbstick_value})") + logger_mp.info(f" Left A/B Buttons: A={state.left_aButton}, B={state.left_bButton}") + logger_mp.info(f" Right Trigger: {state.right_trigger_state}") + logger_mp.info(f" Right Squeeze: {state.right_squeeze_ctrl_state} ({state.right_squeeze_ctrl_value:.2f})") + logger_mp.info(f" Right Thumbstick: {state.right_thumbstick_state} ({state.right_thumbstick_value})") + logger_mp.info(f" Right A/B Buttons: A={state.right_aButton}, B={state.right_bButton}") current_time = time.time() time_elapsed = current_time - start_time sleep_time = max(0, 0.033 - time_elapsed) time.sleep(sleep_time) - # print(f"main process sleep: {sleep_time}") + logger_mp.debug(f"main process sleep: {sleep_time}") except KeyboardInterrupt: running = False - print("KeyboardInterrupt, exiting program...") + logger_mp.info("KeyboardInterrupt, exiting program...") finally: image_shm.unlink() image_shm.close() - print("Finally, exiting program...") + logger_mp.info("Finally, exiting program...") exit(0) if __name__ == '__main__': diff --git a/teleop/robot_control/hand_retargeting.py b/teleop/robot_control/hand_retargeting.py index a0975c5..77c6a18 100644 --- a/teleop/robot_control/hand_retargeting.py +++ b/teleop/robot_control/hand_retargeting.py @@ -2,6 +2,8 @@ from .dex_retargeting.retargeting_config import RetargetingConfig from pathlib import Path import yaml from enum import Enum +import logging_mp +logger_mp = logging_mp.get_logger(__name__) class HandType(Enum): INSPIRE_HAND = "../assets/inspire_hand/inspire_hand.yml" @@ -49,11 +51,11 @@ class HandRetargeting: self.right_dex_retargeting_to_hardware = [ self.right_retargeting_joint_names.index(name) for name in self.right_dex3_api_joint_names] # Archive: This is the joint order of the dex-retargeting library version 0.1.1. - # print([joint.get_name() for joint in self.left_retargeting.optimizer.robot.get_active_joints()]) + # logger_mp.info([joint.get_name() for joint in self.left_retargeting.optimizer.robot.get_active_joints()]) # ['left_hand_thumb_0_joint', 'left_hand_thumb_1_joint', 'left_hand_thumb_2_joint', # 'left_hand_middle_0_joint', 'left_hand_middle_1_joint', # 'left_hand_index_0_joint', 'left_hand_index_1_joint'] - # print([joint.get_name() for joint in self.right_retargeting.optimizer.robot.get_active_joints()]) + # logger_mp.info([joint.get_name() for joint in self.right_retargeting.optimizer.robot.get_active_joints()]) # ['right_hand_thumb_0_joint', 'right_hand_thumb_1_joint', 'right_hand_thumb_2_joint', # 'right_hand_middle_0_joint', 'right_hand_middle_1_joint', # 'right_hand_index_0_joint', 'right_hand_index_1_joint'] @@ -66,11 +68,11 @@ class HandRetargeting: self.right_dex_retargeting_to_hardware = [ self.right_retargeting_joint_names.index(name) for name in self.right_inspire_api_joint_names] except FileNotFoundError: - print(f"Configuration file not found: {config_file_path}") + logger_mp.warning(f"Configuration file not found: {config_file_path}") raise except yaml.YAMLError as e: - print(f"YAML error while reading {config_file_path}: {e}") + logger_mp.warning(f"YAML error while reading {config_file_path}: {e}") raise except Exception as e: - print(f"An error occurred: {e}") + logger_mp.error(f"An error occurred: {e}") raise \ No newline at end of file diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index d728024..1b1a753 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -11,6 +11,9 @@ from unitree_sdk2py.utils.crc import CRC from unitree_sdk2py.idl.unitree_go.msg.dds_ import ( LowCmd_ as go_LowCmd, LowState_ as go_LowState) # idl for h1 from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ +import logging_mp +logger_mp = logging_mp.get_logger(__name__) + kTopicLowCommand_Debug = "rt/lowcmd" kTopicLowCommand_Motion = "rt/arm_sdk" kTopicLowState = "rt/lowstate" @@ -57,7 +60,7 @@ class DataBuffer: class G1_29_ArmController: def __init__(self, debug_mode = True): - print("Initialize G1_29_ArmController...") + logger_mp.info("Initialize G1_29_ArmController...") self.q_target = np.zeros(14) self.tauff_target = np.zeros(14) self.debug_mode = debug_mode @@ -95,7 +98,7 @@ class G1_29_ArmController: while not self.lowstate_buffer.GetData(): time.sleep(0.01) - print("[G1_29_ArmController] Waiting to subscribe dds...") + logger_mp.warning("[G1_29_ArmController] Waiting to subscribe dds...") # initialize hg's lowcmd msg self.crc = CRC() @@ -104,9 +107,9 @@ class G1_29_ArmController: self.msg.mode_machine = self.get_mode_machine() self.all_motor_q = self.get_current_motor_q() - print(f"Current all body motor state q:\n{self.all_motor_q} \n") - print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - print("Lock all joints except two arms...\n") + logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") + logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") + logger_mp.info("Lock all joints except two arms...\n") arm_indices = set(member.value for member in G1_29_JointArmIndex) for id in G1_29_JointIndex: @@ -126,7 +129,7 @@ class G1_29_ArmController: self.msg.motor_cmd[id].kp = self.kp_high self.msg.motor_cmd[id].kd = self.kd_high self.msg.motor_cmd[id].q = self.all_motor_q[id] - print("Lock OK!\n") + logger_mp.info("Lock OK!\n") # initialize publish thread self.publish_thread = threading.Thread(target=self._ctrl_motor_state) @@ -134,7 +137,7 @@ class G1_29_ArmController: self.publish_thread.daemon = True self.publish_thread.start() - print("Initialize G1_29_ArmController OK!\n") + logger_mp.info("Initialize G1_29_ArmController OK!\n") def _subscribe_motor_state(self): while True: @@ -183,8 +186,8 @@ class G1_29_ArmController: all_t_elapsed = current_time - start_time sleep_time = max(0, (self.control_dt - all_t_elapsed)) time.sleep(sleep_time) - # print(f"arm_velocity_limit:{self.arm_velocity_limit}") - # print(f"sleep_time:{sleep_time}") + # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") + # logger_mp.debug(f"sleep_time:{sleep_time}") def ctrl_dual_arm(self, q_target, tauff_target): '''Set control target values q & tau of the left and right arm motors.''' @@ -210,7 +213,7 @@ class G1_29_ArmController: def ctrl_dual_arm_go_home(self): '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' - print("[G1_29_ArmController] ctrl_dual_arm_go_home start...") + logger_mp.info("[G1_29_ArmController] ctrl_dual_arm_go_home start...") with self.ctrl_lock: self.q_target = np.zeros(14) # self.tauff_target = np.zeros(14) @@ -222,7 +225,7 @@ class G1_29_ArmController: for weight in np.arange(1, 0, -0.01): self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; time.sleep(0.02) - print("[G1_29_ArmController] both arms have reached the home position.") + logger_mp.info("[G1_29_ArmController] both arms have reached the home position.") break time.sleep(0.05) @@ -332,7 +335,7 @@ class G1_29_JointIndex(IntEnum): class G1_23_ArmController: def __init__(self): - print("Initialize G1_23_ArmController...") + logger_mp.info("Initialize G1_23_ArmController...") self.q_target = np.zeros(10) self.tauff_target = np.zeros(10) @@ -366,7 +369,7 @@ class G1_23_ArmController: while not self.lowstate_buffer.GetData(): time.sleep(0.01) - print("[G1_23_ArmController] Waiting to subscribe dds...") + logger_mp.warning("[G1_23_ArmController] Waiting to subscribe dds...") # initialize hg's lowcmd msg self.crc = CRC() @@ -375,9 +378,9 @@ class G1_23_ArmController: self.msg.mode_machine = self.get_mode_machine() self.all_motor_q = self.get_current_motor_q() - print(f"Current all body motor state q:\n{self.all_motor_q} \n") - print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - print("Lock all joints except two arms...\n") + logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") + logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") + logger_mp.info("Lock all joints except two arms...\n") arm_indices = set(member.value for member in G1_23_JointArmIndex) for id in G1_23_JointIndex: @@ -397,7 +400,7 @@ class G1_23_ArmController: self.msg.motor_cmd[id].kp = self.kp_high self.msg.motor_cmd[id].kd = self.kd_high self.msg.motor_cmd[id].q = self.all_motor_q[id] - print("Lock OK!\n") + logger_mp.info("Lock OK!\n") # initialize publish thread self.publish_thread = threading.Thread(target=self._ctrl_motor_state) @@ -405,7 +408,7 @@ class G1_23_ArmController: self.publish_thread.daemon = True self.publish_thread.start() - print("Initialize G1_23_ArmController OK!\n") + logger_mp.info("Initialize G1_23_ArmController OK!\n") def _subscribe_motor_state(self): while True: @@ -451,8 +454,8 @@ class G1_23_ArmController: all_t_elapsed = current_time - start_time sleep_time = max(0, (self.control_dt - all_t_elapsed)) time.sleep(sleep_time) - # print(f"arm_velocity_limit:{self.arm_velocity_limit}") - # print(f"sleep_time:{sleep_time}") + # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") + # logger_mp.debug(f"sleep_time:{sleep_time}") def ctrl_dual_arm(self, q_target, tauff_target): '''Set control target values q & tau of the left and right arm motors.''' @@ -478,7 +481,7 @@ class G1_23_ArmController: def ctrl_dual_arm_go_home(self): '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' - print("[G1_23_ArmController] ctrl_dual_arm_go_home start...") + logger_mp.info("[G1_23_ArmController] ctrl_dual_arm_go_home start...") with self.ctrl_lock: self.q_target = np.zeros(10) # self.tauff_target = np.zeros(10) @@ -486,7 +489,7 @@ class G1_23_ArmController: while True: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): - print("[G1_23_ArmController] both arms have reached the home position.") + logger_mp.info("[G1_23_ArmController] both arms have reached the home position.") break time.sleep(0.05) @@ -588,7 +591,7 @@ class G1_23_JointIndex(IntEnum): class H1_2_ArmController: def __init__(self): - print("Initialize H1_2_ArmController...") + logger_mp.info("Initialize H1_2_ArmController...") self.q_target = np.zeros(14) self.tauff_target = np.zeros(14) @@ -622,7 +625,7 @@ class H1_2_ArmController: while not self.lowstate_buffer.GetData(): time.sleep(0.01) - print("[H1_2_ArmController] Waiting to subscribe dds...") + logger_mp.warning("[H1_2_ArmController] Waiting to subscribe dds...") # initialize hg's lowcmd msg self.crc = CRC() @@ -631,9 +634,9 @@ class H1_2_ArmController: self.msg.mode_machine = self.get_mode_machine() self.all_motor_q = self.get_current_motor_q() - print(f"Current all body motor state q:\n{self.all_motor_q} \n") - print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - print("Lock all joints except two arms...\n") + logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") + logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") + logger_mp.info("Lock all joints except two arms...\n") arm_indices = set(member.value for member in H1_2_JointArmIndex) for id in H1_2_JointIndex: @@ -653,7 +656,7 @@ class H1_2_ArmController: self.msg.motor_cmd[id].kp = self.kp_high self.msg.motor_cmd[id].kd = self.kd_high self.msg.motor_cmd[id].q = self.all_motor_q[id] - print("Lock OK!\n") + logger_mp.info("Lock OK!\n") # initialize publish thread self.publish_thread = threading.Thread(target=self._ctrl_motor_state) @@ -661,7 +664,7 @@ class H1_2_ArmController: self.publish_thread.daemon = True self.publish_thread.start() - print("Initialize H1_2_ArmController OK!\n") + logger_mp.info("Initialize H1_2_ArmController OK!\n") def _subscribe_motor_state(self): while True: @@ -707,8 +710,8 @@ class H1_2_ArmController: all_t_elapsed = current_time - start_time sleep_time = max(0, (self.control_dt - all_t_elapsed)) time.sleep(sleep_time) - # print(f"arm_velocity_limit:{self.arm_velocity_limit}") - # print(f"sleep_time:{sleep_time}") + # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") + # logger_mp.debug(f"sleep_time:{sleep_time}") def ctrl_dual_arm(self, q_target, tauff_target): '''Set control target values q & tau of the left and right arm motors.''' @@ -734,7 +737,7 @@ class H1_2_ArmController: def ctrl_dual_arm_go_home(self): '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' - print("[H1_2_ArmController] ctrl_dual_arm_go_home start...") + logger_mp.info("[H1_2_ArmController] ctrl_dual_arm_go_home start...") with self.ctrl_lock: self.q_target = np.zeros(14) # self.tauff_target = np.zeros(14) @@ -742,7 +745,7 @@ class H1_2_ArmController: while True: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): - print("[H1_2_ArmController] both arms have reached the home position.") + logger_mp.info("[H1_2_ArmController] both arms have reached the home position.") break time.sleep(0.05) @@ -851,7 +854,7 @@ class H1_2_JointIndex(IntEnum): class H1_ArmController: def __init__(self): - print("Initialize H1_ArmController...") + logger_mp.info("Initialize H1_ArmController...") self.q_target = np.zeros(8) self.tauff_target = np.zeros(8) @@ -883,7 +886,7 @@ class H1_ArmController: while not self.lowstate_buffer.GetData(): time.sleep(0.01) - print("[H1_ArmController] Waiting to subscribe dds...") + logger_mp.warning("[H1_ArmController] Waiting to subscribe dds...") # initialize h1's lowcmd msg self.crc = CRC() @@ -894,9 +897,9 @@ class H1_ArmController: self.msg.gpio = 0 self.all_motor_q = self.get_current_motor_q() - print(f"Current all body motor state q:\n{self.all_motor_q} \n") - print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - print("Lock all joints except two arms...\n") + logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") + logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") + logger_mp.info("Lock all joints except two arms...\n") for id in H1_JointIndex: if self._Is_weak_motor(id): @@ -908,7 +911,7 @@ class H1_ArmController: self.msg.motor_cmd[id].kd = self.kd_high self.msg.motor_cmd[id].mode = 0x0A self.msg.motor_cmd[id].q = self.all_motor_q[id] - print("Lock OK!\n") + logger_mp.info("Lock OK!\n") # initialize publish thread self.publish_thread = threading.Thread(target=self._ctrl_motor_state) @@ -916,7 +919,7 @@ class H1_ArmController: self.publish_thread.daemon = True self.publish_thread.start() - print("Initialize H1_ArmController OK!\n") + logger_mp.info("Initialize H1_ArmController OK!\n") def _subscribe_motor_state(self): while True: @@ -962,8 +965,8 @@ class H1_ArmController: all_t_elapsed = current_time - start_time sleep_time = max(0, (self.control_dt - all_t_elapsed)) time.sleep(sleep_time) - # print(f"arm_velocity_limit:{self.arm_velocity_limit}") - # print(f"sleep_time:{sleep_time}") + # logger_mp.debug(f"arm_velocity_limit:{self.arm_velocity_limit}") + # logger_mp.debug(f"sleep_time:{sleep_time}") def ctrl_dual_arm(self, q_target, tauff_target): '''Set control target values q & tau of the left and right arm motors.''' @@ -985,7 +988,7 @@ class H1_ArmController: def ctrl_dual_arm_go_home(self): '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' - print("[H1_ArmController] ctrl_dual_arm_go_home start...") + logger_mp.info("[H1_ArmController] ctrl_dual_arm_go_home start...") with self.ctrl_lock: self.q_target = np.zeros(8) # self.tauff_target = np.zeros(8) @@ -993,7 +996,7 @@ class H1_ArmController: while True: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): - print("[H1_ArmController] both arms have reached the home position.") + logger_mp.info("[H1_ArmController] both arms have reached the home position.") break time.sleep(0.05) diff --git a/teleop/robot_control/robot_arm_ik.py b/teleop/robot_control/robot_arm_ik.py index 57c92e0..0a94d11 100644 --- a/teleop/robot_control/robot_arm_ik.py +++ b/teleop/robot_control/robot_arm_ik.py @@ -7,7 +7,8 @@ from pinocchio import casadi as cpin from pinocchio.visualize import MeshcatVisualizer import os import sys - +import logging_mp +logger_mp = logging_mp.get_logger(__name__) parent2_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) sys.path.append(parent2_dir) @@ -83,9 +84,9 @@ class G1_29_ArmIK: # for i in range(self.reduced_robot.model.nframes): # frame = self.reduced_robot.model.frames[i] # frame_id = self.reduced_robot.model.getFrameId(frame.name) - # print(f"Frame ID: {frame_id}, Name: {frame.name}") + # logger_mp.debug(f"Frame ID: {frame_id}, Name: {frame.name}") # for idx, name in enumerate(self.reduced_robot.model.names): - # print(f"{idx}: {name}") + # logger_mp.debug(f"{idx}: {name}") # Creating Casadi models and data for symbolic computing self.cmodel = cpin.Model(self.reduced_robot.model) @@ -237,7 +238,7 @@ class G1_29_ArmIK: return sol_q, sol_tauff except Exception as e: - print(f"ERROR in convergence, plotting debug info.{e}") + logger_mp.error(f"ERROR in convergence, plotting debug info.{e}") sol_q = self.opti.debug.value(self.var_q) self.smooth_filter.add_data(sol_q) @@ -252,7 +253,7 @@ class G1_29_ArmIK: sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv)) - print(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") + logger_mp.error(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") if self.Visualization: self.vis.display(sol_q) # for visualization @@ -311,7 +312,7 @@ class G1_23_ArmIK: # for i in range(self.reduced_robot.model.nframes): # frame = self.reduced_robot.model.frames[i] # frame_id = self.reduced_robot.model.getFrameId(frame.name) - # print(f"Frame ID: {frame_id}, Name: {frame.name}") + # logger_mp.debug(f"Frame ID: {frame_id}, Name: {frame.name}") # Creating Casadi models and data for symbolic computing self.cmodel = cpin.Model(self.reduced_robot.model) @@ -463,7 +464,7 @@ class G1_23_ArmIK: return sol_q, sol_tauff except Exception as e: - print(f"ERROR in convergence, plotting debug info.{e}") + logger_mp.error(f"ERROR in convergence, plotting debug info.{e}") sol_q = self.opti.debug.value(self.var_q) self.smooth_filter.add_data(sol_q) @@ -478,7 +479,7 @@ class G1_23_ArmIK: sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv)) - print(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") + logger_mp.error(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") if self.Visualization: self.vis.display(sol_q) # for visualization @@ -562,7 +563,7 @@ class H1_2_ArmIK: # for i in range(self.reduced_robot.model.nframes): # frame = self.reduced_robot.model.frames[i] # frame_id = self.reduced_robot.model.getFrameId(frame.name) - # print(f"Frame ID: {frame_id}, Name: {frame.name}") + # logger_mp.debug(f"Frame ID: {frame_id}, Name: {frame.name}") # Creating Casadi models and data for symbolic computing self.cmodel = cpin.Model(self.reduced_robot.model) @@ -714,7 +715,7 @@ class H1_2_ArmIK: return sol_q, sol_tauff except Exception as e: - print(f"ERROR in convergence, plotting debug info.{e}") + logger_mp.error(f"ERROR in convergence, plotting debug info.{e}") sol_q = self.opti.debug.value(self.var_q) self.smooth_filter.add_data(sol_q) @@ -729,7 +730,7 @@ class H1_2_ArmIK: sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv)) - print(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") + logger_mp.error(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") if self.Visualization: self.vis.display(sol_q) # for visualization @@ -816,7 +817,7 @@ class H1_ArmIK: # for i in range(self.reduced_robot.model.nframes): # frame = self.reduced_robot.model.frames[i] # frame_id = self.reduced_robot.model.getFrameId(frame.name) - # print(f"Frame ID: {frame_id}, Name: {frame.name}") + # logger_mp.debug(f"Frame ID: {frame_id}, Name: {frame.name}") # Creating Casadi models and data for symbolic computing self.cmodel = cpin.Model(self.reduced_robot.model) @@ -968,7 +969,7 @@ class H1_ArmIK: return sol_q, sol_tauff except Exception as e: - print(f"ERROR in convergence, plotting debug info.{e}") + logger_mp.error(f"ERROR in convergence, plotting debug info.{e}") sol_q = self.opti.debug.value(self.var_q) self.smooth_filter.add_data(sol_q) @@ -983,7 +984,7 @@ class H1_ArmIK: sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv)) - print(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") + logger_mp.error(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}") if self.Visualization: self.vis.display(sol_q) # for visualization diff --git a/teleop/robot_control/robot_hand_inspire.py b/teleop/robot_control/robot_hand_inspire.py index 5c69477..562402e 100644 --- a/teleop/robot_control/robot_hand_inspire.py +++ b/teleop/robot_control/robot_hand_inspire.py @@ -8,7 +8,10 @@ import numpy as np from enum import IntEnum import threading import time -from multiprocessing import Process, shared_memory, Array, Lock +from multiprocessing import Process, Array + +import logging_mp +logger_mp = logging_mp.get_logger(__name__) inspire_tip_indices = [4, 9, 14, 19, 24] Inspire_Num_Motors = 6 @@ -18,7 +21,7 @@ kTopicInspireState = "rt/inspire/state" class Inspire_Controller: def __init__(self, left_hand_array, right_hand_array, dual_hand_data_lock = None, dual_hand_state_array = None, dual_hand_action_array = None, fps = 100.0, Unit_Test = False): - print("Initialize Inspire_Controller...") + logger_mp.info("Initialize Inspire_Controller...") self.fps = fps self.Unit_Test = Unit_Test if not self.Unit_Test: @@ -47,14 +50,14 @@ class Inspire_Controller: if any(self.right_hand_state_array): # any(self.left_hand_state_array) and break time.sleep(0.01) - print("[Inspire_Controller] Waiting to subscribe dds...") + logger_mp.warning("[Inspire_Controller] Waiting to subscribe dds...") hand_control_process = Process(target=self.control_process, args=(left_hand_array, right_hand_array, self.left_hand_state_array, self.right_hand_state_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)) hand_control_process.daemon = True hand_control_process.start() - print("Initialize Inspire_Controller OK!\n") + logger_mp.info("Initialize Inspire_Controller OK!\n") def _subscribe_hand_state(self): while True: @@ -76,7 +79,7 @@ class Inspire_Controller: self.hand_msg.cmds[id].q = right_q_target[idx] self.HandCmb_publisher.Write(self.hand_msg) - # print("hand ctrl publish ok.") + # logger_mp.debug("hand ctrl publish ok.") def control_process(self, left_hand_array, right_hand_array, left_hand_state_array, right_hand_state_array, dual_hand_data_lock = None, dual_hand_state_array = None, dual_hand_action_array = None): @@ -147,7 +150,7 @@ class Inspire_Controller: sleep_time = max(0, (1 / self.fps) - time_elapsed) time.sleep(sleep_time) finally: - print("Inspire_Controller has been closed.") + logger_mp.info("Inspire_Controller has been closed.") # Update hand state, according to the official documentation, https://support.unitree.com/home/en/G1_developer/inspire_dfx_dexterous_hand # the state sequence is as shown in the table below diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index 4787a59..dbd6935 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -20,6 +20,9 @@ sys.path.append(parent2_dir) from teleop.robot_control.hand_retargeting import HandRetargeting, HandType from teleop.utils.weighted_moving_filter import WeightedMovingFilter +import logging_mp +logger_mp = logging_mp.get_logger(__name__) + unitree_tip_indices = [4, 9, 14] # [thumb, index, middle] in OpenXR Dex3_Num_Motors = 7 @@ -49,7 +52,7 @@ class Dex3_1_Controller: Unit_Test: Whether to enable unit testing """ - print("Initialize Dex3_1_Controller...") + logger_mp.info("Initialize Dex3_1_Controller...") self.fps = fps self.Unit_Test = Unit_Test @@ -83,14 +86,14 @@ class Dex3_1_Controller: if any(self.left_hand_state_array) and any(self.right_hand_state_array): break time.sleep(0.01) - print("[Dex3_1_Controller] Waiting to subscribe dds...") + logger_mp.warning("[Dex3_1_Controller] Waiting to subscribe dds...") hand_control_process = Process(target=self.control_process, args=(left_hand_array_in, right_hand_array_in, self.left_hand_state_array, self.right_hand_state_array, dual_hand_data_lock, dual_hand_state_array_out, dual_hand_action_array_out)) hand_control_process.daemon = True hand_control_process.start() - print("Initialize Dex3_1_Controller OK!\n") + logger_mp.info("Initialize Dex3_1_Controller OK!\n") def _subscribe_hand_state(self): while True: @@ -127,7 +130,7 @@ class Dex3_1_Controller: self.LeftHandCmb_publisher.Write(self.left_msg) self.RightHandCmb_publisher.Write(self.right_msg) - # print("hand ctrl publish ok.") + # logger_mp.debug("hand ctrl publish ok.") def control_process(self, left_hand_array_in, right_hand_array_in, left_hand_state_array, right_hand_state_array, dual_hand_data_lock = None, dual_hand_state_array_out = None, dual_hand_action_array_out = None): @@ -204,7 +207,7 @@ class Dex3_1_Controller: sleep_time = max(0, (1 / self.fps) - time_elapsed) time.sleep(sleep_time) finally: - print("Dex3_1_Controller has been closed.") + logger_mp.info("Dex3_1_Controller has been closed.") class Dex3_1_Left_JointIndex(IntEnum): kLeftHandThumb0 = 0 @@ -251,7 +254,7 @@ class Gripper_Controller: Unit_Test: Whether to enable unit testing """ - print("Initialize Gripper_Controller...") + logger_mp.info("Initialize Gripper_Controller...") self.fps = fps self.Unit_Test = Unit_Test @@ -281,14 +284,14 @@ class Gripper_Controller: if any(state != 0.0 for state in self.dual_gripper_state): break time.sleep(0.01) - print("[Gripper_Controller] Waiting to subscribe dds...") + logger_mp.warning("[Gripper_Controller] Waiting to subscribe dds...") self.gripper_control_thread = threading.Thread(target=self.control_thread, args=(left_gripper_value_in, right_gripper_value_in, self.dual_gripper_state, dual_gripper_data_lock, dual_gripper_state_out, dual_gripper_action_out)) self.gripper_control_thread.daemon = True self.gripper_control_thread.start() - print("Initialize Gripper_Controller OK!\n") + logger_mp.info("Initialize Gripper_Controller OK!\n") def _subscribe_gripper_state(self): while True: @@ -304,7 +307,7 @@ class Gripper_Controller: self.gripper_msg.cmds[id].q = gripper_q_target[idx] self.GripperCmb_publisher.Write(self.gripper_msg) - # print("gripper ctrl publish ok.") + # logger_mp.debug("gripper ctrl publish ok.") def control_thread(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_state_in, dual_hand_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None): @@ -374,9 +377,9 @@ class Gripper_Controller: dual_gripper_state_out[:] = dual_gripper_state - np.array([RIGHT_MAPPED_MIN, LEFT_MAPPED_MIN]) dual_gripper_action_out[:] = dual_gripper_action - np.array([RIGHT_MAPPED_MIN, LEFT_MAPPED_MIN]) - # print(f"LEFT: euclidean:{left_euclidean_distance:.4f} \tstate:{dual_gripper_state_out[1]:.4f}\ + # logger_mp.debug(f"LEFT: euclidean:{left_euclidean_distance:.4f} \tstate:{dual_gripper_state_out[1]:.4f}\ # \ttarget_action:{right_target_action - RIGHT_MAPPED_MIN:.4f} \tactual_action:{dual_gripper_action_out[1]:.4f}") - # print(f"RIGHT:euclidean:{right_euclidean_distance:.4f} \tstate:{dual_gripper_state_out[0]:.4f}\ + # logger_mp.debug(f"RIGHT:euclidean:{right_euclidean_distance:.4f} \tstate:{dual_gripper_state_out[0]:.4f}\ # \ttarget_action:{left_target_action - LEFT_MAPPED_MIN:.4f} \tactual_action:{dual_gripper_action_out[0]:.4f}") self.ctrl_dual_gripper(dual_gripper_action) @@ -385,7 +388,7 @@ class Gripper_Controller: sleep_time = max(0, (1 / self.fps) - time_elapsed) time.sleep(sleep_time) finally: - print("Gripper_Controller has been closed.") + logger_mp.info("Gripper_Controller has been closed.") class Gripper_JointIndex(IntEnum): kLeftGripper = 0 @@ -402,7 +405,7 @@ if __name__ == "__main__": parser.add_argument('--gripper', dest='dex', action='store_false', help='Use gripper') parser.set_defaults(dex=True) args = parser.parse_args() - print(f"args:{args}\n") + logger_mp.info(f"args:{args}\n") # image img_config = { @@ -457,5 +460,5 @@ if __name__ == "__main__": right_hand_array[:] = right_hand.flatten() # with dual_hand_data_lock: - # print(f"state : {list(dual_hand_state_array)} \naction: {list(dual_hand_action_array)} \n") + # logger_mp.info(f"state : {list(dual_hand_state_array)} \naction: {list(dual_hand_action_array)} \n") time.sleep(0.01) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 7a1b353..6426b9c 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -4,6 +4,9 @@ import argparse import cv2 from multiprocessing import shared_memory, Value, Array, Lock import threading +import logging_mp +logging_mp.basic_config(level=logging_mp.INFO) +logger_mp = logging_mp.get_logger(__name__) import os import sys @@ -38,7 +41,7 @@ if __name__ == '__main__': parser.set_defaults(debug = True) args = parser.parse_args() - print(f"args:{args}\n") + logger_mp.info(f"args: {args}") # image client: img_config should be the same as the configuration in image_server.py (of Robot's development computing unit) img_config = { @@ -87,8 +90,9 @@ if __name__ == '__main__': # arm if args.arm == 'G1_29': - arm_ctrl = G1_29_ArmController(debug_mode=args.debug) - arm_ik = G1_29_ArmIK() + # arm_ctrl = G1_29_ArmController(debug_mode=args.debug) + # arm_ik = G1_29_ArmIK() + pass elif args.arm == 'G1_23': arm_ctrl = G1_23_ArmController() arm_ik = G1_23_ArmIK() @@ -138,7 +142,7 @@ if __name__ == '__main__': try: user_input = input("Please enter the start signal (enter 'r' to start the subsequent program):\n") if user_input.lower() == 'r': - arm_ctrl.speed_gradual_max() + # arm_ctrl.speed_gradual_max() running = True while running: start_time = time.time() @@ -190,16 +194,16 @@ if __name__ == '__main__': -tele_data.tele_state.left_thumbstick_value[0] * 0.3, -tele_data.tele_state.right_thumbstick_value[0] * 0.3) - # get current robot state data. - current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() - current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() + # # get current robot state data. + # current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() + # current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() - # solve ik using motor data and wrist pose, then use ik results to control arms. - time_ik_start = time.time() - sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) - time_ik_end = time.time() - # print(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") - arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) + # # solve ik using motor data and wrist pose, then use ik results to control arms. + # time_ik_start = time.time() + # sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) + # time_ik_end = time.time() + # logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") + # arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) # record data if args.record: @@ -325,12 +329,12 @@ if __name__ == '__main__': time_elapsed = current_time - start_time sleep_time = max(0, (1 / args.frequency) - time_elapsed) time.sleep(sleep_time) - # print(f"main process sleep: {sleep_time}") + logger_mp.debug(f"main process sleep: {sleep_time}") except KeyboardInterrupt: - print("KeyboardInterrupt, exiting program...") + logger_mp.info("KeyboardInterrupt, exiting program...") finally: - arm_ctrl.ctrl_dual_arm_go_home() + # arm_ctrl.ctrl_dual_arm_go_home() tv_img_shm.unlink() tv_img_shm.close() if WRIST: @@ -338,5 +342,5 @@ if __name__ == '__main__': wrist_img_shm.close() if args.record: recorder.close() - print("Finally, exiting program...") + logger_mp.info("Finally, exiting program...") exit(0) \ No newline at end of file diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index ec05ba1..8f255f6 100644 --- a/teleop/utils/episode_writer.py +++ b/teleop/utils/episode_writer.py @@ -7,22 +7,24 @@ import time from .rerun_visualizer import RerunLogger from queue import Queue, Empty from threading import Thread +import logging_mp +logger_mp = logging_mp.get_logger(__name__) class EpisodeWriter(): def __init__(self, task_dir, frequency=30, image_size=[640, 480], rerun_log = True): """ image_size: [width, height] """ - print("==> EpisodeWriter initializing...\n") + logger_mp.info("==> EpisodeWriter initializing...\n") self.task_dir = task_dir self.frequency = frequency self.image_size = image_size self.rerun_log = rerun_log if self.rerun_log: - print("==> RerunLogger initializing...\n") + logger_mp.info("==> RerunLogger initializing...\n") self.rerun_logger = RerunLogger(prefix="online/", IdxRangeBoundary = 60, memory_limit = "300MB") - print("==> RerunLogger initializing ok.\n") + logger_mp.info("==> RerunLogger initializing ok.\n") self.data = {} self.episode_data = [] @@ -32,10 +34,10 @@ class EpisodeWriter(): episode_dirs = [episode_dir for episode_dir in os.listdir(self.task_dir) if 'episode_' in episode_dir] episode_last = sorted(episode_dirs)[-1] if len(episode_dirs) > 0 else None self.episode_id = 0 if episode_last is None else int(episode_last.split('_')[-1]) - print(f"==> task_dir directory already exist, now self.episode_id is:{self.episode_id}\n") + logger_mp.info(f"==> task_dir directory already exist, now self.episode_id is:{self.episode_id}\n") else: os.makedirs(self.task_dir) - print(f"==> episode directory does not exist, now create one.\n") + logger_mp.info(f"==> episode directory does not exist, now create one.\n") self.data_info() self.text_desc() @@ -47,7 +49,7 @@ class EpisodeWriter(): self.worker_thread = Thread(target=self.process_queue) self.worker_thread.start() - print("==> EpisodeWriter initialized successfully.\n") + logger_mp.info("==> EpisodeWriter initialized successfully.\n") def data_info(self, version='1.0.0', date=None, author=None): self.info = { @@ -87,7 +89,7 @@ class EpisodeWriter(): Once successfully created, this function will only be available again after save_episode complete its save task. """ if not self.is_available: - print("==> The class is currently unavailable for new operations. Please wait until ongoing tasks are completed.") + logger_mp.info("==> The class is currently unavailable for new operations. Please wait until ongoing tasks are completed.") return False # Return False if the class is unavailable # Reset episode-related data and create necessary directories @@ -108,7 +110,7 @@ class EpisodeWriter(): self.online_logger = RerunLogger(prefix="online/", IdxRangeBoundary = 60, memory_limit="300MB") self.is_available = False # After the episode is created, the class is marked as unavailable until the episode is successfully saved - print(f"==> New episode created: {self.episode_dir}") + logger_mp.info(f"==> New episode created: {self.episode_dir}") return True # Return True if the episode is successfully created def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None): @@ -135,7 +137,7 @@ class EpisodeWriter(): try: self._process_item_data(item_data) except Exception as e: - print(f"Error processing item_data (idx={item_data['idx']}): {e}") + logger_mp.info(f"Error processing item_data (idx={item_data['idx']}): {e}") self.item_data_queue.task_done() except Empty: pass @@ -155,7 +157,7 @@ class EpisodeWriter(): for idx_color, (color_key, color) in enumerate(colors.items()): color_name = f'{str(idx).zfill(6)}_{color_key}.jpg' if not cv2.imwrite(os.path.join(self.color_dir, color_name), color): - print(f"Failed to save color image.") + logger_mp.info(f"Failed to save color image.") item_data['colors'][color_key] = os.path.join('colors', color_name) # Save depths @@ -163,7 +165,7 @@ class EpisodeWriter(): for idx_depth, (depth_key, depth) in enumerate(depths.items()): depth_name = f'{str(idx).zfill(6)}_{depth_key}.jpg' if not cv2.imwrite(os.path.join(self.depth_dir, depth_name), depth): - print(f"Failed to save depth image.") + logger_mp.info(f"Failed to save depth image.") item_data['depths'][depth_key] = os.path.join('depths', depth_name) # Save audios @@ -179,7 +181,7 @@ class EpisodeWriter(): # Log data if necessary if self.rerun_log: curent_record_time = time.time() - print(f"==> episode_id:{self.episode_id} item_id:{self.item_id} current_time:{curent_record_time}") + logger_mp.info(f"==> episode_id:{self.episode_id} item_id:{self.item_id} current_time:{curent_record_time}") self.rerun_logger.log_item_data(item_data) def save_episode(self): @@ -187,7 +189,7 @@ class EpisodeWriter(): Trigger the save operation. This sets the save flag, and the process_queue thread will handle it. """ self.need_save = True # Set the save flag - print(f"==> Episode saved start...") + logger_mp.info(f"==> Episode saved start...") def _save_episode(self): """ @@ -200,7 +202,7 @@ class EpisodeWriter(): jsonf.write(json.dumps(self.data, indent=4, ensure_ascii=False)) self.need_save = False # Reset the save flag self.is_available = True # Mark the class as available after saving - print(f"==> Episode saved successfully to {self.json_path}.") + logger_mp.info(f"==> Episode saved successfully to {self.json_path}.") def close(self): """ diff --git a/teleop/utils/rerun_visualizer.py b/teleop/utils/rerun_visualizer.py index f5c8a87..631fd6c 100644 --- a/teleop/utils/rerun_visualizer.py +++ b/teleop/utils/rerun_visualizer.py @@ -188,6 +188,9 @@ if __name__ == "__main__": import gdown import zipfile import os + import logging_mp + logger_mp = logging_mp.get_logger(__name__, level=logging_mp.INFO) + zip_file = "rerun_testdata.zip" zip_file_download_url = "https://drive.google.com/file/d/1f5UuFl1z_gaByg_7jDRj1_NxfJZh2evD/view?usp=sharing" unzip_file_output_dir = "./testdata" @@ -195,16 +198,16 @@ if __name__ == "__main__": if not os.path.exists(zip_file): file_id = zip_file_download_url.split('/')[5] gdown.download(id=file_id, output=zip_file, quiet=False) - print("download ok.") + logger_mp.info("download ok.") if not os.path.exists(unzip_file_output_dir): os.makedirs(unzip_file_output_dir) with zipfile.ZipFile(zip_file, 'r') as zip_ref: zip_ref.extractall(unzip_file_output_dir) - print("uncompress ok.") + logger_mp.info("uncompress ok.") os.remove(zip_file) - print("clean file ok.") + logger_mp.info("clean file ok.") else: - print("rerun_testdata exits.") + logger_mp.info("rerun_testdata exits.") episode_reader = RerunEpisodeReader(task_dir = unzip_file_output_dir) @@ -212,20 +215,20 @@ if __name__ == "__main__": user_input = input("Please enter the start signal (enter 'off' or 'on' to start the subsequent program):\n") if user_input.lower() == 'off': episode_data6 = episode_reader.return_episode_data(6) - print("Starting offline visualization...") + logger_mp.info("Starting offline visualization...") offline_logger = RerunLogger(prefix="offline/") offline_logger.log_episode_data(episode_data6) - print("Offline visualization completed.") + logger_mp.info("Offline visualization completed.") # TEST EXAMPLE 2 : ONLINE DATA TEST, SLIDE WINDOW SIZE IS 60, MEMORY LIMIT IS 50MB if user_input.lower() == 'on': episode_data8 = episode_reader.return_episode_data(8) - print("Starting online visualization with fixed idx size...") + logger_mp.info("Starting online visualization with fixed idx size...") online_logger = RerunLogger(prefix="online/", IdxRangeBoundary = 60, memory_limit='50MB') for item_data in episode_data8: online_logger.log_item_data(item_data) time.sleep(0.033) # 30hz - print("Online visualization completed.") + logger_mp.info("Online visualization completed.") # # TEST DATA OF data_dir @@ -236,9 +239,9 @@ if __name__ == "__main__": # episode_data8 = episode_reader2.return_episode_data(episode_data_number) # if user_input.lower() == 'on': # # Example 2: Offline Visualization with Fixed Time Window - # print("Starting offline visualization with fixed idx size...") + # logger_mp.info("Starting offline visualization with fixed idx size...") # online_logger = RerunLogger(prefix="offline/", IdxRangeBoundary = 60) # for item_data in episode_data8: # online_logger.log_item_data(item_data) # time.sleep(0.033) # 30hz - # print("Offline visualization completed.") \ No newline at end of file + # logger_mp.info("Offline visualization completed.") \ No newline at end of file From 62a3c036a0b93444b03d86dc4bb4fa5140e32eaf Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 10 Jun 2025 17:51:25 +0800 Subject: [PATCH 10/32] [fix] requirements --- requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/requirements.txt b/requirements.txt index 281492b..c5d9c66 100644 --- a/requirements.txt +++ b/requirements.txt @@ -17,3 +17,4 @@ rerun-sdk==0.20.1 nlopt>=2.6.1,<2.8.0 trimesh>=4.4.0 anytree>=2.12.0 +logging-mp==0.1.5 From d9d28120e901f52a03d85c97a45542cea2b82020 Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 10 Jun 2025 17:54:48 +0800 Subject: [PATCH 11/32] [fix] uncomment codes. --- teleop/teleop_hand_and_arm.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 6426b9c..fb02213 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -90,8 +90,8 @@ if __name__ == '__main__': # arm if args.arm == 'G1_29': - # arm_ctrl = G1_29_ArmController(debug_mode=args.debug) - # arm_ik = G1_29_ArmIK() + arm_ctrl = G1_29_ArmController(debug_mode=args.debug) + arm_ik = G1_29_ArmIK() pass elif args.arm == 'G1_23': arm_ctrl = G1_23_ArmController() @@ -142,7 +142,7 @@ if __name__ == '__main__': try: user_input = input("Please enter the start signal (enter 'r' to start the subsequent program):\n") if user_input.lower() == 'r': - # arm_ctrl.speed_gradual_max() + arm_ctrl.speed_gradual_max() running = True while running: start_time = time.time() @@ -194,16 +194,16 @@ if __name__ == '__main__': -tele_data.tele_state.left_thumbstick_value[0] * 0.3, -tele_data.tele_state.right_thumbstick_value[0] * 0.3) - # # get current robot state data. - # current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() - # current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() + # get current robot state data. + current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() + current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() - # # solve ik using motor data and wrist pose, then use ik results to control arms. - # time_ik_start = time.time() - # sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) - # time_ik_end = time.time() - # logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") - # arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) + # solve ik using motor data and wrist pose, then use ik results to control arms. + time_ik_start = time.time() + sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) + time_ik_end = time.time() + logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") + arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) # record data if args.record: @@ -334,7 +334,7 @@ if __name__ == '__main__': except KeyboardInterrupt: logger_mp.info("KeyboardInterrupt, exiting program...") finally: - # arm_ctrl.ctrl_dual_arm_go_home() + arm_ctrl.ctrl_dual_arm_go_home() tv_img_shm.unlink() tv_img_shm.close() if WRIST: From e70c1afecf754445f8433fc40c5eeba7e23ea12b Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 10 Jun 2025 20:12:46 +0800 Subject: [PATCH 12/32] [fix] type bug. --- teleop/open_television/_test_television.py | 4 ++-- teleop/open_television/_test_tv_wrapper.py | 26 +++++++++++++--------- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/teleop/open_television/_test_television.py b/teleop/open_television/_test_television.py index ccb2005..c1ba588 100644 --- a/teleop/open_television/_test_television.py +++ b/teleop/open_television/_test_television.py @@ -75,11 +75,11 @@ def run_test_television(): time.sleep(0.03) except KeyboardInterrupt: running = False - logger_mp.info("KeyboardInterrupt, exiting program...") + logger_mp.warning("KeyboardInterrupt, exiting program...") finally: image_shm.unlink() image_shm.close() - logger_mp.info("Finally, exiting program...") + logger_mp.warning("Finally, exiting program...") exit(0) if __name__ == '__main__': diff --git a/teleop/open_television/_test_tv_wrapper.py b/teleop/open_television/_test_tv_wrapper.py index 47163b0..66ef23c 100644 --- a/teleop/open_television/_test_tv_wrapper.py +++ b/teleop/open_television/_test_tv_wrapper.py @@ -35,21 +35,24 @@ def run_test_tv_wrapper(): teleData = tv_wrapper.get_motion_state_data() logger_mp.info("=== TeleData Snapshot ===") - logger_mp.info("[Head Rotation Matrix]:\n", teleData.head_rotation) - logger_mp.info("[Left EE Pose]:\n", teleData.left_arm_pose) - logger_mp.info("[Right EE Pose]:\n", teleData.right_arm_pose) + logger_mp.info(f"[Head Rotation Matrix]:\n{teleData.head_rotation}") + logger_mp.info(f"[Left EE Pose]:\n{teleData.left_arm_pose}") + logger_mp.info(f"[Right EE Pose]:\n{teleData.right_arm_pose}") if use_hand_track: - logger_mp.info("[Left Hand Position] shape {}:\n{}".format(teleData.left_hand_pos.shape, teleData.left_hand_pos)) - logger_mp.info("[Right Hand Position] shape {}:\n{}".format(teleData.right_hand_pos.shape, teleData.right_hand_pos)) + logger_mp.info(f"[Left Hand Position] shape {teleData.left_hand_pos.shape}:\n{teleData.left_hand_pos}") + logger_mp.info(f"[Right Hand Position] shape {teleData.right_hand_pos.shape}:\n{teleData.right_hand_pos}") + if teleData.left_hand_rot is not None: - logger_mp.info("[Left Hand Rotation] shape {}:\n{}".format(teleData.left_hand_rot.shape, teleData.left_hand_rot)) + logger_mp.info(f"[Left Hand Rotation] shape {teleData.left_hand_rot.shape}:\n{teleData.left_hand_rot}") if teleData.right_hand_rot is not None: - logger_mp.info("[Right Hand Rotation] shape {}:\n{}".format(teleData.right_hand_rot.shape, teleData.right_hand_rot)) + logger_mp.info(f"[Right Hand Rotation] shape {teleData.right_hand_rot.shape}:\n{teleData.right_hand_rot}") + if teleData.left_pinch_value is not None: - logger_mp.info("[Left Pinch Value]: {:.2f}".format(teleData.left_pinch_value)) + logger_mp.info(f"[Left Pinch Value]: {teleData.left_pinch_value:.2f}") if teleData.right_pinch_value is not None: - logger_mp.info("[Right Pinch Value]: {:.2f}".format(teleData.right_pinch_value)) + logger_mp.info(f"[Right Pinch Value]: {teleData.right_pinch_value:.2f}") + if teleData.tele_state: state = teleData.tele_state logger_mp.info("[Hand State]:") @@ -60,6 +63,7 @@ def run_test_tv_wrapper(): else: logger_mp.info(f"[Left Trigger Value]: {teleData.left_trigger_value:.2f}") logger_mp.info(f"[Right Trigger Value]: {teleData.right_trigger_value:.2f}") + if teleData.tele_state: state = teleData.tele_state logger_mp.info("[Controller State]:") @@ -80,11 +84,11 @@ def run_test_tv_wrapper(): except KeyboardInterrupt: running = False - logger_mp.info("KeyboardInterrupt, exiting program...") + logger_mp.warning("KeyboardInterrupt, exiting program...") finally: image_shm.unlink() image_shm.close() - logger_mp.info("Finally, exiting program...") + logger_mp.warning("Finally, exiting program...") exit(0) if __name__ == '__main__': From 6cc44d31926d4de248e4df966d3f59455024594d Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 17 Jun 2025 10:52:34 +0800 Subject: [PATCH 13/32] [update] headless mode (use sshkeyboard lib) for ssh or no screen environment. --- teleop/teleop_hand_and_arm.py | 47 +++++++++++++++++++++-------------- 1 file changed, 29 insertions(+), 18 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index fb02213..033f498 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -21,24 +21,32 @@ from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_C from teleop.robot_control.robot_hand_inspire import Inspire_Controller from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter +from sshkeyboard import listen_keyboard +running = True +recording = False +def on_press(key): + global running, recording + if key == 'q': + running = False + elif key == 's': + recording = not recording + logger_mp.info(f"recording : {recording}") +threading.Thread(target=listen_keyboard, kwargs={"on_press": on_press}, daemon=True).start() + if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--task_dir', type = str, default = './utils/data', help = 'path to save data') parser.add_argument('--frequency', type = float, default = 90.0, help = 'save data\'s frequency') - parser.add_argument('--record', action = 'store_true', help = 'Save data or not') - parser.add_argument('--no-record', dest = 'record', action = 'store_false', help = 'Do not save data') - parser.set_defaults(record = False) - parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') - parser.add_argument('--debug', action = 'store_true', help = 'debug mode') - parser.add_argument('--no-debug', dest = 'debug', action = 'store_false', help = 'motion mode') - parser.set_defaults(debug = True) + parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') + parser.add_argument('--debug', action = 'store_true', help = 'Enable debug mode') + parser.add_argument('--headless', action='store_true', help='Run in headless mode (no display)') args = parser.parse_args() logger_mp.info(f"args: {args}") @@ -135,26 +143,29 @@ if __name__ == '__main__': sport_client.SetTimeout(0.0001) sport_client.Init() - if args.record: + if args.record and args.headless: + recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = False) + elif args.record and not args.headless: recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = True) - recording = False try: user_input = input("Please enter the start signal (enter 'r' to start the subsequent program):\n") if user_input.lower() == 'r': arm_ctrl.speed_gradual_max() - running = True while running: start_time = time.time() - # opencv image - tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) - cv2.imshow("record image", tv_resized_image) - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - running = False - elif key == ord('s') and args.record: - recording = not recording # state flipping + if not args.headless: + tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) + cv2.imshow("record image", tv_resized_image) + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + running = False + elif key == ord('s'): + recording = not recording # state flipping + logger_mp.info(f"recording : {recording}") + + if args.record: if recording: if not recorder.create_episode(): recording = False From 58dbbf8cebb2ec68e37cbd3f5cd1f5888da8b917 Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 17 Jun 2025 11:31:36 +0800 Subject: [PATCH 14/32] [fix] log --- teleop/image_server/image_client.py | 2 +- teleop/robot_control/robot_arm.py | 4 + teleop/robot_control/robot_hand_inspire.py | 1 + teleop/robot_control/robot_hand_unitree.py | 2 + teleop/teleop_hand_and_arm.py | 377 +++++++++++---------- 5 files changed, 199 insertions(+), 187 deletions(-) diff --git a/teleop/image_server/image_client.py b/teleop/image_server/image_client.py index 1ff75dd..9c42581 100644 --- a/teleop/image_server/image_client.py +++ b/teleop/image_server/image_client.py @@ -131,7 +131,7 @@ class ImageClient: self._socket.connect(f"tcp://{self._server_address}:{self._port}") self._socket.setsockopt_string(zmq.SUBSCRIBE, "") - logger_mp.info("\nImage client has started, waiting to receive data...") + logger_mp.info("Image client has started, waiting to receive data...") try: while self.running: # Receive message diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index 1b1a753..d9f3f35 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -99,6 +99,7 @@ class G1_29_ArmController: while not self.lowstate_buffer.GetData(): time.sleep(0.01) logger_mp.warning("[G1_29_ArmController] Waiting to subscribe dds...") + logger_mp.info("[G1_29_ArmController] Subscribe dds ok.") # initialize hg's lowcmd msg self.crc = CRC() @@ -370,6 +371,7 @@ class G1_23_ArmController: while not self.lowstate_buffer.GetData(): time.sleep(0.01) logger_mp.warning("[G1_23_ArmController] Waiting to subscribe dds...") + logger_mp.info("[G1_23_ArmController] Subscribe dds ok.") # initialize hg's lowcmd msg self.crc = CRC() @@ -626,6 +628,7 @@ class H1_2_ArmController: while not self.lowstate_buffer.GetData(): time.sleep(0.01) logger_mp.warning("[H1_2_ArmController] Waiting to subscribe dds...") + logger_mp.info("[H1_2_ArmController] Subscribe dds ok.") # initialize hg's lowcmd msg self.crc = CRC() @@ -887,6 +890,7 @@ class H1_ArmController: while not self.lowstate_buffer.GetData(): time.sleep(0.01) logger_mp.warning("[H1_ArmController] Waiting to subscribe dds...") + logger_mp.info("[H1_ArmController] Subscribe dds ok.") # initialize h1's lowcmd msg self.crc = CRC() diff --git a/teleop/robot_control/robot_hand_inspire.py b/teleop/robot_control/robot_hand_inspire.py index 562402e..47a1e55 100644 --- a/teleop/robot_control/robot_hand_inspire.py +++ b/teleop/robot_control/robot_hand_inspire.py @@ -51,6 +51,7 @@ class Inspire_Controller: break time.sleep(0.01) logger_mp.warning("[Inspire_Controller] Waiting to subscribe dds...") + logger_mp.info("[Inspire_Controller] Subscribe dds ok.") hand_control_process = Process(target=self.control_process, args=(left_hand_array, right_hand_array, self.left_hand_state_array, self.right_hand_state_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)) diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index dbd6935..a7f575f 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -87,6 +87,7 @@ class Dex3_1_Controller: break time.sleep(0.01) logger_mp.warning("[Dex3_1_Controller] Waiting to subscribe dds...") + logger_mp.info("[Dex3_1_Controller] Subscribe dds ok.") hand_control_process = Process(target=self.control_process, args=(left_hand_array_in, right_hand_array_in, self.left_hand_state_array, self.right_hand_state_array, dual_hand_data_lock, dual_hand_state_array_out, dual_hand_action_array_out)) @@ -285,6 +286,7 @@ class Gripper_Controller: break time.sleep(0.01) logger_mp.warning("[Gripper_Controller] Waiting to subscribe dds...") + logger_mp.info("[Gripper_Controller] Subscribe dds ok.") self.gripper_control_thread = threading.Thread(target=self.control_thread, args=(left_gripper_value_in, right_gripper_value_in, self.dual_gripper_state, dual_gripper_data_lock, dual_gripper_state_out, dual_gripper_action_out)) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 033f498..2c773f1 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -24,11 +24,15 @@ from teleop.utils.episode_writer import EpisodeWriter from sshkeyboard import listen_keyboard +start_signal = False running = True recording = False def on_press(key): - global running, recording - if key == 'q': + global running, recording, start_signal + if key == 'r': + start_signal = True + logger_mp.info("[Key] start signal received.") + elif key == 'q': running = False elif key == 's': recording = not recording @@ -100,7 +104,6 @@ if __name__ == '__main__': if args.arm == 'G1_29': arm_ctrl = G1_29_ArmController(debug_mode=args.debug) arm_ik = G1_29_ArmIK() - pass elif args.arm == 'G1_23': arm_ctrl = G1_23_ArmController() arm_ik = G1_23_ArmIK() @@ -149,198 +152,200 @@ if __name__ == '__main__': recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = True) try: - user_input = input("Please enter the start signal (enter 'r' to start the subsequent program):\n") - if user_input.lower() == 'r': - arm_ctrl.speed_gradual_max() - while running: - start_time = time.time() - - if not args.headless: - tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) - cv2.imshow("record image", tv_resized_image) - key = cv2.waitKey(1) & 0xFF - if key == ord('q'): - running = False - elif key == ord('s'): - recording = not recording # state flipping - logger_mp.info(f"recording : {recording}") + logger_mp.info("Please enter the start signal (enter 'r' to start the subsequent program)") + while not start_signal: + time.sleep(0.01) + logger_mp.info("Program start.") + arm_ctrl.speed_gradual_max() + while running: + start_time = time.time() - if args.record: - if recording: - if not recorder.create_episode(): - recording = False - else: - recorder.save_episode() + if not args.headless: + tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) + cv2.imshow("record image", tv_resized_image) + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + running = False + elif key == ord('s'): + recording = not recording # state flipping + logger_mp.info(f"recording : {recording}") - # get input data - tele_data = tv_wrapper.get_motion_state_data() - if (args.ee == 'dex3' or args.ee == 'inspire1') and args.xr_mode == 'hand': - with left_hand_pos_array.get_lock(): - left_hand_pos_array[:] = tele_data.left_hand_pos.flatten() - with right_hand_pos_array.get_lock(): - right_hand_pos_array[:] = tele_data.right_hand_pos.flatten() - elif args.ee == 'gripper' and args.xr_mode == 'controller': - with left_gripper_value.get_lock(): - left_gripper_value.value = tele_data.left_trigger_value - with right_gripper_value.get_lock(): - right_gripper_value.value = tele_data.right_trigger_value - elif args.ee == 'gripper' and args.xr_mode == 'hand': - with left_gripper_value.get_lock(): - left_gripper_value.value = tele_data.left_pinch_value - with right_gripper_value.get_lock(): - right_gripper_value.value = tele_data.right_pinch_value + if args.record: + if recording: + if not recorder.create_episode(): + recording = False else: - pass - - # high level control - if args.xr_mode == 'controller' and not args.debug: - # quit teleoperate - if tele_data.tele_state.right_aButton: - running = False - # command robot to enter damping mode. soft emergency stop function - if tele_data.tele_state.left_thumbstick_state and tele_data.tele_state.right_thumbstick_state: - sport_client.Damp() - # control, limit velocity to within 0.3 - sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, - -tele_data.tele_state.left_thumbstick_value[0] * 0.3, - -tele_data.tele_state.right_thumbstick_value[0] * 0.3) + recorder.save_episode() - # get current robot state data. - current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() - current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() + # get input data + tele_data = tv_wrapper.get_motion_state_data() + if (args.ee == 'dex3' or args.ee == 'inspire1') and args.xr_mode == 'hand': + with left_hand_pos_array.get_lock(): + left_hand_pos_array[:] = tele_data.left_hand_pos.flatten() + with right_hand_pos_array.get_lock(): + right_hand_pos_array[:] = tele_data.right_hand_pos.flatten() + elif args.ee == 'gripper' and args.xr_mode == 'controller': + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_trigger_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_trigger_value + elif args.ee == 'gripper' and args.xr_mode == 'hand': + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_pinch_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_pinch_value + else: + pass + + # high level control + if args.xr_mode == 'controller' and not args.debug: + # quit teleoperate + if tele_data.tele_state.right_aButton: + running = False + # command robot to enter damping mode. soft emergency stop function + if tele_data.tele_state.left_thumbstick_state and tele_data.tele_state.right_thumbstick_state: + sport_client.Damp() + # control, limit velocity to within 0.3 + sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3) - # solve ik using motor data and wrist pose, then use ik results to control arms. - time_ik_start = time.time() - sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) - time_ik_end = time.time() - logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") - arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) + # get current robot state data. + current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() + current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() - # record data - if args.record: - # dex hand or gripper - if args.ee == "dex3" and args.xr_mode == 'hand': - with dual_hand_data_lock: - left_ee_state = dual_hand_state_array[:7] - right_ee_state = dual_hand_state_array[-7:] - left_hand_action = dual_hand_action_array[:7] - right_hand_action = dual_hand_action_array[-7:] - current_body_state = [] - current_body_action = [] - elif args.ee == "gripper" and args.xr_mode == 'hand': - with dual_gripper_data_lock: - left_ee_state = [dual_gripper_state_array[1]] - right_ee_state = [dual_gripper_state_array[0]] - left_hand_action = [dual_gripper_action_array[1]] - right_hand_action = [dual_gripper_action_array[0]] - current_body_state = [] - current_body_action = [] - elif args.ee == "gripper" and args.xr_mode == 'controller': - with dual_gripper_data_lock: - left_ee_state = [dual_gripper_state_array[1]] - right_ee_state = [dual_gripper_state_array[0]] - left_hand_action = [dual_gripper_action_array[1]] - right_hand_action = [dual_gripper_action_array[0]] - current_body_state = arm_ctrl.get_current_motor_q().tolist() - current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3, - -tele_data.tele_state.left_thumbstick_value[0] * 0.3, - -tele_data.tele_state.right_thumbstick_value[0] * 0.3] - elif args.ee == "inspire1" and args.xr_mode == 'hand': - with dual_hand_data_lock: - left_ee_state = dual_hand_state_array[:6] - right_ee_state = dual_hand_state_array[-6:] - left_hand_action = dual_hand_action_array[:6] - right_hand_action = dual_hand_action_array[-6:] - current_body_state = [] - current_body_action = [] - else: - left_ee_state = [] - right_ee_state = [] - left_hand_action = [] - right_hand_action = [] + # solve ik using motor data and wrist pose, then use ik results to control arms. + time_ik_start = time.time() + sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_arm_pose, tele_data.right_arm_pose, current_lr_arm_q, current_lr_arm_dq) + time_ik_end = time.time() + logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") + arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) + + # record data + if args.record: + # dex hand or gripper + if args.ee == "dex3" and args.xr_mode == 'hand': + with dual_hand_data_lock: + left_ee_state = dual_hand_state_array[:7] + right_ee_state = dual_hand_state_array[-7:] + left_hand_action = dual_hand_action_array[:7] + right_hand_action = dual_hand_action_array[-7:] current_body_state = [] current_body_action = [] - # head image - current_tv_image = tv_img_array.copy() - # wrist image - if WRIST: - current_wrist_image = wrist_img_array.copy() - # arm state and action - left_arm_state = current_lr_arm_q[:7] - right_arm_state = current_lr_arm_q[-7:] - left_arm_action = sol_q[:7] - right_arm_action = sol_q[-7:] - if recording: - colors = {} - depths = {} - if BINOCULAR: - colors[f"color_{0}"] = current_tv_image[:, :tv_img_shape[1]//2] - colors[f"color_{1}"] = current_tv_image[:, tv_img_shape[1]//2:] - if WRIST: - colors[f"color_{2}"] = current_wrist_image[:, :wrist_img_shape[1]//2] - colors[f"color_{3}"] = current_wrist_image[:, wrist_img_shape[1]//2:] - else: - colors[f"color_{0}"] = current_tv_image - if WRIST: - colors[f"color_{1}"] = current_wrist_image[:, :wrist_img_shape[1]//2] - colors[f"color_{2}"] = current_wrist_image[:, wrist_img_shape[1]//2:] - states = { - "left_arm": { - "qpos": left_arm_state.tolist(), # numpy.array -> list - "qvel": [], - "torque": [], - }, - "right_arm": { - "qpos": right_arm_state.tolist(), - "qvel": [], - "torque": [], - }, - "left_ee": { - "qpos": left_ee_state, - "qvel": [], - "torque": [], - }, - "right_ee": { - "qpos": right_ee_state, - "qvel": [], - "torque": [], - }, - "body": { - "qpos": current_body_state, - }, - } - actions = { - "left_arm": { - "qpos": left_arm_action.tolist(), - "qvel": [], - "torque": [], - }, - "right_arm": { - "qpos": right_arm_action.tolist(), - "qvel": [], - "torque": [], - }, - "left_ee": { - "qpos": left_hand_action, - "qvel": [], - "torque": [], - }, - "right_ee": { - "qpos": right_hand_action, - "qvel": [], - "torque": [], - }, - "body": { - "qpos": current_body_action, - }, - } - recorder.add_item(colors=colors, depths=depths, states=states, actions=actions) + elif args.ee == "gripper" and args.xr_mode == 'hand': + with dual_gripper_data_lock: + left_ee_state = [dual_gripper_state_array[1]] + right_ee_state = [dual_gripper_state_array[0]] + left_hand_action = [dual_gripper_action_array[1]] + right_hand_action = [dual_gripper_action_array[0]] + current_body_state = [] + current_body_action = [] + elif args.ee == "gripper" and args.xr_mode == 'controller': + with dual_gripper_data_lock: + left_ee_state = [dual_gripper_state_array[1]] + right_ee_state = [dual_gripper_state_array[0]] + left_hand_action = [dual_gripper_action_array[1]] + right_hand_action = [dual_gripper_action_array[0]] + current_body_state = arm_ctrl.get_current_motor_q().tolist() + current_body_action = [-tele_data.tele_state.left_thumbstick_value[1] * 0.3, + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3] + elif args.ee == "inspire1" and args.xr_mode == 'hand': + with dual_hand_data_lock: + left_ee_state = dual_hand_state_array[:6] + right_ee_state = dual_hand_state_array[-6:] + left_hand_action = dual_hand_action_array[:6] + right_hand_action = dual_hand_action_array[-6:] + current_body_state = [] + current_body_action = [] + else: + left_ee_state = [] + right_ee_state = [] + left_hand_action = [] + right_hand_action = [] + current_body_state = [] + current_body_action = [] + # head image + current_tv_image = tv_img_array.copy() + # wrist image + if WRIST: + current_wrist_image = wrist_img_array.copy() + # arm state and action + left_arm_state = current_lr_arm_q[:7] + right_arm_state = current_lr_arm_q[-7:] + left_arm_action = sol_q[:7] + right_arm_action = sol_q[-7:] + if recording: + colors = {} + depths = {} + if BINOCULAR: + colors[f"color_{0}"] = current_tv_image[:, :tv_img_shape[1]//2] + colors[f"color_{1}"] = current_tv_image[:, tv_img_shape[1]//2:] + if WRIST: + colors[f"color_{2}"] = current_wrist_image[:, :wrist_img_shape[1]//2] + colors[f"color_{3}"] = current_wrist_image[:, wrist_img_shape[1]//2:] + else: + colors[f"color_{0}"] = current_tv_image + if WRIST: + colors[f"color_{1}"] = current_wrist_image[:, :wrist_img_shape[1]//2] + colors[f"color_{2}"] = current_wrist_image[:, wrist_img_shape[1]//2:] + states = { + "left_arm": { + "qpos": left_arm_state.tolist(), # numpy.array -> list + "qvel": [], + "torque": [], + }, + "right_arm": { + "qpos": right_arm_state.tolist(), + "qvel": [], + "torque": [], + }, + "left_ee": { + "qpos": left_ee_state, + "qvel": [], + "torque": [], + }, + "right_ee": { + "qpos": right_ee_state, + "qvel": [], + "torque": [], + }, + "body": { + "qpos": current_body_state, + }, + } + actions = { + "left_arm": { + "qpos": left_arm_action.tolist(), + "qvel": [], + "torque": [], + }, + "right_arm": { + "qpos": right_arm_action.tolist(), + "qvel": [], + "torque": [], + }, + "left_ee": { + "qpos": left_hand_action, + "qvel": [], + "torque": [], + }, + "right_ee": { + "qpos": right_hand_action, + "qvel": [], + "torque": [], + }, + "body": { + "qpos": current_body_action, + }, + } + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions) - current_time = time.time() - time_elapsed = current_time - start_time - sleep_time = max(0, (1 / args.frequency) - time_elapsed) - time.sleep(sleep_time) - logger_mp.debug(f"main process sleep: {sleep_time}") + current_time = time.time() + time_elapsed = current_time - start_time + sleep_time = max(0, (1 / args.frequency) - time_elapsed) + time.sleep(sleep_time) + logger_mp.debug(f"main process sleep: {sleep_time}") except KeyboardInterrupt: logger_mp.info("KeyboardInterrupt, exiting program...") From 5b6f852335f96a4241c2c9c95e84f49f18313521 Mon Sep 17 00:00:00 2001 From: silencht Date: Wed, 18 Jun 2025 12:03:44 +0800 Subject: [PATCH 15/32] [fix] bug. --- teleop/teleop_hand_and_arm.py | 34 +++++++++++++++++++--------------- teleop/utils/episode_writer.py | 4 ++-- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 2c773f1..86a9479 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -21,22 +21,23 @@ from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_C from teleop.robot_control.robot_hand_inspire import Inspire_Controller from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter -from sshkeyboard import listen_keyboard +from sshkeyboard import listen_keyboard, stop_listening start_signal = False running = True -recording = False +should_toggle_recording = False +is_recording = False def on_press(key): - global running, recording, start_signal + global running, start_signal, should_toggle_recording if key == 'r': start_signal = True - logger_mp.info("[Key] start signal received.") + logger_mp.info("Program start signal received.") elif key == 'q': + stop_listening() running = False elif key == 's': - recording = not recording - logger_mp.info(f"recording : {recording}") + should_toggle_recording = True threading.Thread(target=listen_keyboard, kwargs={"on_press": on_press}, daemon=True).start() if __name__ == '__main__': @@ -49,7 +50,7 @@ if __name__ == '__main__': parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') - parser.add_argument('--debug', action = 'store_true', help = 'Enable debug mode') + parser.add_argument('--debug', action = 'store_false', help = 'Enable debug mode') parser.add_argument('--headless', action='store_true', help='Run in headless mode (no display)') args = parser.parse_args() @@ -155,7 +156,6 @@ if __name__ == '__main__': logger_mp.info("Please enter the start signal (enter 'r' to start the subsequent program)") while not start_signal: time.sleep(0.01) - logger_mp.info("Program start.") arm_ctrl.speed_gradual_max() while running: start_time = time.time() @@ -165,16 +165,20 @@ if __name__ == '__main__': cv2.imshow("record image", tv_resized_image) key = cv2.waitKey(1) & 0xFF if key == ord('q'): + stop_listening() running = False elif key == ord('s'): - recording = not recording # state flipping - logger_mp.info(f"recording : {recording}") + should_toggle_recording = True - if args.record: - if recording: - if not recorder.create_episode(): - recording = False + if args.record and should_toggle_recording: + should_toggle_recording = False + if not is_recording: + if recorder.create_episode(): + is_recording = True + else: + logger_mp.error("Failed to create episode. Recording not started.") else: + is_recording = False recorder.save_episode() # get input data @@ -275,7 +279,7 @@ if __name__ == '__main__': right_arm_state = current_lr_arm_q[-7:] left_arm_action = sol_q[:7] right_arm_action = sol_q[-7:] - if recording: + if is_recording: colors = {} depths = {} if BINOCULAR: diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index 8f255f6..5cc4830 100644 --- a/teleop/utils/episode_writer.py +++ b/teleop/utils/episode_writer.py @@ -43,7 +43,7 @@ class EpisodeWriter(): self.is_available = True # Indicates whether the class is available for new operations # Initialize the queue and worker thread - self.item_data_queue = Queue(maxsize=100) + self.item_data_queue = Queue(-1) self.stop_worker = False self.need_save = False # Flag to indicate when save_episode is triggered self.worker_thread = Thread(target=self.process_queue) @@ -181,7 +181,7 @@ class EpisodeWriter(): # Log data if necessary if self.rerun_log: curent_record_time = time.time() - logger_mp.info(f"==> episode_id:{self.episode_id} item_id:{self.item_id} current_time:{curent_record_time}") + logger_mp.info(f"==> episode_id:{self.episode_id} item_id:{idx} current_time:{curent_record_time}") self.rerun_logger.log_item_data(item_data) def save_episode(self): From ae1cf8a1750a87302e1d417dbb4f35d022e96996 Mon Sep 17 00:00:00 2001 From: "wei.li" Date: Mon, 23 Jun 2025 20:12:17 +0800 Subject: [PATCH 16/32] [update] Enable unitree_sim_isaaclab simulation environment support --- teleop/robot_control/robot_arm.py | 36 ++++++++++++---- teleop/robot_control/robot_hand_unitree.py | 10 +++-- teleop/teleop_hand_and_arm.py | 48 ++++++++++++++-------- 3 files changed, 66 insertions(+), 28 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index d9f3f35..cdd24c1 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -59,12 +59,12 @@ class DataBuffer: self.data = data class G1_29_ArmController: - def __init__(self, debug_mode = True): + def __init__(self, debug_mode = True, is_use_sim = False): logger_mp.info("Initialize G1_29_ArmController...") self.q_target = np.zeros(14) self.tauff_target = np.zeros(14) self.debug_mode = debug_mode - + self.is_use_sim = is_use_sim self.kp_high = 300.0 self.kd_high = 3.0 self.kp_low = 80.0 @@ -169,7 +169,10 @@ class G1_29_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + if self.is_use_sim: + cliped_arm_q_target = arm_q_target + else: + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) for idx, id in enumerate(G1_29_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] @@ -335,7 +338,9 @@ class G1_29_JointIndex(IntEnum): kNotUsedJoint5 = 34 class G1_23_ArmController: - def __init__(self): + def __init__(self, is_use_sim = False): + self.is_use_sim = is_use_sim + logger_mp.info("Initialize G1_23_ArmController...") self.q_target = np.zeros(10) self.tauff_target = np.zeros(10) @@ -438,7 +443,10 @@ class G1_23_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + if self.is_use_sim: + cliped_arm_q_target = arm_q_target + else: + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) for idx, id in enumerate(G1_23_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] @@ -592,7 +600,9 @@ class G1_23_JointIndex(IntEnum): kNotUsedJoint5 = 34 class H1_2_ArmController: - def __init__(self): + def __init__(self, is_use_sim = False): + self.is_use_sim = is_use_sim + logger_mp.info("Initialize H1_2_ArmController...") self.q_target = np.zeros(14) self.tauff_target = np.zeros(14) @@ -695,7 +705,10 @@ class H1_2_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + if self.is_use_sim: + cliped_arm_q_target = arm_q_target + else: + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) for idx, id in enumerate(H1_2_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] @@ -856,7 +869,9 @@ class H1_2_JointIndex(IntEnum): kNotUsedJoint7 = 34 class H1_ArmController: - def __init__(self): + def __init__(self, is_use_sim = False): + self.is_use_sim = is_use_sim + logger_mp.info("Initialize H1_ArmController...") self.q_target = np.zeros(8) self.tauff_target = np.zeros(8) @@ -951,7 +966,10 @@ class H1_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + if self.is_use_sim: + cliped_arm_q_target = arm_q_target + else: + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) for idx, id in enumerate(H1_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index a7f575f..909801d 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -236,7 +236,7 @@ kTopicGripperState = "rt/unitree_actuator/state" class Gripper_Controller: def __init__(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None, - filter = True, fps = 200.0, Unit_Test = False): + filter = True, fps = 200.0, Unit_Test = False, is_use_sim = False): """ [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process @@ -259,6 +259,8 @@ class Gripper_Controller: self.fps = fps self.Unit_Test = Unit_Test + self.is_use_sim = is_use_sim + if filter: self.smooth_filter = WeightedMovingFilter(np.array([0.5, 0.3, 0.2]), Gripper_Num_Motors) else: @@ -314,8 +316,10 @@ class Gripper_Controller: def control_thread(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_state_in, dual_hand_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None): self.running = True - - DELTA_GRIPPER_CMD = 0.18 # The motor rotates 5.4 radians, the clamping jaw slide open 9 cm, so 0.6 rad <==> 1 cm, 0.18 rad <==> 3 mm + if self.is_use_sim: + DELTA_GRIPPER_CMD = 1.0 + else: + DELTA_GRIPPER_CMD = 0.18 # The motor rotates 5.4 radians, the clamping jaw slide open 9 cm, so 0.6 rad <==> 1 cm, 0.18 rad <==> 3 mm THUMB_INDEX_DISTANCE_MIN = 5.0 THUMB_INDEX_DISTANCE_MAX = 7.0 LEFT_MAPPED_MIN = 0.0 # The minimum initial motor position when the gripper closes at startup. diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 86a9479..461b9f9 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -53,19 +53,35 @@ if __name__ == '__main__': parser.add_argument('--debug', action = 'store_false', help = 'Enable debug mode') parser.add_argument('--headless', action='store_true', help='Run in headless mode (no display)') + parser.add_argument('--is_use_sim', action = 'store_true', help = 'Use simulation or not') + parser.set_defaults(is_use_sim = False) + args = parser.parse_args() logger_mp.info(f"args: {args}") # image client: img_config should be the same as the configuration in image_server.py (of Robot's development computing unit) - img_config = { - 'fps': 30, - 'head_camera_type': 'opencv', - 'head_camera_image_shape': [480, 1280], # Head camera resolution - 'head_camera_id_numbers': [0], - 'wrist_camera_type': 'opencv', - 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution - 'wrist_camera_id_numbers': [2, 4], - } + if args.is_use_sim: + img_config = { + 'fps': 30, + 'head_camera_type': 'opencv', + 'head_camera_image_shape': [480, 640], # Head camera resolution + 'head_camera_id_numbers': [0], + 'wrist_camera_type': 'opencv', + 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + 'wrist_camera_id_numbers': [2, 4], + } + else: + img_config = { + 'fps': 30, + 'head_camera_type': 'opencv', + 'head_camera_image_shape': [480, 1280], # Head camera resolution + 'head_camera_id_numbers': [0], + 'wrist_camera_type': 'opencv', + 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + 'wrist_camera_id_numbers': [2, 4], + } + + ASPECT_RATIO_THRESHOLD = 2.0 # If the aspect ratio exceeds this value, it is considered binocular if len(img_config['head_camera_id_numbers']) > 1 or (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][0] > ASPECT_RATIO_THRESHOLD): BINOCULAR = True @@ -89,9 +105,9 @@ if __name__ == '__main__': wrist_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(wrist_img_shape) * np.uint8().itemsize) wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = wrist_img_shm.buf) img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, - wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name) + wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name,server_address="127.0.0.1") else: - img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name) + img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, server_address="127.0.0.1") image_receive_thread = threading.Thread(target = img_client.receive_process, daemon = True) image_receive_thread.daemon = True @@ -103,16 +119,16 @@ if __name__ == '__main__': # arm if args.arm == 'G1_29': - arm_ctrl = G1_29_ArmController(debug_mode=args.debug) + arm_ctrl = G1_29_ArmController(debug_mode=args.debug, is_use_sim=args.is_use_sim) arm_ik = G1_29_ArmIK() elif args.arm == 'G1_23': - arm_ctrl = G1_23_ArmController() + arm_ctrl = G1_23_ArmController(is_use_sim=args.is_use_sim) arm_ik = G1_23_ArmIK() elif args.arm == 'H1_2': - arm_ctrl = H1_2_ArmController() + arm_ctrl = H1_2_ArmController(is_use_sim=args.is_use_sim) arm_ik = H1_2_ArmIK() elif args.arm == 'H1': - arm_ctrl = H1_ArmController() + arm_ctrl = H1_ArmController(is_use_sim=args.is_use_sim) arm_ik = H1_ArmIK() # end-effector @@ -129,7 +145,7 @@ if __name__ == '__main__': dual_gripper_data_lock = Lock() dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. - gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array) + gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, is_use_sim=args.is_use_sim) elif args.ee == "inspire1": left_hand_pos_array = Array('d', 75, lock = True) # [input] right_hand_pos_array = Array('d', 75, lock = True) # [input] From 0ad5732966e8c40efd142caf0aa3c35ee9aa6a48 Mon Sep 17 00:00:00 2001 From: "wei.li" Date: Mon, 23 Jun 2025 20:55:38 +0800 Subject: [PATCH 17/32] [update] Add DDS communication support for simulation object position resetting --- teleop/teleop_hand_and_arm.py | 30 ++++++++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 461b9f9..e6449f0 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -21,8 +21,22 @@ from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_C from teleop.robot_control.robot_hand_inspire import Inspire_Controller from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ + + from sshkeyboard import listen_keyboard, stop_listening +def publish_reset_category(category: int,publisher): + # construct message + msg = String_(data=str(category)) # pass data parameter directly during initialization + + # create publisher + + # publish message + publisher.Write(msg) + print(f"published reset category: {category}") start_signal = False running = True @@ -47,14 +61,14 @@ if __name__ == '__main__': parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') - parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') + parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'],default='gripper', help='Select end effector controller') parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') parser.add_argument('--debug', action = 'store_false', help = 'Enable debug mode') parser.add_argument('--headless', action='store_true', help='Run in headless mode (no display)') parser.add_argument('--is_use_sim', action = 'store_true', help = 'Use simulation or not') - parser.set_defaults(is_use_sim = False) + parser.set_defaults(is_use_sim = True) args = parser.parse_args() logger_mp.info(f"args: {args}") @@ -155,7 +169,9 @@ if __name__ == '__main__': hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) else: pass - + if args.is_use_sim: + reset_pose_publisher = ChannelPublisher("rt/reset_pose/cmd", String_) + reset_pose_publisher.Init() # xr mode if args.xr_mode == 'controller' and not args.debug: from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient @@ -183,8 +199,13 @@ if __name__ == '__main__': if key == ord('q'): stop_listening() running = False + if args.is_use_sim: + publish_reset_category(2, reset_pose_publisher) elif key == ord('s'): should_toggle_recording = True + elif key == ord('a'): + if args.is_use_sim: + publish_reset_category(2, reset_pose_publisher) if args.record and should_toggle_recording: should_toggle_recording = False @@ -196,7 +217,8 @@ if __name__ == '__main__': else: is_recording = False recorder.save_episode() - + if args.is_use_sim: + publish_reset_category(1, reset_pose_publisher) # get input data tele_data = tv_wrapper.get_motion_state_data() if (args.ee == 'dex3' or args.ee == 'inspire1') and args.xr_mode == 'hand': From 742460a7d5d4eb37894cb2026e73161e758cee45 Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 26 Jun 2025 11:41:48 +0800 Subject: [PATCH 18/32] [fix] minor issues --- teleop/robot_control/robot_arm.py | 36 +++++++------- teleop/robot_control/robot_hand_unitree.py | 6 +-- teleop/teleop_hand_and_arm.py | 55 ++++++++++------------ 3 files changed, 46 insertions(+), 51 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index cdd24c1..49ac742 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -59,12 +59,12 @@ class DataBuffer: self.data = data class G1_29_ArmController: - def __init__(self, debug_mode = True, is_use_sim = False): + def __init__(self, motion_mode = False, simulation_mode = False): logger_mp.info("Initialize G1_29_ArmController...") self.q_target = np.zeros(14) self.tauff_target = np.zeros(14) - self.debug_mode = debug_mode - self.is_use_sim = is_use_sim + self.motion_mode = motion_mode + self.simulation_mode = simulation_mode self.kp_high = 300.0 self.kd_high = 3.0 self.kp_low = 80.0 @@ -82,10 +82,10 @@ class G1_29_ArmController: # initialize lowcmd publisher and lowstate subscriber ChannelFactoryInitialize(0) - if self.debug_mode: - self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) - else: + if self.motion_mode: self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Motion, hg_LowCmd) + else: + self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) self.lowcmd_publisher.Init() self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_LowState) self.lowstate_subscriber.Init() @@ -159,7 +159,7 @@ class G1_29_ArmController: return cliped_arm_q_target def _ctrl_motor_state(self): - if not self.debug_mode: + if self.motion_mode: self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0; while True: @@ -169,7 +169,7 @@ class G1_29_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - if self.is_use_sim: + if self.simulation_mode: cliped_arm_q_target = arm_q_target else: cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) @@ -225,7 +225,7 @@ class G1_29_ArmController: while True: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): - if not self.debug_mode: + if self.motion_mode: for weight in np.arange(1, 0, -0.01): self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; time.sleep(0.02) @@ -338,8 +338,8 @@ class G1_29_JointIndex(IntEnum): kNotUsedJoint5 = 34 class G1_23_ArmController: - def __init__(self, is_use_sim = False): - self.is_use_sim = is_use_sim + def __init__(self, simulation_mode = False): + self.simulation_mode = simulation_mode logger_mp.info("Initialize G1_23_ArmController...") self.q_target = np.zeros(10) @@ -443,7 +443,7 @@ class G1_23_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - if self.is_use_sim: + if self.simulation_mode: cliped_arm_q_target = arm_q_target else: cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) @@ -600,8 +600,8 @@ class G1_23_JointIndex(IntEnum): kNotUsedJoint5 = 34 class H1_2_ArmController: - def __init__(self, is_use_sim = False): - self.is_use_sim = is_use_sim + def __init__(self, simulation_mode = False): + self.simulation_mode = simulation_mode logger_mp.info("Initialize H1_2_ArmController...") self.q_target = np.zeros(14) @@ -705,7 +705,7 @@ class H1_2_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - if self.is_use_sim: + if self.simulation_mode: cliped_arm_q_target = arm_q_target else: cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) @@ -869,8 +869,8 @@ class H1_2_JointIndex(IntEnum): kNotUsedJoint7 = 34 class H1_ArmController: - def __init__(self, is_use_sim = False): - self.is_use_sim = is_use_sim + def __init__(self, simulation_mode = False): + self.simulation_mode = simulation_mode logger_mp.info("Initialize H1_ArmController...") self.q_target = np.zeros(8) @@ -966,7 +966,7 @@ class H1_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - if self.is_use_sim: + if self.simulation_mode: cliped_arm_q_target = arm_q_target else: cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index 909801d..320dc2f 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -236,7 +236,7 @@ kTopicGripperState = "rt/unitree_actuator/state" class Gripper_Controller: def __init__(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None, - filter = True, fps = 200.0, Unit_Test = False, is_use_sim = False): + filter = True, fps = 200.0, Unit_Test = False, simulation_mode = False): """ [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process @@ -259,7 +259,7 @@ class Gripper_Controller: self.fps = fps self.Unit_Test = Unit_Test - self.is_use_sim = is_use_sim + self.simulation_mode = simulation_mode if filter: self.smooth_filter = WeightedMovingFilter(np.array([0.5, 0.3, 0.2]), Gripper_Num_Motors) @@ -316,7 +316,7 @@ class Gripper_Controller: def control_thread(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_state_in, dual_hand_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None): self.running = True - if self.is_use_sim: + if self.simulation_mode: DELTA_GRIPPER_CMD = 1.0 else: DELTA_GRIPPER_CMD = 0.18 # The motor rotates 5.4 radians, the clamping jaw slide open 9 cm, so 0.6 rad <==> 1 cm, 0.18 rad <==> 3 mm diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index e6449f0..9f578be 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -21,23 +21,17 @@ from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_C from teleop.robot_control.robot_hand_inspire import Inspire_Controller from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter +from sshkeyboard import listen_keyboard, stop_listening +# for simulation from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ - - -from sshkeyboard import listen_keyboard, stop_listening - -def publish_reset_category(category: int,publisher): - # construct message - msg = String_(data=str(category)) # pass data parameter directly during initialization - - # create publisher - - # publish message +def publish_reset_category(category: int,publisher): # Scene Reset signal + msg = String_(data=str(category)) publisher.Write(msg) - print(f"published reset category: {category}") + logger_mp.info(f"published reset category: {category}") +# state transition start_signal = False running = True should_toggle_recording = False @@ -61,20 +55,18 @@ if __name__ == '__main__': parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') - parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'],default='gripper', help='Select end effector controller') + parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') - parser.add_argument('--debug', action = 'store_false', help = 'Enable debug mode') - parser.add_argument('--headless', action='store_true', help='Run in headless mode (no display)') - - parser.add_argument('--is_use_sim', action = 'store_true', help = 'Use simulation or not') - parser.set_defaults(is_use_sim = True) + parser.add_argument('--motion', action = 'store_true', help = 'Enable motion control mode') + parser.add_argument('--headless', action='store_true', help='Enable headless mode (no display)') + parser.add_argument('--sim', action = 'store_true', help = 'Enable isaac simulation mode') args = parser.parse_args() logger_mp.info(f"args: {args}") # image client: img_config should be the same as the configuration in image_server.py (of Robot's development computing unit) - if args.is_use_sim: + if args.sim: img_config = { 'fps': 30, 'head_camera_type': 'opencv', @@ -133,16 +125,16 @@ if __name__ == '__main__': # arm if args.arm == 'G1_29': - arm_ctrl = G1_29_ArmController(debug_mode=args.debug, is_use_sim=args.is_use_sim) + arm_ctrl = G1_29_ArmController(motion_mode=args.motion, simulation_mode=args.sim) arm_ik = G1_29_ArmIK() elif args.arm == 'G1_23': - arm_ctrl = G1_23_ArmController(is_use_sim=args.is_use_sim) + arm_ctrl = G1_23_ArmController(simulation_mode=args.sim) arm_ik = G1_23_ArmIK() elif args.arm == 'H1_2': - arm_ctrl = H1_2_ArmController(is_use_sim=args.is_use_sim) + arm_ctrl = H1_2_ArmController(simulation_mode=args.sim) arm_ik = H1_2_ArmIK() elif args.arm == 'H1': - arm_ctrl = H1_ArmController(is_use_sim=args.is_use_sim) + arm_ctrl = H1_ArmController(simulation_mode=args.sim) arm_ik = H1_ArmIK() # end-effector @@ -159,7 +151,7 @@ if __name__ == '__main__': dual_gripper_data_lock = Lock() dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. - gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, is_use_sim=args.is_use_sim) + gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, simulation_mode=args.sim) elif args.ee == "inspire1": left_hand_pos_array = Array('d', 75, lock = True) # [input] right_hand_pos_array = Array('d', 75, lock = True) # [input] @@ -169,11 +161,14 @@ if __name__ == '__main__': hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) else: pass - if args.is_use_sim: + + # simulation mode + if args.sim: reset_pose_publisher = ChannelPublisher("rt/reset_pose/cmd", String_) reset_pose_publisher.Init() + # xr mode - if args.xr_mode == 'controller' and not args.debug: + if args.xr_mode == 'controller' and args.motion: from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient sport_client = LocoClient() sport_client.SetTimeout(0.0001) @@ -199,12 +194,12 @@ if __name__ == '__main__': if key == ord('q'): stop_listening() running = False - if args.is_use_sim: + if args.sim: publish_reset_category(2, reset_pose_publisher) elif key == ord('s'): should_toggle_recording = True elif key == ord('a'): - if args.is_use_sim: + if args.sim: publish_reset_category(2, reset_pose_publisher) if args.record and should_toggle_recording: @@ -217,7 +212,7 @@ if __name__ == '__main__': else: is_recording = False recorder.save_episode() - if args.is_use_sim: + if args.sim: publish_reset_category(1, reset_pose_publisher) # get input data tele_data = tv_wrapper.get_motion_state_data() @@ -240,7 +235,7 @@ if __name__ == '__main__': pass # high level control - if args.xr_mode == 'controller' and not args.debug: + if args.xr_mode == 'controller' and args.motion: # quit teleoperate if tele_data.tele_state.right_aButton: running = False From 124647dfd435d69c4800250ec8026d44e2258b03 Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 26 Jun 2025 12:19:14 +0800 Subject: [PATCH 19/32] [update] a good vuer version 0.0.60. --- teleop/open_television/_test_tv_wrapper.py | 2 +- teleop/open_television/setup.py | 10 ++++++++-- teleop/open_television/television.py | 4 ++-- teleop/open_television/tv_wrapper.py | 8 +++----- 4 files changed, 14 insertions(+), 10 deletions(-) diff --git a/teleop/open_television/_test_tv_wrapper.py b/teleop/open_television/_test_tv_wrapper.py index 66ef23c..ab8579e 100644 --- a/teleop/open_television/_test_tv_wrapper.py +++ b/teleop/open_television/_test_tv_wrapper.py @@ -35,7 +35,7 @@ def run_test_tv_wrapper(): teleData = tv_wrapper.get_motion_state_data() logger_mp.info("=== TeleData Snapshot ===") - logger_mp.info(f"[Head Rotation Matrix]:\n{teleData.head_rotation}") + logger_mp.info(f"[Head Rotation Matrix]:\n{teleData.head_pose}") logger_mp.info(f"[Left EE Pose]:\n{teleData.left_arm_pose}") logger_mp.info(f"[Right EE Pose]:\n{teleData.right_arm_pose}") diff --git a/teleop/open_television/setup.py b/teleop/open_television/setup.py index 6799080..dc471af 100644 --- a/teleop/open_television/setup.py +++ b/teleop/open_television/setup.py @@ -14,7 +14,7 @@ setup( 'aiohttp_cors==0.7.0', 'aiortc==1.8.0', 'av==11.0.0', - # 'vuer==0.0.32rc7', # avp \ pico, hand tracking all work fine. + # 'vuer==0.0.32rc7', # avp \ pico, hand tracking all work fine. but it not support controller tracking. # 'vuer==0.0.35', # avp hand tracking works fine. pico is fine too, but the right eye occasionally goes black for a short time at start. @@ -32,11 +32,17 @@ setup( # from 'vuer==0.0.46' # avp hand tracking work fine. # to - 'vuer==0.0.56', # In pico hand tracking mode, using a hand gesture to click the "Virtual Reality" button + # 'vuer==0.0.56', # In pico hand tracking mode, using a hand gesture to click the "Virtual Reality" button # causes the screen to stay black and stuck loading. But if the button is clicked with the controller instead, everything works fine. # In pico controller tracking mode, using controller to click the "Virtual Reality" button # causes the screen to stay black and stuck loading. But if the button is clicked with a hand gesture instead, everything works fine. # avp \ pico all have hand marker visualization (RGB three-axis color coordinates). + + 'vuer==0.0.60', # a good version + # https://github.com/unitreerobotics/avp_teleoperate/issues/53 + # https://github.com/vuer-ai/vuer/issues/45 + # https://github.com/vuer-ai/vuer/issues/65 + ], python_requires='>=3.8', ) \ No newline at end of file diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index d891fb9..6cba8a5 100644 --- a/teleop/open_television/television.py +++ b/teleop/open_television/television.py @@ -206,8 +206,8 @@ class TeleVision: MotionControllers( stream=True, key="motionControllers", - showLeft=False, - showRight=False, + left=True, + right=True, ), to="bgChildren", ) diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py index cc2e1f4..92ca634 100644 --- a/teleop/open_television/tv_wrapper.py +++ b/teleop/open_television/tv_wrapper.py @@ -166,7 +166,7 @@ class TeleStateData: @dataclass class TeleData: - head_rotation: np.ndarray # (3,3) Head orientation matrix + head_pose: np.ndarray # (4,4) SE(3) pose of head matrix left_arm_pose: np.ndarray # (4,4) SE(3) pose of left arm right_arm_pose: np.ndarray # (4,4) SE(3) pose of right arm left_hand_pos: np.ndarray = None # (25,3) 3D positions of left hand joints @@ -259,7 +259,6 @@ class TeleVisionWrapper: right_IPunitree_Brobot_head_arm[0:3, 3] = right_IPunitree_Brobot_world_arm[0:3, 3] - Brobot_world_head[0:3, 3] # =====coordinate origin offset===== - Brobot_world_head_rot = Brobot_world_head[:3, :3] # The origin of the coordinate for IK Solve is near the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to visualize it. # The origin of the coordinate of IPunitree_Brobot_head_arm is HEAD. # So it is necessary to translate the origin of IPunitree_Brobot_head_arm from HEAD to WAIST. @@ -340,7 +339,7 @@ class TeleVisionWrapper: hand_state = None return TeleData( - head_rotation=Brobot_world_head_rot, + head_pose=Brobot_world_head, left_arm_pose=left_IPunitree_Brobot_waist_arm, right_arm_pose=right_IPunitree_Brobot_waist_arm, left_hand_pos=left_IPunitree_Brobot_arm_hand_pos, @@ -368,7 +367,6 @@ class TeleVisionWrapper: right_IPunitree_Brobot_head_arm[0:3, 3] = right_IPunitree_Brobot_head_arm[0:3, 3] - Brobot_world_head[0:3, 3] # =====coordinate origin offset===== - Brobot_world_head_rot = Brobot_world_head[:3, :3] # The origin of the coordinate for IK Solve is near the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to check it. # The origin of the coordinate of IPunitree_Brobot_head_arm is HEAD. # So it is necessary to translate the origin of IPunitree_Brobot_head_arm from HEAD to WAIST. @@ -403,7 +401,7 @@ class TeleVisionWrapper: controller_state = None return TeleData( - head_rotation=Brobot_world_head_rot, + head_pose=Brobot_world_head, left_arm_pose=left_IPunitree_Brobot_waist_arm, right_arm_pose=right_IPunitree_Brobot_waist_arm, left_trigger_value=10.0 - self.tvuer.left_controller_trigger_value * 10, From aee701fdf813e24524d0a60d37691079851bbaa2 Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 26 Jun 2025 16:11:38 +0800 Subject: [PATCH 20/32] [fix] bug --- teleop/open_television/television.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index 6cba8a5..aa5c527 100644 --- a/teleop/open_television/television.py +++ b/teleop/open_television/television.py @@ -264,8 +264,8 @@ class TeleVision: MotionControllers( stream=True, key="motionControllers", - showLeft=False, - showRight=False, + left=True, + right=True, ), to="bgChildren", ) From 7d91cfb6b89c37e1b754dbf6300785787eeeaf7c Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 26 Jun 2025 20:53:02 +0800 Subject: [PATCH 21/32] [update] support dex3 DexPilot algorithm. --- assets/unitree_hand/unitree_dex3.yml | 40 +++++++------- .../dex_retargeting/optimizer.py | 11 ++-- .../dex_retargeting/retargeting_config.py | 52 ++++++++++--------- teleop/robot_control/hand_retargeting.py | 2 + teleop/robot_control/robot_hand_unitree.py | 11 +--- 5 files changed, 62 insertions(+), 54 deletions(-) diff --git a/assets/unitree_hand/unitree_dex3.yml b/assets/unitree_hand/unitree_dex3.yml index d54677a..dc5ec85 100644 --- a/assets/unitree_hand/unitree_dex3.yml +++ b/assets/unitree_hand/unitree_dex3.yml @@ -1,9 +1,8 @@ left: - type: vector + type: DexPilot # or vector urdf_path: unitree_hand/unitree_dex3_left.urdf # Target refers to the retargeting target, which is the robot hand - # target_joint_names: target_joint_names: [ "left_hand_thumb_0_joint", @@ -14,27 +13,26 @@ left: "left_hand_index_0_joint", "left_hand_index_1_joint", ] - wrist_link_name: None + + # for DexPilot type + wrist_link_name: "base_link" + finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip"] + # If you do not know exactly how it is used, please leave it to None for default. + target_link_human_indices_dexpilot: [[ 9, 14, 14, 0, 0, 0], [ 4, 4, 9, 4, 9, 14]] + + # for vector type target_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"] target_task_link_names: ["thumb_tip","index_tip","middle_tip"] - target_link_human_indices: [[0, 0, 0], [4, 9, 14]] + target_link_human_indices_vector: [[0, 0, 0], [4, 9, 14]] - # Currently, the scaling factor for each finger is individually distinguished in the robot_hand_unitree.py file. - # The Unitree Dex3 has three fingers with the same specifications, so the retarget scaling factors need to be adjusted separately. - # The relevant code is as follows: - # ref_left_value[0] = ref_left_value[0] * 1.15 - # ref_left_value[1] = ref_left_value[1] * 1.05 - # ref_left_value[2] = ref_left_value[2] * 0.95 - # ref_right_value[0] = ref_right_value[0] * 1.15 - # ref_right_value[1] = ref_right_value[1] * 1.05 - # ref_right_value[2] = ref_right_value[2] * 0.95 + # Scaling factor for vector retargeting only + # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 scaling_factor: 1.0 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency low_pass_alpha: 0.2 right: - type: vector + type: DexPilot # or vector urdf_path: unitree_hand/unitree_dex3_right.urdf # Target refers to the retargeting target, which is the robot hand @@ -48,12 +46,18 @@ right: "right_hand_middle_0_joint", "right_hand_middle_1_joint", ] - wrist_link_name: None + # for DexPilot type + wrist_link_name: "base_link" + finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip"] + target_link_human_indices_dexpilot: [[ 9, 14, 14, 0, 0, 0], [ 4, 4, 9, 4, 9, 14]] + + # for vector type target_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"] target_task_link_names: ["thumb_tip", "index_tip", "middle_tip"] - target_link_human_indices: [[0, 0, 0], [4, 9, 14]] + target_link_human_indices_vector: [[0, 0, 0], [4, 9, 14]] + # Scaling factor for vector retargeting only + # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 scaling_factor: 1.0 - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency low_pass_alpha: 0.2 diff --git a/teleop/robot_control/dex_retargeting/optimizer.py b/teleop/robot_control/dex_retargeting/optimizer.py index 9288523..ed891fe 100644 --- a/teleop/robot_control/dex_retargeting/optimizer.py +++ b/teleop/robot_control/dex_retargeting/optimizer.py @@ -324,7 +324,12 @@ class DexPilotOptimizer(Optimizer): origin_link_index, task_link_index = self.generate_link_indices(self.num_fingers) if target_link_human_indices is None: - target_link_human_indices = (np.stack([origin_link_index, task_link_index], axis=0) * 4).astype(int) + logical_indices = np.stack([origin_link_index, task_link_index], axis=0) + target_link_human_indices = np.where( + logical_indices > 0, + logical_indices * 5 - 1, + 0 + ).astype(int) link_names = [wrist_link_name] + finger_tip_link_names target_origin_link_names = [link_names[index] for index in origin_link_index] target_task_link_names = [link_names[index] for index in task_link_index] @@ -370,13 +375,13 @@ class DexPilotOptimizer(Optimizer): origin_link_index = [] task_link_index = [] - # Add indices for connections between fingers + # S1：Add indices for connections between fingers for i in range(1, num_fingers): for j in range(i + 1, num_fingers + 1): origin_link_index.append(j) task_link_index.append(i) - # Add indices for connections to the base (0) + # S2：Add indices for connections to the base (0) for i in range(1, num_fingers + 1): origin_link_index.append(0) task_link_index.append(i) diff --git a/teleop/robot_control/dex_retargeting/retargeting_config.py b/teleop/robot_control/dex_retargeting/retargeting_config.py index d2378c0..f1e8cd8 100644 --- a/teleop/robot_control/dex_retargeting/retargeting_config.py +++ b/teleop/robot_control/dex_retargeting/retargeting_config.py @@ -19,32 +19,34 @@ from .yourdfpy import DUMMY_JOINT_NAMES class RetargetingConfig: type: str urdf_path: str + target_joint_names: Optional[List[str]] = None # Whether to add free joint to the root of the robot. Free joint enable the robot hand move freely in the space add_dummy_free_joint: bool = False - # Source refers to the retargeting input, which usually corresponds to the human hand - # The joint indices of human hand joint which corresponds to each link in the target_link_names - target_link_human_indices: Optional[np.ndarray] = None - + # DexPilot retargeting related # The link on the robot hand which corresponding to the wrist of human hand wrist_link_name: Optional[str] = None + # DexPilot retargeting link names + finger_tip_link_names: Optional[List[str]] = None + target_link_human_indices_dexpilot: Optional[np.ndarray] = None # Position retargeting link names target_link_names: Optional[List[str]] = None + target_link_human_indices_position: Optional[np.ndarray] = None # Vector retargeting link names - target_joint_names: Optional[List[str]] = None target_origin_link_names: Optional[List[str]] = None target_task_link_names: Optional[List[str]] = None - - # DexPilot retargeting link names - finger_tip_link_names: Optional[List[str]] = None + target_link_human_indices_vector: Optional[np.ndarray] = None # Scaling factor for vector retargeting only # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 scaling_factor: float = 1.0 + # Low pass filter + low_pass_alpha: float = 0.1 + # Optimization parameters normal_delta: float = 4e-3 huber_delta: float = 2e-2 @@ -59,9 +61,6 @@ class RetargetingConfig: # Mimic joint tag ignore_mimic_joint: bool = False - # Low pass filter - low_pass_alpha: float = 0.1 - _TYPE = ["vector", "position", "dexpilot"] _DEFAULT_URDF_DIR = "./" @@ -78,24 +77,24 @@ class RetargetingConfig: raise ValueError(f"Vector retargeting requires: target_origin_link_names + target_task_link_names") if len(self.target_task_link_names) != len(self.target_origin_link_names): raise ValueError(f"Vector retargeting origin and task links dim mismatch") - if self.target_link_human_indices.shape != (2, len(self.target_origin_link_names)): + if self.target_link_human_indices_vector.shape != (2, len(self.target_origin_link_names)): raise ValueError(f"Vector retargeting link names and link indices dim mismatch") - if self.target_link_human_indices is None: - raise ValueError(f"Vector retargeting requires: target_link_human_indices") + if self.target_link_human_indices_vector is None: + raise ValueError(f"Vector retargeting requires: target_link_human_indices_vector") elif self.type == "position": if self.target_link_names is None: raise ValueError(f"Position retargeting requires: target_link_names") - self.target_link_human_indices = self.target_link_human_indices.squeeze() - if self.target_link_human_indices.shape != (len(self.target_link_names),): + self.target_link_human_indices_position = self.target_link_human_indices_position.squeeze() + if self.target_link_human_indices_position.shape != (len(self.target_link_names),): raise ValueError(f"Position retargeting link names and link indices dim mismatch") - if self.target_link_human_indices is None: - raise ValueError(f"Position retargeting requires: target_link_human_indices") + if self.target_link_human_indices_position is None: + raise ValueError(f"Position retargeting requires: target_link_human_indices_position") elif self.type == "dexpilot": if self.finger_tip_link_names is None or self.wrist_link_name is None: raise ValueError(f"Position retargeting requires: finger_tip_link_names + wrist_link_name") - if self.target_link_human_indices is not None: + if self.target_link_human_indices_dexpilot is not None: print( "\033[33m", "Target link human indices is provided in the DexPilot retargeting config, which is uncommon.\n" @@ -132,8 +131,13 @@ class RetargetingConfig: @classmethod def from_dict(cls, cfg: Dict[str, Any], override: Optional[Dict] = None): - if "target_link_human_indices" in cfg: - cfg["target_link_human_indices"] = np.array(cfg["target_link_human_indices"]) + if "target_link_human_indices_position" in cfg: + cfg["target_link_human_indices_position"] = np.array(cfg["target_link_human_indices_position"]) + if "target_link_human_indices_vector" in cfg: + cfg["target_link_human_indices_vector"] = np.array(cfg["target_link_human_indices_vector"]) + if "target_link_human_indices_dexpilot" in cfg: + cfg["target_link_human_indices_dexpilot"] = np.array(cfg["target_link_human_indices_dexpilot"]) + if override is not None: for key, value in override.items(): cfg[key] = value @@ -170,7 +174,7 @@ class RetargetingConfig: robot, joint_names, target_link_names=self.target_link_names, - target_link_human_indices=self.target_link_human_indices, + target_link_human_indices=self.target_link_human_indices_position, norm_delta=self.normal_delta, huber_delta=self.huber_delta, ) @@ -180,7 +184,7 @@ class RetargetingConfig: joint_names, target_origin_link_names=self.target_origin_link_names, target_task_link_names=self.target_task_link_names, - target_link_human_indices=self.target_link_human_indices, + target_link_human_indices=self.target_link_human_indices_vector, scaling=self.scaling_factor, norm_delta=self.normal_delta, huber_delta=self.huber_delta, @@ -191,7 +195,7 @@ class RetargetingConfig: joint_names, finger_tip_link_names=self.finger_tip_link_names, wrist_link_name=self.wrist_link_name, - target_link_human_indices=self.target_link_human_indices, + target_link_human_indices=self.target_link_human_indices_dexpilot, scaling=self.scaling_factor, project_dist=self.project_dist, escape_dist=self.escape_dist, diff --git a/teleop/robot_control/hand_retargeting.py b/teleop/robot_control/hand_retargeting.py index 77c6a18..1c67945 100644 --- a/teleop/robot_control/hand_retargeting.py +++ b/teleop/robot_control/hand_retargeting.py @@ -38,6 +38,8 @@ class HandRetargeting: self.left_retargeting_joint_names = self.left_retargeting.joint_names self.right_retargeting_joint_names = self.right_retargeting.joint_names + self.left_indices = self.left_retargeting.optimizer.target_link_human_indices + self.right_indices = self.right_retargeting.optimizer.target_link_human_indices if hand_type == HandType.UNITREE_DEX3 or hand_type == HandType.UNITREE_DEX3_Unit_Test: # In section "Sort by message structure" of https://support.unitree.com/home/en/G1_developer/dexterous_hand diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index 320dc2f..fb04cf1 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -24,7 +24,6 @@ import logging_mp logger_mp = logging_mp.get_logger(__name__) -unitree_tip_indices = [4, 9, 14] # [thumb, index, middle] in OpenXR Dex3_Num_Motors = 7 kTopicDex3LeftCommand = "rt/dex3/left/cmd" kTopicDex3RightCommand = "rt/dex3/right/cmd" @@ -183,14 +182,8 @@ class Dex3_1_Controller: state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) if not np.all(right_hand_data == 0.0) and not np.all(left_hand_data[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. - ref_left_value = left_hand_data[unitree_tip_indices] - ref_right_value = right_hand_data[unitree_tip_indices] - ref_left_value[0] = ref_left_value[0] * 1.15 - ref_left_value[1] = ref_left_value[1] * 1.05 - ref_left_value[2] = ref_left_value[2] * 0.95 - ref_right_value[0] = ref_right_value[0] * 1.15 - ref_right_value[1] = ref_right_value[1] * 1.05 - ref_right_value[2] = ref_right_value[2] * 0.95 + ref_left_value = left_hand_data[self.hand_retargeting.left_indices[1,:]] - left_hand_data[self.hand_retargeting.left_indices[0,:]] + ref_right_value = right_hand_data[self.hand_retargeting.right_indices[1,:]] - right_hand_data[self.hand_retargeting.right_indices[0,:]] left_q_target = self.hand_retargeting.left_retargeting.retarget(ref_left_value)[self.hand_retargeting.right_dex_retargeting_to_hardware] right_q_target = self.hand_retargeting.right_retargeting.retarget(ref_right_value)[self.hand_retargeting.right_dex_retargeting_to_hardware] From 64c01a71eee9d5ff88aaf6f7b2a18ef62de1f380 Mon Sep 17 00:00:00 2001 From: silencht Date: Mon, 30 Jun 2025 11:32:35 +0800 Subject: [PATCH 22/32] [update] support inspire1 DexPilot algorithm. --- assets/inspire_hand/inspire_hand.yml | 63 +++++++++++++++------- teleop/robot_control/robot_hand_inspire.py | 11 ++-- 2 files changed, 48 insertions(+), 26 deletions(-) diff --git a/assets/inspire_hand/inspire_hand.yml b/assets/inspire_hand/inspire_hand.yml index a3aaaf0..8466ce0 100644 --- a/assets/inspire_hand/inspire_hand.yml +++ b/assets/inspire_hand/inspire_hand.yml @@ -1,39 +1,62 @@ left: - type: vector + type: DexPilot # or vector urdf_path: inspire_hand/inspire_hand_left.urdf - wrist_link_name: "L_hand_base_link" # Target refers to the retargeting target, which is the robot hand - target_joint_names: ['L_thumb_proximal_yaw_joint', 'L_thumb_proximal_pitch_joint', - 'L_index_proximal_joint', 'L_middle_proximal_joint', - 'L_ring_proximal_joint', 'L_pinky_proximal_joint' ] + target_joint_names: + [ + 'L_thumb_proximal_yaw_joint', + 'L_thumb_proximal_pitch_joint', + 'L_index_proximal_joint', + 'L_middle_proximal_joint', + 'L_ring_proximal_joint', + 'L_pinky_proximal_joint' + ] + + # for DexPilot type + wrist_link_name: "L_hand_base_link" + finger_tip_link_names: [ "L_thumb_tip", "L_index_tip", "L_middle_tip", "L_ring_tip", "L_pinky_tip" ] + # If you do not know exactly how it is used, please leave it to None for default. + target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] + + # for vector type target_origin_link_names: [ "L_hand_base_link", "L_hand_base_link", "L_hand_base_link", "L_hand_base_link", "L_hand_base_link"] target_task_link_names: [ "L_thumb_tip", "L_index_tip", "L_middle_tip", "L_ring_tip", "L_pinky_tip" ] - scaling_factor: 1.20 - - # Source refers to the retargeting input, which usually corresponds to the human hand - # The joint indices of human hand joint which corresponds to each link in the target_link_names - target_link_human_indices: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] + target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] + # Scaling factor for vector retargeting only + # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 + scaling_factor: 1.20 # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency low_pass_alpha: 0.2 right: - type: vector + type: DexPilot # or vector urdf_path: inspire_hand/inspire_hand_right.urdf - wrist_link_name: "R_hand_base_link" # Target refers to the retargeting target, which is the robot hand - target_joint_names: ['R_thumb_proximal_yaw_joint', 'R_thumb_proximal_pitch_joint', - 'R_index_proximal_joint', 'R_middle_proximal_joint', - 'R_ring_proximal_joint', 'R_pinky_proximal_joint' ] + target_joint_names: + [ + 'R_thumb_proximal_yaw_joint', + 'R_thumb_proximal_pitch_joint', + 'R_index_proximal_joint', + 'R_middle_proximal_joint', + 'R_ring_proximal_joint', + 'R_pinky_proximal_joint' + ] + + # for DexPilot type + wrist_link_name: "R_hand_base_link" + finger_tip_link_names: [ "R_thumb_tip", "R_index_tip", "R_middle_tip", "R_ring_tip", "R_pinky_tip" ] + # If you do not know exactly how it is used, please leave it to None for + target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] + target_origin_link_names: [ "R_hand_base_link", "R_hand_base_link", "R_hand_base_link", "R_hand_base_link", "R_hand_base_link"] target_task_link_names: [ "R_thumb_tip", "R_index_tip", "R_middle_tip", "R_ring_tip", "R_pinky_tip" ] + target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] + + # Scaling factor for vector retargeting only + # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 scaling_factor: 1.20 - - # Source refers to the retargeting input, which usually corresponds to the human hand - # The joint indices of human hand joint which corresponds to each link in the target_link_names - target_link_human_indices: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] - # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency low_pass_alpha: 0.2 \ No newline at end of file diff --git a/teleop/robot_control/robot_hand_inspire.py b/teleop/robot_control/robot_hand_inspire.py index 47a1e55..bec0a63 100644 --- a/teleop/robot_control/robot_hand_inspire.py +++ b/teleop/robot_control/robot_hand_inspire.py @@ -13,7 +13,6 @@ from multiprocessing import Process, Array import logging_mp logger_mp = logging_mp.get_logger(__name__) -inspire_tip_indices = [4, 9, 14, 19, 24] Inspire_Num_Motors = 6 kTopicInspireCommand = "rt/inspire/cmd" kTopicInspireState = "rt/inspire/state" @@ -103,16 +102,16 @@ class Inspire_Controller: start_time = time.time() # get dual hand state with left_hand_array.get_lock(): - left_hand_mat = np.array(left_hand_array[:]).reshape(25, 3).copy() + left_hand_data = np.array(left_hand_array[:]).reshape(25, 3).copy() with right_hand_array.get_lock(): - right_hand_mat = np.array(right_hand_array[:]).reshape(25, 3).copy() + right_hand_data = np.array(right_hand_array[:]).reshape(25, 3).copy() # Read left and right q_state from shared arrays state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) - if not np.all(right_hand_mat == 0.0) and not np.all(left_hand_mat[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. - ref_left_value = left_hand_mat[inspire_tip_indices] - ref_right_value = right_hand_mat[inspire_tip_indices] + if not np.all(right_hand_data == 0.0) and not np.all(left_hand_data[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. + ref_left_value = left_hand_data[self.hand_retargeting.left_indices[1,:]] - left_hand_data[self.hand_retargeting.left_indices[0,:]] + ref_right_value = right_hand_data[self.hand_retargeting.right_indices[1,:]] - right_hand_data[self.hand_retargeting.right_indices[0,:]] left_q_target = self.hand_retargeting.left_retargeting.retarget(ref_left_value)[self.hand_retargeting.left_dex_retargeting_to_hardware] right_q_target = self.hand_retargeting.right_retargeting.retarget(ref_right_value)[self.hand_retargeting.right_dex_retargeting_to_hardware] From 2716c0e7094e7efc0d9b0fdc26fc1e5652ea3bf7 Mon Sep 17 00:00:00 2001 From: silencht Date: Mon, 30 Jun 2025 15:17:53 +0800 Subject: [PATCH 23/32] [fix] sshkeyboard's related issue --- teleop/teleop_hand_and_arm.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 9f578be..d3d29f4 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -24,7 +24,7 @@ from teleop.utils.episode_writer import EpisodeWriter from sshkeyboard import listen_keyboard, stop_listening # for simulation -from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelPublisher from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ def publish_reset_category(category: int,publisher): # Scene Reset signal msg = String_(data=str(category)) @@ -41,12 +41,15 @@ def on_press(key): if key == 'r': start_signal = True logger_mp.info("Program start signal received.") - elif key == 'q': + elif start_signal == True and key == 'q': stop_listening() running = False - elif key == 's': + elif start_signal == True and key == 's': should_toggle_recording = True -threading.Thread(target=listen_keyboard, kwargs={"on_press": on_press}, daemon=True).start() + else: + logger_mp.info(f"{key} was pressed, but no action is defined for this key.") +listen_keyboard_thread = threading.Thread(target=listen_keyboard, kwargs={"on_press": on_press, "until": None, "sequential": False,}, daemon=True) +listen_keyboard_thread.start() if __name__ == '__main__': parser = argparse.ArgumentParser() @@ -395,5 +398,6 @@ if __name__ == '__main__': wrist_img_shm.close() if args.record: recorder.close() + listen_keyboard_thread.join() logger_mp.info("Finally, exiting program...") exit(0) \ No newline at end of file From a40056f7d498f8c749ba5e67eba453ba8094b3a6 Mon Sep 17 00:00:00 2001 From: silencht Date: Mon, 30 Jun 2025 17:55:14 +0800 Subject: [PATCH 24/32] [update] Optimize go home experience. --- teleop/robot_control/robot_arm.py | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index 49ac742..90cbc43 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -218,11 +218,13 @@ class G1_29_ArmController: def ctrl_dual_arm_go_home(self): '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' logger_mp.info("[G1_29_ArmController] ctrl_dual_arm_go_home start...") + max_attempts = 100 + current_attempts = 0 with self.ctrl_lock: self.q_target = np.zeros(14) # self.tauff_target = np.zeros(14) tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements - while True: + while current_attempts < max_attempts: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): if self.motion_mode: @@ -231,6 +233,7 @@ class G1_29_ArmController: time.sleep(0.02) logger_mp.info("[G1_29_ArmController] both arms have reached the home position.") break + attempts += 1 time.sleep(0.05) def speed_gradual_max(self, t = 5.0): @@ -492,15 +495,18 @@ class G1_23_ArmController: def ctrl_dual_arm_go_home(self): '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' logger_mp.info("[G1_23_ArmController] ctrl_dual_arm_go_home start...") + max_attempts = 100 + current_attempts = 0 with self.ctrl_lock: self.q_target = np.zeros(10) # self.tauff_target = np.zeros(10) tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements - while True: + while current_attempts < max_attempts: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): logger_mp.info("[G1_23_ArmController] both arms have reached the home position.") break + current_attempts += 1 time.sleep(0.05) def speed_gradual_max(self, t = 5.0): @@ -754,15 +760,18 @@ class H1_2_ArmController: def ctrl_dual_arm_go_home(self): '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' logger_mp.info("[H1_2_ArmController] ctrl_dual_arm_go_home start...") + max_attempts = 100 + current_attempts = 0 with self.ctrl_lock: self.q_target = np.zeros(14) # self.tauff_target = np.zeros(14) tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements - while True: + while current_attempts < max_attempts: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): logger_mp.info("[H1_2_ArmController] both arms have reached the home position.") break + current_attempts += 1 time.sleep(0.05) def speed_gradual_max(self, t = 5.0): @@ -1011,15 +1020,18 @@ class H1_ArmController: def ctrl_dual_arm_go_home(self): '''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.''' logger_mp.info("[H1_ArmController] ctrl_dual_arm_go_home start...") + max_attempts = 100 + current_attempts = 0 with self.ctrl_lock: self.q_target = np.zeros(8) # self.tauff_target = np.zeros(8) tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements - while True: + while current_attempts < max_attempts: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): logger_mp.info("[H1_ArmController] both arms have reached the home position.") break + current_attempts += 1 time.sleep(0.05) def speed_gradual_max(self, t = 5.0): From 1173549d3c1b83f2f756e668a165ca43e3b26ebe Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 1 Jul 2025 10:15:00 +0800 Subject: [PATCH 25/32] [fix] var name bug --- teleop/robot_control/robot_arm.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index 90cbc43..b589d26 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -12,7 +12,7 @@ from unitree_sdk2py.idl.unitree_go.msg.dds_ import ( LowCmd_ as go_LowCmd, LowS from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ import logging_mp -logger_mp = logging_mp.get_logger(__name__) +logger_mp = logging_mp.get_logger(__name__,level=logging_mp.DEBUG) kTopicLowCommand_Debug = "rt/lowcmd" kTopicLowCommand_Motion = "rt/arm_sdk" @@ -233,7 +233,7 @@ class G1_29_ArmController: time.sleep(0.02) logger_mp.info("[G1_29_ArmController] both arms have reached the home position.") break - attempts += 1 + current_attempts += 1 time.sleep(0.05) def speed_gradual_max(self, t = 5.0): @@ -1104,7 +1104,7 @@ if __name__ == "__main__": import pinocchio as pin arm_ik = G1_29_ArmIK(Unit_Test = True, Visualization = False) - arm = G1_29_ArmController() + arm = G1_29_ArmController(simulation_mode=True) # arm_ik = G1_23_ArmIK(Unit_Test = True, Visualization = False) # arm = G1_23_ArmController() # arm_ik = H1_2_ArmIK(Unit_Test = True, Visualization = False) From 375cdc27605de377c698e2b89cad0e5885724ca6 Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 1 Jul 2025 11:00:08 +0800 Subject: [PATCH 26/32] [fix] minor issue for sim. --- teleop/robot_control/robot_arm.py | 2 +- teleop/teleop_hand_and_arm.py | 12 +++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index b589d26..bcecc0c 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -12,7 +12,7 @@ from unitree_sdk2py.idl.unitree_go.msg.dds_ import ( LowCmd_ as go_LowCmd, LowS from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_ import logging_mp -logger_mp = logging_mp.get_logger(__name__,level=logging_mp.DEBUG) +logger_mp = logging_mp.get_logger(__name__) kTopicLowCommand_Debug = "rt/lowcmd" kTopicLowCommand_Motion = "rt/arm_sdk" diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index d3d29f4..e727f58 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -109,14 +109,20 @@ if __name__ == '__main__': tv_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(tv_img_shape) * np.uint8().itemsize) tv_img_array = np.ndarray(tv_img_shape, dtype = np.uint8, buffer = tv_img_shm.buf) - if WRIST: + if WRIST and args.sim: wrist_img_shape = (img_config['wrist_camera_image_shape'][0], img_config['wrist_camera_image_shape'][1] * 2, 3) wrist_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(wrist_img_shape) * np.uint8().itemsize) wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = wrist_img_shm.buf) img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, - wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name,server_address="127.0.0.1") + wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name, server_address="127.0.0.1") + elif WRIST and not args.sim: + wrist_img_shape = (img_config['wrist_camera_image_shape'][0], img_config['wrist_camera_image_shape'][1] * 2, 3) + wrist_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(wrist_img_shape) * np.uint8().itemsize) + wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = wrist_img_shm.buf) + img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, + wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name) else: - img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, server_address="127.0.0.1") + img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name) image_receive_thread = threading.Thread(target = img_client.receive_process, daemon = True) image_receive_thread.daemon = True From 7c1455c7fb970cdc3bafda9fda32cde4eda057b7 Mon Sep 17 00:00:00 2001 From: "wei.li" Date: Fri, 4 Jul 2025 09:34:10 +0800 Subject: [PATCH 27/32] update episode_writer ,add sim_state --- teleop/robot_control/robot_arm.py | 2 +- teleop/teleop_hand_and_arm.py | 26 +- ...e-unitree-opject-unitree_sim_isaaclab-.txt | 1 + teleop/utils/episode_writer.py | 4 +- teleop/utils/server_api.py | 8 + teleop/utils/sim_state_client.py | 48 +++ teleop/utils/sim_state_topic.py | 285 ++++++++++++++++++ 7 files changed, 365 insertions(+), 9 deletions(-) create mode 100644 teleop/utils/data/-home-unitree-Code-unitree-opject-unitree_sim_isaaclab-.txt create mode 100644 teleop/utils/server_api.py create mode 100644 teleop/utils/sim_state_client.py create mode 100644 teleop/utils/sim_state_topic.py diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index cdd24c1..3a1923b 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -81,7 +81,7 @@ class G1_29_ArmController: self._gradual_time = None # initialize lowcmd publisher and lowstate subscriber - ChannelFactoryInitialize(0) + ChannelFactoryInitialize(1) if self.debug_mode: self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) else: diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index e6449f0..c08d8e5 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -47,10 +47,10 @@ def on_press(key): if key == 'r': start_signal = True logger_mp.info("Program start signal received.") - elif key == 'q': + elif key == 's': stop_listening() running = False - elif key == 's': + elif key == 'q': should_toggle_recording = True threading.Thread(target=listen_keyboard, kwargs={"on_press": on_press}, daemon=True).start() @@ -172,6 +172,14 @@ if __name__ == '__main__': if args.is_use_sim: reset_pose_publisher = ChannelPublisher("rt/reset_pose/cmd", String_) reset_pose_publisher.Init() + from teleop.utils.sim_state_topic import start_sim_state_subscribe + sim_state_subscriber = start_sim_state_subscribe() + # from teleop.utils.sim_state_client import SimStateClient + # sim_state_client = SimStateClient() + # sim_state_client.Init() + # sim_state_client.SetTimeout(5.0) + # sim_state_client.WaitLeaseApplied() + # c=1 # xr mode if args.xr_mode == 'controller' and not args.debug: from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient @@ -196,12 +204,12 @@ if __name__ == '__main__': tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) cv2.imshow("record image", tv_resized_image) key = cv2.waitKey(1) & 0xFF - if key == ord('q'): + if key == ord('s'): stop_listening() running = False if args.is_use_sim: publish_reset_category(2, reset_pose_publisher) - elif key == ord('s'): + elif key == ord('q'): should_toggle_recording = True elif key == ord('a'): if args.is_use_sim: @@ -237,7 +245,7 @@ if __name__ == '__main__': with right_gripper_value.get_lock(): right_gripper_value.value = tele_data.right_pinch_value else: - pass + pass # high level control if args.xr_mode == 'controller' and not args.debug: @@ -381,7 +389,11 @@ if __name__ == '__main__': "qpos": current_body_action, }, } - recorder.add_item(colors=colors, depths=depths, states=states, actions=actions) + if args.is_use_sim: + sim_state = sim_state_subscriber.read_data() + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, sim_state=sim_state) + else: + recorder.add_item(colors=colors, depths=depths, states=states, actions=actions) current_time = time.time() time_elapsed = current_time - start_time @@ -401,4 +413,4 @@ if __name__ == '__main__': if args.record: recorder.close() logger_mp.info("Finally, exiting program...") - exit(0) \ No newline at end of file + exit(0) diff --git a/teleop/utils/data/-home-unitree-Code-unitree-opject-unitree_sim_isaaclab-.txt b/teleop/utils/data/-home-unitree-Code-unitree-opject-unitree_sim_isaaclab-.txt new file mode 100644 index 0000000..5e55b46 --- /dev/null +++ b/teleop/utils/data/-home-unitree-Code-unitree-opject-unitree_sim_isaaclab-.txt @@ -0,0 +1 @@ +/home/unitree/Code/unitree-opject/unitree_sim_isaaclab/data \ No newline at end of file diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index 5cc4830..ea59faf 100644 --- a/teleop/utils/episode_writer.py +++ b/teleop/utils/episode_writer.py @@ -71,6 +71,7 @@ class EpisodeWriter(): "left_ee": [], "right_ee": [], }, + "sim_state": "" } def text_desc(self): self.text = { @@ -113,7 +114,7 @@ class EpisodeWriter(): logger_mp.info(f"==> New episode created: {self.episode_dir}") return True # Return True if the episode is successfully created - def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None): + def add_item(self, colors, depths=None, states=None, actions=None, tactiles=None, audios=None,sim_state=None): # Increment the item ID self.item_id += 1 # Create the item data dictionary @@ -125,6 +126,7 @@ class EpisodeWriter(): 'actions': actions, 'tactiles': tactiles, 'audios': audios, + 'sim_state': sim_state, } # Enqueue the item data self.item_data_queue.put(item_data) diff --git a/teleop/utils/server_api.py b/teleop/utils/server_api.py new file mode 100644 index 0000000..98bfb4b --- /dev/null +++ b/teleop/utils/server_api.py @@ -0,0 +1,8 @@ +# service name +SERVICE_NAME = "get_sim_state" + +# api version +API_VERSION = "1.0.0.1" + +# api id +API_ID_GET_SIM_STATE = 1008 \ No newline at end of file diff --git a/teleop/utils/sim_state_client.py b/teleop/utils/sim_state_client.py new file mode 100644 index 0000000..972824b --- /dev/null +++ b/teleop/utils/sim_state_client.py @@ -0,0 +1,48 @@ +import time +import json +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.rpc.client import Client +from utils.server_api import SERVICE_NAME, API_VERSION, API_ID_GET_SIM_STATE + + +class SimStateClient(Client): + def __init__(self, enableLease: bool = False): + super().__init__(SERVICE_NAME, enableLease) + + def Init(self): + self._RegistApi(API_ID_GET_SIM_STATE, 0) + self._SetApiVerson(API_VERSION) + + def GetSimState_client_call(self): + c, d = self._Call(API_ID_GET_SIM_STATE, json.dumps("")) + return c, d + + + +if __name__ == "__main__": + # initialize channel factory. + ChannelFactoryInitialize(0) + + # create client + client = SimStateClient() + client.Init() + client.SetTimeout(5.0) + + # get server version + code, serverApiVersion = client.GetServerApiVersion() + print("server api version:", serverApiVersion) + + # wait lease applied + client.WaitLeaseApplied() + print("lease applied") + # test api + while True: + try: + c, d = client.GetSimState_client_call() + print("client get sim state ret:", c) + print("sim state data:", d) + except Exception as e: + print("Error getting sim state:", e) + time.sleep(1.0) + + diff --git a/teleop/utils/sim_state_topic.py b/teleop/utils/sim_state_topic.py new file mode 100644 index 0000000..dc5db54 --- /dev/null +++ b/teleop/utils/sim_state_topic.py @@ -0,0 +1,285 @@ +# Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. +# License: Apache License, Version 2.0 +""" +Simple sim state subscriber class +Subscribe to rt/sim_state_cmd topic and write to shared memory +""" + +import threading +import time +import json +from multiprocessing import shared_memory +from typing import Any, Dict, Optional +from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize +from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ + + +class SharedMemoryManager: + """Shared memory manager""" + + def __init__(self, name: str = None, size: int = 512): + """Initialize shared memory manager + + Args: + name: shared memory name, if None, create new one + size: shared memory size (bytes) + """ + self.size = size + self.lock = threading.RLock() # reentrant lock + + if name: + try: + self.shm = shared_memory.SharedMemory(name=name) + self.shm_name = name + self.created = False + except FileNotFoundError: + self.shm = shared_memory.SharedMemory(create=True, size=size) + self.shm_name = self.shm.name + self.created = True + else: + self.shm = shared_memory.SharedMemory(create=True, size=size) + self.shm_name = self.shm.name + self.created = True + + def write_data(self, data: Dict[str, Any]) -> bool: + """Write data to shared memory + + Args: + data: data to write + + Returns: + bool: write success or not + """ + try: + with self.lock: + json_str = json.dumps(data) + json_bytes = json_str.encode('utf-8') + + if len(json_bytes) > self.size - 8: # reserve 8 bytes for length and timestamp + print(f"Warning: Data too large for shared memory ({len(json_bytes)} > {self.size - 8})") + return False + + # write timestamp (4 bytes) and data length (4 bytes) + timestamp = int(time.time()) & 0xFFFFFFFF # 32-bit timestamp, use bitmask to ensure in range + self.shm.buf[0:4] = timestamp.to_bytes(4, 'little') + self.shm.buf[4:8] = len(json_bytes).to_bytes(4, 'little') + + # write data + self.shm.buf[8:8+len(json_bytes)] = json_bytes + return True + + except Exception as e: + print(f"Error writing to shared memory: {e}") + return False + + def read_data(self) -> Optional[Dict[str, Any]]: + """Read data from shared memory + + Returns: + Dict[str, Any]: read data dictionary, return None if failed + """ + try: + with self.lock: + # read timestamp and data length + timestamp = int.from_bytes(self.shm.buf[0:4], 'little') + data_len = int.from_bytes(self.shm.buf[4:8], 'little') + + if data_len == 0: + return None + + # read data + json_bytes = bytes(self.shm.buf[8:8+data_len]) + data = json.loads(json_bytes.decode('utf-8')) + data['_timestamp'] = timestamp # add timestamp information + return data + + except Exception as e: + print(f"Error reading from shared memory: {e}") + return None + + def get_name(self) -> str: + """Get shared memory name""" + return self.shm_name + + def cleanup(self): + """Clean up shared memory""" + if hasattr(self, 'shm') and self.shm: + self.shm.close() + if self.created: + try: + self.shm.unlink() + except: + pass + + def __del__(self): + """Destructor""" + self.cleanup() + + +class SimStateSubscriber: + """Simple sim state subscriber class""" + + def __init__(self, shm_name: str = "sim_state_cmd_data", shm_size: int = 3096): + """Initialize the subscriber + + Args: + shm_name: shared memory name + shm_size: shared memory size + """ + self.shm_name = shm_name + self.shm_size = shm_size + self.running = False + self.subscriber = None + self.subscribe_thread = None + self.shared_memory = None + + # initialize shared memory + self._setup_shared_memory() + + print(f"[SimStateSubscriber] Initialized with shared memory: {shm_name}") + + def _setup_shared_memory(self): + """Setup shared memory""" + try: + self.shared_memory = SharedMemoryManager(self.shm_name, self.shm_size) + print(f"[SimStateSubscriber] Shared memory setup successfully") + except Exception as e: + print(f"[SimStateSubscriber] Failed to setup shared memory: {e}") + + def _message_handler(self, msg: String_): + """Handle received messages""" + try: + # parse the received message + data = json.loads(msg.data) + # print(f"[SimStateSubscriber] Received message: {data}") + + # write to shared memory + if self.shared_memory: + self.shared_memory.write_data(data) + + except Exception as e: + print(f"[SimStateSubscriber] Error processing message: {e}") + + def _subscribe_loop(self): + """Subscribe loop thread""" + print(f"[SimStateSubscriber] Subscribe thread started") + + while self.running: + try: + time.sleep(0.001) # keep thread alive + except Exception as e: + print(f"[SimStateSubscriber] Error in subscribe loop: {e}") + time.sleep(0.01) + + print(f"[SimStateSubscriber] Subscribe thread stopped") + + def start_subscribe(self): + """Start subscribing""" + if self.running: + print(f"[SimStateSubscriber] Already running") + return + + try: + # setup subscriber + self.subscriber = ChannelSubscriber("rt/sim_state", String_) + self.subscriber.Init(self._message_handler, 10) + + self.running = True + print(f"[SimStateSubscriber] Subscriber initialized") + # start subscribe thread + self.subscribe_thread = threading.Thread(target=self._subscribe_loop) + self.subscribe_thread.daemon = True + self.subscribe_thread.start() + + print(f"[SimStateSubscriber] Started subscribing to rt/sim_state_cmd") + + except Exception as e: + print(f"[SimStateSubscriber] Failed to start subscribing: {e}") + self.running = False + + def stop_subscribe(self): + """Stop subscribing""" + if not self.running: + return + + print(f"[SimStateSubscriber] Stopping subscriber...") + self.running = False + + # wait for thread to finish + if self.subscribe_thread: + self.subscribe_thread.join(timeout=1.0) + + print(f"[SimStateSubscriber] Subscriber stopped") + + def read_data(self) -> Optional[Dict[str, Any]]: + """Read data from shared memory + + Returns: + Dict: received data, None if no data or error + """ + try: + if self.shared_memory: + return self.shared_memory.read_data() + return None + except Exception as e: + print(f"[SimStateSubscriber] Error reading data: {e}") + return None + + def is_running(self) -> bool: + """Check if subscriber is running""" + return self.running + + def __del__(self): + """Destructor""" + self.stop_subscribe() + + +# convenient functions +def create_sim_state_subscriber(shm_name: str = "sim_state_cmd_data", shm_size: int = 3096) -> SimStateSubscriber: + """Create a sim state subscriber instance + + Args: + shm_name: shared memory name + shm_size: shared memory size + + Returns: + SimStateSubscriber: subscriber instance + """ + return SimStateSubscriber(shm_name, shm_size) + + +def start_sim_state_subscribe(shm_name: str = "sim_state_cmd_data", shm_size: int = 3096) -> SimStateSubscriber: + """Start sim state subscribing + + Args: + shm_name: shared memory name + shm_size: shared memory size + + Returns: + SimStateSubscriber: started subscriber instance + """ + subscriber = create_sim_state_subscriber(shm_name, shm_size) + subscriber.start_subscribe() + return subscriber + + +# if __name__ == "__main__": +# # example usage +# print("Starting sim state subscriber...") +# ChannelFactoryInitialize(0) +# # create and start subscriber +# subscriber = start_sim_state_subscribe() + +# try: +# # keep running and check for data +# while True: +# data = subscriber.read_data() +# if data: +# print(f"Read data: {data}") +# time.sleep(1) + +# except KeyboardInterrupt: +# print("\nInterrupted by user") +# finally: +# subscriber.stop_subscribe() +# print("Subscriber stopped") \ No newline at end of file From c346a63734e855f0bad7c5e9a53a9ad0004f99d1 Mon Sep 17 00:00:00 2001 From: "wei.li" Date: Fri, 4 Jul 2025 09:55:06 +0800 Subject: [PATCH 28/32] add 'default=gripper' and update tpoic --- teleop/teleop_hand_and_arm.py | 8 +++--- teleop/utils/server_api.py | 8 ------ teleop/utils/sim_state_client.py | 48 -------------------------------- 3 files changed, 4 insertions(+), 60 deletions(-) delete mode 100644 teleop/utils/server_api.py delete mode 100644 teleop/utils/sim_state_client.py diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 5a2c25b..1f827b2 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -58,7 +58,7 @@ if __name__ == '__main__': parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') - parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') + parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'],default="gripper", help='Select end effector controller') parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') parser.add_argument('--motion', action = 'store_true', help = 'Enable motion control mode') @@ -201,12 +201,12 @@ if __name__ == '__main__': tv_resized_image = cv2.resize(tv_img_array, (tv_img_shape[1] // 2, tv_img_shape[0] // 2)) cv2.imshow("record image", tv_resized_image) key = cv2.waitKey(1) & 0xFF - if key == ord('s'): + if key == ord('q'): stop_listening() running = False if args.sim: publish_reset_category(2, reset_pose_publisher) - elif key == ord('q'): + elif key == ord('s'): should_toggle_recording = True elif key == ord('a'): if args.sim: @@ -386,7 +386,7 @@ if __name__ == '__main__': "qpos": current_body_action, }, } - if args.is_use_sim: + if args.sim: sim_state = sim_state_subscriber.read_data() recorder.add_item(colors=colors, depths=depths, states=states, actions=actions, sim_state=sim_state) else: diff --git a/teleop/utils/server_api.py b/teleop/utils/server_api.py deleted file mode 100644 index 98bfb4b..0000000 --- a/teleop/utils/server_api.py +++ /dev/null @@ -1,8 +0,0 @@ -# service name -SERVICE_NAME = "get_sim_state" - -# api version -API_VERSION = "1.0.0.1" - -# api id -API_ID_GET_SIM_STATE = 1008 \ No newline at end of file diff --git a/teleop/utils/sim_state_client.py b/teleop/utils/sim_state_client.py deleted file mode 100644 index 972824b..0000000 --- a/teleop/utils/sim_state_client.py +++ /dev/null @@ -1,48 +0,0 @@ -import time -import json -from unitree_sdk2py.core.channel import ChannelFactoryInitialize -from unitree_sdk2py.rpc.client import Client -from utils.server_api import SERVICE_NAME, API_VERSION, API_ID_GET_SIM_STATE - - -class SimStateClient(Client): - def __init__(self, enableLease: bool = False): - super().__init__(SERVICE_NAME, enableLease) - - def Init(self): - self._RegistApi(API_ID_GET_SIM_STATE, 0) - self._SetApiVerson(API_VERSION) - - def GetSimState_client_call(self): - c, d = self._Call(API_ID_GET_SIM_STATE, json.dumps("")) - return c, d - - - -if __name__ == "__main__": - # initialize channel factory. - ChannelFactoryInitialize(0) - - # create client - client = SimStateClient() - client.Init() - client.SetTimeout(5.0) - - # get server version - code, serverApiVersion = client.GetServerApiVersion() - print("server api version:", serverApiVersion) - - # wait lease applied - client.WaitLeaseApplied() - print("lease applied") - # test api - while True: - try: - c, d = client.GetSimState_client_call() - print("client get sim state ret:", c) - print("sim state data:", d) - except Exception as e: - print("Error getting sim state:", e) - time.sleep(1.0) - - From 39b64fafa05a377f2a8e20f57015e7cb3bd27380 Mon Sep 17 00:00:00 2001 From: silencht Date: Fri, 4 Jul 2025 20:43:19 +0800 Subject: [PATCH 29/32] [fix] minor issue --- teleop/robot_control/robot_arm.py | 2 +- teleop/teleop_hand_and_arm.py | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index 9ee69b0..bcecc0c 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -629,7 +629,7 @@ class H1_2_ArmController: self._gradual_time = None # initialize lowcmd publisher and lowstate subscriber - ChannelFactoryInitialize(1) + ChannelFactoryInitialize(0) self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) self.lowcmd_publisher.Init() self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, hg_LowState) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 1f827b2..ac9f94a 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -58,7 +58,7 @@ if __name__ == '__main__': parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source') parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller') - parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'],default="gripper", help='Select end effector controller') + parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller') parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') parser.add_argument('--motion', action = 'store_true', help = 'Enable motion control mode') @@ -177,13 +177,15 @@ if __name__ == '__main__': reset_pose_publisher.Init() from teleop.utils.sim_state_topic import start_sim_state_subscribe sim_state_subscriber = start_sim_state_subscribe() - # xr mode + + # controller + motion mode if args.xr_mode == 'controller' and args.motion: from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient sport_client = LocoClient() sport_client.SetTimeout(0.0001) sport_client.Init() + # record + headless mode if args.record and args.headless: recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = False) elif args.record and not args.headless: From ff9e53ebcb9d909d2282957212865d96ee9f1fd6 Mon Sep 17 00:00:00 2001 From: silencht Date: Mon, 7 Jul 2025 16:11:57 +0800 Subject: [PATCH 30/32] [big release] Please refer README: Release Note --- .gitmodules | 6 + LICENSE | 7 +- README.md | 455 ++-- README_ja-JP.md | 556 ++-- README_zh-CN.md | 431 ++-- requirements.txt | 20 +- teleop/open_television/__init__.py | 4 - teleop/open_television/_test_television.py | 86 - teleop/open_television/_test_tv_wrapper.py | 95 - teleop/open_television/setup.py | 48 - teleop/open_television/television.py | 514 ---- teleop/open_television/tv_wrapper.py | 410 --- teleop/robot_control/dex-retargeting | 1 + .../dex_retargeting/CITATION.cff | 22 - teleop/robot_control/dex_retargeting/LICENSE | 21 - .../robot_control/dex_retargeting/__init__.py | 1 - .../dex_retargeting/constants.py | 85 - .../dex_retargeting/kinematics_adaptor.py | 102 - .../dex_retargeting/optimizer.py | 516 ---- .../dex_retargeting/optimizer_utils.py | 17 - .../dex_retargeting/retargeting_config.py | 255 -- .../dex_retargeting/robot_wrapper.py | 93 - .../dex_retargeting/seq_retarget.py | 151 -- .../robot_control/dex_retargeting/yourdfpy.py | 2237 ----------------- teleop/robot_control/hand_retargeting.py | 2 +- teleop/robot_control/robot_hand_unitree.py | 4 +- teleop/teleop_hand_and_arm.py | 8 +- teleop/televuer | 1 + ...e-unitree-opject-unitree_sim_isaaclab-.txt | 1 - teleop/utils/sim_state_topic.py | 53 +- 30 files changed, 750 insertions(+), 5452 deletions(-) create mode 100644 .gitmodules delete mode 100644 teleop/open_television/__init__.py delete mode 100644 teleop/open_television/_test_television.py delete mode 100644 teleop/open_television/_test_tv_wrapper.py delete mode 100644 teleop/open_television/setup.py delete mode 100644 teleop/open_television/television.py delete mode 100644 teleop/open_television/tv_wrapper.py create mode 160000 teleop/robot_control/dex-retargeting delete mode 100644 teleop/robot_control/dex_retargeting/CITATION.cff delete mode 100644 teleop/robot_control/dex_retargeting/LICENSE delete mode 100644 teleop/robot_control/dex_retargeting/__init__.py delete mode 100644 teleop/robot_control/dex_retargeting/constants.py delete mode 100644 teleop/robot_control/dex_retargeting/kinematics_adaptor.py delete mode 100644 teleop/robot_control/dex_retargeting/optimizer.py delete mode 100644 teleop/robot_control/dex_retargeting/optimizer_utils.py delete mode 100644 teleop/robot_control/dex_retargeting/retargeting_config.py delete mode 100644 teleop/robot_control/dex_retargeting/robot_wrapper.py delete mode 100644 teleop/robot_control/dex_retargeting/seq_retarget.py delete mode 100644 teleop/robot_control/dex_retargeting/yourdfpy.py create mode 160000 teleop/televuer delete mode 100644 teleop/utils/data/-home-unitree-Code-unitree-opject-unitree_sim_isaaclab-.txt diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..7e27969 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "teleop/televuer"] + path = teleop/televuer + url = https://github.com/silencht/televuer +[submodule "teleop/robot_control/dex-retargeting"] + path = teleop/robot_control/dex-retargeting + url = https://github.com/silencht/dex-retargeting diff --git a/LICENSE b/LICENSE index b241a5c..1c01999 100644 --- a/LICENSE +++ b/LICENSE @@ -23,10 +23,7 @@ This code builds upon following open-source code-bases. Please visit the URLs to 5) https://github.com/casadi/casadi 6) https://github.com/meshcat-dev/meshcat-python 7) https://github.com/zeromq/pyzmq -8) https://github.com/unitreerobotics/unitree_dds_wrapper -9) https://github.com/tonyzhaozh/act -10) https://github.com/facebookresearch/detr -11) https://github.com/Dingry/BunnyVisionPro -12) https://github.com/unitreerobotics/unitree_sdk2_python +8) https://github.com/Dingry/BunnyVisionPro +9) https://github.com/unitreerobotics/unitree_sdk2_python ------------------ diff --git a/README.md b/README.md index 1999e03..0c985f9 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,14 @@
-

avp_teleoperate

-

Unitree Robotics

+

xr_teleoperate

+ + Unitree LOGO +

English | 中文 | 日本語

+ # 📺 Video Demo

@@ -26,219 +29,254 @@

+# 🔖 Release Note + +1. **Upgraded the Vuer library** to support more XR device modes. The project has been renamed from **`avp_teleoperate`** to **`xr_teleoperate`** to better reflect its broader scope — now supporting not only Apple Vision Pro but also Meta Quest 3 (with controllers) and PICO 4 Ultra Enterprise (with controllers). +2. **Modularized** parts of the codebase and introduced Git submodules (`git submodule`) for better structure and maintainability. +3. Added **headless**, **motion**, and **simulation** modes. The startup parameter configuration has been improved for ease of use (see Section 2.2). The **simulation** mode enables environment validation and hardware failure diagnostics. + +4. Changed the default hand retarget algorithm from Vector to **DexPilot**, improving the precision and intuitiveness of fingertip pinching. + +5. Various other improvements and refinements. # 0. 📖 Introduction -This repository implements teleoperation of the **Unitree humanoid robot** using **XR Devices** ( such as Apple Vision Pro、 PICO 4 Ultra Enterprise or Meta Quest 3 ). -Here are the currently supported robots, +This repository implements **teleoperation** control of a **Unitree humanoid robot** using **XR (Extended Reality) devices ** (such as Apple Vision Pro, PICO 4 Ultra Enterprise, or Meta Quest 3). + +Here are the required devices and wiring diagram, + +

+ + System Diagram + +

+ + +The currently supported devices in this repository: - - + + + + + + - - + + - - + + - - + + - - + + - - + + - - + + - - + +
🤖 Robot ⚪ Status 🤖 Robot⚪ Status
G1 (29 DoF)✅ Complete
G1 (29DoF) ✅ Completed G1 (23 DoF)✅ Complete
G1 (23DoF) ✅ Completed H1 (4‑DoF arm)✅ Complete
H1 (Arm 4DoF) ✅ Completed H1_2 (7‑DoF arm)✅ Complete
H1_2 (Arm 7DoF) ✅ Completed Dex1‑1 gripper✅ Complete
Dex3-1 hand ✅ Completed Dex3‑1 dexterous hand✅ Complete
Inspire hand ✅ Completed Inspire dexterous hand✅ Complete
... ... ··· ···
+# 1. 📦 Installation -Here are the required devices and wiring diagram, - -

- - Watch the Document - -

- -This is a network topology diagram, using the G1 robot as an example, +We tested our code on Ubuntu 20.04 and Ubuntu 22.04, other operating systems may be configured differently. This document primarily describes the **default mode**. -

- - Watch the Document - -

- - - - - -# 1. 📦 Prerequisites +For more information, you can refer to [Official Documentation ](https://support.unitree.com/home/zh/Teleoperation) and [OpenTeleVision](https://github.com/OpenTeleVision/TeleVision). -We tested our code on Ubuntu 20.04 and Ubuntu 22.04, other operating systems may be configured differently. +## 1.1 📥 basic -For more information, you can refer to [Official Documentation ](https://support.unitree.com/home/zh/Teleoperation) and [OpenTeleVision](https://github.com/OpenTeleVision/TeleVision). +```bash +# Create a conda environment +(base) unitree@Host:~$ conda create -n tv python=3.10 pinocchio=3.1.0 numpy=1.26.4 -c conda-forge +(base) unitree@Host:~$ conda activate tv +# Clone this repo +(tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/xr_teleoperate.git +(tv) unitree@Host:~$ cd xr_teleoperate +# Shallow clone submodule +(tv) unitree@Host:~/xr_teleoperate$ git submodule update --init --depth 1 +# Install televuer submodule +(tv) unitree@Host:~/xr_teleoperate$ cd teleop/televuer +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ pip install -e . +# Generate the certificate files required for televuer submodule +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ openssl req -x509 -nodes -days 365 -newkey rsa:2048 -keyout key.pem -out cert.pem +# Install dex-retargeting submodule +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ cd ../robot_control/dex-retargeting/ +(tv) unitree@Host:~/xr_teleoperate/teleop/robot_control/dex-retargeting$ pip install -e . +# Install additional dependencies required by this repo +(tv) unitree@Host:~/xr_teleoperate/teleop/robot_control/dex-retargeting$ cd ../../../ +(tv) unitree@Host:~/xr_teleoperate$ pip install -r requirements.txt +``` -## 1.1 🦾 inverse kinematics +## 1.2 🕹️ unitree_sdk2_python ```bash -unitree@Host:~$ conda create -n tv python=3.8 -unitree@Host:~$ conda activate tv -# If you use `pip install`, Make sure pinocchio version is 3.1.0 -(tv) unitree@Host:~$ conda install pinocchio -c conda-forge -(tv) unitree@Host:~$ pip install meshcat -(tv) unitree@Host:~$ pip install casadi +# Install unitree_sdk2_python library which handles communication with the robot +(tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/unitree_sdk2_python.git +(tv) unitree@Host:~$ cd unitree_sdk2_python +(tv) unitree@Host:~$ pip install -e . ``` -> p.s. All identifiers in front of the command are meant for prompting: **Which device and directory the command should be executed on**. +> **Note 1**: The [unitree_dds_wrapper](https://github.com/unitreerobotics/unitree_dds_wrapper) in the original h1_2 branch was a temporary version. It has now been fully migrated to the official Python-based control and communication library: [unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python). +> +> **Note 2**: All identifiers in front of the command are meant for prompting: **Which device and directory the command should be executed on**. > -In the Ubuntu system's `~/.bashrc` file, the default configuration is: `PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ '` +> In the Ubuntu system's `~/.bashrc` file, the default configuration is: `PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ '` > > Taking the command `(tv) unitree@Host:~$ pip install meshcat` as an example: > > - `(tv)` Indicates the shell is in the conda environment named `tv`. ->- `unitree@Host:~` Shows the user `\u` `unitree` is logged into the device `\h` `Host`, with the current working directory `\w` as `$HOME`. +> - `unitree@Host:~` Shows the user `\u` `unitree` is logged into the device `\h` `Host`, with the current working directory `\w` as `$HOME`. > - `$` shows the current shell is Bash (for non-root users). > - `pip install meshcat` is the command `unitree` wants to execute on `Host`. -> +> > You can refer to [Harley Hahn's Guide to Unix and Linux](https://www.harley.com/unix-book/book/chapters/04.html#H) and [Conda User Guide](https://docs.conda.io/projects/conda/en/latest/user-guide/getting-started.html) to learn more. -## 1.2 🕹️ unitree_sdk2_python - -```bash -# Install unitree_sdk2_python. -(tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/unitree_sdk2_python.git -(tv) unitree@Host:~$ cd unitree_sdk2_python -(tv) unitree@Host:~$ pip install -e . -``` - -> p.s. The [unitree_dds_wrapper](https://github.com/unitreerobotics/unitree_dds_wrapper) in the original h1_2 branch was a temporary version. It has now been fully migrated to the official Python-based control and communication library: [unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python). +# 2. 💻 Simulation Deployment +## 2.1 📥 Environment Setup +First, install [unitree_sim_isaaclab](https://github.com/unitreerobotics/unitree_sim_isaaclab). Follow that repo’s README. -# 2. ⚙️ Configuration - -## 2.1 📥 basic +Then launch the simulation with a G1(29 DoF) and Dex3 hand configuration: ```bash -(tv) unitree@Host:~$ cd ~ -(tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/avp_teleoperate.git -(tv) unitree@Host:~$ cd ~/avp_teleoperate -(tv) unitree@Host:~$ pip install -r requirements.txt +(base) unitree@Host:~$ conda activate unitree_sim_env +(unitree_sim_env) unitree@Host:~$ cd ~/unitree_sim_isaaclab +(unitree_sim_env) unitree@Host:~/unitree_sim_isaaclab$ python sim_main.py --device cpu --enable_cameras --task Isaac-PickPlace-Cylinder-G129-Dex3-Joint --enable_dex3_dds --robot_type g129 ``` -## 2.2 🔌 Local streaming +After simulation starts, click once in the window to activate it. The terminal will show: `controller started, start main loop...` -**2.2.1 Apple Vision Pro** +Here is the simulation GUI: -Apple does not allow WebXR on non-https connections. To test the application locally, we need to create a self-signed certificate and install it on the client. You need a ubuntu machine and a router. Connect the Apple Vision Pro and the ubuntu **Host machine** to the same router. +

Simulation UI

-1. install mkcert: https://github.com/FiloSottile/mkcert -2. check **Host machine** local ip address: +## 2.2 🚀 Launch -```bash -(tv) unitree@Host:~/avp_teleoperate$ ifconfig | grep inet -``` +This program supports XR control of a physical robot or in simulation. Choose modes with command-line arguments: -Suppose the local ip address of the **Host machine** is `192.168.123.2` +- **Basic control parameters** -> p.s. You can use `ifconfig` command to check your **Host machine** ip address. +| ⚙️ Parameter | 📜 Description | 🔘 Options | 📌 Default | +| :---------: | :-------------------------------------------: | :----------------------------------------------------------: | :-------: | +| `--xr-mode` | Choose XR input mode | `hand` (**hand tracking**)
`controller` (**controller tracking**) | `hand` | +| `--arm` | Choose robot arm type (see 0. 📖 Introduction) | `G1_29`
`G1_23`
`H1_2`
`H1` | `G1_29` | +| `--ee` | Choose end-effector (see 0. 📖 Introduction) | `dex1`
`dex3`
`inspire1` | none | -3. create certificate: +- **Mode flags** -```bash -(tv) unitree@Host:~/avp_teleoperate$ mkcert -install && mkcert -cert-file cert.pem -key-file key.pem 192.168.123.2 localhost 127.0.0.1 -``` +| ⚙️ Flag | 📜 Description | +| :----------: | :----------------------------------------------------------: | +| `--record` | **Enable data recording**: after pressing **r** to start, press **s** to start/stop saving an episode. Can repeat. | +| `--motion` | **Enable motion control**: allows independent robot control during teleop. In hand mode, use the R3 remote for walking; in controller mode, use joystick for walking | +| `--headless` | Run without GUI (for headless PC2 deployment) | +| `--sim` | Enable **simulation mode** | -place the generated `cert.pem` and `key.pem` files in `teleop` +Assuming hand tracking with G1(29 DoF) + Dex3 in simulation with recording: ```bash -(tv) unitree@Host:~/avp_teleoperate$ cp cert.pem key.pem ~/avp_teleoperate/teleop/ +(tv) unitree@Host:~$ cd ~/xr_teleoperate/teleop/ +(tv) unitree@Host:~/xr_teleoperate/teleop/$ python teleop_hand_and_arm.py --xr-mode=hand --arm=G1_29 --ee=dex3 --sim --record +# Simplified (defaults apply): +(tv) unitree@Host:~/xr_teleoperate/teleop/$ python teleop_hand_and_arm.py --ee=dex3 --sim --record ``` -4. open firewall on server: +After the program starts, the terminal shows: -```bash -(tv) unitree@Host:~/avp_teleoperate$ sudo ufw allow 8012 -``` +

Terminal Start Log

-5. install ca-certificates on Apple Vision Pro: +Next steps: -```bash -(tv) unitree@Host:~/avp_teleoperate$ mkcert -CAROOT -``` +1. Wear your XR headset (e.g. Apple Vision Pro, PICO4, etc.) -Copy the `rootCA.pem` via AirDrop to Apple Vision Pro and install it. +2. Connect to the corresponding Wi‑Fi -Settings > General > About > Certificate Trust Settings. Under "Enable full trust for root certificates", turn on trust for the certificate. +3. Open a browser (e.g. Safari or PICO Browser) and go to: `https://192.168.123.2:8012?ws=wss://192.168.123.2:8012` -> In the new version of Vision OS 2, this step is different: After copying the certificate to the Apple Vision Pro device via AirDrop, a certificate-related information section will appear below the account bar in the top left corner of the Settings app. Tap it to enable trust for the certificate. + > **Note 1**: This IP must match your **Host** IP (check with `ifconfig`). + > **Note 2**: You may see a warning page. Click **Advanced**, then **Proceed to IP (unsafe)**. -Settings > Apps > Safari > Advanced > Feature Flags > Enable WebXR Related Features. +

+ + vuer_unsafe + +

------- +4. In the Vuer web, click **Virtual Reality**. Allow all prompts to start the VR session. -**2.2.2 PICO 4 Ultra Enterprise or Meta Quest 3** +

Vuer UI

-We have tried using hand tracking for teleoperation on the PICO 4 Ultra Enterprise and Meta-Quest 3. +5. You’ll see the robot’s first-person view in the headset. The terminal prints connection info: + + ```bash + websocket is connected. id:dbb8537d-a58c-4c57-b49d-cbb91bd25b90 + default socket worker is up, adding clientEvents + Uplink task running. id:dbb8537d-a58c-4c57-b49d-cbb91bd25b90 + ``` -The system specifications of PICO 4 Ultra Enterprise: +6. Align your arm to the **robot’s initial pose** to avoid sudden movements at start: -> System Version: 5.12.6.U; Android version number: 14; Software version number: c000_cf01_bv1.0.1_sv5.12.6_202412121344_sparrow_b4244_user; browser version: [4.0.28 beta version](https://github.com/vuer-ai/vuer/issues/45#issuecomment-2674918619) +

Initial Pose

-The system specifications of Meta-Quest 3: +7. Press **r** in the terminal to begin teleoperation. You can now control the robot arm and dexterous hand. -> System version: 49829370066100510; Version: 62.0.0.273.343.570372087; Runtime version: 62.0.0.269.341.570372063; OS version: SQ3A.220605.009.A1. +8. During teleoperation, press **s** to start recording; press **s** again to stop and save. Repeatable process. -For more configuration steps, please refer to the [issue](https://github.com/unitreerobotics/avp_teleoperate/issues/32). +

Recording Process

-## 2.3 🔎 Unit Test +> **Note 1**: Recorded data is stored in `xr_teleoperate/teleop/utils/data` by default, with usage instructions at this repo: [unitree_IL_lerobot](https://github.com/unitreerobotics/unitree_IL_lerobot/tree/main?tab=readme-ov-file#data-collection-and-conversion). +> **Note 2**: Please pay attention to your disk space size during data recording. -This step is to verify that the environment is installed correctly. +## 2.3 🔚 Exit -comming soon. +Press **q** in the terminal (or “record image” window) to quit. -# 3. 🚀 Usage +# 3. 🤖 Physical Deployment -Please read the [Official Documentation ](https://support.unitree.com/home/zh/Teleoperation) at least once before starting this program. +Physical deployment steps are similar to simulation, with these key differences: +## 3.1 🖼️ Image Service -## 3.1 🖼️ Image Server +In the simulation environment, the image service is automatically enabled. For physical deployment, you need to manually start the image service based on your specific camera hardware. The steps are as follows: -Copy `image_server.py` in the `avp_teleoperate/teleop/image_server` directory to the **Development Computing Unit PC2** of Unitree Robot (G1/H1/H1_2/etc.), and execute the following command **in the PC2**: +Copy `image_server.py` in the `xr_teleoperate/teleop/image_server` directory to the **Development Computing Unit PC2** of Unitree Robot (G1/H1/H1_2/etc.), ```bash -# p.s.1 You can transfer image_server.py to PC2 via the scp command and then use ssh to remotely login to PC2 to execute it. +# p.s. You can transfer image_server.py to PC2 via the scp command and then use ssh to remotely login to PC2 to execute it. # Assuming the IP address of the development computing unit PC2 is 192.168.123.164, the transmission process is as follows: # log in to PC2 via SSH and create the folder for the image server (tv) unitree@Host:~$ ssh unitree@192.168.123.164 "mkdir -p ~/image_server" # Copy the local image_server.py to the ~/image_server directory on PC2 -(tv) unitree@Host:~$ scp ~/avp_teleoperate/teleop/image_server/image_server.py unitree@192.168.123.164:~/image_server/ +(tv) unitree@Host:~$ scp ~/xr_teleoperate/teleop/image_server/image_server.py unitree@192.168.123.164:~/image_server/ +``` +and execute the following command **in the PC2**: -# p.s.2 Currently, this image transmission program supports two methods for reading images: OpenCV and Realsense SDK. Please refer to the comments in the `ImageServer` class within `image_server.py` to configure your image transmission service according to your camera hardware. +```bash +# p.s. Currently, this image transmission program supports two methods for reading images: OpenCV and Realsense SDK. Please refer to the comments in the `ImageServer` class within `image_server.py` to configure your image transmission service according to your camera hardware. # Now located in Unitree Robot PC2 terminal unitree@PC2:~/image_server$ python image_server.py # You can see the terminal output as follows: @@ -250,12 +288,14 @@ unitree@PC2:~/image_server$ python image_server.py After image service is started, you can use `image_client.py` **in the Host** terminal to test whether the communication is successful: ```bash -(tv) unitree@Host:~/avp_teleoperate/teleop/image_server$ python image_client.py +(tv) unitree@Host:~/xr_teleoperate/teleop/image_server$ python image_client.py ``` -## 3.2 ✋ Inspire hands Server (optional) +## 3.2 ✋ Inspire Hand Service (optional) -> Note: If the selected robot configuration does not use the Inspire dexterous hand (Gen1), please ignore this section. +> **Note 1**: Skip this if your config does not use the Inspire hand. +> **Note 2**: For the G1 robot with [Inspire DFX hand](https://support.unitree.com/home/zh/G1_developer/inspire_dfx_dexterous_hand), see [issue #46](https://github.com/unitreerobotics/xr_teleoperate/issues/46). +> **Note 3**: For [Inspire FTP hand]((https://support.unitree.com/home/zh/G1_developer/inspire_ftp_dexterity_hand)), see [issue #48](https://github.com/unitreerobotics/xr_teleoperate/issues/48). You can refer to [Dexterous Hand Development](https://support.unitree.com/home/zh/H1_developer/Dexterous_hand) to configure related environments and compile control programs. First, use [this URL](https://oss-global-cdn.unitree.com/static/0a8335f7498548d28412c31ea047d4be.zip) to download the dexterous hand control interface program. Copy it to **PC2** of Unitree robots. @@ -275,143 +315,99 @@ unitree@PC2:~/h1_inspire_service/build$ ./h1_hand_example If two hands open and close continuously, it indicates success. Once successful, close the `./h1_hand_example` program in Terminal 2. -## 3.3 🚀 Start +## 3.3 🚀 Launch > ![Warning](https://img.shields.io/badge/Warning-Important-red) > > 1. Everyone must keep a safe distance from the robot to prevent any potential danger! -> > 2. Please make sure to read the [Official Documentation](https://support.unitree.com/home/zh/Teleoperation) at least once before running this program. -> -> 3. Always make sure that the robot has entered [debug mode (L2+R2)](https://support.unitree.com/home/zh/H1_developer/Remote_control) to stop the motion control program, this will avoid potential command conflict problems. -> - -It's best to have two operators to run this program, referred to as **Operator A** and **Operator B**. +> 3. Without `--motion`, always make sure that the robot has entered [debug mode (L2+R2)](https://support.unitree.com/home/zh/H1_developer/Remote_control) to stop the motion control program, this will avoid potential command conflict problems. +> 4. To use motion mode (with `--motion`), ensure the robot is in control mode (via [R3 remote](https://www.unitree.com/R3)). +> 5. In motion mode: +> - Right controller **A** = Exit teleop +> - Both joysticks pressed = soft emergency stop (switch to damping mode) +> - Left joystick = drive directions; +> - right joystick = turning; +> - max speed is limited in the code. - - -First, **Operator B** needs to perform the following steps: - -1. Modify the `img_config` image client configuration under the `if __name__ == '__main__':` section in `~/avp_teleoperate/teleop/teleop_hand_and_arm.py`. It should match the image server parameters you configured on PC2 in Section 3.1. - -2. Choose different launch parameters based on your robot configuration. Here are some example commands: - - ```bash - # 1. G1 (29DoF) Robot + Dex3-1 Dexterous Hand (Note: G1_29 is the default value for --arm, so it can be omitted) - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_29 --hand=dex3 - - # 2. G1 (29DoF) Robot only - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py - - # 3. G1 (23DoF) Robot - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_23 - - # 4. H1_2 Robot + Inspire hand - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=H1_2 --hand=inspire1 - - # 5. H1 Robot - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=H1 - - # 6. If you want to enable data visualization + recording, you can add the --record option - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_23 --record - ``` - -3. If the program starts successfully, the terminal will pause at the final line displaying the message: "Please enter the start signal (enter 'r' to start the subsequent program):" - - - -And then, **Operator A** needs to perform the following steps: - -1. Wear your Apple Vision Pro device. - -2. Open Safari on Apple Vision Pro and visit : https://192.168.123.2:8012?ws=wss://192.168.123.2:8012 - - > p.s. This IP address should match the IP address of your **Host machine**. - -3. Click `Enter VR` and `Allow` to start the VR session. - -4. You will see the robot's first-person perspective in the Apple Vision Pro. - - - -Next, **Operator B** can start teleoperation program by pressing the **r** key in the terminal. - -At this time, **Operator A** can remotely control the robot's arms (and dexterous hands). - -If the `--record` parameter is used, **Operator B** can press **s** key in the opened "record image" window to start recording data, and press **s** again to stop. This operation can be repeated as needed for multiple recordings. - -> p.s.1 Recorded data is stored in `avp_teleoperate/teleop/utils/data` by default, with usage instructions at this repo: [unitree_IL_lerobot](https://github.com/unitreerobotics/unitree_IL_lerobot/tree/main?tab=readme-ov-file#data-collection-and-conversion). -> -> p.s.2 Please pay attention to your disk space size during data recording. +Same as simulation but follow the safety warnings above. ## 3.4 🔚 Exit -To exit the program, **Operator B** can press the **q** key in the 'record image' window. - -> ![Warning](https://img.shields.io/badge/Warning-Important-red) +> ![Warning](https://img.shields.io/badge/Warning-Important-red) +> To avoid damaging the robot, it is recommended to position the robot's arms close to the initial pose before pressing **q** to exit. > -> To avoid damaging the robot, it's best to ensure that **Operator A** positions the robot's arms in a naturally lowered or appropriate position before **Operator B** presses **q** to exit. +> - In **Debug Mode**: After pressing the exit key, both arms will return to the robot's **initial pose** within 5 seconds, and then the control will end. +> +> - In **Motion Mode**: After pressing the exit key, both arms will return to the robot's **motion control pose** within 5 seconds, and then the control will end. + +Same as simulation but follow the safety warnings above. -# 4. 🗺️ Codebase Tutorial +# 4. 🗺️ Codebase Overview ``` -avp_teleoperate/ +xr_teleoperate/ │ ├── assets [Storage of robot URDF-related files] │ +├── hardware [3D‑printed hardware modules] +│ ├── teleop │ ├── image_server -│ │ ├── image_client.py [Used to receive image data from the robot image server] -│ │ ├── image_server.py [Capture images from cameras and send via network (Running on robot's on-board computer)] +│ │ ├── image_client.py [Used to receive image data from the robot image server] +│ │ ├── image_server.py [Capture images from cameras and send via network (Running on robot's Development Computing Unit PC2)] │ │ -│ ├── open_television -│ │ ├── television.py [Using Vuer to capture wrist and hand data from apple vision pro] -│ │ ├── tv_wrapper.py [Post-processing of captured data] +│ ├── televuer +│ │ ├── src/televuer +│ │ ├── television.py [captures XR devices's head, wrist, hand/controller data] +│ │ ├── tv_wrapper.py [Post-processing of captured data] +│ │ ├── test +│ │ ├── _test_television.py [test for television.py] +│ │ ├── _test_tv_wrapper.py [test for tv_wrapper.py] │ │ │ ├── robot_control -│ │ ├── robot_arm_ik.py [Inverse kinematics of the arm] -│ │ ├── robot_arm.py [Control dual arm joints and lock the others] +│ │ ├── src/dex-retargeting [Dexterous hand retargeting algorithm library] +│ │ ├── robot_arm_ik.py [Inverse kinematics of the arm] +│ │ ├── robot_arm.py [Control dual arm joints and lock the others] +│ │ ├── hand_retargeting.py [Dexterous hand retargeting algorithm library Wrapper] │ │ ├── robot_hand_inspire.py [Control inspire hand joints] │ │ ├── robot_hand_unitree.py [Control unitree hand joints] │ │ │ ├── utils -│ │ ├── episode_writer.py [Used to record data for imitation learning] -│ │ ├── mat_tool.py [Some small math tools] +│ │ ├── episode_writer.py [Used to record data for imitation learning] │ │ ├── weighted_moving_filter.py [For filtering joint data] │ │ ├── rerun_visualizer.py [For visualizing data during recording] │ │ -│ │──teleop_hand_and_arm.py [Startup execution code for teleoperation] +│ └── teleop_hand_and_arm.py [Startup execution code for teleoperation] ``` - - # 5. 🛠️ Hardware -## 5.1 📋 List - -| Item | Quantity | Link | Remarks | -| :--------------------------: | :------: | :----------------------------------------------------------: | :---------------------------------------------------------: | -| **Unitree Robot G1** | 1 | https://www.unitree.com/g1 | With development computing unit | -| **Apple Vision Pro** | 1 | https://www.apple.com/apple-vision-pro/ | | -| **Router** | 1 | | | -| **User PC** | 1 | | Recommended graphics card performance at RTX 4080 and above | -| **Head Stereo Camera** | 1 | [For reference only] http://e.tb.cn/h.TaZxgkpfWkNCakg?tk=KKz03Kyu04u | For head | -| **Head Camera Mount** | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/head_stereo_camera_mount.STEP | For mounting head stereo camera, FOV 130° | -| Intel RealSense D405 | 2 | https://www.intelrealsense.com/depth-camera-d405/ | For wrist | -| Wrist Ring Mount | 2 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/wrist_ring_mount.STEP | Used with wrist camera mount | -| Left Wrist Camera Mount | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/left_wrist_D405_camera_mount.STEP | For mounting left wrist RealSense D405 camera | -| Right Wrist Camera Mount | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/right_wrist_D405_camera_mount.STEP | For mounting right wrist RealSense D405 camera | -| M3 hex nuts | 4 | [For reference only] https://a.co/d/1opqtOr | For Wrist fastener | -| M3x12 screws | 4 | [For reference only] https://amzn.asia/d/aU9NHSf | For wrist fastener | -| M3x6 screws | 4 | [For reference only] https://amzn.asia/d/0nEz5dJ | For wrist fastener | -| **M4x14 screws** | 2 | [For reference only] https://amzn.asia/d/cfta55x | For head fastener | -| **M2x4 self-tapping screws** | 4 | [For reference only] https://amzn.asia/d/1msRa5B | For head fastener | +## 5.1 📋 Parts List + +| Item | Quantity | Link(s) | Remarks | +| :--------------------------: | :------: | :----------------------------------------------------------: | :----------------------------------------------------------: | +| **humanoid robot** | 1 | https://www.unitree.com | With development computing unit | +| **XR device** | 1 | https://www.apple.com/apple-vision-pro/
https://www.meta.com/quest/quest-3
https://www.picoxr.com/products/pico4-ultra-enterprise | | +| **Router** | 1 | | Required for **default mode**; **wireless mode** not need. | +| **User PC** | 1 | | In **simulation mode**, please use the [officially recommended](https://docs.isaacsim.omniverse.nvidia.com/4.5.0/installation/requirements.html) hardware resources for deployment. | +| **Head stereo camera** | 1 | [For reference only] http://e.tb.cn/h.TaZxgkpfWkNCakg?tk=KKz03Kyu04u | For robot head view | +| **Head camera mount** | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/head_stereo_camera_mount.STEP | For mounting head stereo camera | +| Intel RealSense D405 camera | 2 | https://www.intelrealsense.com/depth-camera-d405/ | For wrist | +| Wrist ring mount | 2 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/wrist_ring_mount.STEP | Used with wrist camera mount | +| Left wrist D405 mount | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/left_wrist_D405_camera_mount.STEP | For mounting left wrist RealSense D405 camera | +| Right wrist D405 mount | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/right_wrist_D405_camera_mount.STEP | For mounting right wrist RealSense D405 camera | +| M3-1 hex nuts | 4 | [For reference only] https://a.co/d/1opqtOr | For Wrist fastener | +| M3x12 screws | 4 | [For reference only] https://amzn.asia/d/aU9NHSf | For Wrist fastener | +| M3x6 screws | 4 | [For reference only] https://amzn.asia/d/0nEz5dJ | For Wrist fastener | +| **M4x14 screws** | 2 | [For reference only] https://amzn.asia/d/cfta55x | For head fastener | +| **M2x4 self‑tapping screws** | 4 | [For reference only] https://amzn.asia/d/1msRa5B | For head fastener | > Note: The bolded items are essential equipment for teleoperation tasks, while the other items are optional equipment for recording [datasets](https://huggingface.co/unitreerobotics). -## 5.2 🔨 Installation diagram +## 5.2 🔨 Mounting Diagrams @@ -471,15 +467,12 @@ avp_teleoperate/ This code builds upon following open-source code-bases. Please visit the URLs to see the respective LICENSES: -1) https://github.com/OpenTeleVision/TeleVision -2) https://github.com/dexsuite/dex-retargeting -3) https://github.com/vuer-ai/vuer -4) https://github.com/stack-of-tasks/pinocchio -5) https://github.com/casadi/casadi -6) https://github.com/meshcat-dev/meshcat-python -7) https://github.com/zeromq/pyzmq -8) https://github.com/unitreerobotics/unitree_dds_wrapper -9) https://github.com/tonyzhaozh/act -10) https://github.com/facebookresearch/detr -11) https://github.com/Dingry/BunnyVisionPro -12) https://github.com/unitreerobotics/unitree_sdk2_python +1. https://github.com/OpenTeleVision/TeleVision +2. https://github.com/dexsuite/dex-retargeting +3. https://github.com/vuer-ai/vuer +4. https://github.com/stack-of-tasks/pinocchio +5. https://github.com/casadi/casadi +6. https://github.com/meshcat-dev/meshcat-python +7. https://github.com/zeromq/pyzmq +8. https://github.com/Dingry/BunnyVisionPro +9. https://github.com/unitreerobotics/unitree_sdk2_python diff --git a/README_ja-JP.md b/README_ja-JP.md index 2edb0ed..d9bda66 100644 --- a/README_ja-JP.md +++ b/README_ja-JP.md @@ -1,12 +1,14 @@
-

avp_teleoperate

-

Unitree Robotics

+

xr_teleoperate

+ + Unitree LOGO +

English | 中文 | 日本語

-# 📺 ビデオデモ +# 📺 デモ動画

@@ -15,465 +17,395 @@ Video 1 -

G1 (29自由度) + Dex3-1

+

G1 (29DoF) + Dex3-1

Video 2 -

H1_2（腕 7自由度）

+

H1_2 (Arm 7DoF)

+# 🔖 更新内容 + +1. **Vuerライブラリをアップグレード**し、より多くのXRデバイスモードに対応しました。これに伴い、プロジェクト名を **`avp_teleoperate`** から **`xr_teleoperate`** に変更しました。従来の Apple Vision Pro に加え、**Meta Quest 3（コントローラー対応）** や **PICO 4 Ultra Enterprise（コントローラー対応）** にも対応しています。 +2. 一部の機能を**モジュール化**し、Gitサブモジュール（`git submodule`）を用いて管理・読み込みを行うことで、コード構造の明確化と保守性を向上させました。 +3. **ヘッドレスモード**、**運用モード**、**シミュレーションモード**を新たに追加し、起動パラメータの設定を最適化しました（第2.2節参照）。**シミュレーションモード**により、環境構成の検証やハードウェア故障の切り分けが容易になります。 +4. デフォルトの手指マッピングアルゴリズムを Vector から **DexPilot** に変更し、指先のつまみ動作の精度と操作性を向上させました。 +5. その他、さまざまな最適化を実施しました。 # 0. 📖 イントロダクション -このリポジトリは、**Apple Vision Pro** を使用して **Unitree ヒューマノイドロボット** を遠隔操作するためのものです。 -以下は本リポジトリで現在サポートされているロボットの種類です, +このリポジトリでは、**XR（拡張現実）デバイス**（Apple Vision Pro、PICO 4 Ultra Enterprise、Meta Quest 3など）を使用して**Unitreeヒューマノイドロボット**の**遠隔操作**を実装しています。 + +必要なデバイスと配線図は以下の通りです。 + +

+ + システム構成図 + +

+ + +このリポジトリで現在サポートされているデバイス: - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + + + + +
🤖 ロボット ⚪ ステータス 🤖 ロボット⚪ ステータス
G1（29自由度） ✅ 完了 G1 (29 DoF)✅ 実装済み
G1（23自由度） ✅ 完了 G1 (23 DoF)✅ 実装済み
H1（腕 4自由度） ✅ 完了 H1 (4自由度アーム)✅ 実装済み
H1_2（腕 7自由度） ✅ 完了 H1_2 (7自由度アーム)✅ 実装済み
Dex3-1 ハンド ✅ 完了 Dex1‑1グリッパー✅ 実装済み
Inspire ハンド ✅ 完了 Dex3‑1多指ハンド✅ 実装済み
... ... Inspire多指ハンド✅ 実装済み
··· ···
-以下は、必要なデバイスと配線図です： - -

- - Watch the Document - -

- -これはネットワークトポロジー図で、G1ロボットを例にしています： - -

- - Watch the Document - -

- +# 1. 📦 インストール -# 1. 📦 前提条件 +Ubuntu 20.04と22.04でテスト済みです。他のOSでは設定が異なる場合があります。本ドキュメントでは、主に通常モードについて説明します。 -私たちは Ubuntu 20.04 と Ubuntu 22.04 でコードをテストしました。他のオペレーティングシステムでは異なる設定が必要かもしれません。 +詳細は[公式ドキュメント](https://support.unitree.com/home/zh/Teleoperation)と[OpenTeleVision](https://github.com/OpenTeleVision/TeleVision)を参照してください。 -詳細については、[公式ドキュメント](https://support.unitree.com/home/zh/Teleoperation) および [OpenTeleVision](https://github.com/OpenTeleVision/TeleVision) を参照してください。 - -## 1.1 🦾 逆運動学 +## 1.1 📥 基本設定 ```bash -unitree@Host:~$ conda create -n tv python=3.8 -unitree@Host:~$ conda activate tv -# `pip install` を使用する場合、pinocchio のバージョンが 3.1.0 であることを確認してください -(tv) unitree@Host:~$ conda install pinocchio -c conda-forge -(tv) unitree@Host:~$ pip install meshcat -(tv) unitree@Host:~$ pip install casadi +# Create a conda environment +(base) unitree@Host:~$ conda create -n tv python=3.10 pinocchio=3.1.0 numpy=1.26.4 -c conda-forge +(base) unitree@Host:~$ conda activate tv +# Clone this repo +(tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/xr_teleoperate.git +(tv) unitree@Host:~$ cd xr_teleoperate +# Shallow clone submodule +(tv) unitree@Host:~/xr_teleoperate$ git submodule update --init --depth 1 +# Install televuer submodule +(tv) unitree@Host:~/xr_teleoperate$ cd teleop/televuer +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ pip install -e . +# Generate the certificate files required for televuer submodule +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ openssl req -x509 -nodes -days 365 -newkey rsa:2048 -keyout key.pem -out cert.pem +# Install dex-retargeting submodule +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ cd ../robot_control/dex-retargeting/ +(tv) unitree@Host:~/xr_teleoperate/teleop/robot_control/dex-retargeting$ pip install -e . +# Install additional dependencies required by this repo +(tv) unitree@Host:~/xr_teleoperate/teleop/robot_control/dex-retargeting$ cd ../../../ +(tv) unitree@Host:~/xr_teleoperate$ pip install -r requirements.txt ``` -> 注意：コマンドの前にあるすべての識別子は、**どのデバイスとディレクトリでコマンドを実行するべきか**を示すためのものです。 -> -Ubuntu システムの `~/.bashrc` ファイルでは、デフォルトの設定は次の通りです: `PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ '` -> -> 例として、`(tv) unitree@Host:~$ pip install meshcat` コマンドを取り上げます。 -> -> - `(tv)` はシェルが conda 環境 `tv` にあることを示します。 ->- `unitree@Host:~` はユーザー `\u` `unitree` がデバイス `\h` `Host` にログインしており、現在の作業ディレクトリ `\w` が `$HOME` であることを示します。 -> - `$` は現在のシェルが Bash であることを示します（非ルートユーザーの場合）。 -> - `pip install meshcat` は `unitree` が `Host` で実行したいコマンドです。 -> -> 詳細については、[Harley Hahn's Guide to Unix and Linux](https://www.harley.com/unix-book/book/chapters/04.html#H) および [Conda User Guide](https://docs.conda.io/projects/conda/en/latest/user-guide/getting-started.html) を参照してください。 - ## 1.2 🕹️ unitree_sdk2_python ```bash -# unitree_sdk2_python をインストールします。 +# ロボット通信用ライブラリインストール (tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/unitree_sdk2_python.git (tv) unitree@Host:~$ cd unitree_sdk2_python (tv) unitree@Host:~$ pip install -e . ``` -> 注意：元の h1_2 ブランチの [unitree_dds_wrapper](https://github.com/unitreerobotics/unitree_dds_wrapper) は一時的なバージョンであり、現在は公式の Python 版制御通信ライブラリ [unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python) に完全に移行されています。 +> **注1**: 元のh1_2ブランチのunitree_dds_wrapperは暫定版でした。現在は公式Python制御ライブラリunitree_sdk2_pythonに移行済みです。 +> +> **注2**: コマンド前の識別子は「どのデバイスでどのディレクトリで実行するか」を示しています。 +> +> Ubuntuの`~/.bashrc`デフォルト設定: `PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ '` +> +> 例: `(tv) unitree@Host:~$ pip install meshcat` +> +> - `(tv)` conda環境`tv`を表示 +> - `unitree@Host:~` ユーザー`unitree`が`Host`デバイスにログイン、カレントディレクトリは`$HOME` +> - `$` Bashシェル（非rootユーザー） +> - `pip install meshcat` は`Host`で実行するコマンド +# 2. 💻 シミュレーション環境 +## 2.1 📥 環境設定 -# 2. ⚙️ 設定 +まずunitree_sim_isaaclabをインストールし、READMEに従って設定します。 -## 2.1 📥 基本 +G1(29 DoF)とDex3ハンド構成でシミュレーションを起動: ```bash -(tv) unitree@Host:~$ cd ~ -(tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/avp_teleoperate.git -(tv) unitree@Host:~$ cd ~/avp_teleoperate -(tv) unitree@Host:~$ pip install -r requirements.txt +(base) unitree@Host:~$ conda activate unitree_sim_env +(unitree_sim_env) unitree@Host:~$ cd ~/unitree_sim_isaaclab +(unitree_sim_env) unitree@Host:~/unitree_sim_isaaclab$ python sim_main.py --device cpu --enable_cameras --task Isaac-PickPlace-Cylinder-G129-Dex3-Joint --enable_dex3_dds --robot_type g129 ``` -## 2.2 🔌 ローカルストリーミング +シミュレーション起動後、ウィンドウをクリックして有効化。ターミナルに`controller started, start main loop...`と表示されます。 -**Apple Vision Pro** +シミュレーションGUI: -Apple は非 HTTPS 接続での WebXR を許可していません。アプリケーションをローカルでテストするには、自己署名証明書を作成し、クライアントにインストールする必要があります。Ubuntu マシンとルーターが必要です。Apple Vision Pro と Ubuntu **ホストマシン** を同じルーターに接続します。 +

シミュレーションUI

-1. mkcert をインストールします: https://github.com/FiloSottile/mkcert -2. **ホストマシン** のローカル IP アドレスを確認します: +## 2.2 🚀 起動 -```bash -(tv) unitree@Host:~/avp_teleoperate$ ifconfig | grep inet -``` +物理ロボットとシミュレーションの両方でXR制御をサポート。コマンドライン引数でモード選択: -**ホストマシン** のローカル IP アドレスが `192.168.123.2` であると仮定します。 +- **基本制御パラメータ** -> p.s. `ifconfig` コマンドを使用して **ホストマシン** の IP アドレスを確認できます。 +| ⚙️ パラメータ | 📜 説明 | 🔘 オプション | 📌 デフォルト | +| :----------: | :----------------------------------: | :----------------------------------------------------------: | :----------: | +| `--xr-mode` | XR入力モード選択 | `hand` (**ハンドトラッキング**) `controller` (**コントローラートラッキング**) | `hand` | +| `--arm` | ロボットアームタイプ選択 (0. 📖 参照) | `G1_29` `G1_23` `H1_2` `H1` | `G1_29` | +| `--ee` | エンドエフェクタ選択 (0. 📖 参照) | `dex1` `dex3` `inspire1` | none | -3. 証明書を作成します: +- **モードフラグ** -```bash -(tv) unitree@Host:~/avp_teleoperate$ mkcert -install && mkcert -cert-file cert.pem -key-file key.pem 192.168.123.2 localhost 127.0.0.1 -``` +| ⚙️ フラグ | 📜 説明 | +| :----------: | :----------------------------------------------------------: | +| `--record` | **データ記録有効化**: **r**押下で開始後、**s**でエピソード記録開始/停止。繰り返し可能 | +| `--motion` | **モーション制御有効化**: 遠隔操作中に独立したロボット制御を許可。ハンドモードではR3リモコンで歩行、コントローラーモードではジョイスティックで歩行 | +| `--headless` | GUIなしで実行（ヘッドレスPC2展開用） | +| `--sim` | **シミュレーションモード**有効化 | -生成された `cert.pem` と `key.pem` ファイルを `teleop` に配置します。 +G1(29 DoF) + Dex3でハンドトラッキング、シミュレーション、記録モードで起動: ```bash -(tv) unitree@Host:~/avp_teleoperate$ cp cert.pem key.pem ~/avp_teleoperate/teleop/ +(tv) unitree@Host:~$ cd ~/xr_teleoperate/teleop/ +(tv) unitree@Host:~/xr_teleoperate/teleop/$ python teleop_hand_and_arm.py --xr-mode=hand --arm=G1_29 --ee=dex3 --sim --record +# 簡略化（デフォルト適用）: +(tv) unitree@Host:~/xr_teleoperate/teleop/$ python teleop_hand_and_arm.py --ee=dex3 --sim --record ``` -4. サーバーでファイアウォールを開きます: +プログラム起動後、ターミナル表示: -```bash -(tv) unitree@Host:~/avp_teleoperate$ sudo ufw allow 8012 -``` +

ターミナル起動ログ

-5. Apple Vision Pro に ca-certificates をインストールします: +次の手順: -```bash -(tv) unitree@Host:~/avp_teleoperate$ mkcert -CAROOT -``` - -`rootCA.pem` を AirDrop 経由で Apple Vision Pro にコピーし、インストールします。 - -設定 > 一般 > 情報 > 証明書信頼設定。「ルート証明書の完全な信頼を有効にする」の下で、証明書の信頼をオンにします。 - -設定 > アプリ > Safari > 高度な設定 > 機能フラグ > WebXR 関連機能を有効にします。 - -> 注意：新しい Vision OS 2 システムでは、この手順が異なります。証明書を AirDrop 経由で Apple Vision Pro デバイスにコピーした後、設定アプリの左上のアカウント欄の下に証明書関連情報欄が表示されます。それをクリックして、証明書の信頼を有効にします。 +1. XRヘッドセット（Apple Vision Pro、PICO4など）を装着 ------- +2. 対応するWi-Fiに接続 -**2.2.2 PICO 4 Ultra Enterprise or Meta Quest 3** +3. ブラウザ（SafariやPICO Browserなど）で以下にアクセス: `https://192.168.123.2:8012?ws=wss://192.168.123.2:8012` -PICO 4 Ultra Enterprise と Meta-Quest 3 において、ハンドトラッキングを用いたテレオペレーションを試みました。 + > **注1**: このIPは**Host**のIPと一致させる必要あり（`ifconfig`で確認）。 + > ​**​注2​**: 警告ページが表示される場合があります。[詳細設定]→[IPにアクセス（安全ではない）]を選択。 -PICO 4 Ultra Enterprise のシステム仕様： +

vuer_unsafe

-> システムバージョン：5.12.6.U；Android バージョン：14；ソフトウェアバージョン：c000_cf01_bv1.0.1_sv5.12.6_202412121344_sparrow_b4244_user; ブラウザーバージョン: [4.0.28 beta version](https://github.com/vuer-ai/vuer/issues/45#issuecomment-2674918619) +4. Vuerウェブで[Virtual Reality]をクリック。すべてのプロンプトを許可してVRセッションを開始。 -Meta-Quest 3 のシステム仕様： +

Vuer UI

-> システムバージョン：49829370066100510；バージョン：62.0.0.273.343.570372087；ランタイムバージョン：62.0.0.269.341.570372063；OS バージョン：SQ3A.220605.009.A1 +5. ヘッドセットにロボットの一人称視点が表示されます。ターミナルに接続情報が表示: - さらなる設定手順については、その [issue](https://github.com/unitreerobotics/avp_teleoperate/issues/32) をご覧ください。 +```bash +websocket is connected. id:dbb8537d-a58c-4c57-b49d-cbb91bd25b90 +default socket worker is up, adding clientEvents +Uplink task running. id:dbb8537d-a58c-4c57-b49d-cbb91bd25b90 +``` -## 2.3 🔎 ユニットテスト +6. 急な動きを防ぐため、ロボットの**初期姿勢**に腕を合わせる: -このステップは、環境が正しくインストールされているかを確認するためのものです。 +

初期姿勢

-近日公開。 +7. ターミナルで**r**を押して遠隔操作を開始。ロボットアームと多指ハンドを制御できます。 +8. 遠隔操作中、**s**で記録開始、再度**s**で停止・保存。繰り返し可能。 +

記録プロセス

+> **注1**: 記録データはデフォルトで`xr_teleoperate/teleop/utils/data`に保存。unitree_IL_lerobotで使用方法を確認。 +> **注2**: データ記録時はディスク容量に注意してください。 +## 2.3 🔚 終了 -# 3. 🚀 使用方法 +ターミナル（または「record image」ウィンドウ）で**q**を押して終了。 -このプログラムを開始する前に、[公式ドキュメント](https://support.unitree.com/home/zh/Teleoperation) を少なくとも一度は読んでください。 +# 3. 🤖 物理環境展開 +物理環境展開の手順はシミュレーションと似ていますが、以下の点が異なります: -## 3.1 🖼️ 画像サーバー +## 3.1 🖼️ 画像サービス -`avp_teleoperate/teleop/image_server` ディレクトリにある `image_server.py` を Unitree ロボット (G1/H1/H1_2 など) の **開発用コンピューティングユニット PC2** にコピーし、**PC2** で次のコマンドを実行します: +`xr_teleoperate/teleop/image_server`ディレクトリの`image_server.py`をUnitreeロボット(G1/H1/H1_2など)の**開発用計算ユニットPC2**にコピーし。 ```bash -# 注意1：scp コマンドを使用して image_server.py を PC2 に転送し、ssh を使用して PC2 にリモートログインして実行できます。 -# 開発用コンピューティングユニット PC2 の IP アドレスが 192.168.123.164 であると仮定すると、転送プロセスは以下のようになります： -# まず ssh で PC2 にログインし、画像サーバーのフォルダを作成します +# 補足: scpコマンドでimage_server.pyをPC2に転送後、sshでPC2にリモートログインして実行可能 +# 開発用計算ユニットPC2のIPが192.168.123.164の場合の転送手順: +# SSHでPC2にログインし、画像サーバー用フォルダ作成 (tv) unitree@Host:~$ ssh unitree@192.168.123.164 "mkdir -p ~/image_server" -# ローカルの image_server.py を PC2 の ~/image_server ディレクトリにコピーします -(tv) unitree@Host:~$ scp ~/avp_teleoperate/teleop/image_server/image_server.py unitree@192.168.123.164:~/image_server/ +# ローカルのimage_server.pyをPC2の~/image_serverディレクトリにコピー +(tv) unitree@Host:~$ scp ~/xr_teleoperate/teleop/image_server/image_server.py unitree@192.168.123.164:~/image_server/ +``` -# 注意2：現在、この画像転送プログラムは OpenCV と Realsense SDK の 2 つの画像読み取り方法をサポートしています。image_server.py の ImageServer クラスのコメントを読んで、カメラハードウェアに応じて画像転送サービスを設定してください。 -# 現在、Unitree ロボット PC2 端末にいます +**PC2**で以下を実行: + +```bash +# 補足: 現在この画像転送プログラムは、OpenCVとRealsense SDKの2つの画像読み取り方法をサポート。`image_server.py`内の`ImageServer`クラスのコメントを参照し、カメラハードウェアに合わせて画像転送サービスを設定。 +# UnitreeロボットPC2のターミナルで実行 unitree@PC2:~/image_server$ python image_server.py -# 端末に次のように出力されます： +# ターミナルに以下の出力が表示: # {'fps': 30, 'head_camera_type': 'opencv', 'head_camera_image_shape': [480, 1280], 'head_camera_id_numbers': [0]} # [Image Server] Head camera 0 resolution: 480.0 x 1280.0 # [Image Server] Image server has started, waiting for client connections... ``` -画像サービスが開始された後、**ホスト** 端末で `image_client.py` を使用して通信が成功したかどうかをテストできます: +画像サービス起動後、**Host**ターミナルで`image_client.py`を使用して通信テスト可能: ```bash -(tv) unitree@Host:~/avp_teleoperate/teleop/image_server$ python image_client.py +(tv) unitree@Host:~/xr_teleoperate/teleop/image_server$ python image_client.py ``` -## 3.2 ✋ Inspire ハンドサーバー（オプション） +## 3.2 ✋ Inspireハンドサービス（オプション） -> 注意: 選択したロボット構成に Inspire デクスタラスハンドが使用されていない場合、このセクションは無視してください。 +> **Note 1**: Skip this if your config does not use the Inspire hand. +> **Note 2**: For the G1 robot with [Inspire DFX hand](https://support.unitree.com/home/zh/G1_developer/inspire_dfx_dexterous_hand), see [issue #46](https://github.com/unitreerobotics/xr_teleoperate/issues/46). +> **Note 3**: For [Inspire FTP hand]((https://support.unitree.com/home/zh/G1_developer/inspire_ftp_dexterity_hand)), see [issue #48](https://github.com/unitreerobotics/xr_teleoperate/issues/48). -関連する環境を設定し、制御プログラムをコンパイルするには、[デクスタラスハンド開発](https://support.unitree.com/home/zh/H1_developer/Dexterous_hand) を参照できます。まず、[このリンク](https://oss-global-cdn.unitree.com/static/0a8335f7498548d28412c31ea047d4be.zip) を使用してデクスタラスハンド制御インターフェースプログラムをダウンロードし、Unitree ロボットの **PC2** にコピーします。 +多指ハンド開発を参照して関連環境を設定し、制御プログラムをコンパイル。このURLから多指ハンド制御インターフェースプログラムをダウンロードし、Unitreeロボットの**PC2**にコピー。 -Unitree ロボットの **PC2** で次のコマンドを実行します: +Unitreeロボットの**PC2**で以下を実行: ```bash unitree@PC2:~$ sudo apt install libboost-all-dev libspdlog-dev -# プロジェクトをビルドします +# プロジェクトビルド unitree@PC2:~$ cd h1_inspire_service & mkdir build & cd build unitree@PC2:~/h1_inspire_service/build$ cmake .. -DCMAKE_BUILD_TYPE=Release unitree@PC2:~/h1_inspire_service/build$ make -# ターミナル 1. h1 inspire ハンドサービスを実行します +# ターミナル1. h1 inspireハンドサービス実行 unitree@PC2:~/h1_inspire_service/build$ sudo ./inspire_hand -s /dev/ttyUSB0 -# ターミナル 2. サンプルを実行します +# ターミナル2. サンプル実行 unitree@PC2:~/h1_inspire_service/build$ ./h1_hand_example ``` -2 つの手が連続して開閉する場合、成功を示します。成功したら、ターミナル 2 で `./h1_hand_example` プログラムを閉じます。 +両手が連続的に開閉すれば成功。成功後、ターミナル2の`./h1_hand_example`プログラムを終了。 -## 3.3 🚀 スタート +## 3.3 🚀 起動 -> ![Warning](https://img.shields.io/badge/Warning-Important-red) +> ![Warning](https://img.shields.io/badge/Warning-Important-red) > -> 1. すべての人は、潜在的な危険を防ぐためにロボットから安全な距離を保つ必要があります！ -> -> 2. このプログラムを実行する前に、[公式ドキュメント](https://support.unitree.com/home/zh/Teleoperation) を少なくとも一度は読んでください。 -> -> 3. 常にロボットが [デバッグモード (L2+R2)](https://support.unitree.com/home/zh/H1_developer/Remote_control) に入っていることを確認し、モーションコントロールプログラムを停止して、潜在的なコマンドの競合問題を回避します。 -> - -このプログラムを実行するには、**オペレーター A** と **オペレーター B** と呼ばれる 2 人のオペレーターがいるのが最適です。 - - - -まず、**オペレーター B** は次の手順を実行する必要があります： - -1. `~/avp_teleoperate/teleop/teleop_hand_and_arm.py` の `if __name__ == '__main__':` コードの下にある `img_config` 画像クライアント設定を変更します。これは、3.1 節で PC2 に設定した画像サーバーパラメータと同じである必要があります。 -2. ロボット構成に応じて異なる起動パラメータを選択します。 以下は、いくつかのコマンド例です: - -```bash -# 1. G1（29自由度）ロボット + Dex3-1 多指ハンド（※ G1_29 は --arm のデフォルト値なので省略可能） -(tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_29 --hand=dex3 - -# 2. G1（29自由度）ロボットのみ -(tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py - -# 3. G1（23自由度）ロボット -(tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_23 - -# 4. H1_2 ロボット + Inspire Hand -(tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=H1_2 --hand=inspire1 - -# 5. H1 ロボット -(tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=H1 - -# 6. データの可視化および記録を有効にしたい場合は、--record オプションを追加してください -(tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_23 --record -``` - -3. プログラムが正常に起動すると、端末の最後の行に "Please enter the start signal (enter 'r' to start the subsequent program):" というメッセージが表示されます。 - - - -次に、**オペレーター A** は次の手順を実行します： - -1. Apple Vision Pro デバイスを装着します。 - -2. Apple Vision Pro で Safari を開き、次の URL にアクセスします: https://192.168.123.2:8012?ws=wss://192.168.123.2:8012 - - > p.s. この IP アドレスは **ホストマシン** の IP アドレスと一致する必要があります。 - -3. `Enter VR` をクリックし、`Allow` を選択して VR セッションを開始します。 - -4. Apple Vision Pro でロボットの一人称視点が表示されます。 - - - -次に、**オペレーター B** は端末で **r** キーを押して遠隔操作プログラムを開始します。 - -この時点で、**オペレーター A** はロボットのアーム（およびデクスタラスハンド）を遠隔操作できます。 - -`--record` パラメータを使用した場合、**オペレーター B** は開いている "record image" ウィンドウで **s** キーを押してデータの記録を開始し、再度 **s** キーを押して停止できます。必要に応じてこれを繰り返すことができます。 - -> 注意1：記録されたデータはデフォルトで `avp_teleoperate/teleop/utils/data` に保存されます。使用方法については、このリポジトリを参照してください: [unitree_IL_lerobot](https://github.com/unitreerobotics/unitree_IL_lerobot/tree/main?tab=readme-ov-file#data-collection-and-conversion)。 -> -> 注意2：データ記録時にはディスク容量に注意してください。 +> 1. すべての人はロボットから安全な距離を保ち、潜在的な危険を防止してください！ +> 2. このプログラムを実行する前に、少なくとも一度は公式ドキュメントをお読みください。 +> 3. `--motion`なしの場合、ロボットがデバッグモード（L2+R2）に入り、モーション制御プログラムが停止していることを確認してください。これにより潜在的なコマンド競合問題を回避できます。 +> 4. モーションモード（`--motion`あり）を使用する場合、ロボットが制御モード（R3リモコン経由）にあることを確認。 +> 5. モーションモード時: +> - 右コントローラー**A** = 遠隔操作終了 +> - 両ジョイスティック押下 = ソフト非常停止（ダンピングモードに切替） +> - 左ジョイスティック = 移動方向; +> - 右ジョイスティック = 旋回; +> - 最大速度はコード内で制限。 + +シミュレーションと同じですが、上記の安全警告に従ってください。 ## 3.4 🔚 終了 -プログラムを終了するには、**オペレーター B** は 'record image' ウィンドウで **q** キーを押すことができます。 - -> ![Warning](https://img.shields.io/badge/Warning-Important-red) +> ![Warning](https://img.shields.io/badge/Warning-Important-red) > -> ロボットを損傷しないようにするために、**オペレーター A** がロボットのアームを自然に下げた位置または適切な位置に配置した後、**オペレーター B** が **q** を押して終了するのが最適です。 - +> ロボット損傷を防ぐため、終了前にロボットの腕を初期姿勢に近づけることを推奨。 +> +> - **デバッグモード**: 終了キー押下後、両腕は5秒以内にロボットの**初期姿勢**に戻り、制御終了。 +> - **モーションモード**: 終了キー押下後、両腕は5秒以内にロボットの**モーション制御姿勢**に戻り、制御終了。 +シミュレーションと同じですが、上記の安全警告に従ってください。 -# 4. 🗺️ コードベースチュートリアル +# 4. 🗺️ コード構成 ``` -avp_teleoperate/ +xr_teleoperate/ +│ +├── assets [ロボットURDF関連ファイル格納] │ -├── assets [ロボット URDF 関連ファイルの保存] +├── hardware [3Dプリントハードウェアモジュール] │ ├── teleop │ ├── image_server -│ │ ├── image_client.py [ロボット画像サーバーから画像データを受信するために使用] -│ │ ├── image_server.py [カメラから画像をキャプチャし、ネットワーク経由で送信（ロボットのオンボードコンピュータで実行）] +│ │ ├── image_client.py [ロボット画像サーバーから画像データを受信] +│ │ ├── image_server.py [カメラから画像をキャプチャしネットワーク送信（ロボットの開発用計算ユニットPC2で実行）] │ │ -│ ├── open_television -│ │ ├── television.py [Apple Vision Pro から Vuer を使用して手首と手のデータをキャプチャ] -│ │ ├── tv_wrapper.py [キャプチャされたデータの後処理] +│ ├── televuer +│ │ ├── src/televuer +│ │ ├── television.py [XRデバイスの頭部、手首、手・コントローラーのデータを取得] +│ │ ├── tv_wrapper.py [取得データの後処理] +│ │ ├── test +│ │ ├── _test_television.py [television.pyのテスト] +│ │ ├── _test_tv_wrapper.py [tv_wrapper.pyのテスト] │ │ │ ├── robot_control -│ │ ├── robot_arm_ik.py [アームの逆運動学] -│ │ ├── robot_arm.py [デュアルアームジョイントを制御し、他の部分をロック] -│ │ ├── robot_hand_inspire.py [Inspire ハンドジョイントを制御] -│ │ ├── robot_hand_unitree.py [Unitree ハンドジョイントを制御] +│ │ ├── src/dex-retargeting [多指ハンドリターゲティングアルゴリズムライブラリ] +│ │ ├── robot_arm_ik.py [アームの逆運動学] +│ │ ├── robot_arm.py [両腕関節を制御し他をロック] +│ │ ├── hand_retargeting.py [多指ハンドリターゲティングアルゴリズムラッパー] +│ │ ├── robot_hand_inspire.py [inspireハンド関節を制御] +│ │ ├── robot_hand_unitree.py [unitreeハンド関節を制御] │ │ │ ├── utils -│ │ ├── episode_writer.py [模倣学習のデータを記録するために使用] -│ │ ├── mat_tool.py [いくつかの小さな数学ツール] -│ │ ├── weighted_moving_filter.py [ジョイントデータをフィルタリングするため] -│ │ ├── rerun_visualizer.py [記録中のデータを可視化するため] +│ │ ├── episode_writer.py [模倣学習用データ記録] +│ │ ├── weighted_moving_filter.py [関節データのフィルタリング] +│ │ ├── rerun_visualizer.py [記録中のデータ可視化] │ │ -│ │──teleop_hand_and_arm.py [遠隔操作の起動実行コード] +│ └── teleop_hand_and_arm.py [遠隔操作起動実行コード] ``` - - # 5. 🛠️ ハードウェア -## 5.1 📋 リスト - -| アイテム | 数量 | リンク | 備考 | -| :--------------------------: | :------: | :----------------------------------------------------------: | :---------------------------------------------------------: | -| **Unitree ロボット G1** | 1 | https://www.unitree.com/g1 | 開発用コンピューティングユニット付き | -| **Apple Vision Pro** | 1 | https://www.apple.com/apple-vision-pro/ | | -| **ルーター** | 1 | | | -| **ユーザー PC** | 1 | | 推奨グラフィックカード性能は RTX 4080 以上 | -| **ヘッドステレオカメラ** | 1 | [参考のみ] http://e.tb.cn/h.TaZxgkpfWkNCakg?tk=KKz03Kyu04u | ヘッド用 | -| **ヘッドカメラマウント** | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/head_stereo_camera_mount.STEP | ヘッドステレオカメラの取り付け用, FOV 130° | -| Intel RealSense D405 | 2 | https://www.intelrealsense.com/depth-camera-d405/ | リスト用 | -| リストリングマウント | 2 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/wrist_ring_mount.STEP | リストカメラマウントと一緒に使用 | -| 左リストカメラマウント | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/left_wrist_D405_camera_mount.STEP | 左リスト RealSense D405 カメラの取り付け用 | -| 右リストカメラマウント | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/right_wrist_D405_camera_mount.STEP | 右リスト RealSense D405 カメラの取り付け用 | -| M3 六角ナット | 4 | [参考のみ] https://a.co/d/1opqtOr | リストファスナー用 | -| M3x12 ネジ | 4 | [参考のみ] https://amzn.asia/d/aU9NHSf | リストファスナー用 | -| M3x6 ネジ | 4 | [参考のみ] https://amzn.asia/d/0nEz5dJ | リストファスナー用 | -| **M4x14 ネジ** | 2 | [参考のみ] https://amzn.asia/d/cfta55x | ヘッドファスナー用 | -| **M2x4 自タッピングネジ** | 4 | [参考のみ] https://amzn.asia/d/1msRa5B | ヘッドファスナー用 | - -> 注意: 太字のアイテムは遠隔操作タスクに必要な機器であり、他のアイテムは [データセット](https://huggingface.co/unitreerobotics) を記録するためのオプション機器です。 - -## 5.2 🔨 インストール図 +## 5.1 📋 部品リスト - - - - - - - - - - - - - - - - - - -
アイテムシミュレーション実物
ヘッド -

- head -

ヘッドマウント
-

- 		-

- head -

組み立ての側面図
-

- 		-

- head -

組み立ての正面図
-

-
リスト -

- wrist -

リストリングとカメラマウント
-

- 		-

- wrist -

左手の組み立て
-

- 		-

- wrist -

右手の組み立て
-

-
+| アイテム | 数量 | リンク | 備考 | +| :------------------------: | :--: | :----------------------------------------------------------: | :----------------------------------------------------------: | +| **ヒューマノイドロボット** | 1 | https://www.unitree.com | 開発用計算ユニット付属 | +| **XRデバイス** | 1 | https://www.apple.com/apple-vision-pro/ https://www.meta.com/quest/quest-3 https://www.picoxr.com/products/pico4-ultra-enterprise | | +| **ルーター** | 1 | | **デフォルトモード**で必要; **ワイヤレスモード**では不要 | +| **ユーザーPC** | 1 | | **シミュレーションモード**では、公式推奨ハードウェアリソースを使用。 | +| **ヘッドステレオカメラ** | 1 | [参考] http://e.tb.cn/h.TaZxgkpfWkNCakg?tk=KKz03Kyu04u | ロボット頭部視点用 | +| **ヘッドカメラマウント** | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/head_stereo_camera_mount.STEP | ヘッドステレオカメラ取り付け用 | +| Intel RealSense D405カメラ | 2 | https://www.intelrealsense.com/depth-camera-d405/ | 手首用 | +| 手首リングマウント | 2 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/wrist_ring_mount.STEP | 手首カメラマウントと併用 | +| 左手首D405マウント | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/left_wrist_D405_camera_mount.STEP | 左手首RealSense D405カメラ取り付け用 | +| 右手首D405マウント | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/right_wrist_D405_camera_mount.STEP | 右手首RealSense D405カメラ取り付け用 | +| M3-1六角ナット | 4 | [参考] https://a.co/d/1opqtOr | 手首固定用 | +| M3x12ネジ | 4 | [参考] https://amzn.asia/d/aU9NHSf | 手首固定用 | +| M3x6ネジ | 4 | [参考] https://amzn.asia/d/0nEz5dJ | 手首固定用 | +| **M4x14ネジ** | 2 | [参考] https://amzn.asia/d/cfta55x | 頭部固定用 | +| **M2x4自攻ネジ** | 4 | [参考] https://amzn.asia/d/1msRa5B | 頭部固定用 | + +> 注: 太字のアイテムは遠隔操作タスクに必須の設備、その他はデータセット記録用のオプション設備。 -> 注意: リストリングマウントは、画像の赤い円で示されているように、ロボットのリストのシームと一致する必要があります。 +## 5.2 🔨 取り付け図 +
アイテム シミュレーション 実機
頭部

頭部

頭部マウント

頭部

取り付け側面図

頭部

取り付け正面図
手首

手首

手首リングとカメラマウント

手首

左手取り付け図

手首

右手取り付け図
+> 注: 手首リングマウントは、ロボットの手首の継ぎ目に合わせて取り付け（画像の赤丸部分）。 # 6. 🙏 謝辞 -このコードは、以下のオープンソースコードベースに基づいて構築されています。各ライセンスを確認するには、以下の URL を訪れてください。 - -1) https://github.com/OpenTeleVision/TeleVision -2) https://github.com/dexsuite/dex-retargeting -3) https://github.com/vuer-ai/vuer -4) https://github.com/stack-of-tasks/pinocchio -5) https://github.com/casadi/casadi -6) https://github.com/meshcat-dev/meshcat-python -7) https://github.com/zeromq/pyzmq -8) https://github.com/unitreerobotics/unitree_dds_wrapper -9) https://github.com/tonyzhaozh/act -10) https://github.com/facebookresearch/detr -11) https://github.com/Dingry/BunnyVisionPro -12) https://github.com/unitreerobotics/unitree_sdk2_python +このコードは以下のオープンソースコードを基にしています。各LICENSEはURLで確認してください: + +1. https://github.com/OpenTeleVision/TeleVision +2. https://github.com/dexsuite/dex-retargeting +3. https://github.com/vuer-ai/vuer +4. https://github.com/stack-of-tasks/pinocchio +5. https://github.com/casadi/casadi +6. https://github.com/meshcat-dev/meshcat-python +7. https://github.com/zeromq/pyzmq +8. https://github.com/Dingry/BunnyVisionPro +9. https://github.com/unitreerobotics/unitree_sdk2_python diff --git a/README_zh-CN.md b/README_zh-CN.md index 448195a..315f3f4 100644 --- a/README_zh-CN.md +++ b/README_zh-CN.md @@ -1,11 +1,14 @@
-

avp_teleoperate

-

Unitree Robotics

+

xr_teleoperate

+ + Unitree LOGO +

English | 中文 | 日本語

+ # 📺 视频演示

@@ -26,14 +29,29 @@

+# 🔖 发布说明 + +1. 升级 [Vuer](https://github.com/vuer-ai/vuer) 库，扩展了设备支持模式。为更准确反映功能范围，项目由 **avp_teleoperate** 更名为 **xr_teleoperate**，从最初仅支持 Apple Vision Pro，扩展至兼容 Meta Quest 3（含手柄） 与 PICO 4 Ultra Enterprise（含手柄） 等多款 XR 设备。 +2. 对部分功能进行了**模块化**拆分，并通过 Git 子模块（git submodule）方式进行管理和加载，提升代码结构的清晰度与维护性。 +3. 新增**无头**、**运控**及**仿真**模式，优化启动参数配置（详见第2.2节），提升使用便捷性。**仿真**模式的加入，方便了环境验证和硬件故障排查。 +4. 将默认手部映射算法从 Vector 切换为 **DexPilot**，优化了指尖捏合的精度与交互体验。 +5. 其他一些优化 # 0. 📖 介绍 -该仓库实现了使用 **XR设备**（比如 Apple Vision Pro、PICO 4 Ultra Enterprise 或 Meta Quest 3） 对 **宇树（Unitree）人形机器人** 的遥操作控制。 +该仓库实现了使用 **XR设备（Extended Reality）**（比如 Apple Vision Pro、PICO 4 Ultra Enterprise 或 Meta Quest 3 等） 对 **宇树（Unitree）人形机器人** 的遥操作控制。 + +以下是系统示意图： + +

+ + Watch the Document + +

-以下是本仓库目前支持的机器人类型： +以下是本仓库目前支持的设备类型： @@ -56,6 +74,10 @@ + + + + @@ -65,179 +87,230 @@ - - + +
H1_2 (手臂7自由度) ✅ 完成
Dex1-1 夹爪 ✅ 完成
Dex3-1 灵巧手 ✅ 完成 ✅ 完成
... ... ··· ···
+# 1. 📦 安装 -以下是需要的设备和接线示意图： - -

- - Watch the Document - -

- -以下是网络拓扑图，以G1机器人为例： - -

- - Watch the Document - -

- - - -# 1. 📦 前置条件 - -我们在 Ubuntu 20.04 和 Ubuntu 22.04 上测试了我们的代码，其他操作系统可能需要不同的配置。 +我们在 Ubuntu 20.04 和 Ubuntu 22.04 上测试了我们的代码，其他操作系统可能需要不同的配置。本文档主要介绍常规模式。 有关更多信息，您可以参考 [官方文档](https://support.unitree.com/home/zh/Teleoperation) 和 [OpenTeleVision](https://github.com/OpenTeleVision/TeleVision)。 -## 1.1 🦾 逆运动学 +## 1.1 📥 基础环境 ```bash -unitree@Host:~$ conda create -n tv python=3.8 -unitree@Host:~$ conda activate tv -# 如果您使用 `pip install`，请确保 pinocchio 版本为 3.1.0 -(tv) unitree@Host:~$ conda install pinocchio -c conda-forge -(tv) unitree@Host:~$ pip install meshcat -(tv) unitree@Host:~$ pip install casadi +# 创建 conda 基础环境 +(base) unitree@Host:~$ conda create -n tv python=3.10 pinocchio=3.1.0 numpy=1.26.4 -c conda-forge +(base) unitree@Host:~$ conda activate tv +# 克隆本仓库 +(tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/xr_teleoperate.git +(tv) unitree@Host:~$ cd xr_teleoperate +# 浅克隆子模块 +(tv) unitree@Host:~/xr_teleoperate$ git submodule update --init --depth 1 +# 安装 televuer 模块 +(tv) unitree@Host:~/xr_teleoperate$ cd teleop/televuer +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ pip install -e . +# 生成 televuer 模块所需的证书文件 +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ openssl req -x509 -nodes -days 365 -newkey rsa:2048 -keyout key.pem -out cert.pem +# 安装 dex-retargeting 模块 +(tv) unitree@Host:~/xr_teleoperate/teleop/televuer$ cd ../robot_control/dex-retargeting/ +(tv) unitree@Host:~/xr_teleoperate/teleop/robot_control/dex-retargeting$ pip install -e . +# 安装本仓库所需的其他依赖库 +(tv) unitree@Host:~/xr_teleoperate/teleop/robot_control/dex-retargeting$ cd ../../../ +(tv) unitree@Host:~/xr_teleoperate$ pip install -r requirements.txt ``` -> 提醒：命令前面的所有标识符是为了提示：该命令应该在哪个设备和目录下执行。 -> -> p.s. 在 Ubuntu 系统 `~/.bashrc` 文件中，默认配置: `PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ '` -> - 以`(tv) unitree@Host:~$ pip install meshcat` 命令为例： -> ->- `(tv)` 表示 shell 此时位于 conda 创建的 tv 环境中； -> ->- `unitree@Host:~` 表示用户标识 unitree 在设备 Host 上登录，当前的工作目录为 `$HOME`； -> ->- $ 表示当前 shell 为 Bash； -> ->- pip install meshcat 是用户标识 unitree 要在 设备 Host 上执行的命令。 -> ->您可以参考 [Harley Hahn's Guide to Unix and Linux](https://www.harley.com/unix-book/book/chapters/04.html#H) 和 [Conda User Guide](https://docs.conda.io/projects/conda/en/latest/user-guide/getting-started.html) 来深入了解这些知识。 - ## 1.2 🕹️ unitree_sdk2_python ```bash -# 安装 unitree_sdk2_python 库 +# 安装 unitree_sdk2_python 库，该库负责开发设备与机器人之间的通信控制功能 (tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/unitree_sdk2_python.git (tv) unitree@Host:~$ cd unitree_sdk2_python (tv) unitree@Host:~$ pip install -e . ``` -> 提醒：原 h1_2 分支中的 [unitree_dds_wrapper](https://github.com/unitreerobotics/unitree_dds_wrapper) 为临时版本，现已全面转换到上述正式的 Python 版控制通信库：[unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python) +> 注意1：原 h1_2 分支中的 [unitree_dds_wrapper](https://github.com/unitreerobotics/unitree_dds_wrapper) 为临时版本，现已全面转换到上述正式的 Python 版控制通信库：[unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python) + +> 注意2：命令前面的所有标识符是为了提示：该命令应该在哪个设备和目录下执行。 +> +> p.s. 在 Ubuntu 系统 `~/.bashrc` 文件中，默认配置: `PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ '` +> +> - 以`(tv) unitree@Host:~$ pip install meshcat` 命令为例： +> +> - `(tv)` 表示 shell 此时位于 conda 创建的 tv 环境中； +> +> - `unitree@Host:~` 表示用户标识 unitree 在设备 Host 上登录，当前的工作目录为 `$HOME`； +> +> - $ 表示当前 shell 为 Bash； +> +> - pip install meshcat 是用户标识 unitree 要在 设备 Host 上执行的命令。 +> +> 您可以参考 [Harley Hahn's Guide to Unix and Linux](https://www.harley.com/unix-book/book/chapters/04.html#H) 和 [Conda User Guide](https://docs.conda.io/projects/conda/en/latest/user-guide/getting-started.html) 来深入了解这些知识。 + +# 2. 💻 仿真部署 -# 2. ⚙️ 配置 +## 2.1 📥 环境配置 -## 2.1 📥 基础 +首先，请安装 [unitree_sim_isaaclab](https://github.com/unitreerobotics/unitree_sim_isaaclab)。具体安装步骤，可参考该仓库 README 文档。 + +其次，启动 unitree_sim_isaaclab 仿真环境。假设使用 G1(29 DoF) 和 Dex3 灵巧手配置进行仿真，则启动命令示例如下： ```bash -(tv) unitree@Host:~$ cd ~ -(tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/avp_teleoperate.git -(tv) unitree@Host:~$ cd ~/avp_teleoperate -(tv) unitree@Host:~$ pip install -r requirements.txt +(base) unitree@Host:~$ conda activate unitree_sim_env +(unitree_sim_env) unitree@Host:~$ cd ~/unitree_sim_isaaclab +(unitree_sim_env) unitree@Host:~/unitree_sim_isaaclab$ python sim_main.py --device cpu --enable_cameras --task Isaac-PickPlace-Cylinder-G129-Dex3-Joint --enable_dex3_dds --robot_type g129 ``` -## 2.2 🔌 本地流媒体 +仿真环境启动后，使用鼠标左键在窗口内点击一次以激活仿真运行状态。此时，终端内输出 `controller started, start main loop...`。 -**Apple Vision Pro** +仿真界面如下图所示： -苹果不允许在非 HTTPS 连接上使用 WebXR。要在本地测试应用程序，我们需要创建一个自签名证书并在客户端上安装它。您需要一台 Ubuntu 机器和一个路由器。将 Apple Vision Pro 和 Ubuntu **主机**连接到同一个路由器。 +

+ + Unitree sim isaaclab + +

-1. 安装 mkcert：https://github.com/FiloSottile/mkcert -2. 检查**主机**本地 IP 地址： -```bash -(tv) unitree@Host:~/avp_teleoperate$ ifconfig | grep inet -``` -假设 **主机** 的本地 IP 地址为 `192.168.123.2` +## 2.2 🚀 启动遥操 -> 提醒：您可以使用 `ifconfig` 命令检查您的 **主机** IP 地址。 +本程序支持通过 XR 设备（比如手势或手柄）来控制实际机器人动作，也支持在虚拟仿真中运行。你可以根据需要，通过命令行参数来配置运行方式。 -3. 创建证书： +以下是本程序的启动参数说明： -```bash -(tv) unitree@Host:~/avp_teleoperate$ mkcert -install && mkcert -cert-file cert.pem -key-file key.pem 192.168.123.2 localhost 127.0.0.1 -``` +- 基础控制参数 -将生成的 `cert.pem` 和 `key.pem` 文件放在 `teleop` 目录中 +| ⚙️ 参数 | 📜 说明 | 🔘 目前可选值 | 📌 默认值 | +| :---------: | :----------------------------------------------: | :------------------------------------------------------: | :------: | +| `--xr-mode` | 选择 XR 输入模式（通过什么方式控制机器人） | `hand`（**手势跟踪**）
`controller`（**手柄跟踪**） | `hand` | +| `--arm` | 选择机器人设备类型（可参考 0. 📖 介绍） | `G1_29`
`G1_23`
`H1_2`
`H1` | `G1_29` | +| `--ee` | 选择手臂的末端执行器设备类型（可参考 0. 📖 介绍） | `dex1`
`dex3`
`inspire1` | 无默认值 | -```bash -(tv) unitree@Host:~/avp_teleoperate$ cp cert.pem key.pem ~/avp_teleoperate/teleop/ -``` +- 模式开关参数 -4. 在服务器上打开防火墙： +| ⚙️ 参数 | 📜 说明 | +| :----------: | :----------------------------------------------------------: | +| `--record` | 【启用**数据录制**模式】
按 **r** 键进入遥操后，按 **s** 键可开启数据录制，再次按 **s** 键可结束录制并保存本次 episode 数据。
继续按下 **s** 键可重复前述过程。 | +| `--motion` | 【启用**运动控制**模式】
开启本模式后，可在机器人运控程序运行下进行遥操作程序。
**手势跟踪**模式下，可使用 [R3遥控器](https://www.unitree.com/cn/R3) 控制机器人正常行走；**手柄跟踪**模式下，也可使用[手柄摇杆控制机器人行走](https://github.com/unitreerobotics/xr_teleoperate/blob/375cdc27605de377c698e2b89cad0e5885724ca6/teleop/teleop_hand_and_arm.py#L247-L257)。 | +| `--headless` | 【启用**无图形界面**模式】
适用于本程序部署在开发计算单元（PC2）等无显示器情况 | +| `--sim` | 【启用[**仿真模式**](https://github.com/unitreerobotics/unitree_sim_isaaclab)】 | -```bash -(tv) unitree@Host:~/avp_teleoperate$ sudo ufw allow 8012 -``` +------ -5. 在 Apple Vision Pro 上安装 CA 证书： +根据上述参数说明以及仿真环境配置，我们假设选择**手势跟踪**来控制 G1(29 DoF) + Dex3 灵巧手设备，同时开启仿真模式和数据录制模式。 + +则启动命令如下所示： ```bash -(tv) unitree@Host:~/avp_teleoperate$ mkcert -CAROOT +(tv) unitree@Host:~$ cd ~/xr_teleoperate/teleop/ +(tv) unitree@Host:~/xr_teleoperate/teleop/$ python teleop_hand_and_arm.py --xr-mode=hand --arm=G1_29 --ee=dex3 --sim --record +# 实际上，由于一些参数存在默认值，该命令也可简化为： +(tv) unitree@Host:~/xr_teleoperate/teleop/$ python teleop_hand_and_arm.py --ee=dex3 --sim --record ``` -通过 AirDrop 将 `rootCA.pem` 复制到 Apple Vision Pro 并安装它。 +程序正常启动后，终端输出信息如下图所示： -设置 > 通用 > 关于本机 > 证书信任设置。在“启用对根证书的完全信任”下，打开对证书的信任。 +

+ + start_terminal_log + +

-设置 > 应用 > Safari > 高级 > 功能标志 > 启用 WebXR 相关功能。 +接下来，执行以下步骤： -> 提醒：在新版本 Vision OS 2 系统中，该步骤有所不同：将证书通过 AirDrop 复制到 Apple Vision Pro 设备后，将会在设置 APP 中左上角账户栏的下方出现证书相关信息栏，点击进去即可启用对该证书的信任。 +1. 戴上您的 XR 头显设备（比如 apple vision pro 或 pico4 ultra enterprise等） ------- +2. 连接对应的 WiFi 热点 -**2.2.2 PICO 4 Ultra Enterprise or Meta Quest 3** +3. 打开浏览器应用（比如 Safari 或 PICO Browser），输入并访问网址：https://192.168.123.2:8012?ws=wss://192.168.123.2:8012 -我们已经尝试在 PICO 4 Ultra Enterprise 和 Meta-Quest 3 上使用手部追踪进行遥操作。 + > 注意1：此 IP 地址应与您的 **主机** IP 地址匹配。该地址可以使用 `ifconfig` 等类似命令查询。 -PICO 4 Ultra Enterprise 的系统规格如下： + > 注意2：此时可能弹出下图所示的警告信息。请点击`Advanced`按钮后，继续点击 `Proceed to ip (unsafe)` 按钮，使用非安全方式继续登录服务器。 -> 系统版本：5.12.6.U；Android 版本号：14；软件版本号：c000_cf01_bv1.0.1_sv5.12.6_202412121344_sparrow_b4244_user -> -> 浏览器版本：[4.0.28 beta version](https://github.com/vuer-ai/vuer/issues/45#issuecomment-2674918619) +

+ + vuer_unsafe + +

+ +4. 进入`Vuer`网页界面后，点击 **`Virtual Reality`** 按钮。在允许后续的所有对话框后，启动 VR 会话。界面如下图所示： + +

+ + vuer + +

+ +5. 此时，您将会在 XR 头显设备中看到机器人的第一人称视野。同时，终端打印出链接建立的信息： + + ```bash + websocket is connected. id:dbb8537d-a58c-4c57-b49d-cbb91bd25b90 + default socket worker is up, adding clientEvents + Uplink task running. id:dbb8537d-a58c-4c57-b49d-cbb91bd25b90 + ``` + +6. 然后，将手臂形状摆放到与**机器人初始姿态**相接近的姿势。这一步是为了避免在实物部署时，初始位姿差距过大导致机器人产生过大的摆动。 -Meta-Quest 3 的系统规格如下： + 机器人初始姿态示意图如下： -> 系统版本：49829370066100510；版本：62.0.0.273.343.570372087；运行时版本：62.0.0.269.341.570372063；操作系统版本：SQ3A.220605.009.A1 +

+ + robot_init_pose + +

-更多配置步骤信息，您可以查看该 [issue](https://github.com/unitreerobotics/avp_teleoperate/issues/32)。 +7. 最后，在终端中按下 **r** 键后，正式开启遥操作程序。此时，您可以远程控制机器人的手臂（和灵巧手） -## 2.3 🔎 单元测试 +8. 在遥操过程中，按 **s** 键可开启数据录制，再次按 **s** 键可结束录制并保存数据（该过程可重复） -此步骤用于验证环境是否正确安装。 + 数据录制过程示意图如下： -即将展现。 +

+ + record + +

+> 注意1：录制的数据默认存储在 `xr_teleoperate/teleop/utils/data` 中。数据使用说明见此仓库： [unitree_IL_lerobot](https://github.com/unitreerobotics/unitree_IL_lerobot/blob/main/README_zh.md#%E6%95%B0%E6%8D%AE%E9%87%87%E9%9B%86%E4%B8%8E%E8%BD%AC%E6%8D%A2)。 +> +> 注意2：请在录制数据时注意您的硬盘空间大小。 + +## 2.3 🔚 退出 + +要退出程序，可以在终端窗口（或 'record image' 窗口）中按下 **q** 键。 -# 3. 🚀 使用方法 -在开始此程序之前，请至少阅读一次 [官方文档](https://support.unitree.com/home/zh/Teleoperation)。 +# 3. 🤖 实物部署 -## 3.1 🖼️ 图像服务器 +实物部署与仿真部署步骤基本相似，下面将重点指出不同之处。 -将 `avp_teleoperate/teleop/image_server` 目录中的 `image_server.py` 复制到宇树机器人（G1/H1/H1_2 等）的 **开发计算单元 PC2**，并在 **PC2** 上执行以下命令： +## 3.1 🖼️ 图像服务 + +仿真环境中已经自动开启了图像服务。实物部署时，需要针对自身相机硬件类型，手动开启图像服务。步骤如下： + +将 `xr_teleoperate/teleop/image_server` 目录中的 `image_server.py` 复制到宇树机器人（G1/H1/H1_2 等）的 **开发计算单元 PC2**。 ```bash -# 提醒1：可以通过scp命令将image_server.py传输到PC2，然后使用ssh远程登录PC2后执行它。 +# 提醒：可以通过scp命令将image_server.py传输到PC2，然后使用ssh远程登录PC2后执行它。 # 假设开发计算单元PC2的ip地址为192.168.123.164，那么传输过程示例如下： # 先ssh登录PC2，创建图像服务器的文件夹 (tv) unitree@Host:~$ ssh unitree@192.168.123.164 "mkdir -p ~/image_server" # 将本地的image_server.py拷贝至PC2的~/image_server目录下 -(tv) unitree@Host:~$ scp ~/avp_teleoperate/teleop/image_server/image_server.py unitree@192.168.123.164:~/image_server/ +(tv) unitree@Host:~$ scp ~/xr_teleoperate/teleop/image_server/image_server.py unitree@192.168.123.164:~/image_server/ +``` -# 提醒2：目前该图像传输程序支持OpenCV和Realsense SDK两种读取图像的方式，请阅读image_server.py的ImageServer类的注释以便您根据自己的相机硬件来配置自己的图像传输服务。 +并在 **PC2** 上执行以下命令： + +```bash +# 提醒：目前该图像传输程序支持OpenCV和Realsense SDK两种读取图像的方式，请阅读image_server.py的ImageServer类的注释以便您根据自己的相机硬件来配置自己的图像传输服务。 # 现在位于宇树机器人 PC2 终端 unitree@PC2:~/image_server$ python image_server.py # 您可以看到终端输出如下： @@ -249,14 +322,18 @@ unitree@PC2:~/image_server$ python image_server.py 在图像服务启动后，您可以在 **主机** 终端上使用 `image_client.py` 测试通信是否成功： ```bash -(tv) unitree@Host:~/avp_teleoperate/teleop/image_server$ python image_client.py +(tv) unitree@Host:~/xr_teleoperate/teleop/image_server$ python image_client.py ``` -## 3.2 ✋ Inspire 手部服务器（可选） +## 3.2 ✋ Inspire 手部服务（可选） -> 注意：如果选择的机器人配置中没有使用一代 Inspire 灵巧手，那么请忽略本节内容。 +> 注意1：如果选择的机器人配置中没有使用 Inspire 系列灵巧手，那么请忽略本节内容。 +> +> 注意2：如果选择的G1机器人配置，且使用 [Inspire DFX 灵巧手](https://support.unitree.com/home/zh/G1_developer/inspire_dfx_dexterous_hand)，那么请参考 [issue #46](https://github.com/unitreerobotics/xr_teleoperate/issues/46)。 +> +> 注意3：如果选择的机器人配置中使用了 [Inspire FTP 灵巧手](https://support.unitree.com/home/zh/G1_developer/inspire_ftp_dexterity_hand)，那么请参考 [issue #48](https://github.com/unitreerobotics/xr_teleoperate/issues/48)。 -您可以参考 [灵巧手开发](https://support.unitree.com/home/zh/H1_developer/Dexterous_hand) 配置相关环境并编译控制程序。首先，使用 [此链接](https://oss-global-cdn.unitree.com/static/0a8335f7498548d28412c31ea047d4be.zip) 下载灵巧手控制接口程序，然后将其复制到宇树机器人的**PC2**。 +您可以参考 [H1-DFX灵巧手开发](https://support.unitree.com/home/zh/H1_developer/Dexterous_hand) 配置相关环境并编译控制程序。首先，使用 [此链接](https://oss-global-cdn.unitree.com/static/0a8335f7498548d28412c31ea047d4be.zip) 下载灵巧手控制接口程序，然后将其复制到宇树机器人的**PC2**。 在宇树机器人的 **PC2** 上，执行命令： @@ -274,107 +351,68 @@ unitree@PC2:~/h1_inspire_service/build$ ./h1_hand_example 如果两只手连续打开和关闭，则表示成功。一旦成功，即可关闭终端 2 中的 `./h1_hand_example` 程序。 -## 3.3 🚀 启动 +## 3.3 🚀 启动遥操 > ![Warning](https://img.shields.io/badge/Warning-Important-red) > > 1. 所有人员必须与机器人保持安全距离，以防止任何潜在的危险！ > 2. 在运行此程序之前，请确保至少阅读一次 [官方文档](https://support.unitree.com/home/zh/Teleoperation)。 -> 3. 请务必确保机器人已经进入[调试模式（L2+R2）](https://support.unitree.com/home/zh/H1_developer/Remote_control)，以停止运动控制程序发送指令，这样可以避免潜在的指令冲突问题。 - -最好有两名操作员来运行此程序，称为 **操作员 A** 和 **操作员 B**。 - +> 3. 没有开启**运动控制**模式（`--motion`）时，请务必确保机器人已经进入 [调试模式（L2+R2）](https://support.unitree.com/home/zh/G1_developer/remote_control)，以停止运动控制程序发送指令，这样可以避免潜在的指令冲突问题。 +> 4. 如果要开启**运动控制**模式遥操作，请提前使用 [R3遥控器](https://www.unitree.com/cn/R3) 确保机器人进入主运控模式。 +> 5. 开启**运动控制**模式（`--motion`）时： +> - 右手柄按键 `A` 为遥操作**退出**功能按键； +> - 左手柄和右手柄的两个摇杆按键同时按下为软急停按键，机器人会退出运控程序并进入阻尼模式，该功能只在必要情况下使用 +> - 左手柄摇杆控制机器人前后左右（最大控制速度已经在程序中进行了限制） +> - 右手柄摇杆控制机器人转向（最大控制速度已经在程序中进行了限制） - -首先，**操作员 B** 需要执行以下步骤： - -1. 修改 `~/avp_teleoperate/teleop/teleop_hand_and_arm.py` 中 `if __name__ == '__main__':` 代码下方的 `img_config` 图像客户端配置，它应该与 3.1 节中您在 PC2 配置的图像服务器参数相同。 - -2. 根据您的机器人配置选择不同的启动参数。以下是一些启动命令示例： - - ```bash - # 1. G1(29DoF)机器人 + Dex3-1 灵巧手 （实际上，G1_29是--arm的默认参数，可以选择不填写） - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_29 --hand=dex3 - - # 2. 仅G1(29DoF)机器人 - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py - - # 3. G1 (23DoF) 机器人 - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_23 - - # 4. H1_2 机器人 + 一代 Inspire 灵巧手 - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=H1_2 --hand=inspire1 - - # 5. H1 机器人 - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=H1 - - # 6. 如果您想开启数据可视化+录制，还可以追加 --record 选项 - (tv) unitree@Host:~/avp_teleoperate/teleop$ python teleop_hand_and_arm.py --arm=G1_23 --record - ``` - -3. 程序如果正常启动，终端最后一行将停留在 “Please enter the start signal (enter 'r' to start the subsequent program):” 的字符串输出。 - - - -然后，**操作员 A** 需要执行以下步骤： - -1. 戴上您的 Apple Vision Pro 设备。 - -2. 在 Apple Vision Pro 上打开 Safari，访问：https://192.168.123.2:8012?ws=wss://192.168.123.2:8012 - - > 注意：此 IP 地址应与您的 **主机** IP 地址匹配。 - -3. 点击 `Enter VR` 并选择 `Allow` 以启动 VR 会话。 - -4. 您将会在Apple Vision Pro中看到机器人的第一人称视野。 - - - -接下来，**操作员 B** 可以在终端中按下 **r** 键以启动远程操作程序。 - -此时，**操作员 A** 可以远程控制机器人的手臂（和灵巧手）。 - -如果使用了`--record`参数，那么**操作员 B** 可以在打开的“record image”窗口中按 **s** 键开始录制数据，再次按 **s** 键停止。可以根据需要重复此操作进行多次录制。 - -> 注意1：录制的数据默认存储在 `avp_teleoperate/teleop/utils/data` 中，使用说明见此仓库： [unitree_IL_lerobot](https://github.com/unitreerobotics/unitree_IL_lerobot/blob/main/README_zh.md#%E6%95%B0%E6%8D%AE%E9%87%87%E9%9B%86%E4%B8%8E%E8%BD%AC%E6%8D%A2)。 -> -> 注意2：请在录制数据时注意您的硬盘空间大小。 +与仿真部署基本一致，但要注意上述警告事项。 ## 3.4 🔚 退出 > ![Warning](https://img.shields.io/badge/Warning-Important-red) > -> 为了避免损坏机器人，最好确保**操作员 A** 将机器人手臂摆放为自然下垂或其他恰当位置后，**操作员B **再按 **q** 退出。 +> 为了避免损坏机器人，最好确保将机器人手臂摆放为与机器人初始姿态附近的恰当位置后，再按 **q** 退出。 +> +> 调试模式下：按下退出键后，机器人双臂将在5秒内返回机器人初始姿态，然后结束控制。 +> +> 运控模式下：按下退出键后，机器人双臂将在5秒内返回机器人运控姿态，然后结束控制。 -要退出程序，**操作员 B** 可以在 'record image' 窗口中按下 **q** 键。 +与仿真部署基本一致，但要注意上述警告事项。 # 4. 🗺️ 代码库教程 ``` -avp_teleoperate/ +xr_teleoperate/ │ ├── assets [存储机器人 URDF 相关文件] │ +├── hardware [存储 3D 打印模组] +│ ├── teleop │ ├── image_server │ │ ├── image_client.py [用于从机器人图像服务器接收图像数据] -│ │ ├── image_server.py [从摄像头捕获图像并通过网络发送（在机器人板载计算单元上运行）] +│ │ ├── image_server.py [从摄像头捕获图像并通过网络发送（在机器人板载计算单元PC2上运行）] │ │ -│ ├── open_television -│ │ ├── television.py [使用 Vuer 从 Apple Vision Pro 捕获腕部和手部数据] -│ │ ├── tv_wrapper.py [对捕获的数据进行后处理] +│ ├── televuer +│ │ ├── src/televuer +│ │ ├── television.py [使用 Vuer 从 XR 设备捕获头部、腕部和手部/手柄等数据] +│ │ ├── tv_wrapper.py [对捕获的数据进行后处理] +│ │ ├── test +│ │ ├── _test_television.py [television.py 的测试程序] +│ │ ├── _test_tv_wrapper.py [tv_wrapper.py 的测试程序] │ │ │ ├── robot_control +│ │ ├── src/dex-retargeting [灵巧手映射算法库] │ │ ├── robot_arm_ik.py [手臂的逆运动学] │ │ ├── robot_arm.py [控制双臂关节并锁定其他部分] +│ │ ├── hand_retargeting.py [灵巧手映射算法库 Wrapper] │ │ ├── robot_hand_inspire.py [控制因时灵巧手] │ │ ├── robot_hand_unitree.py [控制宇树灵巧手] │ │ │ ├── utils │ │ ├── episode_writer.py [用于记录模仿学习的数据] -│ │ ├── mat_tool.py [一些小的数学工具] │ │ ├── weighted_moving_filter.py [用于过滤关节数据的滤波器] │ │ ├── rerun_visualizer.py [用于可视化录制数据] │ │ @@ -387,23 +425,23 @@ avp_teleoperate/ ## 5.1 📋 清单 -| 项目 | 数量 | 链接 | 备注 | -| :-----------------------: | :--: | :----------------------------------------------------------: | :----------------------------: | -| **宇树通用人形机器人 G1** | 1 | https://www.unitree.com/cn/g1 | 需选配开发计算单元版本 | -| **Apple Vision Pro** | 1 | https://www.apple.com.cn/apple-vision-pro/ | | -| **路由器** | 1 | | | -| **用户电脑** | 1 | | 推荐显卡性能在RTX 4080 以上 | -| **头部双目相机** | 1 | [仅供参考] http://e.tb.cn/h.TaZxgkpfWkNCakg?tk=KKz03Kyu04u | 用于机器人头部视野，视场角130° | -| **头部相机支架** | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/head_stereo_camera_mount.STEP | 用于装配头部相机 | -| 英特尔 RealSense D405相机 | 2 | https://www.intelrealsense.com/depth-camera-d405/ | 用于腕部灵巧操作视野 | -| 腕部相机环形支架 | 2 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/wrist_ring_mount.STEP | 与腕部相机支架搭配使用 | -| 左腕相机支架 | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/left_wrist_D405_camera_mount.STEP | 用于装配左腕D405相机 | -| 右腕相机支架 | 1 | https://github.com/unitreerobotics/avp_teleoperate/blob/g1/hardware/right_wrist_D405_camera_mount.STEP | 用于装配右腕D405相机 | -| M3-1 六角螺母 | 4 | [仅供参考] https://a.co/d/gQaLtHD | 用于腕部紧固件 | -| M3x12 螺钉 | 4 | [仅供参考] https://amzn.asia/d/aU9NHSf | 用于腕部紧固件 | -| M3x6 螺钉 | 4 | [仅供参考] https://amzn.asia/d/0nEz5dJ | 用于腕部紧固件 | -| **M4x14 螺钉** | 2 | [仅供参考] https://amzn.asia/d/cfta55x | 用于头部紧固件 | -| **M2x4 自攻螺钉** | 4 | [仅供参考] https://amzn.asia/d/1msRa5B | 用于头部紧固件 | +| 项目 | 数量 | 链接 | 备注 | +| :-----------------------: | :--: | :----------------------------------------------------------: | :----------------------------------------------------------: | +| **宇树通用人形机器人 G1** | 1 | https://www.unitree.com/cn/g1 | 需选配开发计算单元版本 | +| **XR 设备** | 1 | https://www.apple.com.cn/apple-vision-pro/
https://www.meta.com/quest/quest-3
https://www.picoxr.com/products/pico4-ultra-enterprise | | +| 路由器 | 1 | | 常规模式必须，无线模式不需要 | +| **用户电脑** | 1 | | 仿真模式下请使用[官方推荐](https://docs.isaacsim.omniverse.nvidia.com/4.5.0/installation/requirements.html)的硬件资源进行部署使用 | +| **头部双目相机** | 1 | [仅供参考] http://e.tb.cn/h.TaZxgkpfWkNCakg?tk=KKz03Kyu04u | 用于机器人头部视野 | +| **头部相机支架** | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/head_stereo_camera_mount.STEP | 用于装配头部相机 | +| 英特尔 RealSense D405相机 | 2 | https://www.intelrealsense.com/depth-camera-d405/ | 用于腕部灵巧操作视野 | +| 腕部相机环形支架 | 2 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/wrist_ring_mount.STEP | 与腕部相机支架搭配使用 | +| 左腕相机支架 | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/left_wrist_D405_camera_mount.STEP | 用于装配左腕D405相机 | +| 右腕相机支架 | 1 | https://github.com/unitreerobotics/xr_teleoperate/blob/g1/hardware/right_wrist_D405_camera_mount.STEP | 用于装配右腕D405相机 | +| M3-1 六角螺母 | 4 | [仅供参考] https://a.co/d/gQaLtHD | 用于腕部紧固件 | +| M3x12 螺钉 | 4 | [仅供参考] https://amzn.asia/d/aU9NHSf | 用于腕部紧固件 | +| M3x6 螺钉 | 4 | [仅供参考] https://amzn.asia/d/0nEz5dJ | 用于腕部紧固件 | +| **M4x14 螺钉** | 2 | [仅供参考] https://amzn.asia/d/cfta55x | 用于头部紧固件 | +| **M2x4 自攻螺钉** | 4 | [仅供参考] https://amzn.asia/d/1msRa5B | 用于头部紧固件 | > 注意：加粗项目是进行遥操作任务时的必需设备，其余项目是录制[数据集](https://huggingface.co/unitreerobotics)时的可选设备。 @@ -475,8 +513,5 @@ avp_teleoperate/ 5. https://github.com/casadi/casadi 6. https://github.com/meshcat-dev/meshcat-python 7. https://github.com/zeromq/pyzmq -8. https://github.com/unitreerobotics/unitree_dds_wrapper -9. https://github.com/tonyzhaozh/act -10. https://github.com/facebookresearch/detr -11. https://github.com/Dingry/BunnyVisionPro -12. https://github.com/unitreerobotics/unitree_sdk2_python +8. https://github.com/Dingry/BunnyVisionPro +9. https://github.com/unitreerobotics/unitree_sdk2_python diff --git a/requirements.txt b/requirements.txt index c5d9c66..ba96d60 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,20 +1,4 @@ -einops==0.8.0 -h5py==3.11.0 -ipython==8.12.3 matplotlib==3.7.5 -packaging==24.1 -pandas==2.0.3 -params_proto==2.12.1 -pytransform3d==3.5.0 -PyYAML==6.0.1 -scikit_learn==1.3.2 -scipy==1.10.1 -seaborn==0.13.2 -setuptools==69.5.1 -torch==2.3.0 -torchvision==0.18.0 rerun-sdk==0.20.1 -nlopt>=2.6.1,<2.8.0 -trimesh>=4.4.0 -anytree>=2.12.0 -logging-mp==0.1.5 +meshcat==0.3.2 +sshkeyboard==2.3.1 \ No newline at end of file diff --git a/teleop/open_television/__init__.py b/teleop/open_television/__init__.py deleted file mode 100644 index 200cce1..0000000 --- a/teleop/open_television/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ -# open_television/__init__.py -# openssl req -x509 -nodes -days 365 -newkey rsa:2048 -keyout key.pem -out cert.pem -from .television import TeleVision -from .tv_wrapper import TeleVisionWrapper, TeleData, TeleStateData \ No newline at end of file diff --git a/teleop/open_television/_test_television.py b/teleop/open_television/_test_television.py deleted file mode 100644 index c1ba588..0000000 --- a/teleop/open_television/_test_television.py +++ /dev/null @@ -1,86 +0,0 @@ -import os, sys -this_file = os.path.abspath(__file__) -project_root = os.path.abspath(os.path.join(os.path.dirname(this_file), '..')) -if project_root not in sys.path: - sys.path.insert(0, project_root) - -import time -import numpy as np -from multiprocessing import shared_memory -from open_television import TeleVision -import logging_mp -logger_mp = logging_mp.get_logger(__name__, level=logging_mp.INFO) - -def run_test_television(): - # image - image_shape = (480, 640 * 2, 3) - image_shm = shared_memory.SharedMemory(create=True, size=np.prod(image_shape) * np.uint8().itemsize) - image_array = np.ndarray(image_shape, dtype=np.uint8, buffer=image_shm.buf) - - # from image_server.image_client import ImageClient - # import threading - # image_client = ImageClient(tv_img_shape = image_shape, tv_img_shm_name = image_shm.name, image_show=True, server_address="127.0.0.1") - # image_receive_thread = threading.Thread(target = image_client.receive_process, daemon = True) - # image_receive_thread.daemon = True - # image_receive_thread.start() - - # television - use_hand_track = True - tv = TeleVision(binocular = True, use_hand_tracking = use_hand_track, img_shape = image_shape, img_shm_name = image_shm.name, webrtc=False) - - try: - input("Press Enter to start television test...") - running = True - while running: - logger_mp.info("=" * 80) - logger_mp.info("Common Data (always available):") - logger_mp.info(f"head_pose shape: {tv.head_pose.shape}\n{tv.head_pose}\n") - logger_mp.info(f"left_arm_pose shape: {tv.left_arm_pose.shape}\n{tv.left_arm_pose}\n") - logger_mp.info(f"right_arm_pose shape: {tv.right_arm_pose.shape}\n{tv.right_arm_pose}\n") - logger_mp.info("=" * 80) - - if use_hand_track: - logger_mp.info("Hand Tracking Data:") - logger_mp.info(f"left_hand_positions shape: {tv.left_hand_positions.shape}\n{tv.left_hand_positions}\n") - logger_mp.info(f"right_hand_positions shape: {tv.right_hand_positions.shape}\n{tv.right_hand_positions}\n") - logger_mp.info(f"left_hand_orientations shape: {tv.left_hand_orientations.shape}\n{tv.left_hand_orientations}\n") - logger_mp.info(f"right_hand_orientations shape: {tv.right_hand_orientations.shape}\n{tv.right_hand_orientations}\n") - logger_mp.info(f"left_hand_pinch_state: {tv.left_hand_pinch_state}") - logger_mp.info(f"left_hand_pinch_value: {tv.left_hand_pinch_value}") - logger_mp.info(f"left_hand_squeeze_state: {tv.left_hand_squeeze_state}") - logger_mp.info(f"left_hand_squeeze_value: {tv.left_hand_squeeze_value}") - logger_mp.info(f"right_hand_pinch_state: {tv.right_hand_pinch_state}") - logger_mp.info(f"right_hand_pinch_value: {tv.right_hand_pinch_value}") - logger_mp.info(f"right_hand_squeeze_state: {tv.right_hand_squeeze_state}") - logger_mp.info(f"right_hand_squeeze_value: {tv.right_hand_squeeze_value}") - else: - logger_mp.info("Controller Data:") - logger_mp.info(f"left_controller_trigger_state: {tv.left_controller_trigger_state}") - logger_mp.info(f"left_controller_trigger_value: {tv.left_controller_trigger_value}") - logger_mp.info(f"left_controller_squeeze_state: {tv.left_controller_squeeze_state}") - logger_mp.info(f"left_controller_squeeze_value: {tv.left_controller_squeeze_value}") - logger_mp.info(f"left_controller_thumbstick_state: {tv.left_controller_thumbstick_state}") - logger_mp.info(f"left_controller_thumbstick_value: {tv.left_controller_thumbstick_value}") - logger_mp.info(f"left_controller_aButton: {tv.left_controller_aButton}") - logger_mp.info(f"left_controller_bButton: {tv.left_controller_bButton}") - logger_mp.info(f"right_controller_trigger_state: {tv.right_controller_trigger_state}") - logger_mp.info(f"right_controller_trigger_value: {tv.right_controller_trigger_value}") - logger_mp.info(f"right_controller_squeeze_state: {tv.right_controller_squeeze_state}") - logger_mp.info(f"right_controller_squeeze_value: {tv.right_controller_squeeze_value}") - logger_mp.info(f"right_controller_thumbstick_state: {tv.right_controller_thumbstick_state}") - logger_mp.info(f"right_controller_thumbstick_value: {tv.right_controller_thumbstick_value}") - logger_mp.info(f"right_controller_aButton: {tv.right_controller_aButton}") - logger_mp.info(f"right_controller_bButton: {tv.right_controller_bButton}") - logger_mp.info("=" * 80) - time.sleep(0.03) - except KeyboardInterrupt: - running = False - logger_mp.warning("KeyboardInterrupt, exiting program...") - finally: - image_shm.unlink() - image_shm.close() - logger_mp.warning("Finally, exiting program...") - exit(0) - -if __name__ == '__main__': - run_test_television() \ No newline at end of file diff --git a/teleop/open_television/_test_tv_wrapper.py b/teleop/open_television/_test_tv_wrapper.py deleted file mode 100644 index ab8579e..0000000 --- a/teleop/open_television/_test_tv_wrapper.py +++ /dev/null @@ -1,95 +0,0 @@ -import os, sys -this_file = os.path.abspath(__file__) -project_root = os.path.abspath(os.path.join(os.path.dirname(this_file), '..')) -if project_root not in sys.path: - sys.path.insert(0, project_root) - -import numpy as np -import time -from multiprocessing import shared_memory -from open_television import TeleVisionWrapper -import logging_mp -logger_mp = logging_mp.get_logger(__name__, level=logging_mp.INFO) - - -def run_test_tv_wrapper(): - image_shape = (480, 640 * 2, 3) - image_shm = shared_memory.SharedMemory(create=True, size=np.prod(image_shape) * np.uint8().itemsize) - image_array = np.ndarray(image_shape, dtype=np.uint8, buffer=image_shm.buf) - - # from image_server.image_client import ImageClient - # import threading - # image_client = ImageClient(tv_img_shape = image_shape, tv_img_shm_name = image_shm.name, image_show=True, server_address="127.0.0.1") - # image_receive_thread = threading.Thread(target = image_client.receive_process, daemon = True) - # image_receive_thread.daemon = True - # image_receive_thread.start() - - use_hand_track=True - tv_wrapper = TeleVisionWrapper(binocular=True, use_hand_tracking=use_hand_track, img_shape=image_shape, img_shm_name=image_shm.name, - return_state_data=True, return_hand_rot_data = True) - try: - input("Press Enter to start tv_wrapper test...") - running = True - while running: - start_time = time.time() - teleData = tv_wrapper.get_motion_state_data() - - logger_mp.info("=== TeleData Snapshot ===") - logger_mp.info(f"[Head Rotation Matrix]:\n{teleData.head_pose}") - logger_mp.info(f"[Left EE Pose]:\n{teleData.left_arm_pose}") - logger_mp.info(f"[Right EE Pose]:\n{teleData.right_arm_pose}") - - if use_hand_track: - logger_mp.info(f"[Left Hand Position] shape {teleData.left_hand_pos.shape}:\n{teleData.left_hand_pos}") - logger_mp.info(f"[Right Hand Position] shape {teleData.right_hand_pos.shape}:\n{teleData.right_hand_pos}") - - if teleData.left_hand_rot is not None: - logger_mp.info(f"[Left Hand Rotation] shape {teleData.left_hand_rot.shape}:\n{teleData.left_hand_rot}") - if teleData.right_hand_rot is not None: - logger_mp.info(f"[Right Hand Rotation] shape {teleData.right_hand_rot.shape}:\n{teleData.right_hand_rot}") - - if teleData.left_pinch_value is not None: - logger_mp.info(f"[Left Pinch Value]: {teleData.left_pinch_value:.2f}") - if teleData.right_pinch_value is not None: - logger_mp.info(f"[Right Pinch Value]: {teleData.right_pinch_value:.2f}") - - if teleData.tele_state: - state = teleData.tele_state - logger_mp.info("[Hand State]:") - logger_mp.info(f" Left Pinch state: {state.left_pinch_state}") - logger_mp.info(f" Left Squeeze: {state.left_squeeze_state} ({state.left_squeeze_value:.2f})") - logger_mp.info(f" Right Pinch state: {state.right_pinch_state}") - logger_mp.info(f" Right Squeeze: {state.right_squeeze_state} ({state.right_squeeze_value:.2f})") - else: - logger_mp.info(f"[Left Trigger Value]: {teleData.left_trigger_value:.2f}") - logger_mp.info(f"[Right Trigger Value]: {teleData.right_trigger_value:.2f}") - - if teleData.tele_state: - state = teleData.tele_state - logger_mp.info("[Controller State]:") - logger_mp.info(f" Left Trigger: {state.left_trigger_state}") - logger_mp.info(f" Left Squeeze: {state.left_squeeze_ctrl_state} ({state.left_squeeze_ctrl_value:.2f})") - logger_mp.info(f" Left Thumbstick: {state.left_thumbstick_state} ({state.left_thumbstick_value})") - logger_mp.info(f" Left A/B Buttons: A={state.left_aButton}, B={state.left_bButton}") - logger_mp.info(f" Right Trigger: {state.right_trigger_state}") - logger_mp.info(f" Right Squeeze: {state.right_squeeze_ctrl_state} ({state.right_squeeze_ctrl_value:.2f})") - logger_mp.info(f" Right Thumbstick: {state.right_thumbstick_state} ({state.right_thumbstick_value})") - logger_mp.info(f" Right A/B Buttons: A={state.right_aButton}, B={state.right_bButton}") - - current_time = time.time() - time_elapsed = current_time - start_time - sleep_time = max(0, 0.033 - time_elapsed) - time.sleep(sleep_time) - logger_mp.debug(f"main process sleep: {sleep_time}") - - except KeyboardInterrupt: - running = False - logger_mp.warning("KeyboardInterrupt, exiting program...") - finally: - image_shm.unlink() - image_shm.close() - logger_mp.warning("Finally, exiting program...") - exit(0) - -if __name__ == '__main__': - run_test_tv_wrapper() \ No newline at end of file diff --git a/teleop/open_television/setup.py b/teleop/open_television/setup.py deleted file mode 100644 index dc471af..0000000 --- a/teleop/open_television/setup.py +++ /dev/null @@ -1,48 +0,0 @@ -from setuptools import setup, find_packages - -setup( - name='open_television', - version='0.0.1', - description='XR vision and hand/controller interface for unitree robotics', - author='silencht', - packages=find_packages(), - install_requires=[ - 'numpy==1.23.0', - 'opencv_contrib_python==4.10.0.82', - 'opencv_python==4.9.0.80', - 'aiohttp==3.9.5', - 'aiohttp_cors==0.7.0', - 'aiortc==1.8.0', - 'av==11.0.0', - # 'vuer==0.0.32rc7', # avp \ pico, hand tracking all work fine. but it not support controller tracking. - - # 'vuer==0.0.35', # avp hand tracking works fine. pico is fine too, but the right eye occasionally goes black for a short time at start. - - # from 'vuer==0.0.36rc1', # avp hand tracking only shows a flat RGB image — there's no stereo view. Pico (hand / controller tracking) is the same, - # and sometimes the right eye occasionally goes black for a short time at start. - # Both avp / pico can display the hand or controller marker, which looks like a black box. - - # to 'vuer==0.0.42rc16', # avp hand tracking only shows a flat RGB image — there's no stereo view. Pico (hand / controller tracking) is the same, - # and sometimes the right eye occasionally goes black for a short time at start. - # Both avp / pico can display the hand or controller marker, which looks like RGB three-axis color coordinates. - - # from 'vuer==0.0.42', # avp hand tracking only shows a flat RGB image — there's no stereo view. - # to 'vuer==0.0.45', pico hand tracking also only shows a flat RGB image — there's no stereo view. Unable to retrieve hand tracking data. - # pico controller tracking also only shows a flat RGB image — there's no stereo view. Controller data can be retrieved. - - # from 'vuer==0.0.46' # avp hand tracking work fine. - # to - # 'vuer==0.0.56', # In pico hand tracking mode, using a hand gesture to click the "Virtual Reality" button - # causes the screen to stay black and stuck loading. But if the button is clicked with the controller instead, everything works fine. - # In pico controller tracking mode, using controller to click the "Virtual Reality" button - # causes the screen to stay black and stuck loading. But if the button is clicked with a hand gesture instead, everything works fine. - # avp \ pico all have hand marker visualization (RGB three-axis color coordinates). - - 'vuer==0.0.60', # a good version - # https://github.com/unitreerobotics/avp_teleoperate/issues/53 - # https://github.com/vuer-ai/vuer/issues/45 - # https://github.com/vuer-ai/vuer/issues/65 - - ], - python_requires='>=3.8', -) \ No newline at end of file diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py deleted file mode 100644 index aa5c527..0000000 --- a/teleop/open_television/television.py +++ /dev/null @@ -1,514 +0,0 @@ -from vuer import Vuer -from vuer.schemas import ImageBackground, Hands, MotionControllers, WebRTCVideoPlane, WebRTCStereoVideoPlane -from multiprocessing import Value, Array, Process, shared_memory -import numpy as np -import asyncio -import cv2 -import os - - -class TeleVision: - def __init__(self, binocular: bool, use_hand_tracking: bool, img_shape, img_shm_name, cert_file=None, key_file=None, ngrok=False, webrtc=False): - """ - TeleVision class for OpenXR-based VR/AR applications. - This class handles the communication with the Vuer server and manages the shared memory for image and pose data. - - :param binocular: bool, whether the application is binocular (stereoscopic) or monocular. - :param use_hand_tracking: bool, whether to use hand tracking or controller tracking. - :param img_shape: tuple, shape of the image (height, width, channels). - :param img_shm_name: str, name of the shared memory for the image. - :param cert_file: str, path to the SSL certificate file. - :param key_file: str, path to the SSL key file. - :param ngrok: bool, whether to use ngrok for tunneling. - """ - self.binocular = binocular - self.use_hand_tracking = use_hand_tracking - self.img_height = img_shape[0] - if self.binocular: - self.img_width = img_shape[1] // 2 - else: - self.img_width = img_shape[1] - - current_module_dir = os.path.dirname(os.path.abspath(__file__)) - if cert_file is None: - cert_file = os.path.join(current_module_dir, "cert.pem") - if key_file is None: - key_file = os.path.join(current_module_dir, "key.pem") - - if ngrok: - self.vuer = Vuer(host='0.0.0.0', queries=dict(grid=False), queue_len=3) - else: - self.vuer = Vuer(host='0.0.0.0', cert=cert_file, key=key_file, queries=dict(grid=False), queue_len=3) - - self.vuer.add_handler("CAMERA_MOVE")(self.on_cam_move) - if self.use_hand_tracking: - self.vuer.add_handler("HAND_MOVE")(self.on_hand_move) - else: - self.vuer.add_handler("CONTROLLER_MOVE")(self.on_controller_move) - - existing_shm = shared_memory.SharedMemory(name=img_shm_name) - self.img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=existing_shm.buf) - - self.webrtc = webrtc - if self.binocular and not self.webrtc: - self.vuer.spawn(start=False)(self.main_image_binocular) - elif not self.binocular and not self.webrtc: - self.vuer.spawn(start=False)(self.main_image_monocular) - elif self.webrtc: - self.vuer.spawn(start=False)(self.main_image_webrtc) - - self.head_pose_shared = Array('d', 16, lock=True) - self.left_arm_pose_shared = Array('d', 16, lock=True) - self.right_arm_pose_shared = Array('d', 16, lock=True) - if self.use_hand_tracking: - self.left_hand_position_shared = Array('d', 75, lock=True) - self.right_hand_position_shared = Array('d', 75, lock=True) - self.left_hand_orientation_shared = Array('d', 25 * 9, lock=True) - self.right_hand_orientation_shared = Array('d', 25 * 9, lock=True) - - self.left_pinch_state_shared = Value('b', False, lock=True) - self.left_pinch_value_shared = Value('d', 0.0, lock=True) - self.left_squeeze_state_shared = Value('b', False, lock=True) - self.left_squeeze_value_shared = Value('d', 0.0, lock=True) - - self.right_pinch_state_shared = Value('b', False, lock=True) - self.right_pinch_value_shared = Value('d', 0.0, lock=True) - self.right_squeeze_state_shared = Value('b', False, lock=True) - self.right_squeeze_value_shared = Value('d', 0.0, lock=True) - else: - self.left_trigger_state_shared = Value('b', False, lock=True) - self.left_trigger_value_shared = Value('d', 0.0, lock=True) - self.left_squeeze_state_shared = Value('b', False, lock=True) - self.left_squeeze_value_shared = Value('d', 0.0, lock=True) - self.left_thumbstick_state_shared = Value('b', False, lock=True) - self.left_thumbstick_value_shared = Array('d', 2, lock=True) - self.left_aButton_shared = Value('b', False, lock=True) - self.left_bButton_shared = Value('b', False, lock=True) - - self.right_trigger_state_shared = Value('b', False, lock=True) - self.right_trigger_value_shared = Value('d', 0.0, lock=True) - self.right_squeeze_state_shared = Value('b', False, lock=True) - self.right_squeeze_value_shared = Value('d', 0.0, lock=True) - self.right_thumbstick_state_shared = Value('b', False, lock=True) - self.right_thumbstick_value_shared = Array('d', 2, lock=True) - self.right_aButton_shared = Value('b', False, lock=True) - self.right_bButton_shared = Value('b', False, lock=True) - - self.process = Process(target=self.vuer_run) - self.process.daemon = True - self.process.start() - - def vuer_run(self): - self.vuer.run() - - async def on_cam_move(self, event, session, fps=60): - try: - with self.head_pose_shared.get_lock(): - self.head_pose_shared[:] = event.value["camera"]["matrix"] - except: - pass - - async def on_controller_move(self, event, session, fps=60): - try: - with self.left_arm_pose_shared.get_lock(): - self.left_arm_pose_shared[:] = event.value["left"] - with self.right_arm_pose_shared.get_lock(): - self.right_arm_pose_shared[:] = event.value["right"] - - left_controller_state = event.value["leftState"] - right_controller_state = event.value["rightState"] - - def extract_controller_states(state_dict, prefix): - # trigger - with getattr(self, f"{prefix}_trigger_state_shared").get_lock(): - getattr(self, f"{prefix}_trigger_state_shared").value = bool(state_dict.get("trigger", False)) - with getattr(self, f"{prefix}_trigger_value_shared").get_lock(): - getattr(self, f"{prefix}_trigger_value_shared").value = float(state_dict.get("triggerValue", 0.0)) - # squeeze - with getattr(self, f"{prefix}_squeeze_state_shared").get_lock(): - getattr(self, f"{prefix}_squeeze_state_shared").value = bool(state_dict.get("squeeze", False)) - with getattr(self, f"{prefix}_squeeze_value_shared").get_lock(): - getattr(self, f"{prefix}_squeeze_value_shared").value = float(state_dict.get("squeezeValue", 0.0)) - # thumbstick - with getattr(self, f"{prefix}_thumbstick_state_shared").get_lock(): - getattr(self, f"{prefix}_thumbstick_state_shared").value = bool(state_dict.get("thumbstick", False)) - with getattr(self, f"{prefix}_thumbstick_value_shared").get_lock(): - getattr(self, f"{prefix}_thumbstick_value_shared")[:] = state_dict.get("thumbstickValue", [0.0, 0.0]) - # buttons - with getattr(self, f"{prefix}_aButton_shared").get_lock(): - getattr(self, f"{prefix}_aButton_shared").value = bool(state_dict.get("aButton", False)) - with getattr(self, f"{prefix}_bButton_shared").get_lock(): - getattr(self, f"{prefix}_bButton_shared").value = bool(state_dict.get("bButton", False)) - - extract_controller_states(left_controller_state, "left") - extract_controller_states(right_controller_state, "right") - except: - pass - - async def on_hand_move(self, event, session, fps=60): - try: - left_hand_data = event.value["left"] - right_hand_data = event.value["right"] - left_hand_state = event.value["leftState"] - right_hand_state = event.value["rightState"] - - def extract_hand_poses(hand_data, arm_pose_shared, hand_position_shared, hand_orientation_shared): - with arm_pose_shared.get_lock(): - arm_pose_shared[:] = hand_data[0:16] - - with hand_position_shared.get_lock(): - for i in range(25): - base = i * 16 - hand_position_shared[i * 3: i * 3 + 3] = [hand_data[base + 12], hand_data[base + 13], hand_data[base + 14]] - - with hand_orientation_shared.get_lock(): - for i in range(25): - base = i * 16 - hand_orientation_shared[i * 9: i * 9 + 9] = [ - hand_data[base + 0], hand_data[base + 1], hand_data[base + 2], - hand_data[base + 4], hand_data[base + 5], hand_data[base + 6], - hand_data[base + 8], hand_data[base + 9], hand_data[base + 10], - ] - - def extract_hand_states(state_dict, prefix): - # pinch - with getattr(self, f"{prefix}_pinch_state_shared").get_lock(): - getattr(self, f"{prefix}_pinch_state_shared").value = bool(state_dict.get("pinch", False)) - with getattr(self, f"{prefix}_pinch_value_shared").get_lock(): - getattr(self, f"{prefix}_pinch_value_shared").value = float(state_dict.get("pinchValue", 0.0)) - # squeeze - with getattr(self, f"{prefix}_squeeze_state_shared").get_lock(): - getattr(self, f"{prefix}_squeeze_state_shared").value = bool(state_dict.get("squeeze", False)) - with getattr(self, f"{prefix}_squeeze_value_shared").get_lock(): - getattr(self, f"{prefix}_squeeze_value_shared").value = float(state_dict.get("squeezeValue", 0.0)) - - extract_hand_poses(left_hand_data, self.left_arm_pose_shared, self.left_hand_position_shared, self.left_hand_orientation_shared) - extract_hand_poses(right_hand_data, self.right_arm_pose_shared, self.right_hand_position_shared, self.right_hand_orientation_shared) - extract_hand_states(left_hand_state, "left") - extract_hand_states(right_hand_state, "right") - - except: - pass - - async def main_image_binocular(self, session, fps=60): - if self.use_hand_tracking: - session.upsert( - Hands( - stream=True, - key="hands", - hideLeft=True, - hideRight=True - ), - to="bgChildren", - ) - else: - session.upsert( - MotionControllers( - stream=True, - key="motionControllers", - left=True, - right=True, - ), - to="bgChildren", - ) - - while True: - display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB) - # aspect_ratio = self.img_width / self.img_height - session.upsert( - [ - ImageBackground( - display_image[:, :self.img_width], - aspect=1.778, - height=1, - distanceToCamera=1, - # The underlying rendering engine supported a layer binary bitmask for both objects and the camera. - # Below we set the two image planes, left and right, to layers=1 and layers=2. - # Note that these two masks are associated with left eye’s camera and the right eye’s camera. - layers=1, - format="jpeg", - quality=100, - key="background-left", - interpolate=True, - ), - ImageBackground( - display_image[:, self.img_width:], - aspect=1.778, - height=1, - distanceToCamera=1, - layers=2, - format="jpeg", - quality=100, - key="background-right", - interpolate=True, - ), - ], - to="bgChildren", - ) - # 'jpeg' encoding should give you about 30fps with a 16ms wait in-between. - await asyncio.sleep(0.016 * 2) - - async def main_image_monocular(self, session, fps=60): - if self.use_hand_tracking: - session.upsert( - Hands( - stream=True, - key="hands", - hideLeft=True, - hideRight=True - ), - to="bgChildren", - ) - else: - session.upsert( - MotionControllers( - stream=True, - key="motionControllers", - left=True, - right=True, - ), - to="bgChildren", - ) - - while True: - display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB) - # aspect_ratio = self.img_width / self.img_height - session.upsert( - [ - ImageBackground( - display_image, - aspect=1.778, - height=1, - distanceToCamera=1, - format="jpeg", - quality=50, - key="background-mono", - interpolate=True, - ), - ], - to="bgChildren", - ) - await asyncio.sleep(0.016) - - async def main_image_webrtc(self, session, fps=60): - if self.use_hand_tracking: - session.upsert( - Hands( - stream=True, - key="hands", - showLeft=False, - showRight=False - ), - to="bgChildren", - ) - else: - session.upsert( - MotionControllers( - stream=True, - key="motionControllers", - showLeft=False, - showRight=False, - ) - ) - - session.upsert( - WebRTCVideoPlane( - # WebRTCStereoVideoPlane( - src="https://10.0.7.49:8080/offer", - iceServer={}, - key="webrtc", - aspect=1.778, - height = 7, - ), - to="bgChildren", - ) - while True: - await asyncio.sleep(1) - # ==================== common data ==================== - @property - def head_pose(self): - """np.ndarray, shape (4, 4), head SE(3) pose matrix from Vuer (basis OpenXR Convention).""" - with self.head_pose_shared.get_lock(): - return np.array(self.head_pose_shared[:]).reshape(4, 4, order="F") - - @property - def left_arm_pose(self): - """np.ndarray, shape (4, 4), left arm SE(3) pose matrix from Vuer (basis OpenXR Convention).""" - with self.left_arm_pose_shared.get_lock(): - return np.array(self.left_arm_pose_shared[:]).reshape(4, 4, order="F") - - @property - def right_arm_pose(self): - """np.ndarray, shape (4, 4), right arm SE(3) pose matrix from Vuer (basis OpenXR Convention).""" - with self.right_arm_pose_shared.get_lock(): - return np.array(self.right_arm_pose_shared[:]).reshape(4, 4, order="F") - - # ==================== Hand Tracking Data ==================== - @property - def left_hand_positions(self): - """np.ndarray, shape (25, 3), left hand 25 landmarks' 3D positions.""" - with self.left_hand_position_shared.get_lock(): - return np.array(self.left_hand_position_shared[:]).reshape(25, 3) - - @property - def right_hand_positions(self): - """np.ndarray, shape (25, 3), right hand 25 landmarks' 3D positions.""" - with self.right_hand_position_shared.get_lock(): - return np.array(self.right_hand_position_shared[:]).reshape(25, 3) - - @property - def left_hand_orientations(self): - """np.ndarray, shape (25, 3, 3), left hand 25 landmarks' orientations (flattened 3x3 matrices, column-major).""" - with self.left_hand_orientation_shared.get_lock(): - return np.array(self.left_hand_orientation_shared[:]).reshape(25, 9).reshape(25, 3, 3, order="F") - - @property - def right_hand_orientations(self): - """np.ndarray, shape (25, 3, 3), right hand 25 landmarks' orientations (flattened 3x3 matrices, column-major).""" - with self.right_hand_orientation_shared.get_lock(): - return np.array(self.right_hand_orientation_shared[:]).reshape(25, 9).reshape(25, 3, 3, order="F") - - @property - def left_hand_pinch_state(self): - """bool, whether left hand is pinching.""" - with self.left_pinch_state_shared.get_lock(): - return self.left_pinch_state_shared.value - - @property - def left_hand_pinch_value(self): - """float, pinch strength of left hand.""" - with self.left_pinch_value_shared.get_lock(): - return self.left_pinch_value_shared.value - - @property - def left_hand_squeeze_state(self): - """bool, whether left hand is squeezing.""" - with self.left_squeeze_state_shared.get_lock(): - return self.left_squeeze_state_shared.value - - @property - def left_hand_squeeze_value(self): - """float, squeeze strength of left hand.""" - with self.left_squeeze_value_shared.get_lock(): - return self.left_squeeze_value_shared.value - - @property - def right_hand_pinch_state(self): - """bool, whether right hand is pinching.""" - with self.right_pinch_state_shared.get_lock(): - return self.right_pinch_state_shared.value - - @property - def right_hand_pinch_value(self): - """float, pinch strength of right hand.""" - with self.right_pinch_value_shared.get_lock(): - return self.right_pinch_value_shared.value - - @property - def right_hand_squeeze_state(self): - """bool, whether right hand is squeezing.""" - with self.right_squeeze_state_shared.get_lock(): - return self.right_squeeze_state_shared.value - - @property - def right_hand_squeeze_value(self): - """float, squeeze strength of right hand.""" - with self.right_squeeze_value_shared.get_lock(): - return self.right_squeeze_value_shared.value - - # ==================== Controller Data ==================== - @property - def left_controller_trigger_state(self): - """bool, left controller trigger pressed or not.""" - with self.left_trigger_state_shared.get_lock(): - return self.left_trigger_state_shared.value - - @property - def left_controller_trigger_value(self): - """float, left controller trigger analog value (0.0 ~ 1.0).""" - with self.left_trigger_value_shared.get_lock(): - return self.left_trigger_value_shared.value - - @property - def left_controller_squeeze_state(self): - """bool, left controller squeeze pressed or not.""" - with self.left_squeeze_state_shared.get_lock(): - return self.left_squeeze_state_shared.value - - @property - def left_controller_squeeze_value(self): - """float, left controller squeeze analog value (0.0 ~ 1.0).""" - with self.left_squeeze_value_shared.get_lock(): - return self.left_squeeze_value_shared.value - - @property - def left_controller_thumbstick_state(self): - """bool, whether left thumbstick is touched or clicked.""" - with self.left_thumbstick_state_shared.get_lock(): - return self.left_thumbstick_state_shared.value - - @property - def left_controller_thumbstick_value(self): - """np.ndarray, shape (2,), left thumbstick 2D axis values (x, y).""" - with self.left_thumbstick_value_shared.get_lock(): - return np.array(self.left_thumbstick_value_shared[:]) - - @property - def left_controller_aButton(self): - """bool, left controller 'A' button pressed.""" - with self.left_aButton_shared.get_lock(): - return self.left_aButton_shared.value - - @property - def left_controller_bButton(self): - """bool, left controller 'B' button pressed.""" - with self.left_bButton_shared.get_lock(): - return self.left_bButton_shared.value - - @property - def right_controller_trigger_state(self): - """bool, right controller trigger pressed or not.""" - with self.right_trigger_state_shared.get_lock(): - return self.right_trigger_state_shared.value - - @property - def right_controller_trigger_value(self): - """float, right controller trigger analog value (0.0 ~ 1.0).""" - with self.right_trigger_value_shared.get_lock(): - return self.right_trigger_value_shared.value - - @property - def right_controller_squeeze_state(self): - """bool, right controller squeeze pressed or not.""" - with self.right_squeeze_state_shared.get_lock(): - return self.right_squeeze_state_shared.value - - @property - def right_controller_squeeze_value(self): - """float, right controller squeeze analog value (0.0 ~ 1.0).""" - with self.right_squeeze_value_shared.get_lock(): - return self.right_squeeze_value_shared.value - - @property - def right_controller_thumbstick_state(self): - """bool, whether right thumbstick is touched or clicked.""" - with self.right_thumbstick_state_shared.get_lock(): - return self.right_thumbstick_state_shared.value - - @property - def right_controller_thumbstick_value(self): - """np.ndarray, shape (2,), right thumbstick 2D axis values (x, y).""" - with self.right_thumbstick_value_shared.get_lock(): - return np.array(self.right_thumbstick_value_shared[:]) - - @property - def right_controller_aButton(self): - """bool, right controller 'A' button pressed.""" - with self.right_aButton_shared.get_lock(): - return self.right_aButton_shared.value - - @property - def right_controller_bButton(self): - """bool, right controller 'B' button pressed.""" - with self.right_bButton_shared.get_lock(): - return self.right_bButton_shared.value \ No newline at end of file diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py deleted file mode 100644 index 92ca634..0000000 --- a/teleop/open_television/tv_wrapper.py +++ /dev/null @@ -1,410 +0,0 @@ -import numpy as np -from television import TeleVision -from dataclasses import dataclass, field - -""" -(basis) OpenXR Convention : y up, z back, x right. -(basis) Robot Convention : z up, y left, x front. - -under (basis) Robot Convention, humanoid arm's initial pose convention: - - # (initial pose) OpenXR Left Arm Pose Convention (hand tracking): - - the x-axis pointing from wrist toward middle. - - the y-axis pointing from index toward pinky. - - the z-axis pointing from palm toward back of the hand. - - # (initial pose) OpenXR Right Arm Pose Convention (hand tracking): - - the x-axis pointing from wrist toward middle. - - the y-axis pointing from pinky toward index. - - the z-axis pointing from palm toward back of the hand. - - # (initial pose) Unitree Humanoid Left Arm URDF Convention: - - the x-axis pointing from wrist toward middle. - - the y-axis pointing from palm toward back of the hand. - - the z-axis pointing from pinky toward index. - - # (initial pose) Unitree Humanoid Right Arm URDF Convention: - - the x-axis pointing from wrist toward middle. - - the y-axis pointing from back of the hand toward palm. - - the z-axis pointing from pinky toward index. - -under (basis) Robot Convention, humanoid hand's initial pose convention: - - # (initial pose) OpenXR Left Hand Pose Convention (hand tracking): - - the x-axis pointing from wrist toward middle. - - the y-axis pointing from index toward pinky. - - the z-axis pointing from palm toward back of the hand. - - # (initial pose) OpenXR Right Hand Pose Convention (hand tracking): - - the x-axis pointing from wrist toward middle. - - the y-axis pointing from pinky toward index. - - the z-axis pointing from palm toward back of the hand. - - # (initial pose) Unitree Humanoid Left Hand URDF Convention: - - The x-axis pointing from palm toward back of the hand. - - The y-axis pointing from middle toward wrist. - - The z-axis pointing from pinky toward index. - - # (initial pose) Unitree Humanoid Right Hand URDF Convention: - - The x-axis pointing from palm toward back of the hand. - - The y-axis pointing from middle toward wrist. - - The z-axis pointing from index toward pinky. - -p.s. TeleVision (Vuer) obtains all raw data under the (basis) OpenXR Convention. - In addition, arm pose data (hand tracking) follows the (initial pose) OpenXR Arm Pose Convention, - while arm pose data (controller tracking) directly follows the (initial pose) Unitree Humanoid Arm URDF Convention (thus no transform is needed). - Meanwhile, all raw data is in the WORLD frame defined by XR device odometry. - -p.s. From website: https://registry.khronos.org/OpenXR/specs/1.1/man/html/openxr.html. - You can find **(initial pose) OpenXR Left/Right Arm Pose Convention** related information like this below: - "The wrist joint is located at the pivot point of the wrist, which is location invariant when twisting the hand without moving the forearm. - The backward (+Z) direction is parallel to the line from wrist joint to middle finger metacarpal joint, and points away from the finger tips. - The up (+Y) direction points out towards back of the hand and perpendicular to the skin at wrist. - The X direction is perpendicular to the Y and Z directions and follows the right hand rule." - Note: The above context is of course under **(basis) OpenXR Convention**. - -p.s. **Unitree Arm/Hand URDF initial pose Convention** information come from URDF files. -""" - - -def safe_mat_update(prev_mat, mat): - # Return previous matrix and False flag if the new matrix is non-singular (determinant ≠ 0). - det = np.linalg.det(mat) - if not np.isfinite(det) or np.isclose(det, 0.0, atol=1e-6): - return prev_mat, False - return mat, True - -def fast_mat_inv(mat): - ret = np.eye(4) - ret[:3, :3] = mat[:3, :3].T - ret[:3, 3] = -mat[:3, :3].T @ mat[:3, 3] - return ret - -def safe_rot_update(prev_rot_array, rot_array): - dets = np.linalg.det(rot_array) - if not np.all(np.isfinite(dets)) or np.any(np.isclose(dets, 0.0, atol=1e-6)): - return prev_rot_array, False - return rot_array, True - -# constants variable -T_TO_UNITREE_HUMANOID_LEFT_ARM = np.array([[1, 0, 0, 0], - [0, 0,-1, 0], - [0, 1, 0, 0], - [0, 0, 0, 1]]) - -T_TO_UNITREE_HUMANOID_RIGHT_ARM = np.array([[1, 0, 0, 0], - [0, 0, 1, 0], - [0,-1, 0, 0], - [0, 0, 0, 1]]) - -T_TO_UNITREE_HAND = np.array([[0, 0, 1, 0], - [-1, 0, 0, 0], - [0, -1, 0, 0], - [0, 0, 0, 1]]) - -T_ROBOT_OPENXR = np.array([[ 0, 0,-1, 0], - [-1, 0, 0, 0], - [ 0, 1, 0, 0], - [ 0, 0, 0, 1]]) - -T_OPENXR_ROBOT = np.array([[ 0,-1, 0, 0], - [ 0, 0, 1, 0], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - -R_ROBOT_OPENXR = np.array([[ 0, 0,-1], - [-1, 0, 0], - [ 0, 1, 0]]) - -R_OPENXR_ROBOT = np.array([[ 0,-1, 0], - [ 0, 0, 1], - [-1, 0, 0]]) - -CONST_HEAD_POSE = np.array([[1, 0, 0, 0], - [0, 1, 0, 1.5], - [0, 0, 1, -0.2], - [0, 0, 0, 1]]) - -# For Robot initial position -CONST_RIGHT_ARM_POSE = np.array([[1, 0, 0, 0.15], - [0, 1, 0, 1.13], - [0, 0, 1, -0.3], - [0, 0, 0, 1]]) - -CONST_LEFT_ARM_POSE = np.array([[1, 0, 0, -0.15], - [0, 1, 0, 1.13], - [0, 0, 1, -0.3], - [0, 0, 0, 1]]) - -CONST_HAND_ROT = np.tile(np.eye(3)[None, :, :], (25, 1, 1)) - -@dataclass -class TeleStateData: - # hand tracking - left_pinch_state: bool = False # True if index and thumb are pinching - left_squeeze_state: bool = False # True if hand is making a fist - left_squeeze_value: float = 0.0 # (0.0 ~ 1.0) degree of hand squeeze - right_pinch_state: bool = False # True if index and thumb are pinching - right_squeeze_state: bool = False # True if hand is making a fist - right_squeeze_value: float = 0.0 # (0.0 ~ 1.0) degree of hand squeeze - - # controller tracking - left_trigger_state: bool = False # True if trigger is actively pressed - left_squeeze_ctrl_state: bool = False # True if grip button is pressed - left_squeeze_ctrl_value: float = 0.0 # (0.0 ~ 1.0) grip pull depth - left_thumbstick_state: bool = False # True if thumbstick button is pressed - left_thumbstick_value: np.ndarray = field(default_factory=lambda: np.zeros(2)) # 2D vector (x, y), normalized - left_aButton: bool = False # True if A button is pressed - left_bButton: bool = False # True if B button is pressed - right_trigger_state: bool = False # True if trigger is actively pressed - right_squeeze_ctrl_state: bool = False # True if grip button is pressed - right_squeeze_ctrl_value: float = 0.0 # (0.0 ~ 1.0) grip pull depth - right_thumbstick_state: bool = False # True if thumbstick button is pressed - right_thumbstick_value: np.ndarray = field(default_factory=lambda: np.zeros(2)) # 2D vector (x, y), normalized - right_aButton: bool = False # True if A button is pressed - right_bButton: bool = False # True if B button is pressed - -@dataclass -class TeleData: - head_pose: np.ndarray # (4,4) SE(3) pose of head matrix - left_arm_pose: np.ndarray # (4,4) SE(3) pose of left arm - right_arm_pose: np.ndarray # (4,4) SE(3) pose of right arm - left_hand_pos: np.ndarray = None # (25,3) 3D positions of left hand joints - right_hand_pos: np.ndarray = None # (25,3) 3D positions of right hand joints - left_hand_rot: np.ndarray = None # (25,3,3) rotation matrices of left hand joints - right_hand_rot: np.ndarray = None # (25,3,3) rotation matrices of right hand joints - left_pinch_value: float = None # float (1x.0 ~ 0.0) pinch distance between index and thumb - right_pinch_value: float = None # float (1x.0 ~ 0.0) pinch distance between index and thumb - left_trigger_value: float = None # float (10.0 ~ 0.0) trigger pull depth - right_trigger_value: float = None # float (10.0 ~ 0.0) trigger pull depth - tele_state: TeleStateData = field(default_factory=TeleStateData) - - -class TeleVisionWrapper: - def __init__(self, binocular: bool, use_hand_tracking: bool, img_shape, img_shm_name, return_state_data: bool = True, return_hand_rot_data: bool = False, - cert_file = None, key_file = None, ngrok = False, webrtc = False): - """ - TeleVisionWrapper is a wrapper for the TeleVision class, which handles XR device's data suit for robot control. - It initializes the TeleVision instance with the specified parameters and provides a method to get motion state data. - - :param binocular: A boolean indicating whether the head camera device is binocular or not. - :param use_hand_tracking: A boolean indicating whether to use hand tracking or use controller tracking. - :param img_shape: The shape of the image to be processed. - :param img_shm_name: The name of the shared memory for the image. - :param return_state: A boolean indicating whether to return the state of the hand or controller. - :param return_hand_rot: A boolean indicating whether to return the hand rotation data. - :param cert_file: The path to the certificate file for secure connection. - :param key_file: The path to the key file for secure connection. - """ - self.use_hand_tracking = use_hand_tracking - self.return_state_data = return_state_data - self.return_hand_rot_data = return_hand_rot_data - self.tvuer = TeleVision(binocular, use_hand_tracking, img_shape, img_shm_name, cert_file=cert_file, key_file=key_file, - ngrok=ngrok, webrtc=webrtc) - - def get_motion_state_data(self): - """ - Get processed motion state data from the TeleVision instance. - - All returned data are transformed from the OpenXR Convention to the (Robot & Unitree) Convention. - """ - # Variable Naming Convention below, - # ┌────────────┬───────────────────────────┬──────────────────────────────────┬────────────────────────────────────┬────────────────────────────────────┐ - # │left / right│ Bxr │ Brobot │ IPxr │ IPunitree │ - # │────────────│───────────────────────────│──────────────────────────────────│────────────────────────────────────│────────────────────────────────────│ - # │ side │ (basis) OpenXR Convention │ (basis) Robot Convention │ (initial pose) OpenXR Convention │ (initial pose) Unitree Convention │ - # └────────────┴───────────────────────────┴──────────────────────────────────┴────────────────────────────────────┴────────────────────────────────────┘ - # ┌───────────────────────────────────┬─────────────────────┐ - # │ world / arm / head / waist │ arm / head / hand │ - # │───────────────────────────────────│─────────────────────│ - # │ source frame │ target frame │ - # └───────────────────────────────────┴─────────────────────┘ - - # TeleVision (Vuer) obtains all raw data under the (basis) OpenXR Convention. - Bxr_world_head, head_pose_is_valid = safe_mat_update(CONST_HEAD_POSE, self.tvuer.head_pose) - - if self.use_hand_tracking: - # 'Arm' pose data follows (basis) OpenXR Convention and (initial pose) OpenXR Arm Convention. - left_IPxr_Bxr_world_arm, left_arm_is_valid = safe_mat_update(CONST_LEFT_ARM_POSE, self.tvuer.left_arm_pose) - right_IPxr_Bxr_world_arm, right_arm_is_valid = safe_mat_update(CONST_RIGHT_ARM_POSE, self.tvuer.right_arm_pose) - - # Change basis convention - # From (basis) OpenXR Convention to (basis) Robot Convention: - # Brobot_Pose = T_{robot}_{openxr} * Bxr_Pose * T_{robot}_{openxr}^T ==> - # Brobot_Pose = T_{robot}_{openxr} * Bxr_Pose * T_{openxr}_{robot} - # Reason for right multiply T_OPENXR_ROBOT = fast_mat_inv(T_ROBOT_OPENXR): - # This is similarity transformation: B = PAP^{-1}, that is B ~ A - # For example: - # - For a pose data T_r under the (basis) Robot Convention, left-multiplying Brobot_Pose means: - # Brobot_Pose * T_r ==> T_{robot}_{openxr} * PoseMatrix_openxr * T_{openxr}_{robot} * T_r - # - First, transform T_r to the (basis) OpenXR Convention (The function of T_{openxr}_{robot}) - # - Then, apply the rotation PoseMatrix_openxr in the OpenXR Convention (The function of PoseMatrix_openxr) - # - Finally, transform back to the Robot Convention (The function of T_{robot}_{openxr}) - # - This results in the same rotation effect under the Robot Convention as in the OpenXR Convention. - Brobot_world_head = T_ROBOT_OPENXR @ Bxr_world_head @ T_OPENXR_ROBOT - left_IPxr_Brobot_world_arm = T_ROBOT_OPENXR @ left_IPxr_Bxr_world_arm @ T_OPENXR_ROBOT - right_IPxr_Brobot_world_arm = T_ROBOT_OPENXR @ right_IPxr_Bxr_world_arm @ T_OPENXR_ROBOT - - # Change initial pose convention - # From (initial pose) OpenXR Arm Convention to (initial pose) Unitree Humanoid Arm URDF Convention - # Reason for right multiply (T_TO_UNITREE_HUMANOID_LEFT_ARM) : Rotate 90 degrees counterclockwise about its own x-axis. - # Reason for right multiply (T_TO_UNITREE_HUMANOID_RIGHT_ARM): Rotate 90 degrees clockwise about its own x-axis. - left_IPunitree_Brobot_world_arm = left_IPxr_Brobot_world_arm @ (T_TO_UNITREE_HUMANOID_LEFT_ARM if left_arm_is_valid else np.eye(4)) - right_IPunitree_Brobot_world_arm = right_IPxr_Brobot_world_arm @ (T_TO_UNITREE_HUMANOID_RIGHT_ARM if right_arm_is_valid else np.eye(4)) - - # Transfer from WORLD to HEAD coordinate (translation adjustment only) - left_IPunitree_Brobot_head_arm = left_IPunitree_Brobot_world_arm.copy() - right_IPunitree_Brobot_head_arm = right_IPunitree_Brobot_world_arm.copy() - left_IPunitree_Brobot_head_arm[0:3, 3] = left_IPunitree_Brobot_head_arm[0:3, 3] - Brobot_world_head[0:3, 3] - right_IPunitree_Brobot_head_arm[0:3, 3] = right_IPunitree_Brobot_world_arm[0:3, 3] - Brobot_world_head[0:3, 3] - - # =====coordinate origin offset===== - # The origin of the coordinate for IK Solve is near the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to visualize it. - # The origin of the coordinate of IPunitree_Brobot_head_arm is HEAD. - # So it is necessary to translate the origin of IPunitree_Brobot_head_arm from HEAD to WAIST. - left_IPunitree_Brobot_waist_arm = left_IPunitree_Brobot_head_arm.copy() - right_IPunitree_Brobot_waist_arm = right_IPunitree_Brobot_head_arm.copy() - left_IPunitree_Brobot_waist_arm[0, 3] +=0.15 # x - right_IPunitree_Brobot_waist_arm[0,3] +=0.15 - left_IPunitree_Brobot_waist_arm[2, 3] +=0.45 # z - right_IPunitree_Brobot_waist_arm[2,3] +=0.45 - - # -----------------------------------hand position---------------------------------------- - if left_arm_is_valid and right_arm_is_valid: - # Homogeneous, [xyz] to [xyz1] - # np.concatenate([25,3]^T,(1,25)) ==> Bxr_world_hand_pos.shape is (4,25) - # Now under (basis) OpenXR Convention, Bxr_world_hand_pos data like this: - # [x0 x1 x2 ··· x23 x24] - # [y0 y1 y1 ··· y23 y24] - # [z0 z1 z2 ··· z23 z24] - # [ 1 1 1 ··· 1 1] - left_IPxr_Bxr_world_hand_pos = np.concatenate([self.tvuer.left_hand_positions.T, np.ones((1, self.tvuer.left_hand_positions.shape[0]))]) - right_IPxr_Bxr_world_hand_pos = np.concatenate([self.tvuer.right_hand_positions.T, np.ones((1, self.tvuer.right_hand_positions.shape[0]))]) - - # Change basis convention - # From (basis) OpenXR Convention to (basis) Robot Convention - # Just a change of basis for 3D points. No rotation, only translation. So, no need to right-multiply fast_mat_inv(T_ROBOT_OPENXR). - left_IPxr_Brobot_world_hand_pos = T_ROBOT_OPENXR @ left_IPxr_Bxr_world_hand_pos - right_IPxr_Brobot_world_hand_pos = T_ROBOT_OPENXR @ right_IPxr_Bxr_world_hand_pos - - # Transfer from WORLD to ARM frame under (basis) Robot Convention: - # Brobot_{world}_{arm}^T * Brobot_{world}_pos ==> Brobot_{arm}_{world} * Brobot_{world}_pos ==> Brobot_arm_hand_pos, Now it's based on the arm frame. - left_IPxr_Brobot_arm_hand_pos = fast_mat_inv(left_IPxr_Brobot_world_arm) @ left_IPxr_Brobot_world_hand_pos - right_IPxr_Brobot_arm_hand_pos = fast_mat_inv(right_IPxr_Brobot_world_arm) @ right_IPxr_Brobot_world_hand_pos - - # Change initial pose convention - # From (initial pose) XR Hand Convention to (initial pose) Unitree Humanoid Hand URDF Convention: - # T_TO_UNITREE_HAND @ IPxr_Brobot_arm_hand_pos ==> IPunitree_Brobot_arm_hand_pos - # ((4,4) @ (4,25))[0:3, :].T ==> (4,25)[0:3, :].T ==> (3,25).T ==> (25,3) - # Now under (initial pose) Unitree Humanoid Hand URDF Convention, matrix shape like this: - # [x0, y0, z0] - # [x1, y1, z1] - # ··· - # [x23,y23,z23] - # [x24,y24,z24] - left_IPunitree_Brobot_arm_hand_pos = (T_TO_UNITREE_HAND @ left_IPxr_Brobot_arm_hand_pos)[0:3, :].T - right_IPunitree_Brobot_arm_hand_pos = (T_TO_UNITREE_HAND @ right_IPxr_Brobot_arm_hand_pos)[0:3, :].T - else: - left_IPunitree_Brobot_arm_hand_pos = np.zeros((25, 3)) - right_IPunitree_Brobot_arm_hand_pos = np.zeros((25, 3)) - - # -----------------------------------hand rotation---------------------------------------- - if self.return_hand_rot_data: - left_Bxr_world_hand_rot, left_hand_rot_is_valid = safe_rot_update(CONST_HAND_ROT, self.tvuer.left_hand_orientations) # [25, 3, 3] - right_Bxr_world_hand_rot, right_hand_rot_is_valid = safe_rot_update(CONST_HAND_ROT, self.tvuer.right_hand_orientations) - - if left_hand_rot_is_valid and right_hand_rot_is_valid: - left_Bxr_arm_hand_rot = np.einsum('ij,njk->nik', left_IPxr_Bxr_world_arm[:3, :3].T, left_Bxr_world_hand_rot) - right_Bxr_arm_hand_rot = np.einsum('ij,njk->nik', right_IPxr_Bxr_world_arm[:3, :3].T, right_Bxr_world_hand_rot) - # Change basis convention - left_Brobot_arm_hand_rot = np.einsum('ij,njk,kl->nil', R_ROBOT_OPENXR, left_Bxr_arm_hand_rot, R_OPENXR_ROBOT) - right_Brobot_arm_hand_rot = np.einsum('ij,njk,kl->nil', R_ROBOT_OPENXR, right_Bxr_arm_hand_rot, R_OPENXR_ROBOT) - else: - left_Brobot_arm_hand_rot = left_Bxr_world_hand_rot - right_Brobot_arm_hand_rot = right_Bxr_world_hand_rot - else: - left_Brobot_arm_hand_rot = None - right_Brobot_arm_hand_rot = None - - if self.return_state_data: - hand_state = TeleStateData( - left_pinch_state=self.tvuer.left_hand_pinch_state, - left_squeeze_state=self.tvuer.left_hand_squeeze_state, - left_squeeze_value=self.tvuer.left_hand_squeeze_value, - right_pinch_state=self.tvuer.right_hand_pinch_state, - right_squeeze_state=self.tvuer.right_hand_squeeze_state, - right_squeeze_value=self.tvuer.right_hand_squeeze_value, - ) - else: - hand_state = None - - return TeleData( - head_pose=Brobot_world_head, - left_arm_pose=left_IPunitree_Brobot_waist_arm, - right_arm_pose=right_IPunitree_Brobot_waist_arm, - left_hand_pos=left_IPunitree_Brobot_arm_hand_pos, - right_hand_pos=right_IPunitree_Brobot_arm_hand_pos, - left_hand_rot=left_Brobot_arm_hand_rot, - right_hand_rot=right_Brobot_arm_hand_rot, - left_pinch_value=self.tvuer.left_hand_pinch_value * 100.0, - right_pinch_value=self.tvuer.right_hand_pinch_value * 100.0, - tele_state=hand_state - ) - else: - # Controller pose data directly follows the (initial pose) Unitree Humanoid Arm URDF Convention (thus no transform is needed). - left_IPunitree_Bxr_world_arm, left_arm_is_valid = safe_mat_update(CONST_LEFT_ARM_POSE, self.tvuer.left_arm_pose) - right_IPunitree_Bxr_world_arm, right_arm_is_valid = safe_mat_update(CONST_RIGHT_ARM_POSE, self.tvuer.right_arm_pose) - - # Change basis convention - Brobot_world_head = T_ROBOT_OPENXR @ Bxr_world_head @ T_OPENXR_ROBOT - left_IPunitree_Brobot_world_arm = T_ROBOT_OPENXR @ left_IPunitree_Bxr_world_arm @ T_OPENXR_ROBOT - right_IPunitree_Brobot_world_arm = T_ROBOT_OPENXR @ right_IPunitree_Bxr_world_arm @ T_OPENXR_ROBOT - - # Transfer from WORLD to HEAD coordinate (translation adjustment only) - left_IPunitree_Brobot_head_arm = left_IPunitree_Brobot_world_arm.copy() - right_IPunitree_Brobot_head_arm = right_IPunitree_Brobot_world_arm.copy() - left_IPunitree_Brobot_head_arm[0:3, 3] = left_IPunitree_Brobot_head_arm[0:3, 3] - Brobot_world_head[0:3, 3] - right_IPunitree_Brobot_head_arm[0:3, 3] = right_IPunitree_Brobot_head_arm[0:3, 3] - Brobot_world_head[0:3, 3] - - # =====coordinate origin offset===== - # The origin of the coordinate for IK Solve is near the WAIST joint motor. You can use teleop/robot_control/robot_arm_ik.py Unit_Test to check it. - # The origin of the coordinate of IPunitree_Brobot_head_arm is HEAD. - # So it is necessary to translate the origin of IPunitree_Brobot_head_arm from HEAD to WAIST. - left_IPunitree_Brobot_waist_arm = left_IPunitree_Brobot_head_arm.copy() - right_IPunitree_Brobot_waist_arm = right_IPunitree_Brobot_head_arm.copy() - left_IPunitree_Brobot_waist_arm[0, 3] +=0.15 # x - right_IPunitree_Brobot_waist_arm[0,3] +=0.15 - left_IPunitree_Brobot_waist_arm[2, 3] +=0.45 # z - right_IPunitree_Brobot_waist_arm[2,3] +=0.45 - # left_IPunitree_Brobot_waist_arm[1, 3] +=0.02 # y - # right_IPunitree_Brobot_waist_arm[1,3] +=0.02 - - # return data - if self.return_state_data: - controller_state = TeleStateData( - left_trigger_state=self.tvuer.left_controller_trigger_state, - left_squeeze_ctrl_state=self.tvuer.left_controller_squeeze_state, - left_squeeze_ctrl_value=self.tvuer.left_controller_squeeze_value, - left_thumbstick_state=self.tvuer.left_controller_thumbstick_state, - left_thumbstick_value=self.tvuer.left_controller_thumbstick_value, - left_aButton=self.tvuer.left_controller_aButton, - left_bButton=self.tvuer.left_controller_bButton, - right_trigger_state=self.tvuer.right_controller_trigger_state, - right_squeeze_ctrl_state=self.tvuer.right_controller_squeeze_state, - right_squeeze_ctrl_value=self.tvuer.right_controller_squeeze_value, - right_thumbstick_state=self.tvuer.right_controller_thumbstick_state, - right_thumbstick_value=self.tvuer.right_controller_thumbstick_value, - right_aButton=self.tvuer.right_controller_aButton, - right_bButton=self.tvuer.right_controller_bButton, - ) - else: - controller_state = None - - return TeleData( - head_pose=Brobot_world_head, - left_arm_pose=left_IPunitree_Brobot_waist_arm, - right_arm_pose=right_IPunitree_Brobot_waist_arm, - left_trigger_value=10.0 - self.tvuer.left_controller_trigger_value * 10, - right_trigger_value=10.0 - self.tvuer.right_controller_trigger_value * 10, - tele_state=controller_state - ) \ No newline at end of file diff --git a/teleop/robot_control/dex-retargeting b/teleop/robot_control/dex-retargeting new file mode 160000 index 0000000..d7753d3 --- /dev/null +++ b/teleop/robot_control/dex-retargeting @@ -0,0 +1 @@ +Subproject commit d7753d38c9ff11f80bafea6cd168351fd3db9b0e diff --git a/teleop/robot_control/dex_retargeting/CITATION.cff b/teleop/robot_control/dex_retargeting/CITATION.cff deleted file mode 100644 index 770c132..0000000 --- a/teleop/robot_control/dex_retargeting/CITATION.cff +++ /dev/null @@ -1,22 +0,0 @@ -cff-version: 1.2.0 -authors: - - family-names: "Qin" - given-names: "Yuzhe" - - family-names: "Yang" - given-names: "Wei" - - family-names: "Huang" - given-names: "Binghao" - - family-names: "Van Wyk" - given-names: "Karl" - - family-names: "Su" - given-names: "Hao" - - family-names: "Wang" - given-names: "Xiaolong" - - family-names: "Chao" - given-names: "Yu-Wei" - - family-names: "Fox" - given-names: "Dieter" -date-released: 2023-10-25 -title: "AnyTeleop" -message: "Thanks for using dex-retargeting. If you use this software, please cite it as below." -url: "https://github.com/dexsuite/dex-retargeting" diff --git a/teleop/robot_control/dex_retargeting/LICENSE b/teleop/robot_control/dex_retargeting/LICENSE deleted file mode 100644 index 15904fb..0000000 --- a/teleop/robot_control/dex_retargeting/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -The MIT License (MIT) - -Copyright (c) 2023 Yuzhe Qin - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. \ No newline at end of file diff --git a/teleop/robot_control/dex_retargeting/__init__.py b/teleop/robot_control/dex_retargeting/__init__.py deleted file mode 100644 index 3dd3d2d..0000000 --- a/teleop/robot_control/dex_retargeting/__init__.py +++ /dev/null @@ -1 +0,0 @@ -__version__ = "0.4.6" diff --git a/teleop/robot_control/dex_retargeting/constants.py b/teleop/robot_control/dex_retargeting/constants.py deleted file mode 100644 index 4b7147a..0000000 --- a/teleop/robot_control/dex_retargeting/constants.py +++ /dev/null @@ -1,85 +0,0 @@ -import enum -from pathlib import Path -from typing import Optional - -import numpy as np - -OPERATOR2MANO_RIGHT = np.array( - [ - [0, 0, -1], - [-1, 0, 0], - [0, 1, 0], - ] -) - -OPERATOR2MANO_LEFT = np.array( - [ - [0, 0, -1], - [1, 0, 0], - [0, -1, 0], - ] -) - - -class RobotName(enum.Enum): - allegro = enum.auto() - shadow = enum.auto() - svh = enum.auto() - leap = enum.auto() - ability = enum.auto() - inspire = enum.auto() - panda = enum.auto() - - -class RetargetingType(enum.Enum): - vector = enum.auto() # For teleoperation, no finger closing prior - position = enum.auto() # For offline data processing, especially hand-object interaction data - dexpilot = enum.auto() # For teleoperation, with finger closing prior - - -class HandType(enum.Enum): - right = enum.auto() - left = enum.auto() - - -ROBOT_NAME_MAP = { - RobotName.allegro: "allegro_hand", - RobotName.shadow: "shadow_hand", - RobotName.svh: "schunk_svh_hand", - RobotName.leap: "leap_hand", - RobotName.ability: "ability_hand", - RobotName.inspire: "inspire_hand", - RobotName.panda: "panda_gripper", -} - -ROBOT_NAMES = list(ROBOT_NAME_MAP.keys()) - - -def get_default_config_path( - robot_name: RobotName, retargeting_type: RetargetingType, hand_type: HandType -) -> Optional[Path]: - config_path = Path(__file__).parent / "configs" - if retargeting_type is RetargetingType.position: - config_path = config_path / "offline" - else: - config_path = config_path / "teleop" - - robot_name_str = ROBOT_NAME_MAP[robot_name] - hand_type_str = hand_type.name - if "gripper" in robot_name_str: # For gripper robots, only use gripper config file. - if retargeting_type == RetargetingType.dexpilot: - config_name = f"{robot_name_str}_dexpilot.yml" - else: - config_name = f"{robot_name_str}.yml" - else: - if retargeting_type == RetargetingType.dexpilot: - config_name = f"{robot_name_str}_{hand_type_str}_dexpilot.yml" - else: - config_name = f"{robot_name_str}_{hand_type_str}.yml" - return config_path / config_name - - -OPERATOR2MANO = { - HandType.right: OPERATOR2MANO_RIGHT, - HandType.left: OPERATOR2MANO_LEFT, -} diff --git a/teleop/robot_control/dex_retargeting/kinematics_adaptor.py b/teleop/robot_control/dex_retargeting/kinematics_adaptor.py deleted file mode 100644 index 045a49f..0000000 --- a/teleop/robot_control/dex_retargeting/kinematics_adaptor.py +++ /dev/null @@ -1,102 +0,0 @@ -from abc import abstractmethod -from typing import List - -import numpy as np - -from .robot_wrapper import RobotWrapper - - -class KinematicAdaptor: - def __init__(self, robot: RobotWrapper, target_joint_names: List[str]): - self.robot = robot - self.target_joint_names = target_joint_names - - # Index mapping - self.idx_pin2target = np.array([robot.get_joint_index(n) for n in target_joint_names]) - - @abstractmethod - def forward_qpos(self, qpos: np.ndarray) -> np.ndarray: - """ - Adapt the joint position for different kinematics constraints. - Note that the joint order of this qpos is consistent with pinocchio - Args: - qpos: the pinocchio qpos - - Returns: the adapted qpos with the same shape as input - - """ - pass - - @abstractmethod - def backward_jacobian(self, jacobian: np.ndarray) -> np.ndarray: - """ - Adapt the jacobian for different kinematics applications. - Note that the joint order of this Jacobian is consistent with pinocchio - Args: - jacobian: the original jacobian - - Returns: the adapted jacobian with the same shape as input - - """ - pass - - -class MimicJointKinematicAdaptor(KinematicAdaptor): - def __init__( - self, - robot: RobotWrapper, - target_joint_names: List[str], - source_joint_names: List[str], - mimic_joint_names: List[str], - multipliers: List[float], - offsets: List[float], - ): - super().__init__(robot, target_joint_names) - - self.multipliers = np.array(multipliers) - self.offsets = np.array(offsets) - - # Joint name check - union_set = set(mimic_joint_names).intersection(set(target_joint_names)) - if len(union_set) > 0: - raise ValueError( - f"Mimic joint should not be one of the target joints.\n" - f"Mimic joints: {mimic_joint_names}.\n" - f"Target joints: {target_joint_names}\n" - f"You need to specify the target joint names explicitly in your retargeting config" - f" for robot with mimic joint constraints: {target_joint_names}" - ) - - # Indices in the pinocchio - self.idx_pin2source = np.array([robot.get_joint_index(name) for name in source_joint_names]) - self.idx_pin2mimic = np.array([robot.get_joint_index(name) for name in mimic_joint_names]) - - # Indices in the output results - self.idx_target2source = np.array([self.target_joint_names.index(n) for n in source_joint_names]) - - # Dimension check - len_source, len_mimic = self.idx_target2source.shape[0], self.idx_pin2mimic.shape[0] - len_mul, len_offset = self.multipliers.shape[0], self.offsets.shape[0] - if not (len_mimic == len_source == len_mul == len_offset): - raise ValueError( - f"Mimic joints setting dimension mismatch.\n" - f"Source joints: {len_source}, mimic joints: {len_mimic}, multiplier: {len_mul}, offset: {len_offset}" - ) - self.num_active_joints = len(robot.dof_joint_names) - len_mimic - - # Uniqueness check - if len(mimic_joint_names) != len(np.unique(mimic_joint_names)): - raise ValueError(f"Redundant mimic joint names: {mimic_joint_names}") - - def forward_qpos(self, pin_qpos: np.ndarray) -> np.ndarray: - mimic_qpos = pin_qpos[self.idx_pin2source] * self.multipliers + self.offsets - pin_qpos[self.idx_pin2mimic] = mimic_qpos - return pin_qpos - - def backward_jacobian(self, jacobian: np.ndarray) -> np.ndarray: - target_jacobian = jacobian[..., self.idx_pin2target] - mimic_joint_jacobian = jacobian[..., self.idx_pin2mimic] * self.multipliers - - for i, index in enumerate(self.idx_target2source): - target_jacobian[..., index] += mimic_joint_jacobian[..., i] - return target_jacobian diff --git a/teleop/robot_control/dex_retargeting/optimizer.py b/teleop/robot_control/dex_retargeting/optimizer.py deleted file mode 100644 index ed891fe..0000000 --- a/teleop/robot_control/dex_retargeting/optimizer.py +++ /dev/null @@ -1,516 +0,0 @@ -from abc import abstractmethod -from typing import List, Optional - -import nlopt -import numpy as np -import torch - -from .kinematics_adaptor import KinematicAdaptor, MimicJointKinematicAdaptor -from .robot_wrapper import RobotWrapper - - -class Optimizer: - retargeting_type = "BASE" - - def __init__( - self, - robot: RobotWrapper, - target_joint_names: List[str], - target_link_human_indices: np.ndarray, - ): - self.robot = robot - self.num_joints = robot.dof - - joint_names = robot.dof_joint_names - idx_pin2target = [] - for target_joint_name in target_joint_names: - if target_joint_name not in joint_names: - raise ValueError(f"Joint {target_joint_name} given does not appear to be in robot XML.") - idx_pin2target.append(joint_names.index(target_joint_name)) - self.target_joint_names = target_joint_names - self.idx_pin2target = np.array(idx_pin2target) - - self.idx_pin2fixed = np.array([i for i in range(robot.dof) if i not in idx_pin2target], dtype=int) - self.opt = nlopt.opt(nlopt.LD_SLSQP, len(idx_pin2target)) - self.opt_dof = len(idx_pin2target) # This dof includes the mimic joints - - # Target - self.target_link_human_indices = target_link_human_indices - - # Free joint - link_names = robot.link_names - self.has_free_joint = len([name for name in link_names if "dummy" in name]) >= 6 - - # Kinematics adaptor - self.adaptor: Optional[KinematicAdaptor] = None - - def set_joint_limit(self, joint_limits: np.ndarray, epsilon=1e-3): - if joint_limits.shape != (self.opt_dof, 2): - raise ValueError(f"Expect joint limits have shape: {(self.opt_dof, 2)}, but get {joint_limits.shape}") - self.opt.set_lower_bounds((joint_limits[:, 0] - epsilon).tolist()) - self.opt.set_upper_bounds((joint_limits[:, 1] + epsilon).tolist()) - - def get_link_indices(self, target_link_names): - return [self.robot.get_link_index(link_name) for link_name in target_link_names] - - def set_kinematic_adaptor(self, adaptor: KinematicAdaptor): - self.adaptor = adaptor - - # Remove mimic joints from fixed joint list - if isinstance(adaptor, MimicJointKinematicAdaptor): - fixed_idx = self.idx_pin2fixed - mimic_idx = adaptor.idx_pin2mimic - new_fixed_id = np.array([x for x in fixed_idx if x not in mimic_idx], dtype=int) - self.idx_pin2fixed = new_fixed_id - - def retarget(self, ref_value, fixed_qpos, last_qpos): - """ - Compute the retargeting results using non-linear optimization - Args: - ref_value: the reference value in cartesian space as input, different optimizer has different reference - fixed_qpos: the fixed value (not optimized) in retargeting, consistent with self.fixed_joint_names - last_qpos: the last retargeting results or initial value, consistent with function return - - Returns: joint position of robot, the joint order and dim is consistent with self.target_joint_names - - """ - if len(fixed_qpos) != len(self.idx_pin2fixed): - raise ValueError( - f"Optimizer has {len(self.idx_pin2fixed)} joints but non_target_qpos {fixed_qpos} is given" - ) - objective_fn = self.get_objective_function(ref_value, fixed_qpos, np.array(last_qpos).astype(np.float32)) - - self.opt.set_min_objective(objective_fn) - try: - qpos = self.opt.optimize(last_qpos) - return np.array(qpos, dtype=np.float32) - except RuntimeError as e: - print(e) - return np.array(last_qpos, dtype=np.float32) - - @abstractmethod - def get_objective_function(self, ref_value: np.ndarray, fixed_qpos: np.ndarray, last_qpos: np.ndarray): - pass - - @property - def fixed_joint_names(self): - joint_names = self.robot.dof_joint_names - return [joint_names[i] for i in self.idx_pin2fixed] - - -class PositionOptimizer(Optimizer): - retargeting_type = "POSITION" - - def __init__( - self, - robot: RobotWrapper, - target_joint_names: List[str], - target_link_names: List[str], - target_link_human_indices: np.ndarray, - huber_delta=0.02, - norm_delta=4e-3, - ): - super().__init__(robot, target_joint_names, target_link_human_indices) - self.body_names = target_link_names - self.huber_loss = torch.nn.SmoothL1Loss(beta=huber_delta) - self.norm_delta = norm_delta - - # Sanity check and cache link indices - self.target_link_indices = self.get_link_indices(target_link_names) - - self.opt.set_ftol_abs(1e-5) - - def get_objective_function(self, target_pos: np.ndarray, fixed_qpos: np.ndarray, last_qpos: np.ndarray): - qpos = np.zeros(self.num_joints) - qpos[self.idx_pin2fixed] = fixed_qpos - torch_target_pos = torch.as_tensor(target_pos) - torch_target_pos.requires_grad_(False) - - def objective(x: np.ndarray, grad: np.ndarray) -> float: - qpos[self.idx_pin2target] = x - - # Kinematics forwarding for qpos - if self.adaptor is not None: - qpos[:] = self.adaptor.forward_qpos(qpos)[:] - - self.robot.compute_forward_kinematics(qpos) - target_link_poses = [self.robot.get_link_pose(index) for index in self.target_link_indices] - body_pos = np.stack([pose[:3, 3] for pose in target_link_poses], axis=0) # (n ,3) - - # Torch computation for accurate loss and grad - torch_body_pos = torch.as_tensor(body_pos) - torch_body_pos.requires_grad_() - - # Loss term for kinematics retargeting based on 3D position error - huber_distance = self.huber_loss(torch_body_pos, torch_target_pos) - result = huber_distance.cpu().detach().item() - - if grad.size > 0: - jacobians = [] - for i, index in enumerate(self.target_link_indices): - link_body_jacobian = self.robot.compute_single_link_local_jacobian(qpos, index)[:3, ...] - link_pose = target_link_poses[i] - link_rot = link_pose[:3, :3] - link_kinematics_jacobian = link_rot @ link_body_jacobian - jacobians.append(link_kinematics_jacobian) - - # Note: the joint order in this jacobian is consistent pinocchio - jacobians = np.stack(jacobians, axis=0) - huber_distance.backward() - grad_pos = torch_body_pos.grad.cpu().numpy()[:, None, :] - - # Convert the jacobian from pinocchio order to target order - if self.adaptor is not None: - jacobians = self.adaptor.backward_jacobian(jacobians) - else: - jacobians = jacobians[..., self.idx_pin2target] - - # Compute the gradient to the qpos - grad_qpos = np.matmul(grad_pos, jacobians) - grad_qpos = grad_qpos.mean(1).sum(0) - grad_qpos += 2 * self.norm_delta * (x - last_qpos) - - grad[:] = grad_qpos[:] - - return result - - return objective - - -class VectorOptimizer(Optimizer): - retargeting_type = "VECTOR" - - def __init__( - self, - robot: RobotWrapper, - target_joint_names: List[str], - target_origin_link_names: List[str], - target_task_link_names: List[str], - target_link_human_indices: np.ndarray, - huber_delta=0.02, - norm_delta=4e-3, - scaling=1.0, - ): - super().__init__(robot, target_joint_names, target_link_human_indices) - self.origin_link_names = target_origin_link_names - self.task_link_names = target_task_link_names - self.huber_loss = torch.nn.SmoothL1Loss(beta=huber_delta, reduction="mean") - self.norm_delta = norm_delta - self.scaling = scaling - - # Computation cache for better performance - # For one link used in multiple vectors, e.g. hand palm, we do not want to compute it multiple times - self.computed_link_names = list(set(target_origin_link_names).union(set(target_task_link_names))) - self.origin_link_indices = torch.tensor( - [self.computed_link_names.index(name) for name in target_origin_link_names] - ) - self.task_link_indices = torch.tensor([self.computed_link_names.index(name) for name in target_task_link_names]) - - # Cache link indices that will involve in kinematics computation - self.computed_link_indices = self.get_link_indices(self.computed_link_names) - - self.opt.set_ftol_abs(1e-6) - - def get_objective_function(self, target_vector: np.ndarray, fixed_qpos: np.ndarray, last_qpos: np.ndarray): - qpos = np.zeros(self.num_joints) - qpos[self.idx_pin2fixed] = fixed_qpos - torch_target_vec = torch.as_tensor(target_vector) * self.scaling - torch_target_vec.requires_grad_(False) - - def objective(x: np.ndarray, grad: np.ndarray) -> float: - qpos[self.idx_pin2target] = x - - # Kinematics forwarding for qpos - if self.adaptor is not None: - qpos[:] = self.adaptor.forward_qpos(qpos)[:] - - self.robot.compute_forward_kinematics(qpos) - target_link_poses = [self.robot.get_link_pose(index) for index in self.computed_link_indices] - body_pos = np.array([pose[:3, 3] for pose in target_link_poses]) - - # Torch computation for accurate loss and grad - torch_body_pos = torch.as_tensor(body_pos) - torch_body_pos.requires_grad_() - - # Index link for computation - origin_link_pos = torch_body_pos[self.origin_link_indices, :] - task_link_pos = torch_body_pos[self.task_link_indices, :] - robot_vec = task_link_pos - origin_link_pos - - # Loss term for kinematics retargeting based on 3D position error - vec_dist = torch.norm(robot_vec - torch_target_vec, dim=1, keepdim=False) - huber_distance = self.huber_loss(vec_dist, torch.zeros_like(vec_dist)) - result = huber_distance.cpu().detach().item() - - if grad.size > 0: - jacobians = [] - for i, index in enumerate(self.computed_link_indices): - link_body_jacobian = self.robot.compute_single_link_local_jacobian(qpos, index)[:3, ...] - link_pose = target_link_poses[i] - link_rot = link_pose[:3, :3] - link_kinematics_jacobian = link_rot @ link_body_jacobian - jacobians.append(link_kinematics_jacobian) - - # Note: the joint order in this jacobian is consistent pinocchio - jacobians = np.stack(jacobians, axis=0) - huber_distance.backward() - grad_pos = torch_body_pos.grad.cpu().numpy()[:, None, :] - - # Convert the jacobian from pinocchio order to target order - if self.adaptor is not None: - jacobians = self.adaptor.backward_jacobian(jacobians) - else: - jacobians = jacobians[..., self.idx_pin2target] - - grad_qpos = np.matmul(grad_pos, np.array(jacobians)) - grad_qpos = grad_qpos.mean(1).sum(0) - grad_qpos += 2 * self.norm_delta * (x - last_qpos) - - grad[:] = grad_qpos[:] - - return result - - return objective - - -class DexPilotOptimizer(Optimizer): - """Retargeting optimizer using the method proposed in DexPilot - - This is a broader adaptation of the original optimizer delineated in the DexPilot paper. - While the initial DexPilot study focused solely on the four-fingered Allegro Hand, this version of the optimizer - embraces the same principles for both four-fingered and five-fingered hands. It projects the distance between the - thumb and the other fingers to facilitate more stable grasping. - Reference: https://arxiv.org/abs/1910.03135 - - Args: - robot: - target_joint_names: - finger_tip_link_names: - wrist_link_name: - gamma: - project_dist: - escape_dist: - eta1: - eta2: - scaling: - """ - - retargeting_type = "DEXPILOT" - - def __init__( - self, - robot: RobotWrapper, - target_joint_names: List[str], - finger_tip_link_names: List[str], - wrist_link_name: str, - target_link_human_indices: Optional[np.ndarray] = None, - huber_delta=0.03, - norm_delta=4e-3, - # DexPilot parameters - # gamma=2.5e-3, - project_dist=0.03, - escape_dist=0.05, - eta1=1e-4, - eta2=3e-2, - scaling=1.0, - ): - if len(finger_tip_link_names) < 2 or len(finger_tip_link_names) > 5: - raise ValueError( - f"DexPilot optimizer can only be applied to hands with 2 to 5 fingers, but got " - f"{len(finger_tip_link_names)} fingers." - ) - self.num_fingers = len(finger_tip_link_names) - - origin_link_index, task_link_index = self.generate_link_indices(self.num_fingers) - - if target_link_human_indices is None: - logical_indices = np.stack([origin_link_index, task_link_index], axis=0) - target_link_human_indices = np.where( - logical_indices > 0, - logical_indices * 5 - 1, - 0 - ).astype(int) - link_names = [wrist_link_name] + finger_tip_link_names - target_origin_link_names = [link_names[index] for index in origin_link_index] - target_task_link_names = [link_names[index] for index in task_link_index] - - super().__init__(robot, target_joint_names, target_link_human_indices) - self.origin_link_names = target_origin_link_names - self.task_link_names = target_task_link_names - self.scaling = scaling - self.huber_loss = torch.nn.SmoothL1Loss(beta=huber_delta, reduction="none") - self.norm_delta = norm_delta - - # DexPilot parameters - self.project_dist = project_dist - self.escape_dist = escape_dist - self.eta1 = eta1 - self.eta2 = eta2 - - # Computation cache for better performance - # For one link used in multiple vectors, e.g. hand palm, we do not want to compute it multiple times - self.computed_link_names = list(set(target_origin_link_names).union(set(target_task_link_names))) - self.origin_link_indices = torch.tensor( - [self.computed_link_names.index(name) for name in target_origin_link_names] - ) - self.task_link_indices = torch.tensor([self.computed_link_names.index(name) for name in target_task_link_names]) - - # Sanity check and cache link indices - self.computed_link_indices = self.get_link_indices(self.computed_link_names) - - self.opt.set_ftol_abs(1e-6) - - # DexPilot cache - self.projected, self.s2_project_index_origin, self.s2_project_index_task, self.projected_dist = ( - self.set_dexpilot_cache(self.num_fingers, eta1, eta2) - ) - - @staticmethod - def generate_link_indices(num_fingers): - """ - Example: - >>> generate_link_indices(4) - ([2, 3, 4, 3, 4, 4, 0, 0, 0, 0], [1, 1, 1, 2, 2, 3, 1, 2, 3, 4]) - """ - origin_link_index = [] - task_link_index = [] - - # S1：Add indices for connections between fingers - for i in range(1, num_fingers): - for j in range(i + 1, num_fingers + 1): - origin_link_index.append(j) - task_link_index.append(i) - - # S2：Add indices for connections to the base (0) - for i in range(1, num_fingers + 1): - origin_link_index.append(0) - task_link_index.append(i) - - return origin_link_index, task_link_index - - @staticmethod - def set_dexpilot_cache(num_fingers, eta1, eta2): - """ - Example: - >>> set_dexpilot_cache(4, 0.1, 0.2) - (array([False, False, False, False, False, False]), - [1, 2, 2], - [0, 0, 1], - array([0.1, 0.1, 0.1, 0.2, 0.2, 0.2])) - """ - projected = np.zeros(num_fingers * (num_fingers - 1) // 2, dtype=bool) - - s2_project_index_origin = [] - s2_project_index_task = [] - for i in range(0, num_fingers - 2): - for j in range(i + 1, num_fingers - 1): - s2_project_index_origin.append(j) - s2_project_index_task.append(i) - - projected_dist = np.array([eta1] * (num_fingers - 1) + [eta2] * ((num_fingers - 1) * (num_fingers - 2) // 2)) - - return projected, s2_project_index_origin, s2_project_index_task, projected_dist - - def get_objective_function(self, target_vector: np.ndarray, fixed_qpos: np.ndarray, last_qpos: np.ndarray): - qpos = np.zeros(self.num_joints) - qpos[self.idx_pin2fixed] = fixed_qpos - - len_proj = len(self.projected) - len_s2 = len(self.s2_project_index_task) - len_s1 = len_proj - len_s2 - - # Update projection indicator - target_vec_dist = np.linalg.norm(target_vector[:len_proj], axis=1) - self.projected[:len_s1][target_vec_dist[0:len_s1] < self.project_dist] = True - self.projected[:len_s1][target_vec_dist[0:len_s1] > self.escape_dist] = False - self.projected[len_s1:len_proj] = np.logical_and( - self.projected[:len_s1][self.s2_project_index_origin], self.projected[:len_s1][self.s2_project_index_task] - ) - self.projected[len_s1:len_proj] = np.logical_and( - self.projected[len_s1:len_proj], target_vec_dist[len_s1:len_proj] <= 0.03 - ) - - # Update weight vector - normal_weight = np.ones(len_proj, dtype=np.float32) * 1 - high_weight = np.array([200] * len_s1 + [400] * len_s2, dtype=np.float32) - weight = np.where(self.projected, high_weight, normal_weight) - - # We change the weight to 10 instead of 1 here, for vector originate from wrist to fingertips - # This ensures better intuitive mapping due wrong pose detection - weight = torch.from_numpy( - np.concatenate([weight, np.ones(self.num_fingers, dtype=np.float32) * len_proj + self.num_fingers]) - ) - - # Compute reference distance vector - normal_vec = target_vector * self.scaling # (10, 3) - dir_vec = target_vector[:len_proj] / (target_vec_dist[:, None] + 1e-6) # (6, 3) - projected_vec = dir_vec * self.projected_dist[:, None] # (6, 3) - - # Compute final reference vector - reference_vec = np.where(self.projected[:, None], projected_vec, normal_vec[:len_proj]) # (6, 3) - reference_vec = np.concatenate([reference_vec, normal_vec[len_proj:]], axis=0) # (10, 3) - torch_target_vec = torch.as_tensor(reference_vec, dtype=torch.float32) - torch_target_vec.requires_grad_(False) - - def objective(x: np.ndarray, grad: np.ndarray) -> float: - qpos[self.idx_pin2target] = x - - # Kinematics forwarding for qpos - if self.adaptor is not None: - qpos[:] = self.adaptor.forward_qpos(qpos)[:] - - self.robot.compute_forward_kinematics(qpos) - target_link_poses = [self.robot.get_link_pose(index) for index in self.computed_link_indices] - body_pos = np.array([pose[:3, 3] for pose in target_link_poses]) - - # Torch computation for accurate loss and grad - torch_body_pos = torch.as_tensor(body_pos) - torch_body_pos.requires_grad_() - - # Index link for computation - origin_link_pos = torch_body_pos[self.origin_link_indices, :] - task_link_pos = torch_body_pos[self.task_link_indices, :] - robot_vec = task_link_pos - origin_link_pos - - # Loss term for kinematics retargeting based on 3D position error - # Different from the original DexPilot, we use huber loss here instead of the squared dist - vec_dist = torch.norm(robot_vec - torch_target_vec, dim=1, keepdim=False) - huber_distance = ( - self.huber_loss(vec_dist, torch.zeros_like(vec_dist)) * weight / (robot_vec.shape[0]) - ).sum() - huber_distance = huber_distance.sum() - result = huber_distance.cpu().detach().item() - - if grad.size > 0: - jacobians = [] - for i, index in enumerate(self.computed_link_indices): - link_body_jacobian = self.robot.compute_single_link_local_jacobian(qpos, index)[:3, ...] - link_pose = target_link_poses[i] - link_rot = link_pose[:3, :3] - link_kinematics_jacobian = link_rot @ link_body_jacobian - jacobians.append(link_kinematics_jacobian) - - # Note: the joint order in this jacobian is consistent pinocchio - jacobians = np.stack(jacobians, axis=0) - huber_distance.backward() - grad_pos = torch_body_pos.grad.cpu().numpy()[:, None, :] - - # Convert the jacobian from pinocchio order to target order - if self.adaptor is not None: - jacobians = self.adaptor.backward_jacobian(jacobians) - else: - jacobians = jacobians[..., self.idx_pin2target] - - grad_qpos = np.matmul(grad_pos, np.array(jacobians)) - grad_qpos = grad_qpos.mean(1).sum(0) - - # In the original DexPilot, γ = 2.5 × 10−3 is a weight on regularizing the Allegro angles to zero - # which is equivalent to fully opened the hand - # In our implementation, we regularize the joint angles to the previous joint angles - grad_qpos += 2 * self.norm_delta * (x - last_qpos) - - grad[:] = grad_qpos[:] - - return result - - return objective diff --git a/teleop/robot_control/dex_retargeting/optimizer_utils.py b/teleop/robot_control/dex_retargeting/optimizer_utils.py deleted file mode 100644 index 2b54275..0000000 --- a/teleop/robot_control/dex_retargeting/optimizer_utils.py +++ /dev/null @@ -1,17 +0,0 @@ -class LPFilter: - def __init__(self, alpha): - self.alpha = alpha - self.y = None - self.is_init = False - - def next(self, x): - if not self.is_init: - self.y = x - self.is_init = True - return self.y.copy() - self.y = self.y + self.alpha * (x - self.y) - return self.y.copy() - - def reset(self): - self.y = None - self.is_init = False diff --git a/teleop/robot_control/dex_retargeting/retargeting_config.py b/teleop/robot_control/dex_retargeting/retargeting_config.py deleted file mode 100644 index f1e8cd8..0000000 --- a/teleop/robot_control/dex_retargeting/retargeting_config.py +++ /dev/null @@ -1,255 +0,0 @@ -from dataclasses import dataclass -from pathlib import Path -from typing import List, Optional, Dict, Any, Tuple -from typing import Union - -import numpy as np -import yaml -import os - -from . import yourdfpy as urdf -from .kinematics_adaptor import MimicJointKinematicAdaptor -from .optimizer_utils import LPFilter -from .robot_wrapper import RobotWrapper -from .seq_retarget import SeqRetargeting -from .yourdfpy import DUMMY_JOINT_NAMES - - -@dataclass -class RetargetingConfig: - type: str - urdf_path: str - target_joint_names: Optional[List[str]] = None - - # Whether to add free joint to the root of the robot. Free joint enable the robot hand move freely in the space - add_dummy_free_joint: bool = False - - # DexPilot retargeting related - # The link on the robot hand which corresponding to the wrist of human hand - wrist_link_name: Optional[str] = None - # DexPilot retargeting link names - finger_tip_link_names: Optional[List[str]] = None - target_link_human_indices_dexpilot: Optional[np.ndarray] = None - - # Position retargeting link names - target_link_names: Optional[List[str]] = None - target_link_human_indices_position: Optional[np.ndarray] = None - - # Vector retargeting link names - target_origin_link_names: Optional[List[str]] = None - target_task_link_names: Optional[List[str]] = None - target_link_human_indices_vector: Optional[np.ndarray] = None - - # Scaling factor for vector retargeting only - # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 - scaling_factor: float = 1.0 - - # Low pass filter - low_pass_alpha: float = 0.1 - - # Optimization parameters - normal_delta: float = 4e-3 - huber_delta: float = 2e-2 - - # DexPilot optimizer parameters - project_dist: float = 0.03 - escape_dist: float = 0.05 - - # Joint limit tag - has_joint_limits: bool = True - - # Mimic joint tag - ignore_mimic_joint: bool = False - - _TYPE = ["vector", "position", "dexpilot"] - _DEFAULT_URDF_DIR = "./" - - def __post_init__(self): - # Retargeting type check - self.type = self.type.lower() - if self.type not in self._TYPE: - raise ValueError(f"Retargeting type must be one of {self._TYPE}") - - # Vector retargeting requires: target_origin_link_names + target_task_link_names - # Position retargeting requires: target_link_names - if self.type == "vector": - if self.target_origin_link_names is None or self.target_task_link_names is None: - raise ValueError(f"Vector retargeting requires: target_origin_link_names + target_task_link_names") - if len(self.target_task_link_names) != len(self.target_origin_link_names): - raise ValueError(f"Vector retargeting origin and task links dim mismatch") - if self.target_link_human_indices_vector.shape != (2, len(self.target_origin_link_names)): - raise ValueError(f"Vector retargeting link names and link indices dim mismatch") - if self.target_link_human_indices_vector is None: - raise ValueError(f"Vector retargeting requires: target_link_human_indices_vector") - - elif self.type == "position": - if self.target_link_names is None: - raise ValueError(f"Position retargeting requires: target_link_names") - self.target_link_human_indices_position = self.target_link_human_indices_position.squeeze() - if self.target_link_human_indices_position.shape != (len(self.target_link_names),): - raise ValueError(f"Position retargeting link names and link indices dim mismatch") - if self.target_link_human_indices_position is None: - raise ValueError(f"Position retargeting requires: target_link_human_indices_position") - - elif self.type == "dexpilot": - if self.finger_tip_link_names is None or self.wrist_link_name is None: - raise ValueError(f"Position retargeting requires: finger_tip_link_names + wrist_link_name") - if self.target_link_human_indices_dexpilot is not None: - print( - "\033[33m", - "Target link human indices is provided in the DexPilot retargeting config, which is uncommon.\n" - "If you do not know exactly how it is used, please leave it to None for default.\n" - "\033[00m", - ) - - # URDF path check - urdf_path = Path(self.urdf_path) - if not urdf_path.is_absolute(): - urdf_path = self._DEFAULT_URDF_DIR / urdf_path - urdf_path = urdf_path.absolute() - if not urdf_path.exists(): - raise ValueError(f"URDF path {urdf_path} does not exist") - self.urdf_path = str(urdf_path) - - @classmethod - def set_default_urdf_dir(cls, urdf_dir: Union[str, Path]): - path = Path(urdf_dir) - if not path.exists(): - raise ValueError(f"URDF dir {urdf_dir} not exists.") - cls._DEFAULT_URDF_DIR = urdf_dir - - @classmethod - def load_from_file(cls, config_path: Union[str, Path], override: Optional[Dict] = None): - path = Path(config_path) - if not path.is_absolute(): - path = path.absolute() - - with path.open("r") as f: - yaml_config = yaml.load(f, Loader=yaml.FullLoader) - cfg = yaml_config["retargeting"] - return cls.from_dict(cfg, override) - - @classmethod - def from_dict(cls, cfg: Dict[str, Any], override: Optional[Dict] = None): - if "target_link_human_indices_position" in cfg: - cfg["target_link_human_indices_position"] = np.array(cfg["target_link_human_indices_position"]) - if "target_link_human_indices_vector" in cfg: - cfg["target_link_human_indices_vector"] = np.array(cfg["target_link_human_indices_vector"]) - if "target_link_human_indices_dexpilot" in cfg: - cfg["target_link_human_indices_dexpilot"] = np.array(cfg["target_link_human_indices_dexpilot"]) - - if override is not None: - for key, value in override.items(): - cfg[key] = value - config = RetargetingConfig(**cfg) - return config - - def build(self) -> SeqRetargeting: - from .optimizer import ( - VectorOptimizer, - PositionOptimizer, - DexPilotOptimizer, - ) - import tempfile - - # Process the URDF with yourdfpy to better find file path - robot_urdf = urdf.URDF.load( - self.urdf_path, add_dummy_free_joints=self.add_dummy_free_joint, build_scene_graph=False - ) - urdf_name = self.urdf_path.split(os.path.sep)[-1] - temp_dir = tempfile.mkdtemp(prefix="dex_retargeting-") - temp_path = f"{temp_dir}/{urdf_name}" - robot_urdf.write_xml_file(temp_path) - - # Load pinocchio model - robot = RobotWrapper(temp_path) - - # Add 6D dummy joint to target joint names so that it will also be optimized - if self.add_dummy_free_joint and self.target_joint_names is not None: - self.target_joint_names = DUMMY_JOINT_NAMES + self.target_joint_names - joint_names = self.target_joint_names if self.target_joint_names is not None else robot.dof_joint_names - - if self.type == "position": - optimizer = PositionOptimizer( - robot, - joint_names, - target_link_names=self.target_link_names, - target_link_human_indices=self.target_link_human_indices_position, - norm_delta=self.normal_delta, - huber_delta=self.huber_delta, - ) - elif self.type == "vector": - optimizer = VectorOptimizer( - robot, - joint_names, - target_origin_link_names=self.target_origin_link_names, - target_task_link_names=self.target_task_link_names, - target_link_human_indices=self.target_link_human_indices_vector, - scaling=self.scaling_factor, - norm_delta=self.normal_delta, - huber_delta=self.huber_delta, - ) - elif self.type == "dexpilot": - optimizer = DexPilotOptimizer( - robot, - joint_names, - finger_tip_link_names=self.finger_tip_link_names, - wrist_link_name=self.wrist_link_name, - target_link_human_indices=self.target_link_human_indices_dexpilot, - scaling=self.scaling_factor, - project_dist=self.project_dist, - escape_dist=self.escape_dist, - ) - else: - raise RuntimeError() - - if 0 <= self.low_pass_alpha <= 1: - lp_filter = LPFilter(self.low_pass_alpha) - else: - lp_filter = None - - # Parse mimic joints and set kinematics adaptor for optimizer - has_mimic_joints, source_names, mimic_names, multipliers, offsets = parse_mimic_joint(robot_urdf) - if has_mimic_joints and not self.ignore_mimic_joint: - adaptor = MimicJointKinematicAdaptor( - robot, - target_joint_names=joint_names, - source_joint_names=source_names, - mimic_joint_names=mimic_names, - multipliers=multipliers, - offsets=offsets, - ) - optimizer.set_kinematic_adaptor(adaptor) - print( - "\033[34m", - "Mimic joint adaptor enabled. The mimic joint tags in the URDF will be considered during retargeting.\n" - "To disable mimic joint adaptor, consider setting ignore_mimic_joint=True in the configuration.", - "\033[39m", - ) - - retargeting = SeqRetargeting( - optimizer, - has_joint_limits=self.has_joint_limits, - lp_filter=lp_filter, - ) - return retargeting - - -def get_retargeting_config(config_path: Union[str, Path]) -> RetargetingConfig: - config = RetargetingConfig.load_from_file(config_path) - return config - - -def parse_mimic_joint(robot_urdf: urdf.URDF) -> Tuple[bool, List[str], List[str], List[float], List[float]]: - mimic_joint_names = [] - source_joint_names = [] - multipliers = [] - offsets = [] - for name, joint in robot_urdf.joint_map.items(): - if joint.mimic is not None: - mimic_joint_names.append(name) - source_joint_names.append(joint.mimic.joint) - multipliers.append(joint.mimic.multiplier) - offsets.append(joint.mimic.offset) - - return len(mimic_joint_names) > 0, source_joint_names, mimic_joint_names, multipliers, offsets diff --git a/teleop/robot_control/dex_retargeting/robot_wrapper.py b/teleop/robot_control/dex_retargeting/robot_wrapper.py deleted file mode 100644 index 335fb43..0000000 --- a/teleop/robot_control/dex_retargeting/robot_wrapper.py +++ /dev/null @@ -1,93 +0,0 @@ -from typing import List - -import numpy as np -import numpy.typing as npt -import pinocchio as pin - - -class RobotWrapper: - """ - This class does not take mimic joint into consideration - """ - - def __init__(self, urdf_path: str, use_collision=False, use_visual=False): - # Create robot model and data - self.model: pin.Model = pin.buildModelFromUrdf(urdf_path) - self.data: pin.Data = self.model.createData() - - if use_visual or use_collision: - raise NotImplementedError - - self.q0 = pin.neutral(self.model) - if self.model.nv != self.model.nq: - raise NotImplementedError(f"Can not handle robot with special joint.") - - # -------------------------------------------------------------------------- # - # Robot property - # -------------------------------------------------------------------------- # - @property - def joint_names(self) -> List[str]: - return list(self.model.names) - - @property - def dof_joint_names(self) -> List[str]: - nqs = self.model.nqs - return [name for i, name in enumerate(self.model.names) if nqs[i] > 0] - - @property - def dof(self) -> int: - return self.model.nq - - @property - def link_names(self) -> List[str]: - link_names = [] - for i, frame in enumerate(self.model.frames): - link_names.append(frame.name) - return link_names - - @property - def joint_limits(self): - lower = self.model.lowerPositionLimit - upper = self.model.upperPositionLimit - return np.stack([lower, upper], axis=1) - - # -------------------------------------------------------------------------- # - # Query function - # -------------------------------------------------------------------------- # - def get_joint_index(self, name: str): - return self.dof_joint_names.index(name) - - def get_link_index(self, name: str): - if name not in self.link_names: - raise ValueError(f"{name} is not a link name. Valid link names: \n{self.link_names}") - return self.model.getFrameId(name, pin.BODY) - - def get_joint_parent_child_frames(self, joint_name: str): - joint_id = self.model.getFrameId(joint_name) - parent_id = self.model.frames[joint_id].parent - child_id = -1 - for idx, frame in enumerate(self.model.frames): - if frame.previousFrame == joint_id: - child_id = idx - if child_id == -1: - raise ValueError(f"Can not find child link of {joint_name}") - - return parent_id, child_id - - # -------------------------------------------------------------------------- # - # Kinematics function - # -------------------------------------------------------------------------- # - def compute_forward_kinematics(self, qpos: npt.NDArray): - pin.forwardKinematics(self.model, self.data, qpos) - - def get_link_pose(self, link_id: int) -> npt.NDArray: - pose: pin.SE3 = pin.updateFramePlacement(self.model, self.data, link_id) - return pose.homogeneous - - def get_link_pose_inv(self, link_id: int) -> npt.NDArray: - pose: pin.SE3 = pin.updateFramePlacement(self.model, self.data, link_id) - return pose.inverse().homogeneous - - def compute_single_link_local_jacobian(self, qpos, link_id: int) -> npt.NDArray: - J = pin.computeFrameJacobian(self.model, self.data, qpos, link_id) - return J diff --git a/teleop/robot_control/dex_retargeting/seq_retarget.py b/teleop/robot_control/dex_retargeting/seq_retarget.py deleted file mode 100644 index a24529c..0000000 --- a/teleop/robot_control/dex_retargeting/seq_retarget.py +++ /dev/null @@ -1,151 +0,0 @@ -import time -from typing import Optional - -import numpy as np -from pytransform3d import rotations - -from .constants import OPERATOR2MANO, HandType -from .optimizer import Optimizer -from .optimizer_utils import LPFilter - - -class SeqRetargeting: - def __init__( - self, - optimizer: Optimizer, - has_joint_limits=True, - lp_filter: Optional[LPFilter] = None, - ): - self.optimizer = optimizer - robot = self.optimizer.robot - - # Joint limit - self.has_joint_limits = has_joint_limits - joint_limits = np.ones_like(robot.joint_limits) - joint_limits[:, 0] = -1e4 # a large value is equivalent to no limit - joint_limits[:, 1] = 1e4 - if has_joint_limits: - joint_limits[:] = robot.joint_limits[:] - self.optimizer.set_joint_limit(joint_limits[self.optimizer.idx_pin2target]) - self.joint_limits = joint_limits[self.optimizer.idx_pin2target] - - # Temporal information - self.last_qpos = joint_limits.mean(1)[self.optimizer.idx_pin2target].astype(np.float32) - self.accumulated_time = 0 - self.num_retargeting = 0 - - # Filter - self.filter = lp_filter - - # Warm started - self.is_warm_started = False - - def warm_start( - self, - wrist_pos: np.ndarray, - wrist_quat: np.ndarray, - hand_type: HandType = HandType.right, - is_mano_convention: bool = False, - ): - """ - Initialize the wrist joint pose using analytical computation instead of retargeting optimization. - This function is specifically for position retargeting with the flying robot hand, i.e. has 6D free joint - You are not expected to use this function for vector retargeting, e.g. when you are working on teleoperation - - Args: - wrist_pos: position of the hand wrist, typically from human hand pose - wrist_quat: quaternion of the hand wrist, the same convention as the operator frame definition if not is_mano_convention - hand_type: hand type, used to determine the operator2mano matrix - is_mano_convention: whether the wrist_quat is in mano convention - """ - # This function can only be used when the first joints of robot are free joints - - if len(wrist_pos) != 3: - raise ValueError(f"Wrist pos: {wrist_pos} is not a 3-dim vector.") - if len(wrist_quat) != 4: - raise ValueError(f"Wrist quat: {wrist_quat} is not a 4-dim vector.") - - operator2mano = OPERATOR2MANO[hand_type] if is_mano_convention else np.eye(3) - robot = self.optimizer.robot - target_wrist_pose = np.eye(4) - target_wrist_pose[:3, :3] = rotations.matrix_from_quaternion(wrist_quat) @ operator2mano.T - target_wrist_pose[:3, 3] = wrist_pos - - name_list = [ - "dummy_x_translation_joint", - "dummy_y_translation_joint", - "dummy_z_translation_joint", - "dummy_x_rotation_joint", - "dummy_y_rotation_joint", - "dummy_z_rotation_joint", - ] - wrist_link_id = robot.get_joint_parent_child_frames(name_list[5])[1] - - # Set the dummy joints angles to zero - old_qpos = robot.q0 - new_qpos = old_qpos.copy() - for num, joint_name in enumerate(self.optimizer.target_joint_names): - if joint_name in name_list: - new_qpos[num] = 0 - - robot.compute_forward_kinematics(new_qpos) - root2wrist = robot.get_link_pose_inv(wrist_link_id) - target_root_pose = target_wrist_pose @ root2wrist - - euler = rotations.euler_from_matrix(target_root_pose[:3, :3], 0, 1, 2, extrinsic=False) - pose_vec = np.concatenate([target_root_pose[:3, 3], euler]) - - # Find the dummy joints - for num, joint_name in enumerate(self.optimizer.target_joint_names): - if joint_name in name_list: - index = name_list.index(joint_name) - self.last_qpos[num] = pose_vec[index] - - self.is_warm_started = True - - def retarget(self, ref_value, fixed_qpos=np.array([])): - tic = time.perf_counter() - - qpos = self.optimizer.retarget( - ref_value=ref_value.astype(np.float32), - fixed_qpos=fixed_qpos.astype(np.float32), - last_qpos=np.clip(self.last_qpos, self.joint_limits[:, 0], self.joint_limits[:, 1]), - ) - self.accumulated_time += time.perf_counter() - tic - self.num_retargeting += 1 - self.last_qpos = qpos - robot_qpos = np.zeros(self.optimizer.robot.dof) - robot_qpos[self.optimizer.idx_pin2fixed] = fixed_qpos - robot_qpos[self.optimizer.idx_pin2target] = qpos - - if self.optimizer.adaptor is not None: - robot_qpos = self.optimizer.adaptor.forward_qpos(robot_qpos) - - if self.filter is not None: - robot_qpos = self.filter.next(robot_qpos) - return robot_qpos - - def set_qpos(self, robot_qpos: np.ndarray): - target_qpos = robot_qpos[self.optimizer.idx_pin2target] - self.last_qpos = target_qpos - - def get_qpos(self, fixed_qpos: Optional[np.ndarray] = None): - robot_qpos = np.zeros(self.optimizer.robot.dof) - robot_qpos[self.optimizer.idx_pin2target] = self.last_qpos - if fixed_qpos is not None: - robot_qpos[self.optimizer.idx_pin2fixed] = fixed_qpos - return robot_qpos - - def verbose(self): - min_value = self.optimizer.opt.last_optimum_value() - print(f"Retargeting {self.num_retargeting} times takes: {self.accumulated_time}s") - print(f"Last distance: {min_value}") - - def reset(self): - self.last_qpos = self.joint_limits.mean(1).astype(np.float32) - self.num_retargeting = 0 - self.accumulated_time = 0 - - @property - def joint_names(self): - return self.optimizer.robot.dof_joint_names diff --git a/teleop/robot_control/dex_retargeting/yourdfpy.py b/teleop/robot_control/dex_retargeting/yourdfpy.py deleted file mode 100644 index ce8e287..0000000 --- a/teleop/robot_control/dex_retargeting/yourdfpy.py +++ /dev/null @@ -1,2237 +0,0 @@ -# Code from yourdfpy with small modification for deprecated warning -# Source: https://github.com/clemense/yourdfpy/blob/main/src/yourdfpy/urdf.py - -import copy -import logging -import os -from dataclasses import dataclass, field, is_dataclass -from functools import partial -from typing import Dict, List, Optional, Union - -import anytree -import numpy as np -import six -import trimesh -import trimesh.transformations as tra -from anytree import Node, LevelOrderIter -from lxml import etree - -_logger = logging.getLogger(__name__) - - -def _array_eq(arr1, arr2): - if arr1 is None and arr2 is None: - return True - return ( - isinstance(arr1, np.ndarray) - and isinstance(arr2, np.ndarray) - and arr1.shape == arr2.shape - and (arr1 == arr2).all() - ) - - -@dataclass(eq=False) -class TransmissionJoint: - name: str - hardware_interfaces: List[str] = field(default_factory=list) - - def __eq__(self, other): - if not isinstance(other, TransmissionJoint): - return NotImplemented - return ( - self.name == other.name - and all(self_hi in other.hardware_interfaces for self_hi in self.hardware_interfaces) - and all(other_hi in self.hardware_interfaces for other_hi in other.hardware_interfaces) - ) - - -@dataclass(eq=False) -class Actuator: - name: str - mechanical_reduction: Optional[float] = None - # The follwing is only valid for ROS Indigo and prior versions - hardware_interfaces: List[str] = field(default_factory=list) - - def __eq__(self, other): - if not isinstance(other, Actuator): - return NotImplemented - return ( - self.name == other.name - and self.mechanical_reduction == other.mechanical_reduction - and all(self_hi in other.hardware_interfaces for self_hi in self.hardware_interfaces) - and all(other_hi in self.hardware_interfaces for other_hi in other.hardware_interfaces) - ) - - -@dataclass(eq=False) -class Transmission: - name: str - type: Optional[str] = None - joints: List[TransmissionJoint] = field(default_factory=list) - actuators: List[Actuator] = field(default_factory=list) - - def __eq__(self, other): - if not isinstance(other, Transmission): - return NotImplemented - return ( - self.name == other.name - and self.type == other.type - and all(self_joint in other.joints for self_joint in self.joints) - and all(other_joint in self.joints for other_joint in other.joints) - and all(self_actuator in other.actuators for self_actuator in self.actuators) - and all(other_actuator in self.actuators for other_actuator in other.actuators) - ) - - -@dataclass -class Calibration: - rising: Optional[float] = None - falling: Optional[float] = None - - -@dataclass -class Mimic: - joint: str - multiplier: Optional[float] = None - offset: Optional[float] = None - - -@dataclass -class SafetyController: - soft_lower_limit: Optional[float] = None - soft_upper_limit: Optional[float] = None - k_position: Optional[float] = None - k_velocity: Optional[float] = None - - -@dataclass -class Sphere: - radius: float - - -@dataclass -class Cylinder: - radius: float - length: float - - -@dataclass(eq=False) -class Box: - size: np.ndarray - - def __eq__(self, other): - if not isinstance(other, Box): - return NotImplemented - return _array_eq(self.size, other.size) - - -@dataclass(eq=False) -class Mesh: - filename: str - scale: Optional[Union[float, np.ndarray]] = None - - def __eq__(self, other): - if not isinstance(other, Mesh): - return NotImplemented - - if self.filename != other.filename: - return False - - if isinstance(self.scale, float) and isinstance(other.scale, float): - return self.scale == other.scale - - return _array_eq(self.scale, other.scale) - - -@dataclass -class Geometry: - box: Optional[Box] = None - cylinder: Optional[Cylinder] = None - sphere: Optional[Sphere] = None - mesh: Optional[Mesh] = None - - -@dataclass(eq=False) -class Color: - rgba: np.ndarray - - def __eq__(self, other): - if not isinstance(other, Color): - return NotImplemented - return _array_eq(self.rgba, other.rgba) - - -@dataclass -class Texture: - filename: str - - -@dataclass -class Material: - name: Optional[str] = None - color: Optional[Color] = None - texture: Optional[Texture] = None - - -@dataclass(eq=False) -class Visual: - name: Optional[str] = None - origin: Optional[np.ndarray] = None - geometry: Optional[Geometry] = None # That's not really optional according to ROS - material: Optional[Material] = None - - def __eq__(self, other): - if not isinstance(other, Visual): - return NotImplemented - return ( - self.name == other.name - and _array_eq(self.origin, other.origin) - and self.geometry == other.geometry - and self.material == other.material - ) - - -@dataclass(eq=False) -class Collision: - name: str - origin: Optional[np.ndarray] = None - geometry: Geometry = None - - def __eq__(self, other): - if not isinstance(other, Collision): - return NotImplemented - return self.name == other.name and _array_eq(self.origin, other.origin) and self.geometry == other.geometry - - -@dataclass(eq=False) -class Inertial: - origin: Optional[np.ndarray] = None - mass: Optional[float] = None - inertia: Optional[np.ndarray] = None - - def __eq__(self, other): - if not isinstance(other, Inertial): - return NotImplemented - return ( - _array_eq(self.origin, other.origin) and self.mass == other.mass and _array_eq(self.inertia, other.inertia) - ) - - -@dataclass(eq=False) -class Link: - name: str - inertial: Optional[Inertial] = None - visuals: List[Visual] = field(default_factory=list) - collisions: List[Collision] = field(default_factory=list) - - def __eq__(self, other): - if not isinstance(other, Link): - return NotImplemented - return ( - self.name == other.name - and self.inertial == other.inertial - and all(self_visual in other.visuals for self_visual in self.visuals) - and all(other_visual in self.visuals for other_visual in other.visuals) - and all(self_collision in other.collisions for self_collision in self.collisions) - and all(other_collision in self.collisions for other_collision in other.collisions) - ) - - -@dataclass -class Dynamics: - damping: Optional[float] = None - friction: Optional[float] = None - - -@dataclass -class Limit: - effort: Optional[float] = None - velocity: Optional[float] = None - lower: Optional[float] = None - upper: Optional[float] = None - - -@dataclass(eq=False) -class Joint: - name: str - type: str = None - parent: str = None - child: str = None - origin: np.ndarray = None - axis: np.ndarray = None - dynamics: Optional[Dynamics] = None - limit: Optional[Limit] = None - mimic: Optional[Mimic] = None - calibration: Optional[Calibration] = None - safety_controller: Optional[SafetyController] = None - - def __eq__(self, other): - if not isinstance(other, Joint): - return NotImplemented - return ( - self.name == other.name - and self.type == other.type - and self.parent == other.parent - and self.child == other.child - and _array_eq(self.origin, other.origin) - and _array_eq(self.axis, other.axis) - and self.dynamics == other.dynamics - and self.limit == other.limit - and self.mimic == other.mimic - and self.calibration == other.calibration - and self.safety_controller == other.safety_controller - ) - - -@dataclass(eq=False) -class Robot: - name: str - links: List[Link] = field(default_factory=list) - joints: List[Joint] = field(default_factory=list) - materials: List[Material] = field(default_factory=list) - transmission: List[str] = field(default_factory=list) - gazebo: List[str] = field(default_factory=list) - - def __eq__(self, other): - if not isinstance(other, Robot): - return NotImplemented - return ( - self.name == other.name - and all(self_link in other.links for self_link in self.links) - and all(other_link in self.links for other_link in other.links) - and all(self_joint in other.joints for self_joint in self.joints) - and all(other_joint in self.joints for other_joint in other.joints) - and all(self_material in other.materials for self_material in self.materials) - and all(other_material in self.materials for other_material in other.materials) - and all(self_transmission in other.transmission for self_transmission in self.transmission) - and all(other_transmission in self.transmission for other_transmission in other.transmission) - and all(self_gazebo in other.gazebo for self_gazebo in self.gazebo) - and all(other_gazebo in self.gazebo for other_gazebo in other.gazebo) - ) - - -class URDFError(Exception): - """General URDF exception.""" - - def __init__(self, msg): - super(URDFError, self).__init__() - self.msg = msg - - def __str__(self): - return type(self).__name__ + ": " + self.msg - - def __repr__(self): - return type(self).__name__ + '("' + self.msg + '")' - - -class URDFIncompleteError(URDFError): - """Raised when needed data for an object isn't there.""" - - pass - - -class URDFAttributeValueError(URDFError): - """Raised when attribute value is not contained in the set of allowed values.""" - - pass - - -class URDFBrokenRefError(URDFError): - """Raised when a referenced object is not found in the scope.""" - - pass - - -class URDFMalformedError(URDFError): - """Raised when data is found to be corrupted in some way.""" - - pass - - -class URDFUnsupportedError(URDFError): - """Raised when some unexpectedly unsupported feature is found.""" - - pass - - -class URDFSaveValidationError(URDFError): - """Raised when XML validation fails when saving.""" - - pass - - -def _str2float(s): - """Cast string to float if it is not None. Otherwise return None. - - Args: - s (str): String to convert or None. - - Returns: - str or NoneType: The converted string or None. - """ - return float(s) if s is not None else None - - -def apply_visual_color( - geom: trimesh.Trimesh, - visual: Visual, - material_map: Dict[str, Material], -) -> None: - """Apply the color of the visual material to the mesh. - - Args: - geom: Trimesh to color. - visual: Visual description from XML. - material_map: Dictionary mapping material names to their definitions. - """ - if visual.material is None: - return - - if visual.material.color is not None: - color = visual.material.color - elif visual.material.name is not None and visual.material.name in material_map: - color = material_map[visual.material.name].color - else: - return - - if color is None: - return - if isinstance(geom.visual, trimesh.visual.ColorVisuals): - geom.visual.face_colors[:] = [int(255 * channel) for channel in color.rgba] - - -def filename_handler_null(fname): - """A lazy filename handler that simply returns its input. - - Args: - fname (str): A file name. - - Returns: - str: Same file name. - """ - return fname - - -def filename_handler_ignore_directive(fname): - """A filename handler that removes anything before (and including) '://'. - - Args: - fname (str): A file name. - - Returns: - str: The file name without the prefix. - """ - if "://" in fname or ":\\\\" in fname: - return ":".join(fname.split(":")[1:])[2:] - return fname - - -def filename_handler_ignore_directive_package(fname): - """A filename handler that removes the 'package://' directive and the package it refers to. - It subsequently calls filename_handler_ignore_directive, i.e., it removes any other directive. - - Args: - fname (str): A file name. - - Returns: - str: The file name without 'package://' and the package name. - """ - if fname.startswith("package://"): - string_length = len("package://") - return os.path.join(*os.path.normpath(fname[string_length:]).split(os.path.sep)[1:]) - return filename_handler_ignore_directive(fname) - - -def filename_handler_add_prefix(fname, prefix): - """A filename handler that adds a prefix. - - Args: - fname (str): A file name. - prefix (str): A prefix. - - Returns: - str: Prefix plus file name. - """ - return prefix + fname - - -def filename_handler_absolute2relative(fname, dir): - """A filename handler that turns an absolute file name into a relative one. - - Args: - fname (str): A file name. - dir (str): A directory. - - Returns: - str: The file name relative to the directory. - """ - # TODO: that's not right - if fname.startswith(dir): - return fname[len(dir) :] - return fname - - -def filename_handler_relative(fname, dir): - """A filename handler that joins a file name with a directory. - - Args: - fname (str): A file name. - dir (str): A directory. - - Returns: - str: The directory joined with the file name. - """ - return os.path.join(dir, filename_handler_ignore_directive_package(fname)) - - -def filename_handler_relative_to_urdf_file(fname, urdf_fname): - return filename_handler_relative(fname, os.path.dirname(urdf_fname)) - - -def filename_handler_relative_to_urdf_file_recursive(fname, urdf_fname, level=0): - if level == 0: - return filename_handler_relative_to_urdf_file(fname, urdf_fname) - return filename_handler_relative_to_urdf_file_recursive(fname, os.path.split(urdf_fname)[0], level=level - 1) - - -def _create_filename_handlers_to_urdf_file_recursive(urdf_fname): - return [ - partial( - filename_handler_relative_to_urdf_file_recursive, - urdf_fname=urdf_fname, - level=i, - ) - for i in range(len(os.path.normpath(urdf_fname).split(os.path.sep))) - ] - - -def filename_handler_meta(fname, filename_handlers): - """A filename handler that calls other filename handlers until the resulting file name points to an existing file. - - Args: - fname (str): A file name. - filename_handlers (list(fn)): A list of function pointers to filename handlers. - - Returns: - str: The resolved file name that points to an existing file or the input if none of the files exists. - """ - for fn in filename_handlers: - candidate_fname = fn(fname=fname) - _logger.debug(f"Checking filename: {candidate_fname}") - if os.path.isfile(candidate_fname): - return candidate_fname - _logger.warning(f"Unable to resolve filename: {fname}") - return fname - - -def filename_handler_magic(fname, dir): - """A magic filename handler. - - Args: - fname (str): A file name. - dir (str): A directory. - - Returns: - str: The file name that exists or the input if nothing is found. - """ - return filename_handler_meta( - fname=fname, - filename_handlers=[ - partial(filename_handler_relative, dir=dir), - filename_handler_ignore_directive, - ] - + _create_filename_handlers_to_urdf_file_recursive(urdf_fname=dir), - ) - - -def validation_handler_strict(errors): - """A validation handler that does not allow any errors. - - Args: - errors (list[yourdfpy.URDFError]): List of errors. - - Returns: - bool: Whether any errors were found. - """ - return len(errors) == 0 - - -class URDF: - def __init__( - self, - robot: Robot = None, - build_scene_graph: bool = True, - build_collision_scene_graph: bool = False, - load_meshes: bool = True, - load_collision_meshes: bool = False, - filename_handler=None, - mesh_dir: str = "", - force_mesh: bool = False, - force_collision_mesh: bool = True, - build_tree: bool = False, - ): - """A URDF model. - - Args: - robot (Robot): The robot model. Defaults to None. - build_scene_graph (bool, optional): Wheter to build a scene graph to enable transformation queries and forward kinematics. Defaults to True. - build_collision_scene_graph (bool, optional): Wheter to build a scene graph for elements. Defaults to False. - load_meshes (bool, optional): Whether to load the meshes referenced in the elements. Defaults to True. - load_collision_meshes (bool, optional): Whether to load the collision meshes referenced in the elements. Defaults to False. - filename_handler ([type], optional): Any function f(in: str) -> str, that maps filenames in the URDF to actual resources. Can be used to customize treatment of `package://` directives or relative/absolute filenames. Defaults to None. - mesh_dir (str, optional): A root directory used for loading meshes. Defaults to "". - force_mesh (bool, optional): Each loaded geometry will be concatenated into a single one (instead of being turned into a graph; in case the underlying file contains multiple geometries). This might loose texture information but the resulting scene graph will be smaller. Defaults to False. - force_collision_mesh (bool, optional): Same as force_mesh, but for collision scene. Defaults to True. - build_tree (bool, optional): Build the tree structure for global kinematics computation - """ - if filename_handler is None: - self._filename_handler = partial(filename_handler_magic, dir=mesh_dir) - else: - self._filename_handler = filename_handler - - self.robot = robot - self._create_maps() - self._update_actuated_joints() - - self._cfg = self.zero_cfg - - if build_scene_graph or build_collision_scene_graph: - self._base_link = self._determine_base_link() - else: - self._base_link = None - - self._errors = [] - - if build_scene_graph: - self._scene = self._create_scene( - use_collision_geometry=False, - load_geometry=load_meshes, - force_mesh=force_mesh, - force_single_geometry_per_link=force_mesh, - ) - else: - self._scene = None - - if build_collision_scene_graph: - self._scene_collision = self._create_scene( - use_collision_geometry=True, - load_geometry=load_collision_meshes, - force_mesh=force_collision_mesh, - force_single_geometry_per_link=force_collision_mesh, - ) - else: - self._scene_collision = None - - if build_tree: - self.tree_root = self.build_tree() - else: - self.tree_root = None - - @property - def scene(self) -> trimesh.Scene: - """A scene object representing the URDF model. - - Returns: - trimesh.Scene: A trimesh scene object. - """ - return self._scene - - @property - def collision_scene(self) -> trimesh.Scene: - """A scene object representing the elements of the URDF model - - Returns: - trimesh.Scene: A trimesh scene object. - """ - return self._scene_collision - - @property - def link_map(self) -> dict: - """A dictionary mapping link names to link objects. - - Returns: - dict: Mapping from link name (str) to Link. - """ - return self._link_map - - @property - def joint_map(self) -> dict: - """A dictionary mapping joint names to joint objects. - - Returns: - dict: Mapping from joint name (str) to Joint. - """ - return self._joint_map - - @property - def joint_names(self): - """List of joint names. - - Returns: - list[str]: List of joint names of the URDF model. - """ - return [j.name for j in self.robot.joints] - - @property - def actuated_joints(self): - """List of actuated joints. This excludes mimic and fixed joints. - - Returns: - list[Joint]: List of actuated joints of the URDF model. - """ - return self._actuated_joints - - @property - def actuated_dof_indices(self): - """List of DOF indices per actuated joint. Can be used to reference configuration. - - Returns: - list[list[int]]: List of DOF indices per actuated joint. - """ - return self._actuated_dof_indices - - @property - def actuated_joint_indices(self): - """List of indices of all joints that are actuated, i.e., not of type mimic or fixed. - - Returns: - list[int]: List of indices of actuated joints. - """ - return self._actuated_joint_indices - - @property - def actuated_joint_names(self): - """List of names of actuated joints. This excludes mimic and fixed joints. - - Returns: - list[str]: List of names of actuated joints of the URDF model. - """ - return [j.name for j in self._actuated_joints] - - @property - def num_actuated_joints(self): - """Number of actuated joints. - - Returns: - int: Number of actuated joints. - """ - return len(self.actuated_joints) - - @property - def num_dofs(self): - """Number of degrees of freedom of actuated joints. Depending on the type of the joint, the number of DOFs might vary. - - Returns: - int: Degrees of freedom. - """ - total_num_dofs = 0 - for j in self._actuated_joints: - if j.type in ["revolute", "prismatic", "continuous"]: - total_num_dofs += 1 - elif j.type == "floating": - total_num_dofs += 6 - elif j.type == "planar": - total_num_dofs += 2 - return total_num_dofs - - @property - def zero_cfg(self): - """Return the zero configuration. - - Returns: - np.ndarray: The zero configuration. - """ - return np.zeros(self.num_dofs) - - @property - def center_cfg(self): - """Return center configuration of URDF model by using the average of each joint's limits if present, otherwise zero. - - Returns: - (n), float: Default configuration of URDF model. - """ - config = [] - config_names = [] - for j in self._actuated_joints: - if j.type == "revolute" or j.type == "prismatic": - if j.limit is not None: - cfg = [j.limit.lower + 0.5 * (j.limit.upper - j.limit.lower)] - else: - cfg = [0.0] - elif j.type == "continuous": - cfg = [0.0] - elif j.type == "floating": - cfg = [0.0] * 6 - elif j.type == "planar": - cfg = [0.0] * 2 - - config.append(cfg) - config_names.append(j.name) - - for i, j in enumerate(self.robot.joints): - if j.mimic is not None: - index = config_names.index(j.mimic.joint) - config[i][0] = config[index][0] * j.mimic.multiplier + j.mimic.offset - - if len(config) == 0: - return np.array([], dtype=np.float64) - return np.concatenate(config) - - @property - def cfg(self): - """Current configuration. - - Returns: - np.ndarray: Current configuration of URDF model. - """ - return self._cfg - - @property - def base_link(self): - """Name of URDF base/root link. - - Returns: - str: Name of base link of URDF model. - """ - return self._base_link - - @property - def errors(self) -> list: - """A list with validation errors. - - Returns: - list: A list of validation errors. - """ - return self._errors - - def clear_errors(self): - """Clear the validation error log.""" - self._errors = [] - - def show(self, collision_geometry=False, callback=None): - """Open a simpler viewer displaying the URDF model. - - Args: - collision_geometry (bool, optional): Whether to display the or elements. Defaults to False. - """ - if collision_geometry: - if self._scene_collision is None: - raise ValueError( - "No collision scene available. Use build_collision_scene_graph=True and load_collision_meshes=True during loading." - ) - else: - self._scene_collision.show(callback=callback) - else: - if self._scene is None: - raise ValueError("No scene available. Use build_scene_graph=True and load_meshes=True during loading.") - elif len(self._scene.bounds_corners) < 1: - raise ValueError( - "Scene is empty, maybe meshes failed to load? Use build_scene_graph=True and load_meshes=True during loading." - ) - else: - self._scene.show(callback=callback) - - def validate(self, validation_fn=None) -> bool: - """Validate URDF model. - - Args: - validation_fn (function, optional): A function f(list[yourdfpy.URDFError]) -> bool. None uses the strict handler (any error leads to False). Defaults to None. - - Returns: - bool: Whether the model is valid. - """ - self._errors = [] - self._validate_robot(self.robot) - - if validation_fn is None: - validation_fn = validation_handler_strict - - return validation_fn(self._errors) - - def _create_maps(self): - self._material_map = {} - for m in self.robot.materials: - self._material_map[m.name] = m - - self._joint_map = {} - for j in self.robot.joints: - self._joint_map[j.name] = j - - self._link_map = {} - for l in self.robot.links: - self._link_map[l.name] = l - - def _update_actuated_joints(self): - self._actuated_joints = [] - self._actuated_joint_indices = [] - self._actuated_dof_indices = [] - - dof_indices_cnt = 0 - for i, j in enumerate(self.robot.joints): - if j.mimic is None and j.type != "fixed": - self._actuated_joints.append(j) - self._actuated_joint_indices.append(i) - - if j.type in ["prismatic", "revolute", "continuous"]: - self._actuated_dof_indices.append([dof_indices_cnt]) - dof_indices_cnt += 1 - elif j.type == "floating": - self._actuated_dof_indices.append([dof_indices_cnt, dof_indices_cnt + 1, dof_indices_cnt + 2]) - dof_indices_cnt += 3 - elif j.type == "planar": - self._actuated_dof_indices.append([dof_indices_cnt, dof_indices_cnt + 1]) - dof_indices_cnt += 2 - - def _validate_required_attribute(self, attribute, error_msg, allowed_values=None): - if attribute is None: - self._errors.append(URDFIncompleteError(error_msg)) - elif isinstance(attribute, str) and len(attribute) == 0: - self._errors.append(URDFIncompleteError(error_msg)) - - if allowed_values is not None and attribute is not None: - if attribute not in allowed_values: - self._errors.append(URDFAttributeValueError(error_msg)) - - @staticmethod - def load(fname_or_file, add_dummy_free_joints=False, **kwargs): - """Load URDF file from filename or file object. - - Args: - fname_or_file (str or file object): A filename or file object, file-like object, stream representing the URDF file. - **build_scene_graph (bool, optional): Wheter to build a scene graph to enable transformation queries and forward kinematics. Defaults to True. - **build_collision_scene_graph (bool, optional): Wheter to build a scene graph for elements. Defaults to False. - **load_meshes (bool, optional): Whether to load the meshes referenced in the elements. Defaults to True. - **load_collision_meshes (bool, optional): Whether to load the collision meshes referenced in the elements. Defaults to False. - **filename_handler ([type], optional): Any function f(in: str) -> str, that maps filenames in the URDF to actual resources. Can be used to customize treatment of `package://` directives or relative/absolute filenames. Defaults to None. - **mesh_dir (str, optional): A root directory used for loading meshes. Defaults to "". - **force_mesh (bool, optional): Each loaded geometry will be concatenated into a single one (instead of being turned into a graph; in case the underlying file contains multiple geometries). This might loose texture information but the resulting scene graph will be smaller. Defaults to False. - **force_collision_mesh (bool, optional): Same as force_mesh, but for collision scene. Defaults to True. - - Raises: - ValueError: If filename does not exist. - - Returns: - yourdfpy.URDF: URDF model. - """ - if isinstance(fname_or_file, six.string_types): - if not os.path.isfile(fname_or_file): - raise ValueError("{} is not a file".format(fname_or_file)) - - if not "mesh_dir" in kwargs: - kwargs["mesh_dir"] = os.path.dirname(fname_or_file) - - try: - parser = etree.XMLParser(remove_blank_text=True) - tree = etree.parse(fname_or_file, parser=parser) - xml_root = tree.getroot() - except Exception as e: - _logger.error(e) - _logger.error("Using different parsing approach.") - - events = ("start", "end", "start-ns", "end-ns") - xml = etree.iterparse(fname_or_file, recover=True, events=events) - - # Iterate through all XML elements - for action, elem in xml: - # Skip comments and processing instructions, - # because they do not have names - if not (isinstance(elem, etree._Comment) or isinstance(elem, etree._ProcessingInstruction)): - # Remove a namespace URI in the element's name - # elem.tag = etree.QName(elem).localname - if action == "end" and ":" in elem.tag: - elem.getparent().remove(elem) - - xml_root = xml.root - - # Remove comments - etree.strip_tags(xml_root, etree.Comment) - etree.cleanup_namespaces(xml_root) - - return URDF( - robot=URDF._parse_robot(xml_element=xml_root, add_dummy_free_joints=add_dummy_free_joints), **kwargs - ) - - def contains(self, key, value, element=None) -> bool: - """Checks recursively whether the URDF tree contains the provided key-value pair. - - Args: - key (str): A key. - value (str): A value. - element (etree.Element, optional): The XML element from which to start the recursive search. None means URDF root. Defaults to None. - - Returns: - bool: Whether the key-value pair was found. - """ - if element is None: - element = self.robot - - result = False - for field in element.__dataclass_fields__: - field_value = getattr(element, field) - if is_dataclass(field_value): - result = result or self.contains(key=key, value=value, element=field_value) - elif isinstance(field_value, list) and len(field_value) > 0 and is_dataclass(field_value[0]): - for field_value_element in field_value: - result = result or self.contains(key=key, value=value, element=field_value_element) - else: - if key == field and value == field_value: - result = True - return result - - def _determine_base_link(self): - """Get the base link of the URDF tree by extracting all links without parents. - In case multiple links could be root chose the first. - - Returns: - str: Name of the base link. - """ - link_names = [l.name for l in self.robot.links] - - for j in self.robot.joints: - link_names.remove(j.child) - - if len(link_names) == 0: - # raise Error? - return None - - return link_names[0] - - def _forward_kinematics_joint(self, joint, q=None): - origin = np.eye(4) if joint.origin is None else joint.origin - - if joint.mimic is not None: - if joint.mimic.joint in self.actuated_joint_names: - mimic_joint_index = self.actuated_joint_names.index(joint.mimic.joint) - q = self._cfg[mimic_joint_index] * joint.mimic.multiplier + joint.mimic.offset - else: - _logger.warning( - f"Joint '{joint.name}' is supposed to mimic '{joint.mimic.joint}'. But this joint is not actuated - will assume (0.0 + offset)." - ) - q = 0.0 + joint.mimic.offset - - if joint.type in ["revolute", "prismatic", "continuous"]: - if q is None: - # Use internal cfg vector for forward kinematics - q = float(self.cfg[self.actuated_dof_indices[self.actuated_joint_names.index(joint.name)]]) - - if joint.type == "prismatic": - matrix = origin @ tra.translation_matrix(q * joint.axis) - else: - matrix = origin @ tra.rotation_matrix(q, joint.axis) - else: - # this includes: floating, planar, fixed - matrix = origin - - return matrix, q - - def update_cfg(self, configuration): - """Update joint configuration of URDF; does forward kinematics. - - Args: - configuration (dict, list[float], tuple[float] or np.ndarray): A mapping from joints or joint names to configuration values, or a list containing a value for each actuated joint. - - Raises: - ValueError: Raised if dimensionality of configuration does not match number of actuated joints of URDF model. - TypeError: Raised if configuration is neither a dict, list, tuple or np.ndarray. - """ - joint_cfg = [] - - if isinstance(configuration, dict): - for joint in configuration: - if isinstance(joint, six.string_types): - joint_cfg.append((self._joint_map[joint], configuration[joint])) - elif isinstance(joint, Joint): - # TODO: Joint is not hashable; so this branch will not succeed - joint_cfg.append((joint, configuration[joint])) - elif isinstance(configuration, (list, tuple, np.ndarray)): - if len(configuration) == len(self.robot.joints): - for joint, value in zip(self.robot.joints, configuration): - joint_cfg.append((joint, value)) - elif len(configuration) == self.num_actuated_joints: - for joint, value in zip(self._actuated_joints, configuration): - joint_cfg.append((joint, value)) - else: - raise ValueError( - f"Dimensionality of configuration ({len(configuration)}) doesn't match number of all ({len(self.robot.joints)}) or actuated joints ({self.num_actuated_joints})." - ) - else: - raise TypeError("Invalid type for configuration") - - # append all mimic joints in the update - for j, q in joint_cfg + [(j, 0.0) for j in self.robot.joints if j.mimic is not None]: - matrix, joint_q = self._forward_kinematics_joint(j, q=q) - - # update internal configuration vector - only consider actuated joints - if j.name in self.actuated_joint_names: - self._cfg[self.actuated_dof_indices[self.actuated_joint_names.index(j.name)]] = joint_q - - if self._scene is not None: - self._scene.graph.update(frame_from=j.parent, frame_to=j.child, matrix=matrix) - if self._scene_collision is not None: - self._scene_collision.graph.update(frame_from=j.parent, frame_to=j.child, matrix=matrix) - - def get_transform(self, frame_to, frame_from=None, collision_geometry=False): - """Get the transform from one frame to another. - - Args: - frame_to (str): Node name. - frame_from (str, optional): Node name. If None it will be set to self.base_frame. Defaults to None. - collision_geometry (bool, optional): Whether to use the collision geometry scene graph (instead of the visual geometry). Defaults to False. - - Raises: - ValueError: Raised if scene graph wasn't constructed during intialization. - - Returns: - (4, 4) float: Homogeneous transformation matrix - """ - if collision_geometry: - if self._scene_collision is None: - raise ValueError("No collision scene available. Use build_collision_scene_graph=True during loading.") - else: - return self._scene_collision.graph.get(frame_to=frame_to, frame_from=frame_from)[0] - else: - if self._scene is None: - raise ValueError("No scene available. Use build_scene_graph=True during loading.") - else: - return self._scene.graph.get(frame_to=frame_to, frame_from=frame_from)[0] - - def _link_mesh(self, link, collision_geometry=True): - geometries = link.collisions if collision_geometry else link.visuals - - if len(geometries) == 0: - return None - - meshes = [] - for g in geometries: - for m in g.geometry.meshes: - m = m.copy() - pose = g.origin - if g.geometry.mesh is not None: - if g.geometry.mesh.scale is not None: - S = np.eye(4) - S[:3, :3] = np.diag(g.geometry.mesh.scale) - pose = pose.dot(S) - m.apply_transform(pose) - meshes.append(m) - if len(meshes) == 0: - return None - self._collision_mesh = meshes[0] + meshes[1:] - return self._collision_mesh - - def _geometry2trimeshscene(self, geometry, load_file, force_mesh, skip_materials): - new_s = None - if geometry.box is not None: - new_s = trimesh.primitives.Box(extents=geometry.box.size).scene() - elif geometry.sphere is not None: - new_s = trimesh.primitives.Sphere(radius=geometry.sphere.radius).scene() - elif geometry.cylinder is not None: - new_s = trimesh.primitives.Cylinder( - radius=geometry.cylinder.radius, height=geometry.cylinder.length - ).scene() - elif geometry.mesh is not None and load_file: - new_filename = self._filename_handler(fname=geometry.mesh.filename) - - if os.path.isfile(new_filename): - _logger.debug(f"Loading {geometry.mesh.filename} as {new_filename}") - - if force_mesh: - new_g = trimesh.load( - new_filename, - ignore_broken=True, - force="mesh", - skip_materials=skip_materials, - ) - - # add original filename - if "file_path" not in new_g.metadata: - new_g.metadata["file_path"] = os.path.abspath(new_filename) - new_g.metadata["file_name"] = os.path.basename(new_filename) - - new_s = trimesh.Scene() - new_s.add_geometry(new_g) - else: - new_s = trimesh.load( - new_filename, - ignore_broken=True, - force="scene", - skip_materials=skip_materials, - ) - - if "file_path" in new_s.metadata: - for i, (_, geom) in enumerate(new_s.geometry.items()): - if "file_path" not in geom.metadata: - geom.metadata["file_path"] = new_s.metadata["file_path"] - geom.metadata["file_name"] = new_s.metadata["file_name"] - geom.metadata["file_element"] = i - - # scale mesh appropriately - if geometry.mesh.scale is not None: - if isinstance(geometry.mesh.scale, float): - new_s = new_s.scaled(geometry.mesh.scale) - elif isinstance(geometry.mesh.scale, np.ndarray): - new_s = new_s.scaled(geometry.mesh.scale) - else: - _logger.warning(f"Warning: Can't interpret scale '{geometry.mesh.scale}'") - else: - _logger.warning(f"Can't find {new_filename}") - return new_s - - def _add_geometries_to_scene( - self, - s, - geometries, - link_name, - load_geometry, - force_mesh, - force_single_geometry, - skip_materials, - ): - if force_single_geometry: - tmp_scene = trimesh.Scene(base_frame=link_name) - - first_geom_name = None - - for v in geometries: - if v.geometry is not None: - if first_geom_name is None: - first_geom_name = v.name - - new_s = self._geometry2trimeshscene( - geometry=v.geometry, - load_file=load_geometry, - force_mesh=force_mesh, - skip_materials=skip_materials, - ) - if new_s is not None: - origin = v.origin if v.origin is not None else np.eye(4) - - if force_single_geometry: - for name, geom in new_s.geometry.items(): - if isinstance(v, Visual): - apply_visual_color(geom, v, self._material_map) - tmp_scene.add_geometry( - geometry=geom, - geom_name=v.name, - parent_node_name=link_name, - transform=origin @ new_s.graph.get(name)[0], - ) - else: - # The following map is used to deal with glb format - # when the graph node and geometry have different names - geom_name_map = {new_s.graph[node_name][1]: node_name for node_name in new_s.graph.nodes} - for name, geom in new_s.geometry.items(): - if isinstance(v, Visual): - apply_visual_color(geom, v, self._material_map) - s.add_geometry( - geometry=geom, - geom_name=v.name, - parent_node_name=link_name, - transform=origin @ new_s.graph.get(geom_name_map[name])[0], - ) - - if force_single_geometry and len(tmp_scene.geometry) > 0: - s.add_geometry( - geometry=tmp_scene.dump(concatenate=True), - geom_name=first_geom_name, - parent_node_name=link_name, - transform=np.eye(4), - ) - - def _create_scene( - self, - use_collision_geometry=False, - load_geometry=True, - force_mesh=False, - force_single_geometry_per_link=False, - ): - s = trimesh.scene.Scene(base_frame=self._base_link) - - for j in self.robot.joints: - matrix, _ = self._forward_kinematics_joint(j) - - s.graph.update(frame_from=j.parent, frame_to=j.child, matrix=matrix) - - for l in self.robot.links: - if l.name not in s.graph.nodes and l.name != s.graph.base_frame: - _logger.warning(f"{l.name} not connected via joints. Will add link to base frame.") - s.graph.update(frame_from=s.graph.base_frame, frame_to=l.name) - - meshes = l.collisions if use_collision_geometry else l.visuals - self._add_geometries_to_scene( - s, - geometries=meshes, - link_name=l.name, - load_geometry=load_geometry, - force_mesh=force_mesh, - force_single_geometry=force_single_geometry_per_link, - skip_materials=use_collision_geometry, - ) - - return s - - def _successors(self, node): - """ - Get all nodes of the scene that succeeds a specified node. - - Parameters - ------------ - node : any - Hashable key in `scene.graph` - - Returns - ----------- - subnodes : set[str] - Set of nodes. - """ - # get every node that is a successor to specified node - # this includes `node` - return self._scene.graph.transforms.successors(node) - - def _create_subrobot(self, robot_name, root_link_name): - subrobot = Robot(name=robot_name) - subnodes = self._successors(node=root_link_name) - - if len(subnodes) > 0: - for node in subnodes: - if node in self.link_map: - subrobot.links.append(copy.deepcopy(self.link_map[node])) - for joint_name, joint in self.joint_map.items(): - if joint.parent in subnodes and joint.child in subnodes: - subrobot.joints.append(copy.deepcopy(self.joint_map[joint_name])) - - return subrobot - - def split_along_joints(self, joint_type="floating", **kwargs): - """Split URDF model along a particular joint type. - The result is a set of URDF models which together compose the original URDF. - - Args: - joint_type (str, or list[str], optional): Type of joint to use for splitting. Defaults to "floating". - **kwargs: Arguments delegated to URDF constructor of new URDF models. - - Returns: - list[(np.ndarray, yourdfpy.URDF)]: A list of tuples (np.ndarray, yourdfpy.URDF) whereas each homogeneous 4x4 matrix describes the root transformation of the respective URDF model w.r.t. the original URDF. - """ - root_urdf = URDF(robot=copy.deepcopy(self.robot), build_scene_graph=False, load_meshes=False) - result = [] - - joint_types = joint_type if isinstance(joint_type, list) else [joint_type] - - # find all relevant joints - joint_names = [j.name for j in self.robot.joints if j.type in joint_types] - for joint_name in joint_names: - root_link = self.link_map[self.joint_map[joint_name].child] - new_robot = self._create_subrobot( - robot_name=root_link.name, - root_link_name=root_link.name, - ) - - result.append( - ( - self._scene.graph.get(root_link.name)[0], - URDF(robot=new_robot, **kwargs), - ) - ) - - # remove links and joints from root robot - for j in new_robot.joints: - root_urdf.robot.joints.remove(root_urdf.joint_map[j.name]) - for l in new_robot.links: - root_urdf.robot.links.remove(root_urdf.link_map[l.name]) - - # remove joint that connects root urdf to root_link - if root_link.name in [j.child for j in root_urdf.robot.joints]: - root_urdf.robot.joints.remove( - root_urdf.robot.joints[[j.child for j in root_urdf.robot.joints].index(root_link.name)] - ) - - result.insert(0, (np.eye(4), URDF(robot=root_urdf.robot, **kwargs))) - - return result - - def validate_filenames(self): - for l in self.robot.links: - meshes = [m.geometry.mesh for m in l.collisions + l.visuals if m.geometry.mesh is not None] - for m in meshes: - _logger.debug(m.filename, "-->", self._filename_handler(m.filename)) - if not os.path.isfile(self._filename_handler(m.filename)): - return False - return True - - def write_xml(self): - """Write URDF model to an XML element hierarchy. - - Returns: - etree.ElementTree: XML data. - """ - xml_element = self._write_robot(self.robot) - return etree.ElementTree(xml_element) - - def write_xml_string(self, **kwargs): - """Write URDF model to a string. - - Returns: - str: String of the xml representation of the URDF model. - """ - xml_element = self.write_xml() - return etree.tostring(xml_element, xml_declaration=True, *kwargs) - - def write_xml_file(self, fname): - """Write URDF model to an xml file. - - Args: - fname (str): Filename of the file to be written. Usually ends in `.urdf`. - """ - xml_element = self.write_xml() - xml_element.write(fname, xml_declaration=True, pretty_print=True) - - def _parse_mimic(xml_element): - if xml_element is None: - return None - - return Mimic( - joint=xml_element.get("joint"), - multiplier=_str2float(xml_element.get("multiplier", 1.0)), - offset=_str2float(xml_element.get("offset", 0.0)), - ) - - def _write_mimic(self, xml_parent, mimic): - etree.SubElement( - xml_parent, - "mimic", - attrib={ - "joint": mimic.joint, - "multiplier": str(mimic.multiplier), - "offset": str(mimic.offset), - }, - ) - - def _parse_safety_controller(xml_element): - if xml_element is None: - return None - - return SafetyController( - soft_lower_limit=_str2float(xml_element.get("soft_lower_limit")), - soft_upper_limit=_str2float(xml_element.get("soft_upper_limit")), - k_position=_str2float(xml_element.get("k_position")), - k_velocity=_str2float(xml_element.get("k_velocity")), - ) - - def _write_safety_controller(self, xml_parent, safety_controller): - etree.SubElement( - xml_parent, - "safety_controller", - attrib={ - "soft_lower_limit": str(safety_controller.soft_lower_limit), - "soft_upper_limit": str(safety_controller.soft_upper_limit), - "k_position": str(safety_controller.k_position), - "k_velocity": str(safety_controller.k_velocity), - }, - ) - - def _parse_transmission_joint(xml_element): - if xml_element is None: - return None - - transmission_joint = TransmissionJoint(name=xml_element.get("name")) - - for h in xml_element.findall("hardware_interface"): - transmission_joint.hardware_interfaces.append(h.text) - - return transmission_joint - - def _write_transmission_joint(self, xml_parent, transmission_joint): - xml_element = etree.SubElement( - xml_parent, - "joint", - attrib={ - "name": str(transmission_joint.name), - }, - ) - for h in transmission_joint.hardware_interfaces: - tmp = etree.SubElement( - xml_element, - "hardwareInterface", - ) - tmp.text = h - - def _parse_actuator(xml_element): - if xml_element is None: - return None - - actuator = Actuator(name=xml_element.get("name")) - if xml_element.find("mechanicalReduction"): - actuator.mechanical_reduction = float(xml_element.find("mechanicalReduction").text) - - for h in xml_element.findall("hardwareInterface"): - actuator.hardware_interfaces.append(h.text) - - return actuator - - def _write_actuator(self, xml_parent, actuator): - xml_element = etree.SubElement( - xml_parent, - "actuator", - attrib={ - "name": str(actuator.name), - }, - ) - if actuator.mechanical_reduction is not None: - tmp = etree.SubElement("mechanicalReduction") - tmp.text = str(actuator.mechanical_reduction) - - for h in actuator.hardware_interfaces: - tmp = etree.SubElement( - xml_element, - "hardwareInterface", - ) - tmp.text = h - - def _parse_transmission(xml_element): - if xml_element is None: - return None - - transmission = Transmission(name=xml_element.get("name")) - - for j in xml_element.findall("joint"): - transmission.joints.append(URDF._parse_transmission_joint(j)) - for a in xml_element.findall("actuator"): - transmission.actuators.append(URDF._parse_actuator(a)) - - return transmission - - def _write_transmission(self, xml_parent, transmission): - xml_element = etree.SubElement( - xml_parent, - "transmission", - attrib={ - "name": str(transmission.name), - }, - ) - - for j in transmission.joints: - self._write_transmission_joint(xml_element, j) - - for a in transmission.actuators: - self._write_actuator(xml_element, a) - - def _parse_calibration(xml_element): - if xml_element is None: - return None - - return Calibration( - rising=_str2float(xml_element.get("rising")), - falling=_str2float(xml_element.get("falling")), - ) - - def _write_calibration(self, xml_parent, calibration): - etree.SubElement( - xml_parent, - "calibration", - attrib={ - "rising": str(calibration.rising), - "falling": str(calibration.falling), - }, - ) - - def _parse_box(xml_element): - return Box(size=np.array(xml_element.attrib["size"].split(), dtype=float)) - - def _write_box(self, xml_parent, box): - etree.SubElement(xml_parent, "box", attrib={"size": " ".join(map(str, box.size))}) - - def _parse_cylinder(xml_element): - return Cylinder( - radius=float(xml_element.attrib["radius"]), - length=float(xml_element.attrib["length"]), - ) - - def _write_cylinder(self, xml_parent, cylinder): - etree.SubElement( - xml_parent, - "cylinder", - attrib={"radius": str(cylinder.radius), "length": str(cylinder.length)}, - ) - - def _parse_sphere(xml_element): - return Sphere(radius=float(xml_element.attrib["radius"])) - - def _write_sphere(self, xml_parent, sphere): - etree.SubElement(xml_parent, "sphere", attrib={"radius": str(sphere.radius)}) - - def _parse_scale(xml_element): - if "scale" in xml_element.attrib: - s = xml_element.get("scale").split() - if len(s) == 0: - return None - elif len(s) == 1: - return float(s[0]) - else: - return np.array(list(map(float, s))) - return None - - def _write_scale(self, xml_parent, scale): - if scale is not None: - if isinstance(scale, float) or isinstance(scale, int): - xml_parent.set("scale", " ".join([str(scale)] * 3)) - else: - xml_parent.set("scale", " ".join(map(str, scale))) - - def _parse_mesh(xml_element): - return Mesh(filename=xml_element.get("filename"), scale=URDF._parse_scale(xml_element)) - - def _write_mesh(self, xml_parent, mesh): - # TODO: turn into different filename handler - xml_element = etree.SubElement( - xml_parent, - "mesh", - attrib={"filename": self._filename_handler(mesh.filename)}, - ) - - self._write_scale(xml_element, mesh.scale) - - def _parse_geometry(xml_element): - geometry = Geometry() - if xml_element[0].tag == "box": - geometry.box = URDF._parse_box(xml_element[0]) - elif xml_element[0].tag == "cylinder": - geometry.cylinder = URDF._parse_cylinder(xml_element[0]) - elif xml_element[0].tag == "sphere": - geometry.sphere = URDF._parse_sphere(xml_element[0]) - elif xml_element[0].tag == "mesh": - geometry.mesh = URDF._parse_mesh(xml_element[0]) - else: - raise ValueError(f"Unknown tag: {xml_element[0].tag}") - - return geometry - - def _validate_geometry(self, geometry): - if geometry is None: - self._errors.append(URDFIncompleteError(" is missing.")) - - num_nones = sum( - [ - x is not None - for x in [ - geometry.box, - geometry.cylinder, - geometry.sphere, - geometry.mesh, - ] - ] - ) - if num_nones < 1: - self._errors.append( - URDFIncompleteError( - "One of , , , needs to be defined as a child of ." - ) - ) - elif num_nones > 1: - self._errors.append( - URDFError( - "Too many of , , , defined as a child of . Only one allowed." - ) - ) - - def _write_geometry(self, xml_parent, geometry): - if geometry is None: - return - - xml_element = etree.SubElement(xml_parent, "geometry") - if geometry.box is not None: - self._write_box(xml_element, geometry.box) - elif geometry.cylinder is not None: - self._write_cylinder(xml_element, geometry.cylinder) - elif geometry.sphere is not None: - self._write_sphere(xml_element, geometry.sphere) - elif geometry.mesh is not None: - self._write_mesh(xml_element, geometry.mesh) - - def _parse_origin(xml_element): - if xml_element is None: - return None - - xyz = xml_element.get("xyz", default="0 0 0") - rpy = xml_element.get("rpy", default="0 0 0") - - return tra.compose_matrix( - translate=np.array(list(map(float, xyz.split()))), - angles=np.array(list(map(float, rpy.split()))), - ) - - def _write_origin(self, xml_parent, origin): - if origin is None: - return - - etree.SubElement( - xml_parent, - "origin", - attrib={ - "xyz": " ".join(map(str, tra.translation_from_matrix(origin))), - "rpy": " ".join(map(str, tra.euler_from_matrix(origin))), - }, - ) - - def _parse_color(xml_element): - if xml_element is None: - return None - - rgba = xml_element.get("rgba", default="1 1 1 1") - - return Color(rgba=np.array(list(map(float, rgba.split())))) - - def _write_color(self, xml_parent, color): - if color is None: - return - - etree.SubElement(xml_parent, "color", attrib={"rgba": " ".join(map(str, color.rgba))}) - - def _parse_texture(xml_element): - if xml_element is None: - return None - - # TODO: use texture filename handler - return Texture(filename=xml_element.get("filename", default=None)) - - def _write_texture(self, xml_parent, texture): - if texture is None: - return - - # TODO: use texture filename handler - etree.SubElement(xml_parent, "texture", attrib={"filename": texture.filename}) - - def _parse_material(xml_element): - if xml_element is None: - return None - - material = Material(name=xml_element.get("name")) - material.color = URDF._parse_color(xml_element.find("color")) - material.texture = URDF._parse_texture(xml_element.find("texture")) - - return material - - def _write_material(self, xml_parent, material): - if material is None: - return - - attrib = {"name": material.name} if material.name is not None else {} - xml_element = etree.SubElement( - xml_parent, - "material", - attrib=attrib, - ) - - self._write_color(xml_element, material.color) - self._write_texture(xml_element, material.texture) - - def _parse_visual(xml_element): - visual = Visual(name=xml_element.get("name")) - - visual.geometry = URDF._parse_geometry(xml_element.find("geometry")) - visual.origin = URDF._parse_origin(xml_element.find("origin")) - visual.material = URDF._parse_material(xml_element.find("material")) - - return visual - - def _validate_visual(self, visual): - self._validate_geometry(visual.geometry) - - def _write_visual(self, xml_parent, visual): - attrib = {"name": visual.name} if visual.name is not None else {} - xml_element = etree.SubElement( - xml_parent, - "visual", - attrib=attrib, - ) - - self._write_geometry(xml_element, visual.geometry) - self._write_origin(xml_element, visual.origin) - self._write_material(xml_element, visual.material) - - def _parse_collision(xml_element): - collision = Collision(name=xml_element.get("name")) - - collision.geometry = URDF._parse_geometry(xml_element.find("geometry")) - collision.origin = URDF._parse_origin(xml_element.find("origin")) - - return collision - - def _validate_collision(self, collision): - self._validate_geometry(collision.geometry) - - def _write_collision(self, xml_parent, collision): - attrib = {"name": collision.name} if collision.name is not None else {} - xml_element = etree.SubElement( - xml_parent, - "collision", - attrib=attrib, - ) - - self._write_geometry(xml_element, collision.geometry) - self._write_origin(xml_element, collision.origin) - - def _parse_inertia(xml_element): - if xml_element is None: - return None - - x = xml_element - - return np.array( - [ - [ - x.get("ixx", default=1.0), - x.get("ixy", default=0.0), - x.get("ixz", default=0.0), - ], - [ - x.get("ixy", default=0.0), - x.get("iyy", default=1.0), - x.get("iyz", default=0.0), - ], - [ - x.get("ixz", default=0.0), - x.get("iyz", default=0.0), - x.get("izz", default=1.0), - ], - ], - dtype=np.float64, - ) - - def _write_inertia(self, xml_parent, inertia): - if inertia is None: - return None - - etree.SubElement( - xml_parent, - "inertia", - attrib={ - "ixx": str(inertia[0, 0]), - "ixy": str(inertia[0, 1]), - "ixz": str(inertia[0, 2]), - "iyy": str(inertia[1, 1]), - "iyz": str(inertia[1, 2]), - "izz": str(inertia[2, 2]), - }, - ) - - def _parse_mass(xml_element): - if xml_element is None: - return None - - return _str2float(xml_element.get("value", default=0.0)) - - def _write_mass(self, xml_parent, mass): - if mass is None: - return - - etree.SubElement( - xml_parent, - "mass", - attrib={ - "value": str(mass), - }, - ) - - def _parse_inertial(xml_element): - if xml_element is None: - return None - - inertial = Inertial() - inertial.origin = URDF._parse_origin(xml_element.find("origin")) - inertial.inertia = URDF._parse_inertia(xml_element.find("inertia")) - inertial.mass = URDF._parse_mass(xml_element.find("mass")) - - return inertial - - def _write_inertial(self, xml_parent, inertial): - if inertial is None: - return - - xml_element = etree.SubElement(xml_parent, "inertial") - - self._write_origin(xml_element, inertial.origin) - self._write_mass(xml_element, inertial.mass) - self._write_inertia(xml_element, inertial.inertia) - - def _parse_link(xml_element): - link = Link(name=xml_element.attrib["name"]) - - link.inertial = URDF._parse_inertial(xml_element.find("inertial")) - - for v in xml_element.findall("visual"): - link.visuals.append(URDF._parse_visual(v)) - - for c in xml_element.findall("collision"): - link.collisions.append(URDF._parse_collision(c)) - - return link - - def _validate_link(self, link): - self._validate_required_attribute(attribute=link.name, error_msg="The tag misses a 'name' attribute.") - - for v in link.visuals: - self._validate_visual(v) - - for c in link.collisions: - self._validate_collision(c) - - def _write_link(self, xml_parent, link): - xml_element = etree.SubElement( - xml_parent, - "link", - attrib={ - "name": link.name, - }, - ) - - self._write_inertial(xml_element, link.inertial) - for visual in link.visuals: - self._write_visual(xml_element, visual) - for collision in link.collisions: - self._write_collision(xml_element, collision) - - def _parse_axis(xml_element): - if xml_element is None: - return np.array([1.0, 0, 0]) - - xyz = xml_element.get("xyz", "1 0 0") - results = [] - for x in xyz.split(): - try: - x = float(x) - except ValueError: - x = 0 - results.append(x) - return np.array(results) - # return np.array(list(map(float, xyz.split()))) - - def _write_axis(self, xml_parent, axis): - if axis is None: - return - - etree.SubElement(xml_parent, "axis", attrib={"xyz": " ".join(map(str, axis))}) - - def _parse_limit(xml_element): - if xml_element is None: - return None - - return Limit( - effort=_str2float(xml_element.get("effort", default=None)), - velocity=_str2float(xml_element.get("velocity", default=None)), - lower=_str2float(xml_element.get("lower", default=None)), - upper=_str2float(xml_element.get("upper", default=None)), - ) - - def _validate_limit(self, limit, type): - if type in ["revolute", "prismatic"]: - self._validate_required_attribute( - limit, - error_msg="The of a (prismatic, revolute) joint is missing.", - ) - - if limit is not None: - self._validate_required_attribute( - limit.upper, - error_msg="Tag of joint is missing attribute 'upper'.", - ) - self._validate_required_attribute( - limit.lower, - error_msg="Tag of joint is missing attribute 'lower'.", - ) - - if limit is not None: - self._validate_required_attribute( - limit.effort, - error_msg="Tag of joint is missing attribute 'effort'.", - ) - - self._validate_required_attribute( - limit.velocity, - error_msg="Tag of joint is missing attribute 'velocity'.", - ) - - def _write_limit(self, xml_parent, limit): - if limit is None: - return - - attrib = {} - if limit.effort is not None: - attrib["effort"] = str(limit.effort) - if limit.velocity is not None: - attrib["velocity"] = str(limit.velocity) - if limit.lower is not None: - attrib["lower"] = str(limit.lower) - if limit.upper is not None: - attrib["upper"] = str(limit.upper) - - etree.SubElement( - xml_parent, - "limit", - attrib=attrib, - ) - - def _parse_dynamics(xml_element): - if xml_element is None: - return None - - dynamics = Dynamics() - dynamics.damping = xml_element.get("damping", default=None) - dynamics.friction = xml_element.get("friction", default=None) - - return dynamics - - def _write_dynamics(self, xml_parent, dynamics): - if dynamics is None: - return - - attrib = {} - if dynamics.damping is not None: - attrib["damping"] = str(dynamics.damping) - if dynamics.friction is not None: - attrib["friction"] = str(dynamics.friction) - - etree.SubElement( - xml_parent, - "dynamics", - attrib=attrib, - ) - - def _parse_joint(xml_element): - joint = Joint(name=xml_element.attrib["name"]) - - joint.type = xml_element.get("type", default=None) - joint.parent = xml_element.find("parent").get("link") - joint.child = xml_element.find("child").get("link") - joint.origin = URDF._parse_origin(xml_element.find("origin")) - joint.axis = URDF._parse_axis(xml_element.find("axis")) - joint.limit = URDF._parse_limit(xml_element.find("limit")) - joint.dynamics = URDF._parse_dynamics(xml_element.find("dynamics")) - joint.mimic = URDF._parse_mimic(xml_element.find("mimic")) - joint.calibration = URDF._parse_calibration(xml_element.find("calibration")) - joint.safety_controller = URDF._parse_safety_controller(xml_element.find("safety_controller")) - - return joint - - def _validate_joint(self, joint): - self._validate_required_attribute( - attribute=joint.name, - error_msg="The tag misses a 'name' attribute.", - ) - - allowed_types = [ - "revolute", - "continuous", - "prismatic", - "fixed", - "floating", - "planar", - ] - self._validate_required_attribute( - attribute=joint.type, - error_msg=f"The tag misses a 'type' attribute or value is not part of allowed values [{', '.join(allowed_types)}].", - allowed_values=allowed_types, - ) - - self._validate_required_attribute( - joint.parent, - error_msg=f"The of a is missing.", - ) - - self._validate_required_attribute( - joint.child, - error_msg=f"The of a is missing.", - ) - - self._validate_limit(joint.limit, type=joint.type) - - def _write_joint(self, xml_parent, joint): - xml_element = etree.SubElement( - xml_parent, - "joint", - attrib={ - "name": joint.name, - "type": joint.type, - }, - ) - - etree.SubElement(xml_element, "parent", attrib={"link": joint.parent}) - etree.SubElement(xml_element, "child", attrib={"link": joint.child}) - self._write_origin(xml_element, joint.origin) - self._write_axis(xml_element, joint.axis) - self._write_limit(xml_element, joint.limit) - self._write_dynamics(xml_element, joint.dynamics) - - @staticmethod - def _parse_robot(xml_element, add_dummy_free_joints=False): - robot = Robot(name=xml_element.attrib["name"]) - - for l in xml_element.findall("link"): - robot.links.append(URDF._parse_link(l)) - for j in xml_element.findall("joint"): - robot.joints.append(URDF._parse_joint(j)) - for m in xml_element.findall("material"): - robot.materials.append(URDF._parse_material(m)) - - if add_dummy_free_joints: - # Determine root link - link_names = [l.name for l in robot.links] - for j in robot.joints: - link_names.remove(j.child) - - if len(link_names) == 0: - raise RuntimeError(f"No root link found for robot.") - - root_link_name = link_names[0] - _add_dummy_joints(robot, root_link_name) - - return robot - - def _validate_robot(self, robot): - if robot is not None: - self._validate_required_attribute( - attribute=robot.name, - error_msg="The tag misses a 'name' attribute.", - ) - - for l in robot.links: - self._validate_link(l) - - for j in robot.joints: - self._validate_joint(j) - - def _write_robot(self, robot): - xml_element = etree.Element("robot", attrib={"name": robot.name}) - for link in robot.links: - self._write_link(xml_element, link) - for joint in robot.joints: - self._write_joint(xml_element, joint) - for material in robot.materials: - self._write_material(xml_element, material) - - return xml_element - - def __eq__(self, other): - if not isinstance(other, URDF): - raise NotImplemented - return self.robot == other.robot - - @property - def filename_handler(self): - return self._filename_handler - - def build_tree(self): - parent_child_map: Dict[str, List[str]] = {} - for joint in self.robot.joints: - if joint.parent in parent_child_map: - parent_child_map[joint.parent].append(joint.child) - else: - parent_child_map[joint.parent] = [joint.child] - - # Sort link with bfs order - bfs_link_list = [self.base_link] - to_be_handle_list = [self.base_link] - while len(to_be_handle_list) > 0: - parent = to_be_handle_list.pop(0) - if parent not in parent_child_map: - continue - - children = parent_child_map[parent] - to_be_handle_list.extend(children) - bfs_link_list.extend(children) - bfs_joint_list = [] - for link_name in bfs_link_list[1:]: - joint_index = [i for i in range(len(self.robot.joints)) if self.robot.joints[i].child == link_name][0] - bfs_joint_list.append(self.robot.joints[joint_index]) - - # Build tree - root = Node(self.base_link, matrix=np.eye(4)) - for joint in bfs_joint_list: - matrix, _ = self._forward_kinematics_joint(joint, 0) - parent_node = anytree.search.findall_by_attr(root, value=joint.parent)[0] - node = Node(joint.child, parent=parent_node, matrix=matrix) - return root - - def update_kinematics(self, configuration): - joint_cfg = [] - - if isinstance(configuration, dict): - for joint in configuration: - if isinstance(joint, six.string_types): - joint_cfg.append((self._joint_map[joint], configuration[joint])) - elif isinstance(joint, Joint): - # TODO: Joint is not hashable; so this branch will not succeed - joint_cfg.append((joint, configuration[joint])) - elif isinstance(configuration, (list, tuple, np.ndarray)): - if len(configuration) == len(self.robot.joints): - for joint, value in zip(self.robot.joints, configuration): - joint_cfg.append((joint, value)) - elif len(configuration) == self.num_actuated_joints: - for joint, value in zip(self._actuated_joints, configuration): - joint_cfg.append((joint, value)) - else: - raise ValueError( - f"Dimensionality of configuration ({len(configuration)}) doesn't match number of all ({len(self.robot.joints)}) or actuated joints ({self.num_actuated_joints})." - ) - else: - raise TypeError("Invalid type for configuration") - - # append all mimic joints in the update - for j, q in joint_cfg + [(j, 0.0) for j in self.robot.joints if j.mimic is not None]: - matrix, _ = self._forward_kinematics_joint(j, q=q) - node = anytree.search.findall_by_attr(self.tree_root, j.child)[0] - node.matrix = matrix - - for node in LevelOrderIter(self.tree_root): - if node.name == self.base_link: - node.global_pose = np.eye(4) - else: - node.global_pose = node.parent.global_pose @ node.matrix - - def get_link_global_transform(self, link_name): - node = anytree.search.findall_by_attr(self.tree_root, link_name)[0] - - return node.global_pose - - -def _add_dummy_joints(robot: Robot, root_link_name: str): - # Prepare link and joint properties - translation_range = (-5, 5) - rotation_range = (-2 * np.pi, 2 * np.pi) - joint_types = ["prismatic"] * 3 + ["revolute"] * 3 - joint_limit = [translation_range] * 3 + [rotation_range] * 3 - joint_name = DUMMY_JOINT_NAMES.copy() - link_name = [f"dummy_{name}_translation_link" for name in "xyz"] + [f"dummy_{name}_rotation_link" for name in "xyz"] - - links = [] - joints = [] - - for i in range(6): - inertial = Inertial( - mass=0.01, inertia=np.array([[1e-4, 0, 0], [0, 1e-4, 0], [0, 0, 1e-4]]), origin=np.identity(4) - ) - link = Link(name=link_name[i], inertial=inertial) - links.append(link) - - joint_axis = np.zeros(3, dtype=int) - joint_axis[i % 3] = 1 - limit = Limit(lower=joint_limit[i][0], upper=joint_limit[i][1], velocity=3.14, effort=10) - - child_name = link_name[i + 1] if i < 5 else root_link_name - joint = Joint( - name=joint_name[i], - type=joint_types[i], - parent=link_name[i], - child=child_name, - origin=np.identity(4), - axis=joint_axis, - limit=limit, - ) - joints.append(joint) - - robot.joints = joints + robot.joints - robot.links = links + robot.links - - -DUMMY_JOINT_NAMES = [f"dummy_{name}_translation_joint" for name in "xyz"] + [ - f"dummy_{name}_rotation_joint" for name in "xyz" -] diff --git a/teleop/robot_control/hand_retargeting.py b/teleop/robot_control/hand_retargeting.py index 1c67945..27f1d40 100644 --- a/teleop/robot_control/hand_retargeting.py +++ b/teleop/robot_control/hand_retargeting.py @@ -1,4 +1,4 @@ -from .dex_retargeting.retargeting_config import RetargetingConfig +from dex_retargeting import RetargetingConfig from pathlib import Path import yaml from enum import Enum diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index fb04cf1..3061851 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -396,7 +396,7 @@ class Gripper_JointIndex(IntEnum): if __name__ == "__main__": import argparse - from teleop.open_television.tv_wrapper import TeleVisionWrapper + from televuer import TeleVuerWrapper from teleop.image_server.image_client import ImageClient parser = argparse.ArgumentParser() @@ -432,7 +432,7 @@ if __name__ == "__main__": image_receive_thread.start() # television and arm - tv_wrapper = TeleVisionWrapper(BINOCULAR, tv_img_shape, img_shm.name) + tv_wrapper = TeleVuerWrapper(BINOCULAR, tv_img_shape, img_shm.name) if args.dex: left_hand_array = Array('d', 75, lock=True) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index ac9f94a..5ed2c6a 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -14,7 +14,7 @@ current_dir = os.path.dirname(os.path.abspath(__file__)) parent_dir = os.path.dirname(current_dir) sys.path.append(parent_dir) -from teleop.open_television import TeleVisionWrapper +from televuer import TeleVuerWrapper from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController, H1_ArmController from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK, H1_ArmIK from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_Controller @@ -129,8 +129,8 @@ if __name__ == '__main__': image_receive_thread.start() # television: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. - tv_wrapper = TeleVisionWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == 'hand', img_shape=tv_img_shape, img_shm_name=tv_img_shm.name, - return_state_data=True, return_hand_rot_data = False) + tv_wrapper = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == 'hand', img_shape=tv_img_shape, img_shm_name=tv_img_shm.name, + return_state_data=True, return_hand_rot_data = False) # arm if args.arm == 'G1_29': @@ -411,6 +411,8 @@ if __name__ == '__main__': wrist_img_shm.close() if args.record: recorder.close() + if args.sim: + sim_state_subscriber.stop_subscribe() listen_keyboard_thread.join() logger_mp.info("Finally, exiting program...") exit(0) diff --git a/teleop/televuer b/teleop/televuer new file mode 160000 index 0000000..34f4475 --- /dev/null +++ b/teleop/televuer @@ -0,0 +1 @@ +Subproject commit 34f4475fca12166d2c52f2469385a851f614fd4e diff --git a/teleop/utils/data/-home-unitree-Code-unitree-opject-unitree_sim_isaaclab-.txt b/teleop/utils/data/-home-unitree-Code-unitree-opject-unitree_sim_isaaclab-.txt deleted file mode 100644 index 5e55b46..0000000 --- a/teleop/utils/data/-home-unitree-Code-unitree-opject-unitree_sim_isaaclab-.txt +++ /dev/null @@ -1 +0,0 @@ -/home/unitree/Code/unitree-opject/unitree_sim_isaaclab/data \ No newline at end of file diff --git a/teleop/utils/sim_state_topic.py b/teleop/utils/sim_state_topic.py index dc5db54..0dd7947 100644 --- a/teleop/utils/sim_state_topic.py +++ b/teleop/utils/sim_state_topic.py @@ -13,6 +13,9 @@ from typing import Any, Dict, Optional from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize from unitree_sdk2py.idl.std_msgs.msg.dds_ import String_ +import logging_mp +logger_mp = logging_mp.get_logger(__name__) + class SharedMemoryManager: """Shared memory manager""" @@ -56,7 +59,7 @@ class SharedMemoryManager: json_bytes = json_str.encode('utf-8') if len(json_bytes) > self.size - 8: # reserve 8 bytes for length and timestamp - print(f"Warning: Data too large for shared memory ({len(json_bytes)} > {self.size - 8})") + logger_mp.warning(f"Data too large for shared memory ({len(json_bytes)} > {self.size - 8})") return False # write timestamp (4 bytes) and data length (4 bytes) @@ -69,7 +72,7 @@ class SharedMemoryManager: return True except Exception as e: - print(f"Error writing to shared memory: {e}") + logger_mp.error(f"Error writing to shared memory: {e}") return False def read_data(self) -> Optional[Dict[str, Any]]: @@ -94,7 +97,7 @@ class SharedMemoryManager: return data except Exception as e: - print(f"Error reading from shared memory: {e}") + logger_mp.error(f"Error reading from shared memory: {e}") return None def get_name(self) -> str: @@ -136,47 +139,46 @@ class SimStateSubscriber: # initialize shared memory self._setup_shared_memory() - print(f"[SimStateSubscriber] Initialized with shared memory: {shm_name}") + logger_mp.info(f"[SimStateSubscriber] Initialized with shared memory: {shm_name}") def _setup_shared_memory(self): """Setup shared memory""" try: self.shared_memory = SharedMemoryManager(self.shm_name, self.shm_size) - print(f"[SimStateSubscriber] Shared memory setup successfully") + logger_mp.info(f"[SimStateSubscriber] Shared memory setup successfully") except Exception as e: - print(f"[SimStateSubscriber] Failed to setup shared memory: {e}") + logger_mp.error(f"[SimStateSubscriber] Failed to setup shared memory: {e}") def _message_handler(self, msg: String_): """Handle received messages""" try: # parse the received message data = json.loads(msg.data) - # print(f"[SimStateSubscriber] Received message: {data}") # write to shared memory if self.shared_memory: self.shared_memory.write_data(data) except Exception as e: - print(f"[SimStateSubscriber] Error processing message: {e}") + logger_mp.error(f"[SimStateSubscriber] Error processing message: {e}") def _subscribe_loop(self): """Subscribe loop thread""" - print(f"[SimStateSubscriber] Subscribe thread started") + logger_mp.info(f"[SimStateSubscriber] Subscribe thread started") while self.running: try: time.sleep(0.001) # keep thread alive except Exception as e: - print(f"[SimStateSubscriber] Error in subscribe loop: {e}") + logger_mp.error(f"[SimStateSubscriber] Error in subscribe loop: {e}") time.sleep(0.01) - print(f"[SimStateSubscriber] Subscribe thread stopped") + logger_mp.info(f"[SimStateSubscriber] Subscribe thread stopped") def start_subscribe(self): """Start subscribing""" if self.running: - print(f"[SimStateSubscriber] Already running") + logger_mp.info(f"[SimStateSubscriber] Already running") return try: @@ -185,31 +187,34 @@ class SimStateSubscriber: self.subscriber.Init(self._message_handler, 10) self.running = True - print(f"[SimStateSubscriber] Subscriber initialized") + logger_mp.info(f"[SimStateSubscriber] Subscriber initialized") # start subscribe thread self.subscribe_thread = threading.Thread(target=self._subscribe_loop) self.subscribe_thread.daemon = True self.subscribe_thread.start() - print(f"[SimStateSubscriber] Started subscribing to rt/sim_state_cmd") + logger_mp.info(f"[SimStateSubscriber] Started subscribing to rt/sim_state_cmd") except Exception as e: - print(f"[SimStateSubscriber] Failed to start subscribing: {e}") + logger_mp.error(f"[SimStateSubscriber] Failed to start subscribing: {e}") self.running = False def stop_subscribe(self): """Stop subscribing""" if not self.running: return - - print(f"[SimStateSubscriber] Stopping subscriber...") + + logger_mp.info(f"[SimStateSubscriber] Stopping subscriber...") self.running = False # wait for thread to finish if self.subscribe_thread: self.subscribe_thread.join(timeout=1.0) - - print(f"[SimStateSubscriber] Subscriber stopped") + + if self.shared_memory: + # cleanup shared memory + self.shared_memory.cleanup() + logger_mp.info(f"[SimStateSubscriber] Subscriber stopped") def read_data(self) -> Optional[Dict[str, Any]]: """Read data from shared memory @@ -222,7 +227,7 @@ class SimStateSubscriber: return self.shared_memory.read_data() return None except Exception as e: - print(f"[SimStateSubscriber] Error reading data: {e}") + logger_mp.error(f"[SimStateSubscriber] Error reading data: {e}") return None def is_running(self) -> bool: @@ -265,7 +270,7 @@ def start_sim_state_subscribe(shm_name: str = "sim_state_cmd_data", shm_size: in # if __name__ == "__main__": # # example usage -# print("Starting sim state subscriber...") +# logger_mp.info("Starting sim state subscriber...") # ChannelFactoryInitialize(0) # # create and start subscriber # subscriber = start_sim_state_subscribe() @@ -275,11 +280,11 @@ def start_sim_state_subscribe(shm_name: str = "sim_state_cmd_data", shm_size: in # while True: # data = subscriber.read_data() # if data: -# print(f"Read data: {data}") +# logger_mp.info(f"Read data: {data}") # time.sleep(1) # except KeyboardInterrupt: -# print("\nInterrupted by user") +# logger_mp.warning("\nInterrupted by user") # finally: # subscriber.stop_subscribe() -# print("Subscriber stopped") \ No newline at end of file +# logger_mp.info("Subscriber stopped") \ No newline at end of file From 1abb9d95b84adae363442f3d8092f7978fde9e76 Mon Sep 17 00:00:00 2001 From: silencht Date: Mon, 7 Jul 2025 17:55:24 +0800 Subject: [PATCH 31/32] [fix] minor issue --- README.md | 3 ++- README_ja-JP.md | 3 ++- README_zh-CN.md | 3 ++- teleop/utils/rerun_visualizer.py | 1 + 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 0c985f9..6935836 100644 --- a/README.md +++ b/README.md @@ -29,6 +29,7 @@

+ # 🔖 Release Note 1. **Upgraded the Vuer library** to support more XR device modes. The project has been renamed from **`avp_teleoperate`** to **`xr_teleoperate`** to better reflect its broader scope — now supporting not only Apple Vision Pro but also Meta Quest 3 (with controllers) and PICO 4 Ultra Enterprise (with controllers). @@ -134,7 +135,7 @@ For more information, you can refer to [Official Documentation ](https://support # Install unitree_sdk2_python library which handles communication with the robot (tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/unitree_sdk2_python.git (tv) unitree@Host:~$ cd unitree_sdk2_python -(tv) unitree@Host:~$ pip install -e . +(tv) unitree@Host:~/unitree_sdk2_python$ pip install -e . ``` > **Note 1**: The [unitree_dds_wrapper](https://github.com/unitreerobotics/unitree_dds_wrapper) in the original h1_2 branch was a temporary version. It has now been fully migrated to the official Python-based control and communication library: [unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python). diff --git a/README_ja-JP.md b/README_ja-JP.md index d9bda66..f77c001 100644 --- a/README_ja-JP.md +++ b/README_ja-JP.md @@ -28,6 +28,7 @@

+ # 🔖 更新内容 1. **Vuerライブラリをアップグレード**し、より多くのXRデバイスモードに対応しました。これに伴い、プロジェクト名を **`avp_teleoperate`** から **`xr_teleoperate`** に変更しました。従来の Apple Vision Pro に加え、**Meta Quest 3（コントローラー対応）** や **PICO 4 Ultra Enterprise（コントローラー対応）** にも対応しています。 @@ -129,7 +130,7 @@ Ubuntu 20.04と22.04でテスト済みです。他のOSでは設定が異なる # ロボット通信用ライブラリインストール (tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/unitree_sdk2_python.git (tv) unitree@Host:~$ cd unitree_sdk2_python -(tv) unitree@Host:~$ pip install -e . +(tv) unitree@Host:~/unitree_sdk2_python$ pip install -e . ``` > **注1**: 元のh1_2ブランチのunitree_dds_wrapperは暫定版でした。現在は公式Python制御ライブラリunitree_sdk2_pythonに移行済みです。 diff --git a/README_zh-CN.md b/README_zh-CN.md index 315f3f4..36cc857 100644 --- a/README_zh-CN.md +++ b/README_zh-CN.md @@ -29,6 +29,7 @@

+ # 🔖 发布说明 1. 升级 [Vuer](https://github.com/vuer-ai/vuer) 库，扩展了设备支持模式。为更准确反映功能范围，项目由 **avp_teleoperate** 更名为 **xr_teleoperate**，从最初仅支持 Apple Vision Pro，扩展至兼容 Meta Quest 3（含手柄） 与 PICO 4 Ultra Enterprise（含手柄） 等多款 XR 设备。 @@ -129,7 +130,7 @@ # 安装 unitree_sdk2_python 库，该库负责开发设备与机器人之间的通信控制功能 (tv) unitree@Host:~$ git clone https://github.com/unitreerobotics/unitree_sdk2_python.git (tv) unitree@Host:~$ cd unitree_sdk2_python -(tv) unitree@Host:~$ pip install -e . +(tv) unitree@Host:~/unitree_sdk2_python$ pip install -e . ``` > 注意1：原 h1_2 分支中的 [unitree_dds_wrapper](https://github.com/unitreerobotics/unitree_dds_wrapper) 为临时版本，现已全面转换到上述正式的 Python 版控制通信库：[unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python) diff --git a/teleop/utils/rerun_visualizer.py b/teleop/utils/rerun_visualizer.py index 631fd6c..cad4c7b 100644 --- a/teleop/utils/rerun_visualizer.py +++ b/teleop/utils/rerun_visualizer.py @@ -5,6 +5,7 @@ import time import rerun as rr import rerun.blueprint as rrb from datetime import datetime +os.environ["RUST_LOG"] = "error" class RerunEpisodeReader: def __init__(self, task_dir = ".", json_file="data.json"): From 0e4a287a8ca18d6c8e8617a34556e1ebf6078692 Mon Sep 17 00:00:00 2001 From: silencht Date: Mon, 7 Jul 2025 18:06:38 +0800 Subject: [PATCH 32/32] [fix] minor issue --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 6935836..66aca15 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ # 0. 📖 Introduction -This repository implements **teleoperation** control of a **Unitree humanoid robot** using **XR (Extended Reality) devices ** (such as Apple Vision Pro, PICO 4 Ultra Enterprise, or Meta Quest 3). +This repository implements **teleoperation** control of a **Unitree humanoid robot** using **XR (Extended Reality) devices** (such as Apple Vision Pro, PICO 4 Ultra Enterprise, or Meta Quest 3). Here are the required devices and wiring diagram,