510 lines
19 KiB
Python
Executable File
510 lines
19 KiB
Python
Executable File
"""RM75 机械臂关节反馈、关节透传和停止适配层。"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import ipaddress
|
|
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
|
|
read_duration_ms: float | None = None
|
|
update_interval_ms: float | None = None
|
|
motion_ready: bool = True
|
|
|
|
|
|
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._initial_joint_positions = [
|
|
math.radians(value) for value in initial_joint_degrees
|
|
]
|
|
self._joint_positions = list(self._initial_joint_positions)
|
|
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 read_joint_state(self) -> JointStateSnapshot:
|
|
return self.get_latest_joint_state()
|
|
|
|
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 move_to_initial_pose(self) -> None:
|
|
self._joint_positions = list(self._initial_joint_positions)
|
|
self.last_joint_target = list(self._joint_positions)
|
|
|
|
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,
|
|
realtime_push_host_ip: str,
|
|
realtime_push_port: int,
|
|
realtime_push_cycle_ms: int = 5,
|
|
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
|
|
try:
|
|
self._realtime_push_host_ip = str(
|
|
ipaddress.IPv4Address(realtime_push_host_ip)
|
|
)
|
|
except ipaddress.AddressValueError as exc:
|
|
raise ValueError(
|
|
"realtime_push_host_ip must be a valid IPv4 address"
|
|
) from exc
|
|
if not 1 <= realtime_push_port <= 65535:
|
|
raise ValueError("realtime_push_port must be between 1 and 65535")
|
|
if realtime_push_cycle_ms <= 0 or realtime_push_cycle_ms % 5 != 0:
|
|
raise ValueError(
|
|
"realtime_push_cycle_ms must be a positive multiple of 5"
|
|
)
|
|
self._realtime_push_port = realtime_push_port
|
|
self._realtime_push_cycle_ms = realtime_push_cycle_ms
|
|
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_ready = threading.Event()
|
|
self._realtime_callback: Any | None = None
|
|
self._accept_realtime_feedback = False
|
|
self._feedback_fault_logged = False
|
|
self._last_motion_status: tuple[Any, ...] | None = None
|
|
|
|
def connect(self) -> None:
|
|
try:
|
|
from Robotic_Arm.rm_robot_interface import (
|
|
RoboticArm,
|
|
rm_realtime_arm_state_callback_ptr,
|
|
rm_realtime_push_config_t,
|
|
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)
|
|
try:
|
|
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_ready.clear()
|
|
self._accept_realtime_feedback = True
|
|
self._realtime_callback = rm_realtime_arm_state_callback_ptr(
|
|
self._on_realtime_arm_state
|
|
)
|
|
self._arm.rm_realtime_arm_state_call_back(
|
|
self._realtime_callback
|
|
)
|
|
config = rm_realtime_push_config_t(
|
|
self._realtime_push_cycle_ms // 5,
|
|
True,
|
|
self._realtime_push_port,
|
|
0,
|
|
self._realtime_push_host_ip,
|
|
)
|
|
self._check_return(
|
|
self._arm.rm_set_realtime_push(config),
|
|
"rm_set_realtime_push",
|
|
)
|
|
if not self._feedback_ready.wait(timeout=2.0):
|
|
raise RuntimeError(
|
|
"RealMan UDP realtime feedback did not receive a valid "
|
|
"frame within 2 seconds"
|
|
)
|
|
self._log_info(
|
|
"RealMan UDP realtime feedback ready: "
|
|
f"host={self._realtime_push_host_ip}:"
|
|
f"{self._realtime_push_port}, "
|
|
f"cycle={self._realtime_push_cycle_ms} ms"
|
|
)
|
|
except Exception:
|
|
self._accept_realtime_feedback = False
|
|
try:
|
|
self._arm.rm_delete_robot_arm()
|
|
except Exception:
|
|
pass
|
|
self._arm = None
|
|
self._realtime_callback = None
|
|
raise
|
|
|
|
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,
|
|
self._latest_joint_state.read_duration_ms,
|
|
self._latest_joint_state.update_interval_ms,
|
|
self._latest_joint_state.motion_ready,
|
|
)
|
|
|
|
def read_joint_state(self) -> JointStateSnapshot:
|
|
self._require_arm()
|
|
started_at = time.monotonic()
|
|
result = self._arm.rm_get_joint_degree()
|
|
finished_at = time.monotonic()
|
|
if not isinstance(result, tuple) or len(result) != 2:
|
|
raise RuntimeError(
|
|
f"rm_get_joint_degree returned invalid result: {result!r}"
|
|
)
|
|
self._check_return(result, "rm_get_joint_degree")
|
|
return JointStateSnapshot(
|
|
self._joint_positions_from_degrees(
|
|
result[1],
|
|
"rm_get_joint_degree",
|
|
),
|
|
finished_at,
|
|
(finished_at - started_at) * 1000.0,
|
|
)
|
|
|
|
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._accept_realtime_feedback = False
|
|
self.stop()
|
|
try:
|
|
self._arm.rm_delete_robot_arm()
|
|
finally:
|
|
self._arm = None
|
|
self._realtime_callback = None
|
|
|
|
def _require_arm(self) -> None:
|
|
if self._arm is None:
|
|
raise RuntimeError("睿尔曼机械臂尚未连接")
|
|
|
|
def _on_realtime_arm_state(self, data: Any) -> None:
|
|
if not self._accept_realtime_feedback:
|
|
return
|
|
try:
|
|
if data is None or int(data.errCode) != 0:
|
|
raise ValueError("invalid realtime feedback error code")
|
|
arm_ip = data.arm_ip
|
|
if isinstance(arm_ip, bytes):
|
|
arm_ip = arm_ip.decode("utf-8").split("\x00", 1)[0]
|
|
if str(arm_ip) != self._robot_ip:
|
|
raise ValueError(
|
|
f"unexpected realtime feedback source: {arm_ip}"
|
|
)
|
|
positions = self._joint_positions_from_degrees(
|
|
data.joint_status.joint_position,
|
|
"RM75 UDP feedback",
|
|
)
|
|
joint_enabled = [
|
|
bool(value) for value in data.joint_status.joint_en_flag
|
|
]
|
|
joint_errors = [
|
|
int(value) for value in data.joint_status.joint_err_code
|
|
]
|
|
if len(joint_enabled) != 7 or len(joint_errors) != 7:
|
|
raise ValueError(
|
|
"RM75 UDP feedback must contain 7 joint states"
|
|
)
|
|
arm_error_count = int(data.err.err_len)
|
|
arm_errors = [
|
|
int(value) for value in list(data.err.err)[:arm_error_count]
|
|
]
|
|
arm_errors = [code for code in arm_errors if code != 0]
|
|
arm_current_status = int(data.arm_current_status)
|
|
motion_ready = (
|
|
0 <= arm_current_status <= 8
|
|
and all(joint_enabled)
|
|
and not any(joint_errors)
|
|
and not arm_errors
|
|
)
|
|
motion_status = (
|
|
arm_current_status,
|
|
tuple(joint_enabled),
|
|
tuple(joint_errors),
|
|
tuple(arm_errors),
|
|
motion_ready,
|
|
)
|
|
received_at = time.monotonic()
|
|
with self._joint_state_lock:
|
|
update_interval_ms = (
|
|
None
|
|
if self._latest_joint_state is None
|
|
else (
|
|
received_at
|
|
- self._latest_joint_state.received_at
|
|
)
|
|
* 1000.0
|
|
)
|
|
self._latest_joint_state = JointStateSnapshot(
|
|
positions,
|
|
received_at,
|
|
None,
|
|
update_interval_ms,
|
|
motion_ready,
|
|
)
|
|
self._log_motion_status_transition(motion_status)
|
|
self._feedback_fault_logged = False
|
|
self._feedback_ready.set()
|
|
except Exception as exc:
|
|
with self._joint_state_lock:
|
|
if self._latest_joint_state is not None:
|
|
current = self._latest_joint_state
|
|
self._latest_joint_state = JointStateSnapshot(
|
|
list(current.positions),
|
|
current.received_at,
|
|
current.read_duration_ms,
|
|
current.update_interval_ms,
|
|
False,
|
|
)
|
|
if not self._feedback_fault_logged:
|
|
self._log_warn(
|
|
f"RealMan UDP realtime feedback invalid: {exc}"
|
|
)
|
|
self._feedback_fault_logged = True
|
|
|
|
@staticmethod
|
|
def _joint_positions_from_degrees(
|
|
values: Any,
|
|
source: str,
|
|
) -> list[float]:
|
|
try:
|
|
degrees = list(values)
|
|
except TypeError as exc:
|
|
raise ValueError(
|
|
f"{source} must contain 7 numeric joints"
|
|
) from exc
|
|
if len(degrees) != 7 or not all(
|
|
isinstance(value, Number) for value in degrees
|
|
):
|
|
raise ValueError(
|
|
f"{source} must contain 7 numeric joints"
|
|
)
|
|
positions = [math.radians(float(value)) for value in degrees]
|
|
if not all(math.isfinite(value) for value in positions):
|
|
raise ValueError(f"{source} contains NaN/Inf")
|
|
return positions
|
|
|
|
def _log_motion_status_transition(
|
|
self,
|
|
status: tuple[Any, ...],
|
|
) -> None:
|
|
previous = self._last_motion_status
|
|
if status == previous:
|
|
return
|
|
self._last_motion_status = status
|
|
arm_status, joint_enabled, joint_errors, arm_errors, ready = status
|
|
details = (
|
|
f"arm_status={arm_status}, "
|
|
f"joint_enabled={list(joint_enabled)}, "
|
|
f"joint_errors={list(joint_errors)}, "
|
|
f"arm_errors={list(arm_errors)}"
|
|
)
|
|
if not ready:
|
|
self._log_warn(f"RealMan UDP 报警或掉使能：{details}")
|
|
elif previous is not None and not previous[-1]:
|
|
self._log_info(f"RealMan UDP 运动状态恢复正常：{details}")
|
|
|
|
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:
|
|
self._require_arm()
|
|
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
|