优化飞行手柄

robot_client/robots/realman/motors_bus.py

@@ -26,11 +26,6 @@ class RealmanMotorsBus(MotorsBus):
         calibration: dict[str, MotorCalibration] | None = None,
     ):
         super().__init__(port,motors, calibration)
-        self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E)
-        if mock:
-            self.rmarm.rm_set_arm_run_mode(0)#仿真模式
-        else:
-            self.rmarm.rm_set_arm_run_mode(1)
         address  = port.split(':')
         self.ip = address[0]
         self.port = int(address[1])
@@ -38,6 +33,8 @@ class RealmanMotorsBus(MotorsBus):
         self.gripper_range = gripper_range
         self.init_joint_position = joint
         self.safe_disable_position = joint
+        self.mock = mock
+        self.rmarm = RoboticArm(rm_thread_mode_e.RM_DUAL_MODE_E)
         self.handle = None
         self.init_pose = None
 
@@ -58,8 +55,11 @@ class RealmanMotorsBus(MotorsBus):
         '''
         使能机械臂并检测使能状态,尝试5s,如果使能超时则退出程序
         '''
-
         self.handle = self.rmarm.rm_create_robot_arm(self.ip, self.port)
+        if self.mock:
+            self.rmarm.rm_set_arm_run_mode(0)#仿真模式
+        else:
+            self.rmarm.rm_set_arm_run_mode(1)
         enable_flag = False
         loop_flag = False
         # 设置超时时间（秒）
@@ -178,8 +178,8 @@ class RealmanMotorsBus(MotorsBus):
             values = list(actionData.values())
             for k,v in enumerate(self.init_pose):
                 self.init_pose[k]+=values[k]
-            print(f"init_pose:{self.init_pose[:-1]}")
-            self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 0, 0)
+            r = self.rmarm.rm_movej_p(self.init_pose[:-1], 50, 0, 0, 0)
+            print(f"r:{r},pose:{self.init_pose}")
             # self.init_pose[6]+=actionData['gripper']
             # self.rmarm.rm_set_gripper_position(int(self.init_pose[6]*1000), False, 0)
     

robot_client/teleoperators/flight_stick/flight_stick.py

@@ -15,6 +15,10 @@ class FlightStick(Xbox):
         # 根据控制模式调整步长
         current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0)
         current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0)
+        if self.joystick.get_button(2):  # A按钮
+                self.fine_control_mode = True
+        if self.joystick.get_button(3):  # B按钮
+                self.fine_control_mode = False
         
         # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
         print(f"精细控制模式: {self.fine_control_mode}")
@@ -26,17 +30,16 @@ class FlightStick(Xbox):
         hat_left = hat[0] == -1  # X-
         hat_right = hat[0] == 1  # X+
         # 计算各轴速度
-        pose_speeds['x.vel'] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0)  # X
-        pose_speeds['y.vel'] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0)  # Y
+        pose_speeds['x.vel'] = current_linear_step if hat_left else (-current_linear_step if hat_right else 0.0)  # X
+        pose_speeds['y.vel'] = -current_linear_step if hat_up else (current_linear_step if hat_down else 0.0)  # Y
         
         # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
-        # 油门控制Z轴
-        gas_axis = -self.joystick.get_axis(2)
-        
-        # 设置Z速度
-        z_mapping = self.apply_nonlinear_mapping(gas_axis)
-        # print(f"Z轴非线性映射: {gas_axis} -> {z_mapping}")
-        pose_speeds['z.vel'] = z_mapping * current_linear_step  # Z
+        if self.joystick.get_button(8):  # X按钮
+            pose_speeds['z.vel'] = current_linear_step
+        elif self.joystick.get_button(9):  # Y按钮
+            pose_speeds['z.vel'] = -current_linear_step
+        else:
+            pose_speeds['z.vel'] = 0
 
         
         # 主摇杆X轴控制RX旋转
@@ -56,6 +59,11 @@ class FlightStick(Xbox):
         rz_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
         # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
         pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2  # RZ
-        
-        pose_speeds['gripper.vel'] = 0
+        if self.joystick.get_button(0):  # X按钮
+            pose_speeds['gripper.vel'] = current_linear_step
+        elif self.joystick.get_button(1):  # Y按钮
+            pose_speeds['gripper.vel'] = -current_linear_step
+        else:
+            pose_speeds['gripper.vel'] = 0
+        print(f"pose_speeds: {pose_speeds}")
         return pose_speeds

robot_client/teleoperators/xbox/xbox.py

@@ -122,6 +122,7 @@ class Xbox(Teleoperator):
         gripper_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
         # print(f"gripper轴非线性映射: {self.joystick.get_axis(0)} -> {gripper_mapping}")
         pose_speeds['gripper.vel'] = gripper_mapping * current_angular_step * 2  # RY
+        print(f"pose_speeds: {pose_speeds}")
         
         return pose_speeds