1002142102@qq.com
cc31254055
2.增加多臂多控制器模式 3.末端姿态由欧拉角控制切换到四元数控制 4.增加vr手柄控制器 Signed-off-by: 1002142102@qq.com <1002142102@qq.com>
69 lines
3.1 KiB
Python
69 lines
3.1 KiB
Python
import pygame
|
|
from .config import FlightStickConfig
|
|
from ..xbox import Xbox
|
|
"""
|
|
FlightStick类用于检测和监控飞行手柄。
|
|
"""
|
|
class FlightStick(Xbox):
|
|
config_class = FlightStickConfig
|
|
name = "flight_stick"
|
|
|
|
def get_action(self):
|
|
pose_speeds = {}
|
|
"""更新末端位姿控制"""
|
|
# 根据控制模式调整步长
|
|
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}")
|
|
# print(f"{self.joystick.get_axis(0)},{self.joystick.get_axis(1)},{self.joystick.get_axis(2)},{self.joystick.get_axis(3)}")
|
|
# 方向键控制XY
|
|
hat = self.joystick.get_hat(0)
|
|
hat_up = hat[1] == 1 # Y+
|
|
hat_down = hat[1] == -1 # Y-
|
|
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
|
|
|
|
# print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
|
|
if self.joystick.get_button(8): # X按钮
|
|
pose_speeds['z.vel'] = current_linear_step/3
|
|
elif self.joystick.get_button(9): # Y按钮
|
|
pose_speeds['z.vel'] = -current_linear_step/3
|
|
else:
|
|
pose_speeds['z.vel'] = 0
|
|
|
|
|
|
# 主摇杆X轴控制RX旋转
|
|
# 设置RX旋转速度
|
|
rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(0))
|
|
# print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
|
|
pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2 # RX
|
|
|
|
# 主摇杆Y轴控制RY旋转
|
|
# 设置RY旋转速度
|
|
ry_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
|
|
# print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
|
|
pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2 # RY
|
|
|
|
# 主摇杆左右旋转轴 控制RZ旋转
|
|
# 设置RZ旋转速度
|
|
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
|
|
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
|