From f657ff8bea7e7c334343f90444472b27b7a1f470 Mon Sep 17 00:00:00 2001 From: silencht Date: Tue, 22 Jul 2025 16:36:15 +0800 Subject: [PATCH] [fix] sshkeboard exit bug --- teleop/robot_control/robot_arm.py | 2 +- teleop/teleop_hand_and_arm.py | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index f8d1f8d..af47e1d 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -232,7 +232,7 @@ class G1_29_ArmController: current_q = self.get_current_dual_arm_q() if np.all(np.abs(current_q) < tolerance): if self.motion_mode: - for weight in np.arange(1, 0, -0.01): + for weight in np.linspace(1, 0, num=101): self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight; time.sleep(0.02) logger_mp.info("[G1_29_ArmController] both arms have reached the home position.") diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 323986a..2569cb7 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -213,7 +213,6 @@ if __name__ == '__main__': cv2.imshow("record image", tv_resized_image) key = cv2.waitKey(1) & 0xFF if key == ord('q'): - stop_listening() running = False if args.sim: publish_reset_category(2, reset_pose_publisher) @@ -260,13 +259,14 @@ if __name__ == '__main__': # quit teleoperate if tele_data.tele_state.right_aButton: running = False + stop_listening() # 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() # 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) + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3) # get current robot state data. current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() @@ -306,8 +306,8 @@ if __name__ == '__main__': right_hand_action = [dual_gripper_action_array[1]] current_body_state = arm_ctrl.get_current_motor_q().tolist() current_body_action = [-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] + -tele_data.tele_state.left_thumbstick_value[0] * 0.3, + -tele_data.tele_state.right_thumbstick_value[0] * 0.3] elif (args.ee == "inspire1" or args.ee == "brainco") and args.xr_mode == "hand": with dual_hand_data_lock: left_ee_state = dual_hand_state_array[:6]