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

Record初步实现

Browse Source
This commit is contained in:
1002142102@qq.com
2025-12-08 17:43:12 +08:00
parent 2b523acc52
commit 1d09062f79
6 changed files with 140 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
robot_client/teleoperators/flight_stick/flight_stick.py
View File

@@ -26,8 +26,8 @@ class FlightStick(Xbox):
hat_left = hat[0] == -1 # X-
hat_right = hat[0] == 1 # X+
# 计算各轴速度
pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X
pose_speeds[1] = 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轴
@@ -36,25 +36,26 @@ class FlightStick(Xbox):
# 设置Z速度
z_mapping = self.apply_nonlinear_mapping(gas_axis)
# print(f"Z轴非线性映射: {gas_axis} -> {z_mapping}")
pose_speeds[2] = z_mapping * current_linear_step # Z
pose_speeds['z.vel'] = z_mapping * current_linear_step # Z
# 主摇杆X轴控制RX旋转
# 设置RX旋转速度
rx_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
# print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
pose_speeds[3] = rx_mapping * current_angular_step * 2 # RX
pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2 # RX
# 主摇杆Y轴控制RY旋转
# 设置RY旋转速度
ry_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(0))
# print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
pose_speeds[4] = ry_mapping * current_angular_step * 2 # RY
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[5] = rz_mapping * current_angular_step * 2 # RZ
pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2 # RZ
pose_speeds['gripper.vel'] = 0
return pose_speeds

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

@@ -8,7 +8,7 @@ Xbox类用于检测和监控飞行手柄。
"""
class Xbox(Teleoperator):
config_class = XboxConfig
name = "flight_stick"
name = "xbox"
def __init__(self, config: XboxConfig):
self.config = config
# 控制参数
@@ -26,8 +26,8 @@ class Xbox(Teleoperator):
def action_features(self):
return {
"dtype": "float32",
"shape": (len(self.pose.keys()),),
"names": self.pose.keys(),
"shape": (7,),
"names": {"x.vel":0, "y.vel":1, "z.vel":2, "rx.vel.vel":3, "ry.vel":4, "rz.vel":5, "gripper.vel":6},
}
@property
@@ -69,7 +69,7 @@ class Xbox(Teleoperator):
def get_action(self):
pygame.event.pump()
pose_speeds = [0.0] * 6
pose_speeds = {}
"""更新末端位姿控制"""
# 根据控制模式调整步长
current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0)
@@ -90,25 +90,25 @@ class Xbox(Teleoperator):
hat_right = hat[0] == 1 # X+
# print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
# 计算各轴速度
pose_speeds[0] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0) # X
pose_speeds[1] = 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
# 右摇杆X轴
z_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(1))
print(f"Z轴非线性映射: {self.joystick.get_axis(1)} -> {z_mapping}")
pose_speeds[2] = z_mapping * current_angular_step * 2 # RY
pose_speeds['z.vel'] = z_mapping * current_angular_step * 2 # RY
#左摇杆X轴控制RX旋转
# 设置RX旋转速度
rx_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(2))
# print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
pose_speeds[3] = rx_mapping * current_angular_step * 2 # RX
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))
# print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
pose_speeds[4] = ry_mapping * current_angular_step * 2 # RY
pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2 # RY
# 左右扳机控制RZ旋转
# 设置RZ旋转速度
@@ -117,7 +117,9 @@ class Xbox(Teleoperator):
print(f"左右扳机: {left_tiger} {right_tiger}")
rz_mapping = self.apply_nonlinear_mapping(left_tiger - right_tiger)
# print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
pose_speeds[5] = rz_mapping * current_angular_step * 2 # RZ
pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2 # RZ
pose_speeds['gripper.vel'] = 0
return pose_speeds