395 lines
15 KiB
Python
Executable File
395 lines
15 KiB
Python
Executable File
"""RM75 机械臂适配层。
|
||
|
||
对上提供统一的当前位姿读取、笛卡尔速度发送和停止接口；对下根据配置
|
||
选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
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]
|
||
|
||
|
||
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_velocity(self, velocity: list[float], follow: bool) -> None:
|
||
del follow
|
||
self.last_velocity = velocity
|
||
# 模拟模式只做简单积分，便于观察控制器是否在按预期更新末端位置。
|
||
self._pose.x += velocity[0] * self._dt
|
||
self._pose.y += velocity[1] * self._dt
|
||
self._pose.z += velocity[2] * self._dt
|
||
self._pose.rx += velocity[3] * self._dt
|
||
self._pose.ry += velocity[4] * self._dt
|
||
self._pose.rz += velocity[5] * self._dt
|
||
|
||
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,
|
||
dt: float,
|
||
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,
|
||
initial_tcp_pose: list[float] | None = None,
|
||
init_move_speed: int = 20,
|
||
command_mode: str = "velocity",
|
||
canfd_trajectory_mode: int = 2,
|
||
canfd_radio: int = 0,
|
||
) -> None:
|
||
self._robot_ip = robot_ip
|
||
self._robot_port = robot_port
|
||
self._dt_ms = int(round(dt * 1000.0))
|
||
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._initial_tcp_pose = initial_tcp_pose
|
||
self._init_move_speed = init_move_speed
|
||
self._command_mode = command_mode
|
||
self._canfd_trajectory_mode = canfd_trajectory_mode
|
||
self._canfd_radio = canfd_radio
|
||
self._scissorgripper: int | None = None
|
||
self._arm: Any | None = None
|
||
if self._command_mode not in ("velocity", "pose_canfd"):
|
||
raise ValueError("command_mode must be one of: velocity, pose_canfd")
|
||
|
||
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}, dt_ms={self._dt_ms}, "
|
||
f"command_mode={self._command_mode}"
|
||
)
|
||
if self._configure_safety_limits:
|
||
self._apply_safety_limits()
|
||
if self._move_to_initial_pose_on_connect:
|
||
self._move_to_initial_pose()
|
||
if self._command_mode == "velocity":
|
||
self._init_movev_canfd()
|
||
else:
|
||
self._log_info("command_mode=pose_canfd，跳过 rm_set_movev_canfd_init。")
|
||
|
||
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_velocity(self, velocity: list[float], follow: bool) -> None:
|
||
self._require_arm()
|
||
if self._command_mode == "pose_canfd":
|
||
return
|
||
ret = self._arm.rm_movev_canfd(velocity, follow, 0, 0)
|
||
self._check_return(ret, "rm_movev_canfd")
|
||
|
||
def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
|
||
self._require_arm()
|
||
if self._command_mode != "pose_canfd":
|
||
raise RuntimeError("send_cartesian_target requires command_mode=pose_canfd")
|
||
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 uses_pose_targets(self) -> bool:
|
||
return self._command_mode == "pose_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:
|
||
if self._command_mode == "velocity":
|
||
self._arm.rm_movev_canfd([0.0] * 6, False, 0, 0)
|
||
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 or self._initial_tcp_pose is None:
|
||
raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose 和 initial_tcp_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)")
|
||
ret = self._arm.rm_movel(self._initial_tcp_pose, self._init_move_speed, 0, 0, 1)
|
||
self._check_return(ret, "rm_movel(initial_tcp_pose)")
|
||
|
||
def _init_movev_canfd(self) -> None:
|
||
attempts = 3
|
||
candidates = [self._avoid_singularity]
|
||
if self._avoid_singularity != 0:
|
||
candidates.append(0)
|
||
|
||
# 某些现场控制器初始化避奇异模式会超时，失败后自动降级到 0 再重试。
|
||
last_ret: Any = None
|
||
for avoid_singularity in candidates:
|
||
if avoid_singularity != self._avoid_singularity:
|
||
self._log_warn("rm_set_movev_canfd_init 降级为 avoid_singularity=0 后重试。")
|
||
|
||
for attempt in range(1, attempts + 1):
|
||
ret = self._arm.rm_set_movev_canfd_init(
|
||
avoid_singularity,
|
||
self._frame_type,
|
||
self._dt_ms,
|
||
)
|
||
last_ret = ret
|
||
code = self._return_code(ret)
|
||
if code == 0:
|
||
self._avoid_singularity = avoid_singularity
|
||
self._log_info(
|
||
"rm_set_movev_canfd_init 成功："
|
||
f"avoid_singularity={avoid_singularity}, "
|
||
f"frame_type={self._frame_type}, dt_ms={self._dt_ms}"
|
||
)
|
||
return
|
||
|
||
self._log_warn(
|
||
"rm_set_movev_canfd_init 失败："
|
||
f"avoid_singularity={avoid_singularity}, "
|
||
f"frame_type={self._frame_type}, dt_ms={self._dt_ms}, "
|
||
f"attempt={attempt}/{attempts}, ret={ret!r}"
|
||
)
|
||
if attempt < attempts:
|
||
time.sleep(0.2)
|
||
|
||
self._check_return(last_ret, "rm_set_movev_canfd_init")
|
||
|
||
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
|