diff --git a/teleop/robot_control/robot_hand_inspire.py b/teleop/robot_control/robot_hand_inspire.py index 3c907b9..5c69477 100644 --- a/teleop/robot_control/robot_hand_inspire.py +++ b/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 diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index be3f67c..1051fc4 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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": [], diff --git a/teleop/utils/rerun_visualizer.py b/teleop/utils/rerun_visualizer.py index c11a10e..9b2f057 100644 --- a/teleop/utils/rerun_visualizer.py +++ b/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))