241 lines
9.0 KiB
Python
241 lines
9.0 KiB
Python
"""RealMan 机械臂末端外设配置工具。
|
||
|
||
scissorgripper 参数约定：
|
||
0：工具板 IO 控制的剪刀夹爪
|
||
1：Modbus 控制的电动夹爪
|
||
2：控制器 DO3/DO4 控制的小型剪刀夹爪
|
||
|
||
当前 XR 工程只在真机连接阶段做外设初始化；主遥操作流程不调用开合命令。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import time
|
||
from dataclasses import dataclass
|
||
from pathlib import Path
|
||
from typing import Any
|
||
|
||
import yaml
|
||
|
||
|
||
@dataclass(frozen=True)
|
||
class PeripheralConfig:
|
||
scissorgripper: int
|
||
tools_in_ee: dict[str, list[list[float]]]
|
||
set_initial_tool_state: bool = False
|
||
|
||
|
||
def load_peripheral_config(config_file: str, arm: str) -> PeripheralConfig:
|
||
"""从 bringup YAML 读取指定左右臂的外设配置。"""
|
||
path = Path(config_file).expanduser()
|
||
if not path.is_file():
|
||
raise FileNotFoundError(f"外设配置文件不存在：{path}")
|
||
|
||
with path.open("r", encoding="utf-8") as stream:
|
||
data = yaml.safe_load(stream) or {}
|
||
if not isinstance(data, dict):
|
||
raise ValueError(f"外设配置文件格式错误：{path}")
|
||
|
||
tools_in_ee = _load_tools_in_ee(data.get("tools_in_ee"), path)
|
||
arm_configs = data.get("arms", {})
|
||
if not isinstance(arm_configs, dict) or arm not in arm_configs:
|
||
raise ValueError(f"{path} 中缺少 arms.{arm} 外设配置")
|
||
|
||
arm_config = arm_configs[arm]
|
||
if not isinstance(arm_config, dict):
|
||
raise ValueError(f"{path} 中 arms.{arm} 必须是映射")
|
||
|
||
scissorgripper = int(arm_config["scissorgripper"])
|
||
tool_count = len(tools_in_ee)
|
||
if not -tool_count <= scissorgripper < tool_count:
|
||
raise ValueError(
|
||
f"arms.{arm}.scissorgripper={scissorgripper} 超出工具列表范围"
|
||
)
|
||
|
||
set_initial_tool_state = _as_bool(
|
||
arm_config.get(
|
||
"set_initial_tool_state",
|
||
data.get("set_initial_tool_state", False),
|
||
)
|
||
)
|
||
return PeripheralConfig(scissorgripper, tools_in_ee, set_initial_tool_state)
|
||
|
||
|
||
def _load_tools_in_ee(raw_tools: Any, path: Path) -> dict[str, list[list[float]]]:
|
||
if not isinstance(raw_tools, dict) or not raw_tools:
|
||
raise ValueError(f"{path} 中缺少 tools_in_ee 配置")
|
||
|
||
tools_in_ee: dict[str, list[list[float]]] = {}
|
||
for tool_name, tool_config in raw_tools.items():
|
||
if not isinstance(tool_config, dict):
|
||
raise ValueError(f"tools_in_ee.{tool_name} 必须是映射")
|
||
pose = _float_list(tool_config.get("pose"), 7, f"tools_in_ee.{tool_name}.pose")
|
||
load = _float_list(tool_config.get("load"), 7, f"tools_in_ee.{tool_name}.load")
|
||
tools_in_ee[str(tool_name)] = [pose, load]
|
||
return tools_in_ee
|
||
|
||
|
||
def _float_list(value: Any, expected_length: int, name: str) -> list[float]:
|
||
if not isinstance(value, (list, tuple)) or len(value) != expected_length:
|
||
raise ValueError(f"{name} 必须包含 {expected_length} 个数值")
|
||
return [float(item) for item in value]
|
||
|
||
|
||
def _as_bool(value: Any) -> bool:
|
||
if isinstance(value, bool):
|
||
return value
|
||
if isinstance(value, str):
|
||
return value.strip().lower() in ("1", "true", "yes", "on")
|
||
return bool(value)
|
||
|
||
|
||
def _tool_name_for_index(tools_in_ee: dict[str, list[list[float]]], scissorgripper: int) -> str:
|
||
return list(tools_in_ee.keys())[scissorgripper]
|
||
|
||
|
||
def cal_tool_frame(handle, scissorgripper, tools_in_ee):
|
||
"""根据末端工具配置生成 RealMan 工具坐标系。"""
|
||
from Robotic_Arm.rm_robot_interface import rm_frame_t
|
||
|
||
tool_name = _tool_name_for_index(tools_in_ee, scissorgripper)
|
||
tool_pose = tools_in_ee[tool_name][0]
|
||
tool_load = tools_in_ee[tool_name][1]
|
||
eu = handle.rm_algo_quaternion2euler([tool_pose[6], tool_pose[3], tool_pose[4], tool_pose[5]])
|
||
tool_frame = rm_frame_t(tool_name, [tool_pose[0], tool_pose[1], tool_pose[2], eu[0], eu[1], eu[2]], tool_load[0],
|
||
tool_load[1], tool_load[2], tool_load[3])
|
||
return tool_frame, tool_name
|
||
|
||
|
||
def alg_init(scissorgripper, tools_in_ee):
|
||
"""初始化算法模型、工具坐标系和关节运动限制。"""
|
||
from Robotic_Arm.rm_robot_interface import Algo, rm_force_type_e, rm_robot_arm_model_e
|
||
|
||
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E
|
||
force_type = rm_force_type_e.RM_MODEL_RM_B_E
|
||
algo_handle = Algo(arm_model, force_type)
|
||
tool_frame, tool_name = cal_tool_frame(algo_handle, scissorgripper, tools_in_ee)
|
||
algo_handle.rm_algo_set_toolframe(tool_frame)
|
||
|
||
algo_handle.rm_algo_set_joint_max_limit([150.0, 110.0, 170.0, 130, 175.0, 125.0, 179.0])
|
||
algo_handle.rm_algo_set_joint_min_limit([-150.0, -30.0, -170.0, -130, -175.0, -125.0, -179.0])
|
||
algo_handle.rm_algo_set_joint_max_speed([60.0, 60.0, 60.0, 60.0, 60.0, 60.0, 60.0])
|
||
algo_handle.rm_algo_set_joint_max_acc([
|
||
166.6666717529297,
|
||
166.6666717529297,
|
||
166.6666717529297,
|
||
166.6666717529297,
|
||
166.6666717529297,
|
||
166.6666717529297,
|
||
166.6666717529297,
|
||
])
|
||
algo_handle.rm_algo_set_redundant_parameter_traversal_mode(False)
|
||
|
||
return algo_handle
|
||
|
||
|
||
def peripheral_cfg(
|
||
robot,
|
||
scissorgripper,
|
||
tools_in_ee,
|
||
*,
|
||
set_initial_tool_state: bool = False,
|
||
):
|
||
"""配置机械臂控制器、当前工具坐标系和末端工具通信方式。"""
|
||
# 控制器输出 12V；IO1/IO2 配置为输入，IO3/IO4 配置为输出。
|
||
robot.rm_set_voltage(2, True)
|
||
time.sleep(0.2)
|
||
robot.rm_set_io_mode(1, 0, 50, 2)
|
||
robot.rm_set_io_mode(2, 0, 50, 2)
|
||
robot.rm_set_io_mode(3, 1, 50, 2)
|
||
robot.rm_set_io_mode(4, 1, 50, 2)
|
||
|
||
time.sleep(0.2)
|
||
tool_frame, tool_name = cal_tool_frame(robot, scissorgripper, tools_in_ee)
|
||
robot.rm_set_manual_tool_frame(frame=tool_frame)
|
||
robot.rm_change_tool_frame(tool_name)
|
||
|
||
if scissorgripper == 0:
|
||
# 剪刀夹爪通过工具板数字输出控制。
|
||
robot.rm_set_tool_voltage(voltage_type=2)
|
||
time.sleep(0.2)
|
||
robot.rm_set_tool_IO_mode(1, 1)
|
||
robot.rm_set_tool_IO_mode(2, 1)
|
||
|
||
if set_initial_tool_state:
|
||
robot.rm_set_tool_do_state(1, 0)
|
||
robot.rm_set_tool_do_state(2, 1)
|
||
elif scissorgripper == 1:
|
||
# 电动夹爪通过工具板 Modbus 寄存器控制。
|
||
from Robotic_Arm.rm_robot_interface import rm_peripheral_read_write_params_t
|
||
|
||
modbus_sts = robot.rm_set_modbus_mode(port=1, baudrate=115200, timeout=2)
|
||
if modbus_sts != 0:
|
||
print(f"警告：Modbus 模式设置失败：{modbus_sts}")
|
||
else:
|
||
print("Modbus 模式配置成功")
|
||
|
||
addr = 1
|
||
# 依次设置目标速度、目标力矩、目标加速度和目标减速度。
|
||
reg_value = [255, 60, 255, 255]
|
||
for i, reg_addr in enumerate([11, 12, 13, 14]):
|
||
write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
|
||
robot.rm_write_single_register(write_params, reg_value[i])
|
||
time.sleep(0.5)
|
||
|
||
if set_initial_tool_state:
|
||
set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
|
||
time.sleep(1.5)
|
||
set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)
|
||
|
||
elif scissorgripper == 2:
|
||
# 小型剪刀夹爪通过控制器 DO3/DO4 控制。
|
||
if set_initial_tool_state:
|
||
robot.rm_set_do_state(io_num=3, state=1)
|
||
robot.rm_set_do_state(io_num=4, state=0)
|
||
time.sleep(2)
|
||
robot.rm_set_do_state(io_num=3, state=0)
|
||
robot.rm_set_do_state(io_num=4, state=1)
|
||
|
||
return robot
|
||
|
||
|
||
def set_tool_position(robot, percent, device=1, scissorgripper=0):
|
||
"""设置末端工具开合位置：0.0 表示闭合，1.0 表示完全张开。"""
|
||
if scissorgripper == 1:
|
||
from Robotic_Arm.rm_robot_interface import rm_peripheral_read_write_params_t
|
||
|
||
pos_value = int(percent * 255)
|
||
write_params = rm_peripheral_read_write_params_t(1, 10, device, 1)
|
||
robot.rm_write_single_register(write_params, pos_value)
|
||
# 写触发寄存器后夹爪才会执行目标位置。
|
||
write_params = rm_peripheral_read_write_params_t(1, 15, device, 1)
|
||
time.sleep(0.5)
|
||
robot.rm_write_single_register(write_params, 0x01)
|
||
time.sleep(0.5)
|
||
|
||
elif scissorgripper == 0:
|
||
# 剪刀夹爪只有全闭合和全张开两个状态。
|
||
if percent == 0:
|
||
robot.rm_set_tool_do_state(1, 1)
|
||
robot.rm_set_tool_do_state(2, 0)
|
||
elif percent == 1:
|
||
robot.rm_set_tool_do_state(1, 0)
|
||
robot.rm_set_tool_do_state(2, 1)
|
||
elif scissorgripper == 2:
|
||
# 小型剪刀夹爪同样只有全闭合和全张开两个状态。
|
||
if percent == 0:
|
||
robot.rm_set_do_state(3, 1)
|
||
robot.rm_set_do_state(4, 0)
|
||
elif percent == 1:
|
||
robot.rm_set_do_state(3, 0)
|
||
robot.rm_set_do_state(4, 1)
|
||
|
||
|
||
def tool_exe(robot, scissorgripper, cmd):
|
||
"""根据布尔命令执行末端工具开合。"""
|
||
switch_state = cmd
|
||
if switch_state:
|
||
set_tool_position(robot, 1, device=1, scissorgripper=scissorgripper)
|
||
else:
|
||
set_tool_position(robot, 0, device=1, scissorgripper=scissorgripper)
|