76 lines
2.5 KiB
Python
76 lines
2.5 KiB
Python
import time
|
|
import logging
|
|
from typing import Dict
|
|
from dataclasses import dataclass
|
|
from lerobot.common.robot_devices.teleop.gamepad import RealmanAlohaMaster, DummyEndposeMaster
|
|
|
|
|
|
@dataclass
|
|
class ControllerConfig:
|
|
"""控制器配置"""
|
|
init_joint: list
|
|
init_pose: list
|
|
max_gripper: int
|
|
min_gripper: int
|
|
config_file: str
|
|
end_control_info: dict
|
|
|
|
|
|
class HybridController:
|
|
def __init__(self, init_info):
|
|
self.config = self._parse_init_info(init_info)
|
|
self.joint = self.config.init_joint.copy()
|
|
self.pose = self.config.init_pose.copy()
|
|
|
|
self.joint_arm = RealmanAlohaMaster(self.config)
|
|
self.endpose_arm = DummyEndposeMaster(self.config)
|
|
|
|
def _parse_init_info(self, init_info: dict) -> ControllerConfig:
|
|
"""解析初始化信息"""
|
|
return ControllerConfig(
|
|
init_joint=init_info['init_joint'],
|
|
init_pose=init_info.get('init_pose', [0]*12),
|
|
max_gripper=init_info['max_gripper'],
|
|
min_gripper=init_info['min_gripper'],
|
|
config_file=init_info['servo_config_file'],
|
|
end_control_info=init_info['end_control_info']
|
|
)
|
|
|
|
def get_action(self, state) -> Dict:
|
|
"""获取控制动作"""
|
|
try:
|
|
endpose_action = self.endpose_arm.get_action(state)
|
|
return endpose_action
|
|
# return self.joint_arm.get_action()
|
|
|
|
except Exception as e:
|
|
logging.error(f"获取动作失败: {e}")
|
|
|
|
def stop(self):
|
|
self.joint_arm.stop()
|
|
|
|
def reset(self):
|
|
"""重置控制器"""
|
|
self.joint = self.config.init_joint.copy()
|
|
self.pose = self.config.init_pose.copy()
|
|
self.joint_control_mode = True
|
|
|
|
|
|
# 使用示例
|
|
if __name__ == "__main__":
|
|
init_info = {
|
|
'init_joint': [-175, 90, 90, 45, 90, -90, 10, 175, 90, 90, -45, 90, 90, 10],
|
|
'init_pose': [[-0.0305, 0.125938, 0.13153, 3.141, 0.698, -1.57, -0.030486, -0.11487, 0.144707, 3.141, 0.698, 1.57]],
|
|
'max_gripper': 990,
|
|
'min_gripper': 10,
|
|
'servo_config_file': '/home/maic/LYT/lerobot/lerobot/common/robot_devices/teleop/servo_dual.yaml',
|
|
'end_control_info': {'left': '0300b14bff1100003708000010010000', 'right': '030003f05e0400008e02000010010000'}
|
|
}
|
|
arm_controller = HybridController(init_info)
|
|
time.sleep(1)
|
|
try:
|
|
while True:
|
|
print(arm_controller.get_action())
|
|
time.sleep(1)
|
|
except KeyboardInterrupt:
|
|
arm_controller.stop() |