[fix] bug.

teleop/robot_control/robot_hand_inspire.py

@@ -147,7 +147,7 @@ class Inspire_Controller:
                 sleep_time = max(0, (1 / self.fps) - time_elapsed)
                 time.sleep(sleep_time)
         finally:
-            print("Dex3_1_Controller has been closed.")
+            print("Inspire_Controller has been closed.")
 
 # Update hand state, according to the official documentation, https://support.unitree.com/home/en/G1_developer/inspire_dfx_dexterous_hand
 # the state sequence is as shown in the table below

teleop/teleop_hand_and_arm.py

@@ -23,7 +23,7 @@ from teleop.utils.episode_writer import EpisodeWriter
 if __name__ == '__main__':
     parser = argparse.ArgumentParser()
     parser.add_argument('--task_dir', type = str, default = './utils/data', help = 'path to save data')
-    parser.add_argument('--frequency', type = float, default = 30.0, help = 'save data\'s frequency')
+    parser.add_argument('--frequency', type = float, default = 90.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')
@@ -83,7 +83,7 @@ if __name__ == '__main__':
 
     # arm
     if args.arm == 'G1_29':
-        arm_ctrl = G1_29_ArmController()
+        arm_ctrl = G1_29_ArmController(debug_mode=True)
         arm_ik = G1_29_ArmIK()
     elif args.arm == 'G1_23':
         arm_ctrl = G1_23_ArmController()
@@ -203,12 +203,16 @@ if __name__ == '__main__':
                             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_body_state = []
+                            current_body_action = []
                     elif args.ee == "gripper" and args.xr_mode == 'hand':
                         with dual_gripper_data_lock:
                             left_hand_state = [dual_gripper_state_array[1]]
                             right_hand_state = [dual_gripper_state_array[0]]
                             left_hand_action = [dual_gripper_action_array[1]]
                             right_hand_action = [dual_gripper_action_array[0]]
+                            current_body_state = []
+                            current_body_action = []
                     elif args.ee == "gripper" and args.xr_mode == 'controller':
                         with dual_gripper_data_lock:
                             left_hand_state = [dual_gripper_state_array[1]]
@@ -225,6 +229,8 @@ if __name__ == '__main__':
                             right_hand_state = dual_hand_state_array[-6:]
                             left_hand_action = dual_hand_action_array[:6]
                             right_hand_action = dual_hand_action_array[-6:]
+                            current_body_state = []
+                            current_body_action = []
                     else:
                         left_hand_state = []
                         right_hand_state = []
@@ -267,12 +273,12 @@ if __name__ == '__main__':
                                 "qvel":   [],                          
                                 "torque": [],                         
                             },                        
-                            "left_hand_pos": {                                                                    
+                            "left_hand": {                                                                    
                                 "qpos":   left_hand_state,           
                                 "qvel":   [],                           
                                 "torque": [],                          
                             }, 
-                            "right_hand_pos": {                                                                    
+                            "right_hand": {                                                                    
                                 "qpos":   right_hand_state,       
                                 "qvel":   [],                           
                                 "torque": [],  
@@ -292,12 +298,12 @@ if __name__ == '__main__':
                                 "qvel":   [],       
                                 "torque": [],       
                             },                         
-                            "left_hand_pos": {                                   
+                            "left_hand": {                                   
                                 "qpos":   left_hand_action,       
                                 "qvel":   [],       
                                 "torque": [],       
                             }, 
-                            "right_hand_pos": {                                   
+                            "right_hand": {                                   
                                 "qpos":   right_hand_action,       
                                 "qvel":   [],       
                                 "torque": [], 

teleop/utils/rerun_visualizer.py

@@ -141,7 +141,7 @@ class RerunLogger:
         # Log states
         states = item_data.get('states', {}) or {}
         for part, state_info in states.items():
-            if state_info:
+            if part != "body" and state_info:
                 values = state_info.get('qpos', [])
                 for idx, val in enumerate(values):
                     rr.log(f"{self.prefix}{part}/states/qpos/{idx}", rr.Scalar(val))
@@ -149,7 +149,7 @@ class RerunLogger:
         # Log actions
         actions = item_data.get('actions', {}) or {}
         for part, action_info in actions.items():
-            if action_info:
+            if part != "body" and action_info:
                 values = action_info.get('qpos', [])
                 for idx, val in enumerate(values):
                     rr.log(f"{self.prefix}{part}/actions/qpos/{idx}", rr.Scalar(val))