From 04c146e59a466a4e7867963a421b2d5026690c7b Mon Sep 17 00:00:00 2001 From: silencht Date: Fri, 25 Oct 2024 14:20:43 +0800 Subject: [PATCH] [update] 1. discard temp dds 2.mono|bino camera support 3. optim hand_unitree 4. more comments --- teleop/image_server/image_client.py | 22 ++ teleop/image_server/image_server.py | 28 ++- teleop/open_television/television.py | 43 +++- teleop/open_television/tv_wrapper.py | 4 +- teleop/robot_control/hand_retargeting.py | 36 ++- teleop/robot_control/robot_arm.py | 130 +++------- teleop/robot_control/robot_hand_unitree.py | 280 ++++++++++++++++----- teleop/teleop_hand_and_arm.py | 64 +++-- 8 files changed, 404 insertions(+), 203 deletions(-) diff --git a/teleop/image_server/image_client.py b/teleop/image_server/image_client.py index 45e4375..8471b3a 100644 --- a/teleop/image_server/image_client.py +++ b/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 diff --git a/teleop/image_server/image_server.py b/teleop/image_server/image_server.py index 6723b23..8eaf439 100644 --- a/teleop/image_server/image_server.py +++ b/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() \ No newline at end of file diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index 2d0a0db..965d888 100644 --- a/teleop/open_television/television.py +++ b/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: diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py index d4ce189..1bcbd85 100644 --- a/teleop/open_television/tv_wrapper.py +++ b/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): diff --git a/teleop/robot_control/hand_retargeting.py b/teleop/robot_control/hand_retargeting.py index 294eb81..97d8437 100644 --- a/teleop/robot_control/hand_retargeting.py +++ b/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() \ No newline at end of file + 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 \ No newline at end of file diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index 2466efe..f1224fe 100644 --- a/teleop/robot_control/robot_arm.py +++ b/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() diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index db845e6..60ec95a 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/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.") \ No newline at end of file + 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) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 75eed7a..37b176c 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 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": [], },