tangger/lerobot
1
0
Fork 1
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

1.增加xbox控制器和飞行手柄控制器

Browse Source 
2.增加多臂多控制器模式
3.末端姿态由欧拉角控制切换到四元数控制
4.增加vr手柄控制器

Signed-off-by: 1002142102@qq.com <1002142102@qq.com>
This commit is contained in:
1002142102@qq.com
2025-12-24 14:02:34 +08:00
parent becdc2c894
commit cc31254055
23 changed files with 1510 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
robot_client/teleoperators/xbox/xbox.py
View File

@@ -1,8 +1,11 @@
import queue
import threading
import pygame
import time
import sys
from lerobot.teleoperators import Teleoperator
from .config import XboxConfig
from ...utlis.pygame_event import PygameEvent
"""
Xbox类用于检测和监控飞行手柄。
"""
@@ -21,6 +24,8 @@ class Xbox(Teleoperator):
self.fine_control_mode = False
self.reset()
def reset(self):
self.running = False
self.action_data ={"x.vel":0, "y.vel":0, "z.vel":0, "rx.vel.vel":0, "ry.vel":0, "rz.vel":0, "gripper.vel":0}
self.joystick = None
@property
def action_features(self):
@@ -37,8 +42,8 @@ class Xbox(Teleoperator):
@property
def is_connected(self):
return self.joystick is not None
def connect(self, calibrate = True):
def update(self,q:queue.Queue):
# 初始化pygame
pygame.init()
pygame.joystick.init()
@@ -47,16 +52,23 @@ class Xbox(Teleoperator):
print(f"检测到 {joystick_count} 个手柄设备")
if joystick_count == 0:
print("未检测到任何手柄设备！")
q.put((False,"未检测到任何手柄设备！"))
return False
if joystick_count<self.config.index+1:
print(f"未检测到配置中指定的索引设备！")
q.put((False,"未检测到配置中指定的索引设备！"))
return False
# 初始化所有检测到的手柄
self.joystick = pygame.joystick.Joystick(self.config.index)
print(f"已连接设备：{self.joystick.get_name()}")
q.put((True,f"已连接设备：{self.joystick.get_name()}"))
while self.running:
self.action_data = self.update_action()
pygame.joystick.quit()
pygame.quit()
def connect(self, calibrate = True):
PygameEvent.start()
self.joystick = PygameEvent.getState(self.config.index)
@property
def is_calibrated(self):
return True
@@ -68,7 +80,6 @@ class Xbox(Teleoperator):
pass
def get_action(self):
pygame.event.pump()
pose_speeds = {}
"""更新末端位姿控制"""
# 根据控制模式调整步长
@@ -80,7 +91,7 @@ class Xbox(Teleoperator):
self.fine_control_mode = False
# print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
print(f"精细控制模式: {self.fine_control_mode}")
# print(f"精细控制模式: {self.fine_control_mode}")
# 方向键控制XY
hat = self.joystick.get_hat(0)
@@ -95,19 +106,19 @@ class Xbox(Teleoperator):
# 左右扳机控制z轴
right_tiger = ((self.joystick.get_axis(5)+1)/2)
left_tiger = ((self.joystick.get_axis(4)+1)/2)
left_tiger = ((self.joystick.get_axis(2)+1)/2)
z_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger)
pose_speeds['z.vel'] = z_mapping * current_angular_step * 2
pose_speeds['z.vel'] = z_mapping * current_linear_step
#右摇杆X轴控制RX旋转
# 设置RX旋转速度
rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(2))
rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
# 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(3))
ry_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(4))
# print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2 # RY
@@ -122,9 +133,64 @@ 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
# def update_action(self):
# pygame.event.pump()
# 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(0): # A按钮
# self.fine_control_mode = True
# if self.joystick.get_button(1): # B按钮
# self.fine_control_mode = False
# # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
# # print(f"精细控制模式: {self.fine_control_mode}")
# # 方向键控制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+
# # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
# # 计算各轴速度
# 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
# # 左右扳机控制z轴
# right_tiger = ((self.joystick.get_axis(5)+1)/2)
# left_tiger = ((self.joystick.get_axis(2)+1)/2)
# z_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger)
# pose_speeds['z.vel'] = z_mapping * current_angular_step * 2
# #右摇杆X轴控制RX旋转
# # 设置RX旋转速度
# rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
# # 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(4))
# # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
# pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2 # RY
# # 左摇杆X轴RZ旋转
# # 设置RZ旋转速度
# rz_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(0))
# # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
# pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2 # RZ
# # 左摇杆Y轴控制夹爪
# # 设置夹爪开合速度
# 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
# return pose_speeds
def apply_nonlinear_mapping(self, value):
"""应用非线性映射以提高控制精度"""
@@ -141,5 +207,3 @@ class Xbox(Teleoperator):
def disconnect(self):
self.reset()
pygame.joystick.quit()
pygame.quit()