[update] 1. discard temp dds 2.mono|bino camera support 3. optim hand_unitree 4. more comments

teleop/image_server/image_client.py

@@ -8,6 +8,20 @@ from multiprocessing import shared_memory
 
 class ImageClient:
     def __init__(self, img_shape = None, img_shm_name = None, image_show = False, server_address = "192.168.123.164", port = 5555, Unit_Test = False):
+        """
+        img_shape: User's expected camera resolution shape (H, W, C). It should match the output of the image service terminal.
+
+        img_shm_name: Shared memory is used to easily transfer images across processes to the Vuer.
+        
+        image_show: Whether to display received images in real time.
+
+        server_address: The ip address to execute the image server script.
+
+        port: The port number to bind to. It should be the same as the image server.
+
+        Unit_Test: When both server and client are True, it can be used to test the image transfer latency, \
+                   network jitter, frame loss rate and other information.
+        """
         self.running = True
         self._image_show = image_show
         self._server_address = server_address
@@ -148,6 +162,14 @@ class ImageClient:
             self._close()
 
 if __name__ == "__main__":
+    # example1
+    # img_shape = (720, 1280, 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(img_shape = img_shape, img_shm_name = img_shm.name)
+    # img_client.receive_process()
+
+    # example2
     # Initialize the client with performance evaluation enabled
     # client = ImageClient(image_show = True, server_address='127.0.0.1', Unit_Test=True) # local test
     client = ImageClient(image_show = True, server_address='192.168.123.164', Unit_Test=False) # deployment test

teleop/image_server/image_server.py

@@ -5,16 +5,32 @@ import struct
 from collections import deque
 
 class ImageServer:
-    def __init__(self, fps = 30, port = 5555, Unit_Test = False):
+    def __init__(self, img_shape = (480, 640 * 2, 3), fps = 30, port = 5555, Unit_Test = False):
+        """
+        img_shape: User's expected camera resolution shape (H, W, C). 
+        
+        p.s.1: 'W' of binocular camera according to binocular width (instead of monocular).
+
+        p.s.2: User expectations are not necessarily the end result. The final img_shape value needs to be determined from \
+               the terminal output (i.e. "Image Resolution: width is ···")
+        
+        fps: user's expected camera frames per second.
+        
+        port: The port number to bind to, where you can receive messages from subscribers.
+        
+        Unit_Test: When both server and client are True, it can be used to test the image transfer latency, \
+                   network jitter, frame loss rate and other information.
+        """
+        self.img_shape = img_shape
         self.fps = fps
         self.port = port
         self.enable_performance_eval = Unit_Test
 
-        # initiate binocular camera: 480 * (640 * 2)
+        # initiate camera
         self.cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
         self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
-        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640 * 2)
-        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.img_shape[1])
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.img_shape[0])
         self.cap.set(cv2.CAP_PROP_FPS, self.fps)
 
         # set ZeroMQ context and socket
@@ -91,6 +107,6 @@ class ImageServer:
             self._close()
 
 if __name__ == "__main__":
-    # server = ImageServer(Unit_Test = True)   # test
-    server = ImageServer(Unit_Test = False)  # deployment
+    # server = ImageServer(img_shape = (720, 640 * 2, 3), Unit_Test = True)   # test
+    server = ImageServer(img_shape = (720, 1280 * 2, 3), Unit_Test = False)  # deployment
     server.send_process()

teleop/open_television/television.py

@@ -11,9 +11,13 @@ Value = context._default_context.Value
 
 
 class TeleVision:
-    def __init__(self, img_shape, img_shm_name, cert_file="./cert.pem", key_file="./key.pem", ngrok=False):
+    def __init__(self, binocular, img_shape, img_shm_name, cert_file="./cert.pem", key_file="./key.pem", ngrok=False):
+        self.binocular = binocular
         self.img_height = img_shape[0]
-        self.img_width  = img_shape[1] // 2
+        if binocular:
+            self.img_width  = img_shape[1] // 2
+        else:
+            self.img_width  = img_shape[1]
 
         if ngrok:
             self.vuer = Vuer(host='0.0.0.0', queries=dict(grid=False), queue_len=3)
@@ -25,8 +29,11 @@ class TeleVision:
 
         existing_shm = shared_memory.SharedMemory(name=img_shm_name)
         self.img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=existing_shm.buf)
-        self.vuer.spawn(start=False)(self.main_image)
-        # self.vuer.spawn(start=False)(self.main_image_stereo)
+
+        if binocular:
+            self.vuer.spawn(start=False)(self.main_image_binocular)
+        else:
+            self.vuer.spawn(start=False)(self.main_image_monocular)
 
         self.left_hand_shared = Array('d', 16, lock=True)
         self.right_hand_shared = Array('d', 16, lock=True)
@@ -60,11 +67,11 @@ class TeleVision:
         except: 
             pass
     
-    async def main_image(self, session, fps=60):
+    async def main_image_binocular(self, session, fps=60):
         session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False)
         while True:
             display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB)
-            aspect_ratio = self.img_width / self.img_height
+            # aspect_ratio = self.img_width / self.img_height
             session.upsert(
                 [
                     ImageBackground(
@@ -98,6 +105,28 @@ class TeleVision:
             # '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):
+        session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False)
+        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)
+
     @property
     def left_hand(self):
         return np.array(self.left_hand_shared[:]).reshape(4, 4, order="F")
@@ -142,7 +171,7 @@ if __name__ == '__main__':
     image_receive_thread.start()
 
     # television
-    tv = TeleVision(img_shape, img_shm.name)
+    tv = TeleVision(True, img_shape, img_shm.name)
     print("vuer unit test program running...")
     print("you can press ^C to interrupt program.")
     while True:

teleop/open_television/tv_wrapper.py

@@ -64,8 +64,8 @@ under (basis) Robot Convention, hand's initial pose convention:
 """
 
 class TeleVisionWrapper:
-    def __init__(self, img_shape, img_shm_name):
-        self.tv = TeleVision(img_shape, img_shm_name)
+    def __init__(self, binocular, img_shape, img_shm_name):
+        self.tv = TeleVision(binocular, img_shape, img_shm_name)
 
     def get_data(self):
 

teleop/robot_control/hand_retargeting.py

@@ -6,17 +6,37 @@ from enum import Enum
 class HandType(Enum):
     INSPIRE_HAND = "../assets/inspire_hand/inspire_hand.yml"
     UNITREE_DEX3 = "../assets/unitree_hand/unitree_dex3.yml"
+    UNITREE_DEX3_Unit_Test = "../../assets/unitree_hand/unitree_dex3.yml"
 
 class HandRetargeting:
     def __init__(self, hand_type: HandType):
-        RetargetingConfig.set_default_urdf_dir('../assets')
+        if hand_type == HandType.UNITREE_DEX3:
+            RetargetingConfig.set_default_urdf_dir('../assets')
+        elif hand_type == HandType.UNITREE_DEX3_Unit_Test:
+            RetargetingConfig.set_default_urdf_dir('../../assets')
+        elif hand_type == HandType.INSPIRE_HAND:
+            RetargetingConfig.set_default_urdf_dir('../assets')
 
-        config_file_path = hand_type.value
+        config_file_path = Path(hand_type.value)
 
-        with Path(config_file_path).open('r') as f:
-            self.cfg = yaml.safe_load(f)
+        try:
+            with config_file_path.open('r') as f:
+                self.cfg = yaml.safe_load(f)
+                
+            if 'left' not in self.cfg or 'right' not in self.cfg:
+                raise ValueError("Configuration file must contain 'left' and 'right' keys.")
 
-        left_retargeting_config = RetargetingConfig.from_dict(self.cfg['left'])
-        right_retargeting_config = RetargetingConfig.from_dict(self.cfg['right'])
-        self.left_retargeting = left_retargeting_config.build()
-        self.right_retargeting = right_retargeting_config.build()
+            left_retargeting_config = RetargetingConfig.from_dict(self.cfg['left'])
+            right_retargeting_config = RetargetingConfig.from_dict(self.cfg['right'])
+            self.left_retargeting = left_retargeting_config.build()
+            self.right_retargeting = right_retargeting_config.build()
+        
+        except FileNotFoundError:
+            print(f"Configuration file not found: {config_file_path}")
+            raise
+        except yaml.YAMLError as e:
+            print(f"YAML error while reading {config_file_path}: {e}")
+            raise
+        except Exception as e:
+            print(f"An error occurred: {e}")
+            raise

teleop/robot_control/robot_arm.py

@@ -1,14 +1,13 @@
 import numpy as np
 import threading
 import time
-
-from unitree_dds_wrapper.idl import unitree_hg
-from unitree_dds_wrapper.publisher import Publisher
-from unitree_dds_wrapper.subscription import Subscription
-
-import struct
 from enum import IntEnum
 
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize # dds
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_                                 # idl
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_
+from unitree_sdk2py.utils.crc import CRC
+
 kTopicLowCommand = "rt/lowcmd"
 kTopicLowState = "rt/lowstate"
 G1_29_Num_Motors = 35
@@ -42,16 +41,6 @@ class G1_29_ArmController:
         self.q_target = np.zeros(14)
         self.tauff_target = np.zeros(14)
 
-        self.msg = unitree_hg.msg.dds_.LowCmd_()
-        self.msg.mode_machine = 3 # g1 is 3, h1_2 is 4
-        self.__packFmtHGLowCmd = '<2B2x' + 'B3x5fI' * 35 + '5I'
-        self.msg.head = [0xFE, 0xEF]
-        
-        self.lowcmd_publisher = Publisher(unitree_hg.msg.dds_.LowCmd_, kTopicLowCommand)
-        self.lowstate_subscriber = Subscription(unitree_hg.msg.dds_.LowState_, kTopicLowState)
-
-        self.lowstate_buffer = DataBuffer()
-
         self.kp_high = 100.0
         self.kd_high = 3.0
         self.kp_low = 80.0
@@ -67,14 +56,29 @@ class G1_29_ArmController:
         self._gradual_start_time = None
         self._gradual_time = None
 
+        # initialize lowcmd publisher and lowstate subscriber
+        ChannelFactoryInitialize(0)
+        self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand, LowCmd_)
+        self.lowcmd_publisher.Init()
+        self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, LowState_)
+        self.lowstate_subscriber.Init()
+        self.lowstate_buffer = DataBuffer()
+
+        # initialize subscribe thread
         self.subscribe_thread = threading.Thread(target=self._subscribe_motor_state)
         self.subscribe_thread.daemon = True
         self.subscribe_thread.start()
 
         while not self.lowstate_buffer.GetData():
             time.sleep(0.01)
-            print("Waiting to subscribe dds...")
-        
+            print("[G1_29_ArmController] Waiting to subscribe dds...")
+
+        # initialize hg's lowcmd msg
+        self.crc = CRC()
+        self.msg = unitree_hg_msg_dds__LowCmd_()
+        self.msg.mode_pr = 0
+        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")
@@ -96,11 +100,9 @@ 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]
-        self.pre_communication()
-        self.lowcmd_publisher.msg = self.msg
-        self.lowcmd_publisher.write()
         print("Lock OK!\n")
 
+        # initialize publish thread
         self.publish_thread = threading.Thread(target=self._ctrl_motor_state)
         self.ctrl_lock = threading.Lock()
         self.publish_thread.daemon = True
@@ -108,61 +110,16 @@ class G1_29_ArmController:
 
         print("Initialize G1_29_ArmController OK!\n")
 
-    def __Trans(self, packData):
-        calcData = []
-        calcLen = ((len(packData)>>2)-1)
-
-        for i in range(calcLen):
-            d = ((packData[i*4+3] << 24) | (packData[i*4+2] << 16) | (packData[i*4+1] << 8) | (packData[i*4]))
-            calcData.append(d)
-
-        return calcData
-    
-    def __Crc32(self, data):
-        bit = 0
-        crc = 0xFFFFFFFF
-        polynomial = 0x04c11db7
-
-        for i in range(len(data)):
-            bit = 1 << 31
-            current = data[i]
-
-            for b in range(32):
-                if crc & 0x80000000:
-                    crc = (crc << 1) & 0xFFFFFFFF
-                    crc ^= polynomial
-                else:
-                    crc = (crc << 1) & 0xFFFFFFFF
-
-                if current & bit:
-                    crc ^= polynomial
-
-                bit >>= 1
-        
-        return crc
-    
-    def __pack_crc(self):
-        origData = []
-        origData.append(self.msg.mode_pr)
-        origData.append(self.msg.mode_machine)
-
-        for i in range(35):
-            origData.append(self.msg.motor_cmd[i].mode)
-            origData.append(self.msg.motor_cmd[i].q)
-            origData.append(self.msg.motor_cmd[i].dq)
-            origData.append(self.msg.motor_cmd[i].tau)
-            origData.append(self.msg.motor_cmd[i].kp)
-            origData.append(self.msg.motor_cmd[i].kd)
-            origData.append(self.msg.motor_cmd[i].reserve)
-
-        origData.extend(self.msg.reserve)
-        origData.append(self.msg.crc)
-        calcdata = struct.pack(self.__packFmtHGLowCmd, *origData)
-        calcdata =  self.__Trans(calcdata)
-        self.msg.crc = self.__Crc32(calcdata)
-
-    def pre_communication(self):
-        self.__pack_crc()
+    def _subscribe_motor_state(self):
+        while True:
+            msg = self.lowstate_subscriber.Read()
+            if msg is not None:
+                lowstate = G1_29_LowState()
+                for id in range(G1_29_Num_Motors):
+                    lowstate.motor_state[id].q  = msg.motor_state[id].q
+                    lowstate.motor_state[id].dq = msg.motor_state[id].dq
+                self.lowstate_buffer.SetData(lowstate)
+            time.sleep(0.002)
 
     def clip_arm_q_target(self, target_q, velocity_limit):
         current_q = self.get_current_dual_arm_q()
@@ -186,9 +143,8 @@ class G1_29_ArmController:
                 self.msg.motor_cmd[id].dq = 0
                 self.msg.motor_cmd[id].tau = arm_tauff_target[idx]      
 
-            self.pre_communication()
-            self.lowcmd_publisher.msg = self.msg
-            self.lowcmd_publisher.write()
+            self.msg.crc = self.crc.Crc(self.msg)
+            self.lowcmd_publisher.Write(self.msg)
 
             if self._speed_gradual_max is True:
                 t_elapsed = start_time - self._gradual_start_time
@@ -206,7 +162,11 @@ class G1_29_ArmController:
         with self.ctrl_lock:
             self.q_target = q_target
             self.tauff_target = tauff_target
-            
+
+    def get_mode_machine(self):
+        '''Return current dds mode machine.'''
+        return self.lowstate_subscriber.Read().mode_machine
+    
     def get_current_motor_q(self):
         '''Return current state q of all body motors.'''
         return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in G1_29_JointIndex])
@@ -219,16 +179,6 @@ class G1_29_ArmController:
         '''Return current state dq of the left and right arm motors.'''
         return np.array([self.lowstate_buffer.GetData().motor_state[id].dq for id in G1_29_JointArmIndex])
 
-    def _subscribe_motor_state(self):
-        while True:
-            if self.lowstate_subscriber.msg:
-                lowstate = G1_29_LowState()
-                for id in range(G1_29_Num_Motors):
-                    lowstate.motor_state[id].q  = self.lowstate_subscriber.msg.motor_state[id].q
-                    lowstate.motor_state[id].dq = self.lowstate_subscriber.msg.motor_state[id].dq
-                self.lowstate_buffer.SetData(lowstate)
-            time.sleep(0.002)
-
     def speed_gradual_max(self, t = 5.0):
         '''Parameter t is the total time required for arms velocity to gradually increase to its maximum value, in seconds. The default is 5.0.'''
         self._gradual_start_time = time.time()

teleop/robot_control/robot_hand_unitree.py

@@ -1,97 +1,249 @@
-from unitree_dds_wrapper.robots.trihand.trihand_pub_cmd import UnitreeTrihand as trihand_pub
-from unitree_dds_wrapper.robots.trihand.trihand_sub_state import UnitreeTrihand as trihand_sub
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize # dds
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_                               # idl
+from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandCmd_
 
 import numpy as np
+from enum import IntEnum
 import time
-from multiprocessing import Array
-from teleop.robot_control.hand_retargeting import HandRetargeting, HandType
+import os
+import sys
 import threading
+from multiprocessing import Process, shared_memory, Array, Lock
+
+parent2_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+sys.path.append(parent2_dir)
+from teleop.robot_control.hand_retargeting import HandRetargeting, HandType
+
 
 unitree_tip_indices = [4, 9, 14]
 
+Dex3_Num_Motors = 7
+kTopicDex3LeftCommand = "rt/dex3/left/cmd"
+kTopicDex3RightCommand = "rt/dex3/right/cmd"
+kTopicDex3LeftState = "rt/dex3/left/state"
+kTopicDex3RightState = "rt/dex3/right/state"
+
+
 class Dex3_1_Controller:
-    def __init__(self, fps = 100.0):
-        self.dex3_pub = trihand_pub()
-
-        kp = np.full(7, 1.5)
-        kd = np.full(7, 0.2)
-        q = np.full(7,0.0)
-        dq = np.full(7,0.0)
-        tau = np.full(7,0.0)
-        self.dex3_pub.left_hand.kp = kp
-        self.dex3_pub.left_hand.kd = kd
-        self.dex3_pub.left_hand.q = q 
-        self.dex3_pub.left_hand.dq = dq
-        self.dex3_pub.left_hand.tau = tau
-
-        self.dex3_pub.right_hand.kp = kp
-        self.dex3_pub.right_hand.kd = kd
-        self.dex3_pub.right_hand.q = q
-        self.dex3_pub.right_hand.dq = dq
-        self.dex3_pub.right_hand.tau = tau
-
-        self.dual_hand_state_array = [0.0] * 14
-        self.lr_hand_state_lock = threading.Lock()
-        # self.dual_hand_state_array = Array('d', 14, lock=True)
-
-        self.sub_state = trihand_sub()
-        self.sub_state.wait_for_connection()
-
-        self.subscribe_state_thread = threading.Thread(target=self.subscribe_state)
+    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):
+        """
+        [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
+
+        right_hand_array: [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_action_array: [output] Return left(7), right(7) hand motor action
+
+        fps: Control frequency
+
+        Unit_Test: Whether to enable unit testing
+        """
+        print("Initialize Dex3_1_Controller...")
+
+        self.fps = fps
+        self.Unit_Test = Unit_Test
+        if not self.Unit_Test:
+            self.hand_retargeting = HandRetargeting(HandType.UNITREE_DEX3)
+        else:
+            self.hand_retargeting = HandRetargeting(HandType.UNITREE_DEX3_Unit_Test)
+            ChannelFactoryInitialize(0)
+
+        # initialize handcmd publisher and handstate subscriber
+        self.LeftHandCmb_publisher = ChannelPublisher(kTopicDex3LeftCommand, HandCmd_)
+        self.LeftHandCmb_publisher.Init()
+        self.RightHandCmb_publisher = ChannelPublisher(kTopicDex3RightCommand, HandCmd_)
+        self.RightHandCmb_publisher.Init()
+
+        self.LeftHandState_subscriber = ChannelSubscriber(kTopicDex3LeftState, HandState_)
+        self.LeftHandState_subscriber.Init()
+        self.RightHandState_subscriber = ChannelSubscriber(kTopicDex3RightState, HandState_)
+        self.RightHandState_subscriber.Init()
+
+        # Shared Arrays for hand states
+        self.left_hand_state_array  = Array('d', Dex3_Num_Motors, lock=True)  
+        self.right_hand_state_array = Array('d', Dex3_Num_Motors, lock=True)
+
+        # initialize subscribe thread
+        self.subscribe_state_thread = threading.Thread(target=self._subscribe_hand_state)
         self.subscribe_state_thread.daemon = True
         self.subscribe_state_thread.start()
 
-        self.hand_retargeting = HandRetargeting(HandType.UNITREE_DEX3)
+        while True:
+            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...")
 
-        self.running = True
-        self.fps = fps
+        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("UnitreeDex3 Controller init ok.\n")
+        print("Initialize Dex3_1_Controller OK!\n")
 
-    def subscribe_state(self):
+    def _subscribe_hand_state(self):
         while True:
-            lq,rq= self.sub_state.sub()
-            with self.lr_hand_state_lock:
-                self.dual_hand_state_array[:] = np.concatenate((lq, rq))
-            # self.dual_hand_state_array[:] = np.concatenate((lq,rq))
+            left_hand_msg  = self.LeftHandState_subscriber.Read()
+            right_hand_msg = self.RightHandState_subscriber.Read()
+            if left_hand_msg is not None and right_hand_msg is not None:
+                # Update left hand state
+                for idx, id in enumerate(Dex3_1_Left_JointIndex):
+                    self.left_hand_state_array[idx] = left_hand_msg.motor_state[id].q
+                # Update right hand state
+                for idx, id in enumerate(Dex3_1_Right_JointIndex):
+                    self.right_hand_state_array[idx] = right_hand_msg.motor_state[id].q
             time.sleep(0.002)
-            
-
-    def ctrl(self, left_angles, right_angles):
+    
+    class _RIS_Mode:
+        def __init__(self, id=0, status=0x01, timeout=0):
+            self.motor_mode = 0
+            self.id = id & 0x0F  # 4 bits for id
+            self.status = status & 0x07  # 3 bits for status
+            self.timeout = timeout & 0x01  # 1 bit for timeout
+
+        def _mode_to_uint8(self):
+            self.motor_mode |= (self.id & 0x0F)
+            self.motor_mode |= (self.status & 0x07) << 4
+            self.motor_mode |= (self.timeout & 0x01) << 7
+            return self.motor_mode
+
+    def ctrl_dual_hand(self, left_q_target, right_q_target):
         """set current left, right hand motor state target q"""
-        self.dex3_pub.left_hand.q  = left_angles
-        self.dex3_pub.right_hand.q  = right_angles
-        self.dex3_pub.pub()
-        # print("hand ctrl publish ok.")
+        for idx, id in enumerate(Dex3_1_Left_JointIndex):
+            self.left_msg.motor_cmd[id].q = left_q_target[idx]
+        for idx, id in enumerate(Dex3_1_Right_JointIndex):
+            self.right_msg.motor_cmd[id].q = right_q_target[idx]
 
-    def get_current_dual_hand_q(self):
-        """return current left, right hand motor state q"""
-        with self.lr_hand_state_lock:
-            return self.dual_hand_state_array[:].copy()
+        self.LeftHandCmb_publisher.Write(self.left_msg)
+        self.RightHandCmb_publisher.Write(self.right_msg)
+        # print("hand ctrl publish ok.")
     
-    def control_process(self, left_hand_array, right_hand_array, dual_hand_state_array = None, dual_hand_aciton_array = None):
-        left_qpos  = np.full(7, 0)
-        right_qpos = np.full(7, 0)
+    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):
+        self.running = True
+
+        left_q_target  = np.full(Dex3_Num_Motors, 0)
+        right_q_target = np.full(Dex3_Num_Motors, 0)
+
+        q = 0.0
+        dq = 0.0
+        tau = 0.0
+        kp = 1.5
+        kd = 0.2
+
+        # initialize dex3-1's left hand cmd msg
+        self.left_msg  = unitree_hg_msg_dds__HandCmd_()
+        for id in Dex3_1_Left_JointIndex:
+            ris_mode = self._RIS_Mode(id = id, status = 0x01)
+            motor_mode = ris_mode._mode_to_uint8()
+            self.left_msg.motor_cmd[id].mode = motor_mode
+            self.left_msg.motor_cmd[id].q    = q
+            self.left_msg.motor_cmd[id].dq   = dq
+            self.left_msg.motor_cmd[id].tau  = tau
+            self.left_msg.motor_cmd[id].kp   = kp
+            self.left_msg.motor_cmd[id].kd   = kd
+
+        # initialize dex3-1's right hand cmd msg
+        self.right_msg = unitree_hg_msg_dds__HandCmd_()
+        for id in Dex3_1_Right_JointIndex:
+            ris_mode = self._RIS_Mode(id = id, status = 0x01)
+            motor_mode = ris_mode._mode_to_uint8()
+            self.right_msg.motor_cmd[id].mode = motor_mode  
+            self.right_msg.motor_cmd[id].q    = q
+            self.right_msg.motor_cmd[id].dq   = dq
+            self.right_msg.motor_cmd[id].tau  = tau
+            self.right_msg.motor_cmd[id].kp   = kp
+            self.right_msg.motor_cmd[id].kd   = kd  
+
         try:
             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()
 
-                is_initial = np.all(left_hand_mat == 0.0)
-                if not is_initial:
-                    left_qpos  = self.hand_retargeting.left_retargeting.retarget(left_hand_mat[unitree_tip_indices])[[0,1,2,3,4,5,6]]
-                    right_qpos = self.hand_retargeting.right_retargeting.retarget(right_hand_mat[unitree_tip_indices])[[0,1,2,3,4,5,6]]
+                # 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[:])))
 
-                self.ctrl(left_qpos, right_qpos)
+                is_hand_data_initialized = not np.all(left_hand_mat == 0.0)
+                if is_hand_data_initialized:
+                    left_q_target  = self.hand_retargeting.left_retargeting.retarget(left_hand_mat[unitree_tip_indices])[[0,1,2,3,4,5,6]]
+                    right_q_target = self.hand_retargeting.right_retargeting.retarget(right_hand_mat[unitree_tip_indices])[[0,1,2,3,4,5,6]]
 
-                if dual_hand_state_array and dual_hand_aciton_array:
-                    dual_hand_state_array[:] = self.get_current_dual_hand_q()
-                    dual_hand_aciton_array[:] = np.concatenate((left_qpos, right_qpos))
+                # get dual hand action
+                action_data = np.concatenate((left_q_target, right_q_target))    
+                if dual_hand_state_array and dual_hand_action_array:
+                    with dual_hand_data_lock:
+                        dual_hand_state_array[:] = state_data
+                        dual_hand_action_array[:] = action_data
 
+                self.ctrl_dual_hand(left_q_target, right_q_target)
                 current_time = time.time()
                 time_elapsed = current_time - start_time
                 sleep_time = max(0, (1 / self.fps) - time_elapsed)
                 time.sleep(sleep_time)
         finally:
-            print("Dex3_1_Controller has been closed.")
+            print("Dex3_1_Controller has been closed.")
+
+class Dex3_1_Left_JointIndex(IntEnum):
+    kLeftHandZero = 0   # thumb_0
+    kLeftHandOne = 1    # thumb_1
+    kLeftHandTwo = 2    # thumb_2
+    kLeftHandFive = 3   # middle_0
+    kLeftHandSix = 4    # middle_0
+    kLeftHandThree = 5  # index_0
+    kLeftHandFour = 6   # index_1
+
+class Dex3_1_Right_JointIndex(IntEnum):
+    kRightHandZero = 0   # thumb_0
+    kRightHandOne = 1    # thumb_1
+    kRightHandTwo = 2    # thumb_2
+    kRightHandThree = 3  # index_0
+    kRightHandFour = 4   # index_1
+    kRightHandFive = 5   # middle_0
+    kRightHandSix = 6    # middle_1
+
+
+if __name__ == "__main__":
+    import argparse
+    from teleop.open_television.tv_wrapper import TeleVisionWrapper
+    from teleop.image_server.image_client import ImageClient
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--binocular', action='store_true', help='Use binocular camera')
+    parser.add_argument('--monocular', dest='binocular', action='store_false', help='Use monocular camera')
+    parser.set_defaults(binocular=True)
+    args = parser.parse_args()
+    print(f"args:{args}\n")
+
+    # image
+    img_shape = (720, 1280, 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(img_shape = img_shape, img_shm_name = img_shm.name)
+    image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True)
+    image_receive_thread.daemon = True
+    image_receive_thread.start()
+
+    # television and arm
+    tv_wrapper = TeleVisionWrapper(args.binocular, img_shape, img_shm.name)
+
+    left_hand_array = Array('d', 75, lock=True)
+    right_hand_array = Array('d', 75, lock=True)
+    dual_hand_data_lock = Lock()
+    dual_hand_state_array = Array('d', 14, lock=False)  # current left, right hand state(14) data.
+    dual_hand_action_array = Array('d', 14, lock=False) # 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, Unit_Test = True)
+
+
+    user_input = input("Please enter the start signal (enter 's' to start the subsequent program):\n")
+    if user_input.lower() == 's':
+        while True:
+            print(f"{dual_hand_state_array[1]}")
+            time.sleep(0.1)

teleop/teleop_hand_and_arm.py

@@ -2,7 +2,7 @@ import numpy as np
 import time
 import argparse
 import cv2
-from multiprocessing import Process, shared_memory, Array
+from multiprocessing import shared_memory, Array, Lock
 import threading
 
 import os 
@@ -21,14 +21,21 @@ from teleop.utils.episode_writer import EpisodeWriter
 
 if __name__ == '__main__':
     parser = argparse.ArgumentParser()
-    parser.add_argument('--record', type=bool, default=True, help='save data or not')
     parser.add_argument('--task_dir', type=str, default='data', help='path to save data')
     parser.add_argument('--frequency', type=int, 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('--binocular', action='store_true', help='Use binocular camera')
+    parser.add_argument('--monocular', dest='binocular', action='store_false', help='Use monocular camera')
+    parser.set_defaults(binocular=True)
     args = parser.parse_args()
     print(f"args:{args}\n")
 
     # image
-    img_shape = (480, 640 * 2, 3)
+    img_shape = (720, 2560, 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(img_shape = img_shape, img_shm_name = img_shm.name)
@@ -37,19 +44,17 @@ if __name__ == '__main__':
     image_receive_thread.start()
 
     # television and arm
-    tv_wrapper = TeleVisionWrapper(img_shape, img_shm.name)
+    tv_wrapper = TeleVisionWrapper(args.binocular, img_shape, img_shm.name)
     arm_ctrl = G1_29_ArmController()
     arm_ik = G1_29_ArmIK()
 
     # hand
-    hand_ctrl = Dex3_1_Controller()
-    left_hand_array = Array('d', 75, lock=True)
-    right_hand_array = Array('d', 75, lock=True)
-    dual_hand_state_array  = Array('d', 14, lock=True) # current left, right hand state data
-    dual_hand_aciton_array = Array('d', 14, lock=True) # current left and right hand action data to be controlled
-    hand_control_process = Process(target=hand_ctrl.control_process, args=(left_hand_array, right_hand_array, dual_hand_state_array, dual_hand_aciton_array))
-    hand_control_process.daemon = True
-    hand_control_process.start()
+    left_hand_array = Array('d', 75, lock=True)         # [input]
+    right_hand_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)
     
     if args.record:
         recorder = EpisodeWriter(task_dir=args.task_dir, frequency=args.frequency)
@@ -71,10 +76,12 @@ if __name__ == '__main__':
                 left_hand_array[:] = left_hand.flatten()
                 right_hand_array[:] = right_hand.flatten()
 
-                # get current arm motor data. solve ik using motor data and wrist pose, then use ik results to control arms.
-                time_ik_start = time.time()
+                # get current 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(left_wrist, right_wrist, 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)}")
@@ -82,18 +89,18 @@ if __name__ == '__main__':
 
                 # record data
                 if args.record:
+                    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:]
+
                     current_image = img_array.copy()
-                    left_image =  current_image[:, :640]
-                    right_image = current_image[:, 640:]
                     left_arm_state  = current_lr_arm_q[:7]
                     right_arm_state = current_lr_arm_q[-7:]
-                    left_hand_state = dual_hand_state_array[:7]
-                    right_hand_state = dual_hand_state_array[-7:]
                     left_arm_action = sol_q[:7]
                     right_arm_action = sol_q[-7:]
-                    left_hand_action = dual_hand_aciton_array[:7]
-                    right_hand_action = dual_hand_aciton_array[-7:]
-        
+
                     cv2.imshow("record image", current_image)
                     key = cv2.waitKey(1) & 0xFF
                     if key == ord('q'):
@@ -101,19 +108,24 @@ if __name__ == '__main__':
                     elif key == ord('s'):
                         press_key_s_count += 1
                         if press_key_s_count % 2 == 1:
-                            print("Start recording...")
+                            print("==> Start recording...")
                             recording = True
                             recorder.create_episode()
+                            print("==> Create episode ok.")
                         else:
-                            print("End recording...")
+                            print("==> End recording...")
                             recording = False
                             recorder.save_episode()
+                            print("==> Save episode ok.")
 
                     if recording:
                         colors = {}
                         depths = {}
-                        colors[f"color_{0}"] = left_image
-                        colors[f"color_{1}"] = right_image
+                        if args.binocular:
+                            colors[f"color_{0}"] = current_image[:, :img_shape[1]//2]
+                            colors[f"color_{1}"] = current_image[:, img_shape[1]//2:]
+                        else:
+                            colors[f"color_{0}"] = current_image
                         states = {
                             "left_arm": {                                                                    
                                 "qpos":   left_arm_state.tolist(),    # numpy.array -> list
@@ -126,7 +138,7 @@ if __name__ == '__main__':
                                 "torque": [],                         
                             },                        
                             "left_hand": {                                                                    
-                                "qpos":   left_hand_state,            # Array returns a list after slicing
+                                "qpos":   left_hand_state,           
                                 "qvel":   [],                           
                                 "torque": [],                          
                             },