From c0cc780ba9dc870d8ced39bb009469726a07ce91 Mon Sep 17 00:00:00 2001 From: silencht Date: Thu, 22 May 2025 20:55:25 +0800 Subject: [PATCH] [fix] bug --- teleop/robot_control/robot_arm.py | 4 ++-- teleop/teleop_hand_and_arm.py | 27 +++++++++++++++++---------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index 6194cd4..d728024 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -155,7 +155,7 @@ class G1_29_ArmController: return cliped_arm_q_target def _ctrl_motor_state(self): - if self.debug_mode: + if not self.debug_mode: self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0; while True: @@ -218,7 +218,7 @@ class G1_29_ArmController: while True: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): - if self.debug_mode: + if not self.debug_mode: for weight in np.arange(1, 0, -0.01): self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; time.sleep(0.02) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 1051fc4..9b0d216 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -33,6 +33,10 @@ if __name__ == '__main__': 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'], help='Select end effector controller') + parser.add_argument('--debug', action = 'store_true', help = 'debug mode') + parser.add_argument('--no-debug', dest = 'debug', action = 'store_false', help = 'motion mode') + parser.set_defaults(debug = True) + args = parser.parse_args() print(f"args:{args}\n") @@ -83,7 +87,7 @@ if __name__ == '__main__': # arm if args.arm == 'G1_29': - arm_ctrl = G1_29_ArmController(debug_mode=True) + arm_ctrl = G1_29_ArmController(debug_mode=args.debug) arm_ik = G1_29_ArmIK() elif args.arm == 'G1_23': arm_ctrl = G1_23_ArmController() @@ -121,7 +125,7 @@ if __name__ == '__main__': pass # xr mode - if args.xr_mode == 'controller': + if args.xr_mode == 'controller' and not args.debug: from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient sport_client = LocoClient() sport_client.SetTimeout(0.0001) @@ -165,23 +169,26 @@ if __name__ == '__main__': 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': + with left_gripper_value.get_lock(): + left_gripper_value.value = tele_data.left_pinch_value + with right_gripper_value.get_lock(): + right_gripper_value.value = tele_data.right_pinch_value + else: + pass + + # high level control + if args.xr_mode == 'controller' and not args.debug: # quit teleoperate if tele_data.tele_state.right_aButton: running = False # command robot to enter damping mode. soft emergency stop function if tele_data.tele_state.left_thumbstick_state and tele_data.tele_state.right_thumbstick_state: sport_client.Damp() - # high level control, limit velocity to within 0.3 + # control, limit velocity to within 0.3 sport_client.Move(-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 == 'gripper' 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(): - right_gripper_value.value = tele_data.right_pinch_value - else: - pass # get current robot state data. current_lr_arm_q = arm_ctrl.get_current_dual_arm_q()