diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index d9f3f35..cdd24c1 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -59,12 +59,12 @@ class DataBuffer: self.data = data class G1_29_ArmController: - def __init__(self, debug_mode = True): + def __init__(self, debug_mode = True, is_use_sim = False): logger_mp.info("Initialize G1_29_ArmController...") self.q_target = np.zeros(14) self.tauff_target = np.zeros(14) self.debug_mode = debug_mode - + self.is_use_sim = is_use_sim self.kp_high = 300.0 self.kd_high = 3.0 self.kp_low = 80.0 @@ -169,7 +169,10 @@ class G1_29_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + if self.is_use_sim: + cliped_arm_q_target = arm_q_target + else: + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) for idx, id in enumerate(G1_29_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] @@ -335,7 +338,9 @@ class G1_29_JointIndex(IntEnum): kNotUsedJoint5 = 34 class G1_23_ArmController: - def __init__(self): + def __init__(self, is_use_sim = False): + self.is_use_sim = is_use_sim + logger_mp.info("Initialize G1_23_ArmController...") self.q_target = np.zeros(10) self.tauff_target = np.zeros(10) @@ -438,7 +443,10 @@ class G1_23_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + if self.is_use_sim: + cliped_arm_q_target = arm_q_target + else: + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) for idx, id in enumerate(G1_23_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] @@ -592,7 +600,9 @@ class G1_23_JointIndex(IntEnum): kNotUsedJoint5 = 34 class H1_2_ArmController: - def __init__(self): + def __init__(self, is_use_sim = False): + self.is_use_sim = is_use_sim + logger_mp.info("Initialize H1_2_ArmController...") self.q_target = np.zeros(14) self.tauff_target = np.zeros(14) @@ -695,7 +705,10 @@ class H1_2_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + if self.is_use_sim: + cliped_arm_q_target = arm_q_target + else: + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) for idx, id in enumerate(H1_2_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] @@ -856,7 +869,9 @@ class H1_2_JointIndex(IntEnum): kNotUsedJoint7 = 34 class H1_ArmController: - def __init__(self): + def __init__(self, is_use_sim = False): + self.is_use_sim = is_use_sim + logger_mp.info("Initialize H1_ArmController...") self.q_target = np.zeros(8) self.tauff_target = np.zeros(8) @@ -951,7 +966,10 @@ class H1_ArmController: arm_q_target = self.q_target arm_tauff_target = self.tauff_target - cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) + if self.is_use_sim: + cliped_arm_q_target = arm_q_target + else: + cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit) for idx, id in enumerate(H1_JointArmIndex): self.msg.motor_cmd[id].q = cliped_arm_q_target[idx] diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index a7f575f..909801d 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -236,7 +236,7 @@ kTopicGripperState = "rt/unitree_actuator/state" class Gripper_Controller: def __init__(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None, - filter = True, fps = 200.0, Unit_Test = False): + filter = True, fps = 200.0, Unit_Test = False, is_use_sim = False): """ [note] A *_array type parameter requires using a multiprocessing Array, because it needs to be passed to the internal child process @@ -259,6 +259,8 @@ class Gripper_Controller: self.fps = fps self.Unit_Test = Unit_Test + self.is_use_sim = is_use_sim + if filter: self.smooth_filter = WeightedMovingFilter(np.array([0.5, 0.3, 0.2]), Gripper_Num_Motors) else: @@ -314,8 +316,10 @@ class Gripper_Controller: def control_thread(self, left_gripper_value_in, right_gripper_value_in, dual_gripper_state_in, dual_hand_data_lock = None, dual_gripper_state_out = None, dual_gripper_action_out = None): self.running = True - - DELTA_GRIPPER_CMD = 0.18 # The motor rotates 5.4 radians, the clamping jaw slide open 9 cm, so 0.6 rad <==> 1 cm, 0.18 rad <==> 3 mm + if self.is_use_sim: + DELTA_GRIPPER_CMD = 1.0 + else: + DELTA_GRIPPER_CMD = 0.18 # The motor rotates 5.4 radians, the clamping jaw slide open 9 cm, so 0.6 rad <==> 1 cm, 0.18 rad <==> 3 mm THUMB_INDEX_DISTANCE_MIN = 5.0 THUMB_INDEX_DISTANCE_MAX = 7.0 LEFT_MAPPED_MIN = 0.0 # The minimum initial motor position when the gripper closes at startup. diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 86a9479..461b9f9 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -53,19 +53,35 @@ if __name__ == '__main__': parser.add_argument('--debug', action = 'store_false', help = 'Enable debug mode') parser.add_argument('--headless', action='store_true', help='Run in headless mode (no display)') + parser.add_argument('--is_use_sim', action = 'store_true', help = 'Use simulation or not') + parser.set_defaults(is_use_sim = False) + args = parser.parse_args() logger_mp.info(f"args: {args}") # image client: img_config should be the same as the configuration in image_server.py (of Robot's development computing unit) - img_config = { - 'fps': 30, - 'head_camera_type': 'opencv', - 'head_camera_image_shape': [480, 1280], # Head camera resolution - 'head_camera_id_numbers': [0], - 'wrist_camera_type': 'opencv', - 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution - 'wrist_camera_id_numbers': [2, 4], - } + if args.is_use_sim: + img_config = { + 'fps': 30, + 'head_camera_type': 'opencv', + 'head_camera_image_shape': [480, 640], # Head camera resolution + 'head_camera_id_numbers': [0], + 'wrist_camera_type': 'opencv', + 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + 'wrist_camera_id_numbers': [2, 4], + } + else: + img_config = { + 'fps': 30, + 'head_camera_type': 'opencv', + 'head_camera_image_shape': [480, 1280], # Head camera resolution + 'head_camera_id_numbers': [0], + 'wrist_camera_type': 'opencv', + 'wrist_camera_image_shape': [480, 640], # Wrist camera resolution + 'wrist_camera_id_numbers': [2, 4], + } + + ASPECT_RATIO_THRESHOLD = 2.0 # If the aspect ratio exceeds this value, it is considered binocular if len(img_config['head_camera_id_numbers']) > 1 or (img_config['head_camera_image_shape'][1] / img_config['head_camera_image_shape'][0] > ASPECT_RATIO_THRESHOLD): BINOCULAR = True @@ -89,9 +105,9 @@ if __name__ == '__main__': wrist_img_shm = shared_memory.SharedMemory(create = True, size = np.prod(wrist_img_shape) * np.uint8().itemsize) wrist_img_array = np.ndarray(wrist_img_shape, dtype = np.uint8, buffer = wrist_img_shm.buf) img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, - wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name) + wrist_img_shape = wrist_img_shape, wrist_img_shm_name = wrist_img_shm.name,server_address="127.0.0.1") else: - img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name) + img_client = ImageClient(tv_img_shape = tv_img_shape, tv_img_shm_name = tv_img_shm.name, server_address="127.0.0.1") image_receive_thread = threading.Thread(target = img_client.receive_process, daemon = True) image_receive_thread.daemon = True @@ -103,16 +119,16 @@ if __name__ == '__main__': # arm if args.arm == 'G1_29': - arm_ctrl = G1_29_ArmController(debug_mode=args.debug) + arm_ctrl = G1_29_ArmController(debug_mode=args.debug, is_use_sim=args.is_use_sim) arm_ik = G1_29_ArmIK() elif args.arm == 'G1_23': - arm_ctrl = G1_23_ArmController() + arm_ctrl = G1_23_ArmController(is_use_sim=args.is_use_sim) arm_ik = G1_23_ArmIK() elif args.arm == 'H1_2': - arm_ctrl = H1_2_ArmController() + arm_ctrl = H1_2_ArmController(is_use_sim=args.is_use_sim) arm_ik = H1_2_ArmIK() elif args.arm == 'H1': - arm_ctrl = H1_ArmController() + arm_ctrl = H1_ArmController(is_use_sim=args.is_use_sim) arm_ik = H1_ArmIK() # end-effector @@ -129,7 +145,7 @@ if __name__ == '__main__': dual_gripper_data_lock = Lock() dual_gripper_state_array = Array('d', 2, lock=False) # current left, right gripper state(2) data. dual_gripper_action_array = Array('d', 2, lock=False) # current left, right gripper action(2) data. - gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array) + gripper_ctrl = Gripper_Controller(left_gripper_value, right_gripper_value, dual_gripper_data_lock, dual_gripper_state_array, dual_gripper_action_array, is_use_sim=args.is_use_sim) elif args.ee == "inspire1": left_hand_pos_array = Array('d', 75, lock = True) # [input] right_hand_pos_array = Array('d', 75, lock = True) # [input]