334 lines
12 KiB
Python
Executable File
334 lines
12 KiB
Python
Executable File
"""RM75 机械臂适配层。
|
||
|
||
对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
|
||
选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
from dataclasses import dataclass
|
||
from numbers import Number
|
||
from typing import Any
|
||
|
||
|
||
def _angle_delta(target: float, current: float) -> float:
|
||
return math.atan2(math.sin(target - current), math.cos(target - current))
|
||
|
||
|
||
@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]
|
||
|
||
|
||
class MockRealManAdapter:
|
||
"""无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
|
||
|
||
def __init__(self, initial_pose: list[float], dt: float) -> None:
|
||
self._pose = ArmPose(*initial_pose[:6])
|
||
self._dt = dt
|
||
self.last_velocity = [0.0] * 6
|
||
self.last_tool_open: bool | None = None
|
||
|
||
def connect(self) -> None:
|
||
return
|
||
|
||
def get_current_pose(self) -> ArmPose:
|
||
return self._pose
|
||
|
||
def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
|
||
del follow
|
||
self.last_velocity = [
|
||
(pose.x - self._pose.x) / self._dt,
|
||
(pose.y - self._pose.y) / self._dt,
|
||
(pose.z - self._pose.z) / self._dt,
|
||
_angle_delta(pose.rx, self._pose.rx) / self._dt,
|
||
_angle_delta(pose.ry, self._pose.ry) / self._dt,
|
||
_angle_delta(pose.rz, self._pose.rz) / self._dt,
|
||
]
|
||
self._pose = pose
|
||
|
||
def stop(self) -> None:
|
||
self.last_velocity = [0.0] * 6
|
||
|
||
def close(self) -> None:
|
||
self.stop()
|
||
|
||
def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
|
||
del config_file, 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,
|
||
frame_type: int,
|
||
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
|
||
self._frame_type = frame_type
|
||
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
|
||
|
||
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}, "
|
||
f"frame_type={self._frame_type}, command=rm_movep_canfd"
|
||
)
|
||
if self._configure_safety_limits:
|
||
self._apply_safety_limits()
|
||
if self._move_to_initial_pose_on_connect:
|
||
self._move_to_initial_pose()
|
||
|
||
def get_current_pose(self) -> ArmPose:
|
||
self._require_arm()
|
||
state = self._arm.rm_get_current_arm_state()
|
||
pose = self._find_pose(state)
|
||
if pose is None:
|
||
raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
|
||
return ArmPose(*pose[:6])
|
||
|
||
def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
|
||
self._require_arm()
|
||
ret = self._arm.rm_movep_canfd(
|
||
[pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
|
||
follow,
|
||
self._canfd_trajectory_mode,
|
||
self._canfd_radio,
|
||
)
|
||
self._check_return(ret, "rm_movep_canfd")
|
||
|
||
def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
|
||
self._require_arm()
|
||
from .fun_peripheral import load_peripheral_config, peripheral_cfg
|
||
|
||
config = load_peripheral_config(config_file, peripheral_arm)
|
||
self._scissorgripper = config.scissorgripper
|
||
tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
|
||
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()
|
||
try:
|
||
self._arm.rm_delete_robot_arm()
|
||
finally:
|
||
self._arm = None
|
||
|
||
def _require_arm(self) -> None:
|
||
if self._arm is None:
|
||
raise RuntimeError("睿尔曼机械臂尚未连接")
|
||
|
||
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
|
||
|
||
@classmethod
|
||
def _find_pose(cls, obj: Any) -> list[float] | None:
|
||
# 不同 SDK 版本返回字段可能略有差异，因此递归查找常见 TCP 位姿字段。
|
||
if isinstance(obj, dict):
|
||
for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
|
||
pose = cls._as_pose(obj.get(key))
|
||
if pose is not None:
|
||
return pose
|
||
for value in obj.values():
|
||
pose = cls._find_pose(value)
|
||
if pose is not None:
|
||
return pose
|
||
elif isinstance(obj, (list, tuple)):
|
||
pose = cls._as_pose(obj)
|
||
if pose is not None:
|
||
return pose
|
||
for value in obj:
|
||
pose = cls._find_pose(value)
|
||
if pose is not None:
|
||
return pose
|
||
elif hasattr(obj, "to_dictionary"):
|
||
try:
|
||
return cls._find_pose(obj.to_dictionary(7))
|
||
except TypeError:
|
||
return cls._find_pose(obj.to_dictionary())
|
||
elif hasattr(obj, "to_dict"):
|
||
return cls._find_pose(obj.to_dict())
|
||
else:
|
||
for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
|
||
if hasattr(obj, key):
|
||
pose = cls._as_pose(getattr(obj, key))
|
||
if pose is not None:
|
||
return pose
|
||
return None
|
||
|
||
@staticmethod
|
||
def _as_pose(value: Any) -> list[float] | None:
|
||
if isinstance(value, (list, tuple)) and len(value) >= 6:
|
||
if all(isinstance(item, Number) for item in value[:6]):
|
||
return [float(item) for item in value[:6]]
|
||
if isinstance(value, dict):
|
||
position = value.get("position")
|
||
euler = value.get("euler")
|
||
if isinstance(position, dict) and isinstance(euler, dict):
|
||
keys = ("x", "y", "z")
|
||
rpy_keys = ("rx", "ry", "rz")
|
||
if all(key in position for key in keys) and all(key in euler for key in rpy_keys):
|
||
return [
|
||
float(position["x"]),
|
||
float(position["y"]),
|
||
float(position["z"]),
|
||
float(euler["rx"]),
|
||
float(euler["ry"]),
|
||
float(euler["rz"]),
|
||
]
|
||
if all(hasattr(value, attr) for attr in ("position", "euler")):
|
||
position = getattr(value, "position")
|
||
euler = getattr(value, "euler")
|
||
if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
|
||
hasattr(euler, key) for key in ("rx", "ry", "rz")
|
||
):
|
||
return [
|
||
float(position.x),
|
||
float(position.y),
|
||
float(position.z),
|
||
float(euler.rx),
|
||
float(euler.ry),
|
||
float(euler.rz),
|
||
]
|
||
return None
|