1002142102@qq.com
cc31254055
2.增加多臂多控制器模式 3.末端姿态由欧拉角控制切换到四元数控制 4.增加vr手柄控制器 Signed-off-by: 1002142102@qq.com <1002142102@qq.com>
65 lines
1.8 KiB
Python
65 lines
1.8 KiB
Python
import queue
|
|
import threading
|
|
import pygame
|
|
import time
|
|
import sys
|
|
from lerobot.teleoperators import Teleoperator,make_teleoperator_from_config
|
|
from .config import MixConfig
|
|
"""
|
|
混合控制器。
|
|
"""
|
|
class Mix(Teleoperator):
|
|
config_class = MixConfig
|
|
name = "mix"
|
|
def __init__(self, config: MixConfig):
|
|
self.config = config
|
|
self.teleopList = {}
|
|
for name,teleop in self.config.teleopList.items():
|
|
self.teleopList[name] = make_teleoperator_from_config(teleop)
|
|
@property
|
|
def action_features(self):
|
|
action_features = {}
|
|
for name,teleop in self.teleopList.items():
|
|
for i,item in enumerate(teleop.action_features()["names"].keys()):
|
|
action_features[f"{name}.{item}"] = i
|
|
return {
|
|
"dtype": "float32",
|
|
"shape": (len(action_features),),
|
|
"names": action_features,
|
|
}
|
|
|
|
@property
|
|
def feedback_features(self):
|
|
return {}
|
|
|
|
@property
|
|
def is_connected(self):
|
|
return all([teleop.is_connected for teleop in self.teleopList.values()])
|
|
|
|
def connect(self, calibrate = True):
|
|
for teleop in self.teleopList.values():
|
|
teleop.connect(calibrate)
|
|
@property
|
|
def is_calibrated(self):
|
|
return True
|
|
|
|
def calibrate(self):
|
|
return all([teleop.calibrate() for teleop in self.teleopList.values()])
|
|
|
|
def configure(self):
|
|
pass
|
|
|
|
def get_action(self):
|
|
data = {}
|
|
for index,teleop in self.teleopList.items():
|
|
for key,item in teleop.get_action().items():
|
|
data[f"{index}.{key}"] = item
|
|
return data
|
|
|
|
def send_feedback(self, feedback):
|
|
pass
|
|
|
|
def disconnect(self):
|
|
for teleop in self.teleopList.values():
|
|
teleop.disconnect()
|