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