320 lines
12 KiB
Python
Executable File
320 lines
12 KiB
Python
Executable File
"""RM75 机械臂关节反馈、关节透传和停止适配层。"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
import threading
|
|
import time
|
|
from dataclasses import dataclass
|
|
from numbers import Number
|
|
from typing import Any
|
|
|
|
|
|
@dataclass
|
|
class ArmPose:
|
|
x: float
|
|
y: float
|
|
z: float
|
|
rx: float = 0.0
|
|
ry: float = 0.0
|
|
rz: float = 0.0
|
|
|
|
def xyz(self) -> list[float]:
|
|
return [self.x, self.y, self.z]
|
|
|
|
def rpy(self) -> list[float]:
|
|
return [self.rx, self.ry, self.rz]
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class JointStateSnapshot:
|
|
positions: list[float]
|
|
received_at: float
|
|
|
|
|
|
class MockRealManAdapter:
|
|
"""不导入厂商 SDK 的关节状态 mock。"""
|
|
|
|
def __init__(self, initial_joint_degrees: list[float]) -> None:
|
|
if len(initial_joint_degrees) != 7 or not all(
|
|
math.isfinite(value) for value in initial_joint_degrees
|
|
):
|
|
raise ValueError("initial joint pose must contain 7 finite values")
|
|
self._joint_positions = [
|
|
math.radians(value) for value in initial_joint_degrees
|
|
]
|
|
self.last_joint_target: list[float] | None = None
|
|
self.last_tool_open: bool | None = None
|
|
|
|
def connect(self) -> None:
|
|
return
|
|
|
|
def get_latest_joint_state(self) -> JointStateSnapshot:
|
|
return JointStateSnapshot(
|
|
list(self._joint_positions),
|
|
time.monotonic(),
|
|
)
|
|
|
|
def send_joint_target(self, joints: list[float], follow: bool) -> None:
|
|
del follow
|
|
if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
|
|
raise ValueError("joint target must contain 7 finite values")
|
|
self._joint_positions = list(joints)
|
|
self.last_joint_target = list(joints)
|
|
|
|
def stop(self) -> None:
|
|
return
|
|
|
|
def close(self) -> None:
|
|
self.stop()
|
|
|
|
def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
|
|
del config, peripheral_arm
|
|
|
|
def set_tool_enabled(self, open_tool: bool) -> None:
|
|
self.last_tool_open = open_tool
|
|
|
|
|
|
class RealManAdapter:
|
|
"""复用一个睿尔曼 Python API2 连接的关节适配层。"""
|
|
|
|
def __init__(
|
|
self,
|
|
robot_ip: str,
|
|
robot_port: int,
|
|
avoid_singularity: int,
|
|
feedback_period: float,
|
|
logger: Any | None = None,
|
|
configure_safety_limits: bool = True,
|
|
max_line_speed: float = 1.0,
|
|
max_angular_speed: float = 1.5,
|
|
max_line_acc: float = 1.0,
|
|
max_angular_acc: float = 2.0,
|
|
joint_max_speed: float = 180.0,
|
|
joint_max_acc: float = 180.0,
|
|
move_to_initial_pose_on_connect: bool = False,
|
|
initial_joint_pose: list[float] | None = None,
|
|
init_move_speed: int = 20,
|
|
canfd_trajectory_mode: int = 2,
|
|
canfd_radio: int = 0,
|
|
) -> None:
|
|
self._robot_ip = robot_ip
|
|
self._robot_port = robot_port
|
|
self._avoid_singularity = avoid_singularity
|
|
if feedback_period <= 0.0:
|
|
raise ValueError("feedback_period must be positive")
|
|
self._feedback_period = feedback_period
|
|
self._logger = logger
|
|
self._configure_safety_limits = configure_safety_limits
|
|
self._max_line_speed = max_line_speed
|
|
self._max_angular_speed = max_angular_speed
|
|
self._max_line_acc = max_line_acc
|
|
self._max_angular_acc = max_angular_acc
|
|
self._joint_max_speed = joint_max_speed
|
|
self._joint_max_acc = joint_max_acc
|
|
self._move_to_initial_pose_on_connect = move_to_initial_pose_on_connect
|
|
self._initial_joint_pose = initial_joint_pose
|
|
self._init_move_speed = init_move_speed
|
|
self._canfd_trajectory_mode = canfd_trajectory_mode
|
|
self._canfd_radio = canfd_radio
|
|
self._scissorgripper: int | None = None
|
|
self._arm: Any | None = None
|
|
self._joint_state_lock = threading.Lock()
|
|
self._latest_joint_state: JointStateSnapshot | None = None
|
|
self._feedback_stop = threading.Event()
|
|
self._feedback_thread: threading.Thread | None = None
|
|
self._feedback_fault_logged = False
|
|
|
|
def connect(self) -> None:
|
|
try:
|
|
from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e
|
|
except ImportError as exc:
|
|
raise RuntimeError(
|
|
"未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。"
|
|
) from exc
|
|
|
|
self._arm = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
|
|
handle = self._arm.rm_create_robot_arm(self._robot_ip, self._robot_port)
|
|
self._check_robot_handle(handle)
|
|
self._log_info(
|
|
"RealMan connected: "
|
|
f"ip={self._robot_ip}, port={self._robot_port}, "
|
|
f"avoid_singularity={self._avoid_singularity}, "
|
|
"command=rm_movej_canfd"
|
|
)
|
|
if self._configure_safety_limits:
|
|
self._apply_safety_limits()
|
|
if self._move_to_initial_pose_on_connect:
|
|
self._move_to_initial_pose()
|
|
self._feedback_stop.clear()
|
|
self._feedback_thread = threading.Thread(
|
|
target=self._feedback_loop,
|
|
name=f"rm75_feedback_{self._robot_ip}",
|
|
daemon=True,
|
|
)
|
|
self._feedback_thread.start()
|
|
|
|
def get_latest_joint_state(self) -> JointStateSnapshot | None:
|
|
with self._joint_state_lock:
|
|
if self._latest_joint_state is None:
|
|
return None
|
|
return JointStateSnapshot(
|
|
list(self._latest_joint_state.positions),
|
|
self._latest_joint_state.received_at,
|
|
)
|
|
|
|
def send_joint_target(self, joints: list[float], follow: bool) -> None:
|
|
self._require_arm()
|
|
if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
|
|
raise ValueError("joint target must contain 7 finite values")
|
|
ret = self._arm.rm_movej_canfd(
|
|
[math.degrees(value) for value in joints],
|
|
follow,
|
|
0,
|
|
self._canfd_trajectory_mode,
|
|
self._canfd_radio,
|
|
)
|
|
self._check_return(ret, "rm_movej_canfd")
|
|
|
|
def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
|
|
self._require_arm()
|
|
from .fun_peripheral import peripheral_cfg
|
|
|
|
self._scissorgripper = config.scissorgripper
|
|
tool_name = config.tool_name
|
|
self._log_info(
|
|
"开始配置 RealMan 末端外设："
|
|
f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
|
|
f"tool={tool_name}, set_initial_tool_state={config.set_initial_tool_state}"
|
|
)
|
|
peripheral_cfg(
|
|
self._arm,
|
|
config.scissorgripper,
|
|
config.tools_in_ee,
|
|
set_initial_tool_state=config.set_initial_tool_state,
|
|
)
|
|
self._log_info(f"RealMan 末端外设配置完成：arm={peripheral_arm}, tool={tool_name}")
|
|
|
|
def set_tool_enabled(self, open_tool: bool) -> None:
|
|
self._require_arm()
|
|
if self._scissorgripper is None:
|
|
raise RuntimeError("末端外设尚未初始化，无法执行开合命令")
|
|
|
|
from .fun_peripheral import tool_exe
|
|
|
|
tool_exe(self._arm, self._scissorgripper, open_tool)
|
|
|
|
def stop(self) -> None:
|
|
if self._arm is None:
|
|
return
|
|
try:
|
|
self._arm.rm_set_arm_slow_stop()
|
|
except Exception:
|
|
pass
|
|
|
|
def close(self) -> None:
|
|
if self._arm is None:
|
|
return
|
|
self.stop()
|
|
self._feedback_stop.set()
|
|
if self._feedback_thread is not None:
|
|
self._feedback_thread.join(timeout=3.0)
|
|
if self._feedback_thread.is_alive():
|
|
self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。")
|
|
self._feedback_thread = None
|
|
try:
|
|
self._arm.rm_delete_robot_arm()
|
|
finally:
|
|
self._arm = None
|
|
|
|
def _require_arm(self) -> None:
|
|
if self._arm is None:
|
|
raise RuntimeError("睿尔曼机械臂尚未连接")
|
|
|
|
def _feedback_loop(self) -> None:
|
|
while not self._feedback_stop.is_set():
|
|
try:
|
|
self._read_joint_state_once()
|
|
self._feedback_fault_logged = False
|
|
except Exception as exc:
|
|
if not self._feedback_fault_logged:
|
|
self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
|
|
self._feedback_fault_logged = True
|
|
self._feedback_stop.wait(self._feedback_period)
|
|
|
|
def _read_joint_state_once(self) -> None:
|
|
self._require_arm()
|
|
result = self._arm.rm_get_joint_degree()
|
|
self._check_return(result, "rm_get_joint_degree")
|
|
if not isinstance(result, tuple) or len(result) < 2:
|
|
raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
|
|
degrees = result[1]
|
|
if (
|
|
not isinstance(degrees, (list, tuple))
|
|
or len(degrees) != 7
|
|
or not all(isinstance(value, Number) for value in degrees)
|
|
):
|
|
raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}")
|
|
positions = [math.radians(float(value)) for value in degrees]
|
|
if not all(math.isfinite(value) for value in positions):
|
|
raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
|
|
snapshot = JointStateSnapshot(positions, time.monotonic())
|
|
with self._joint_state_lock:
|
|
self._latest_joint_state = snapshot
|
|
|
|
def _apply_safety_limits(self) -> None:
|
|
# 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
|
|
self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
|
|
self._try_call("rm_set_arm_max_line_speed", self._max_line_speed)
|
|
self._try_call("rm_set_arm_max_angular_speed", self._max_angular_speed)
|
|
self._try_call("rm_set_arm_max_line_acc", self._max_line_acc)
|
|
self._try_call("rm_set_arm_max_angular_acc", self._max_angular_acc)
|
|
for joint_index in range(1, 8):
|
|
self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed)
|
|
self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
|
|
|
|
def _move_to_initial_pose(self) -> None:
|
|
if self._initial_joint_pose is None:
|
|
raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
|
|
|
|
ret = self._arm.rm_movej(self._initial_joint_pose, self._init_move_speed, 0, 0, 1)
|
|
self._check_return(ret, "rm_movej(initial_joint_pose)")
|
|
|
|
def _try_call(self, name: str, *args: Any) -> None:
|
|
func = getattr(self._arm, name, None)
|
|
if func is None:
|
|
self._log_warn(f"当前睿尔曼 SDK 不支持 {name}，跳过该安全配置。")
|
|
return
|
|
|
|
try:
|
|
ret = func(*args)
|
|
self._check_return(ret, name)
|
|
except Exception as exc:
|
|
self._log_warn(f"{name} 安全配置失败，继续使用软件侧限幅：{exc}")
|
|
|
|
def _log_warn(self, message: str) -> None:
|
|
if self._logger is not None:
|
|
self._logger.warn(message)
|
|
|
|
def _log_info(self, message: str) -> None:
|
|
if self._logger is not None:
|
|
self._logger.info(message)
|
|
|
|
def _check_robot_handle(self, handle: Any) -> None:
|
|
handle_id = getattr(handle, "id", None)
|
|
if handle_id == -1:
|
|
raise RuntimeError(
|
|
"rm_create_robot_arm failed: TCP may be reachable, but the RealMan "
|
|
f"controller did not return robot info from {self._robot_ip}:{self._robot_port}"
|
|
)
|
|
|
|
@staticmethod
|
|
def _check_return(ret: Any, name: str) -> None:
|
|
code = RealManAdapter._return_code(ret)
|
|
if isinstance(code, int) and code != 0:
|
|
raise RuntimeError(f"{name} failed with code {code}: {ret!r}")
|
|
|
|
@staticmethod
|
|
def _return_code(ret: Any) -> Any:
|
|
return ret[0] if isinstance(ret, tuple) and ret else ret
|