diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index 347d618..1870fdf 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -232,7 +232,7 @@ Gripper_Num_Motors = 2 kTopicGripperCommand = "rt/unitree_actuator/cmd" kTopicGripperState = "rt/unitree_actuator/state" -class Gripper_Controller: +class Dex1_1_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, simulation_mode = False): """ @@ -253,7 +253,7 @@ class Gripper_Controller: Unit_Test: Whether to enable unit testing """ - logger_mp.info("Initialize Gripper_Controller...") + logger_mp.info("Initialize Dex1_1_Gripper_Controller...") self.fps = fps self.Unit_Test = Unit_Test @@ -287,15 +287,15 @@ class Gripper_Controller: if any(state != 0.0 for state in self.dual_gripper_state): break time.sleep(0.01) - logger_mp.warning("[Gripper_Controller] Waiting to subscribe dds...") - logger_mp.info("[Gripper_Controller] Subscribe dds ok.") + logger_mp.warning("[Dex1_1_Gripper_Controller] Waiting to subscribe dds...") + logger_mp.info("[Dex1_1_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)) self.gripper_control_thread.daemon = True self.gripper_control_thread.start() - logger_mp.info("Initialize Gripper_Controller OK!\n") + logger_mp.info("Initialize Dex1_1_Gripper_Controller OK!\n") def _subscribe_gripper_state(self): while True: @@ -394,7 +394,7 @@ class Gripper_Controller: sleep_time = max(0, (1 / self.fps) - time_elapsed) time.sleep(sleep_time) finally: - logger_mp.info("Gripper_Controller has been closed.") + logger_mp.info("Dex1_1_Gripper_Controller has been closed.") class Gripper_JointIndex(IntEnum): kLeftGripper = 0 @@ -454,7 +454,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_hand_array, right_hand_array, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, Unit_Test = True) + gripper_ctrl = Dex1_1_Gripper_Controller(left_hand_array, right_hand_array, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, Unit_Test = True) user_input = input("Please enter the start signal (enter 's' to start the subsequent program):\n") diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 4f04899..62fc2ff 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -17,7 +17,7 @@ sys.path.append(parent_dir) 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 +from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Dex1_1_Gripper_Controller from teleop.robot_control.robot_hand_inspire import Inspire_Controller from teleop.robot_control.robot_hand_brainco import Brainco_Controller from teleop.image_server.image_client import ImageClient @@ -60,7 +60,7 @@ if __name__ == '__main__': # basic control parameters 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', 'brainco'], help='Select end effector controller') + parser.add_argument('--ee', type=str, choices=['dex1', 'dex3', 'inspire1', 'brainco'], help='Select end effector controller') # mode flags parser.add_argument('--record', action = 'store_true', help = 'Enable data recording') parser.add_argument('--motion', action = 'store_true', help = 'Enable motion control mode') @@ -131,20 +131,20 @@ 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 = TeleVuerWrapper(binocular=BINOCULAR, use_hand_tracking=args.xr_mode == 'hand', img_shape=tv_img_shape, img_shm_name=tv_img_shm.name, + 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': + if args.arm == "G1_29": arm_ctrl = G1_29_ArmController(motion_mode=args.motion, simulation_mode=args.sim) arm_ik = G1_29_ArmIK() - elif args.arm == 'G1_23': + elif args.arm == "G1_23": arm_ctrl = G1_23_ArmController(simulation_mode=args.sim) arm_ik = G1_23_ArmIK() - elif args.arm == 'H1_2': + elif args.arm == "H1_2": arm_ctrl = H1_2_ArmController(simulation_mode=args.sim) arm_ik = H1_2_ArmIK() - elif args.arm == 'H1': + elif args.arm == "H1": arm_ctrl = H1_ArmController(simulation_mode=args.sim) arm_ik = H1_ArmIK() @@ -155,28 +155,28 @@ if __name__ == '__main__': 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_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) - elif args.ee == "gripper": + 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, simulation_mode=args.sim) + elif args.ee == "dex1": 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_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, simulation_mode=args.sim) + gripper_ctrl = Dex1_1_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] 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_pos_array, right_hand_pos_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, simulation_mode=args.sim) elif args.ee == "brainco": 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 = Brainco_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) + hand_ctrl = Brainco_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array, simulation_mode=args.sim) else: pass @@ -188,7 +188,7 @@ if __name__ == '__main__': sim_state_subscriber = start_sim_state_subscribe() # controller + motion mode - if args.xr_mode == 'controller' and args.motion: + 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) @@ -237,17 +237,17 @@ if __name__ == '__main__': 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' or args.ee == 'brainco') and args.xr_mode == 'hand': + if (args.ee == "dex3" or args.ee == "inspire1" or args.ee == "brainco") 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': + elif args.ee == "dex1" 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': + elif args.ee == "dex1" 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(): @@ -256,7 +256,7 @@ if __name__ == '__main__': pass # high level control - if args.xr_mode == 'controller' and args.motion: + if args.xr_mode == "controller" and args.motion: # quit teleoperate if tele_data.tele_state.right_aButton: running = False @@ -282,7 +282,7 @@ if __name__ == '__main__': # record data if args.record: # dex hand or gripper - if args.ee == "dex3" and args.xr_mode == 'hand': + 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:] @@ -290,7 +290,7 @@ if __name__ == '__main__': right_hand_action = dual_hand_action_array[-7:] current_body_state = [] current_body_action = [] - elif args.ee == "gripper" and args.xr_mode == 'hand': + elif args.ee == "dex1" 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]] @@ -298,7 +298,7 @@ if __name__ == '__main__': right_hand_action = [dual_gripper_action_array[0]] current_body_state = [] current_body_action = [] - elif args.ee == "gripper" and args.xr_mode == 'controller': + elif args.ee == "dex1" 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]] @@ -308,7 +308,7 @@ if __name__ == '__main__': 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" or args.ee == 'brainco') and args.xr_mode == 'hand': + elif (args.ee == "inspire1" or args.ee == "brainco") 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:]