[update] Enable unitree_sim_isaaclab simulation environment support

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,6 +169,9 @@ class G1_29_ArmController:
                 arm_q_target     = self.q_target
                 arm_tauff_target = self.tauff_target
 
+            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):
@@ -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,6 +443,9 @@ class G1_23_ArmController:
                 arm_q_target     = self.q_target
                 arm_tauff_target = self.tauff_target
 
+            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):
@@ -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,6 +705,9 @@ class H1_2_ArmController:
                 arm_q_target     = self.q_target
                 arm_tauff_target = self.tauff_target
 
+            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):
@@ -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,6 +966,9 @@ class H1_ArmController:
                 arm_q_target     = self.q_target
                 arm_tauff_target = self.tauff_target
 
+            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):

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,7 +316,9 @@ 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
-
+        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

teleop/teleop_hand_and_arm.py

@@ -53,10 +53,24 @@ 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)
+    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',
@@ -66,6 +80,8 @@ if __name__ == '__main__':
             '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]