#!/usr/bin/env python3
"""CobottaCtrl に近い I/F を持つ Universal Robots 制御クラス。
K.NAKADA, kengo.nakada@gmail.com
依存:
pip install ur-rtde
公開 I/F の単位:
P座標 [x, y, z, rx, ry, rz] : mm, degree
J座標 [j1, ..., j6] : degree
speed / accel / decel : 0 < 値 <= 100 の百分率
UR の姿勢 rx, ry, rz は Euler 角ではなく rotation vector である。
本クラスでは rotation vector の各成分を degree 表記して公開する。
RTDE 境界では位置を m、角度を rad に変換する。
互換上の注意:
* P/J/I/F は UR コントローラ変数ではなく、このクラス内の仮想変数である。
constructor で自動決定した JSON ファイルへ常時保存する。
* @0 は blend=0、@P は default_blend_radius として move_path() で扱う。
単一点の move() ではブレンドは適用されない。
* Cobotta 固有の PacScript、ファイル、専用ハンド機能は提供しない。
"""
from __future__ import annotations
import json
import math
import re
import threading
import time
from pathlib import Path
from typing import Any, Dict, List, Optional, Sequence, Tuple, Union
from rtde_control import RTDEControlInterface
from rtde_io import RTDEIOInterface
from rtde_receive import RTDEReceiveInterface
from dashboard_client import DashboardClient
from ur.robot_var_manager import RobotVarManager
from ur.ur_error import UrError, UrPrimaryErrorReceiver
from ur.ur_util import MotionMode
from x_logger import XLogger
[docs]
class UrRtde:
"""ur_rtde を CobottaCtrl 風に扱うラッパークラス。
CobottaCtrl の公開 I/F を既存アプリケーションから差し替えやすくする
ことを目的にした薄い互換層である。UR の RTDE は Cobotta/RC8 の
変数系、@P、PacScript、TakeArm/GiveArm と同じ概念を持たないため、
それらは「互換名を持つローカル実装」または「安全な no-op」として
扱う。
位置単位は CobottaCtrl 側に合わせて公開 I/F では mm と degree とし、
RTDE 呼び出し直前に m と rad へ変換する。UR の姿勢成分 rx, ry, rz は
Euler 角ではなく rotation vector である点に注意すること。
"""
_VAR_RE = re.compile(r"(P|J|F|I|IO)(\d+)$", re.IGNORECASE)
_POSITION_VAR_RE = re.compile(r"^(?:@\S+\s+)?([PJ])(\d+)$", re.IGNORECASE)
def __init__(
self,
robot_ip: Optional[str] = None,
logger: Optional[Any] = None,
log_level: Optional[str] = None,
config: Optional[Any] = None,
check_ik: bool = True,
) -> None:
"""UR 制御インスタンスを初期化する。
CobottaCtrl と同じ差し替え思想に従い、constructor にロボット種別固有の
低レベル設定を増やさない。UR では b-CAP / ORiN2 backend が存在しないため、
`backend` や `bcap_port` は公開引数に置かない。
Args:
robot_ip: UR コントローラの IP アドレス。`config.ROBOT_IP` が存在する
場合は config 側を優先する。
logger: 外部 logger。省略時は XLogger を生成する。
log_level: logger 未指定時の XLogger level。
config: 設定オブジェクト。`ROBOT_IP`, `ROBOT_VAR_FILE`,
`UR_USE_DASHBOARD`, `UR_MAX_*` などがあれば使用する。
"""
if logger is None:
import logging
log_level = log_level or "INFO"
logging.basicConfig(level=log_level.upper())
logger = logging.getLogger(__name__)
if config is not None:
if robot_ip is not None:
raise ValueError("robot_ip と config は同時に指定できません。")
# ロボット接続先IPアドレス
robot_ip = getattr(config, "ROBOT_IP", None)
if robot_ip is None:
robot_ip = getattr(config, "UR_IP", None)
if robot_ip is None:
raise ValueError("robot_ip が指定されていません。")
# ------- 各種デフォルトパラメータ
if config is not None:
# ロボット接続先IPアドレス
robot_ip = getattr(config, "ROBOT_IP", getattr(config, "UR_IP", robot_ip))
# Dashboardインターフェースを使用するかどうか
use_dashboard_value = getattr(config, "UR_USE_DASHBOARD", False)
# 永続変数保存用JSONファイル名
variable_file = getattr(
config, "ROBOT_VAR_FILE", getattr(config, "UR_VAR_FILE", None)
)
# 直線移動の最大速度 [m/s]
max_linear_speed_value = getattr(config, "UR_MAX_LINEAR_SPEED", 0.25)
# 直線移動の最大加速度 [m/s^2]
max_linear_acceleration_value = getattr(
config, "UR_MAX_LINEAR_ACCELERATION", 1.2
)
# 関節移動の最大速度 [rad/s]
max_joint_speed_value = getattr(config, "UR_MAX_JOINT_SPEED", 1.05)
# 関節移動の最大加速度 [rad/s^2]
max_joint_acceleration_value = getattr(
config, "UR_MAX_JOINT_ACCELERATION", 1.4
)
# MoveL/MoveJで使用するデフォルトのブレンド半径 [m]
default_blend_radius_value = getattr(
config, "UR_DEFAULT_BLEND_RADIUS", 0.005
)
# 保護停止後の安全再起動を許可するかどうか
allow_restart_safety_value = getattr(
config, "UR_ALLOW_RESTART_SAFETY", False
)
check_ik_value = getattr(
config,
"UR_CHECK_IK",
check_ik,
)
else:
use_dashboard_value = False
variable_file = None
max_linear_speed_value = 0.25
max_linear_acceleration_value = 1.2
max_joint_speed_value = 1.05
max_joint_acceleration_value = 1.4
default_blend_radius_value = 0.005
allow_restart_safety_value = False
check_ik_value = check_ik
use_dashboard = bool(use_dashboard_value)
max_linear_speed = float(max_linear_speed_value)
max_linear_acceleration = float(max_linear_acceleration_value)
max_joint_speed = float(max_joint_speed_value)
max_joint_acceleration = float(max_joint_acceleration_value)
default_blend_radius = float(default_blend_radius_value)
allow_restart_safety = bool(allow_restart_safety_value)
check_ik = bool(check_ik_value)
# 保存ファイル名が未指定の場合は、ロボットIPごとに固有のJSONファイル名を自動生成
if variable_file is None:
safe_ip = str(robot_ip).replace(".", "_").replace(":", "_")
variable_file = f"ur_ctrl_variables_{safe_ip}.json"
self._logger = logger
self._robot_ip = robot_ip # 接続先ロボットのIPアドレス
#####################
# 主要インスタンス
self._rtde_c = None # RTDE Control インターフェース
self._rtde_r = None # RTDE Receive インターフェース
self._rtde_io = None # RTDE IO インターフェース
self._dashboard = None # Dashboard インターフェース
self._primary_error_receiver = UrPrimaryErrorReceiver(
self._robot_ip,
self._logger,
)
#####################
# Dashboard インターフェースを使用するかどうか
self._use_dashboard = use_dashboard
self._check_ik = check_ik
# 排他制御用ロック
self._lock = threading.RLock()
# 永続変数保存用JSONファイル
self._variable_file = Path(variable_file)
# CobottaCtrl / CobottaOrin2 と同じ動的変数思想に合わせる。
# P1, P101, I1, F1 は固定プロパティではなく、名前解決された変数名である。
# UR にはコントローラ内部 P レジスタがないため、値は RobotVarManager に保持し、
# RobotVarManager 側の autosave/autoload により JSON へ保存する。
self._var_manager = RobotVarManager(
logger=logger,
filename=str(variable_file),
autosave=True,
autoload=True,
)
# 現在の動作モード（PTP / LINE）
self._motion_mode = MotionMode.PTP
# 直前の動作モード
self._last_motion_mode = MotionMode.PTP
# 現在の座標系（P：直交座標、J：関節座標）
self._coord_type = "P"
# MoveP 用ブレンド設定
self._path_blend = "@P"
self._speed = 100.0 # 現在の移動速度 [%]
self._jspeed = 100.0 # 現在の関節移動速度 [%]
self._accel = 100.0 # 現在の加速度 [%]
self._decel = 100.0 # 現在の減速度 [%]
self._max_linear_speed = max_linear_speed # 直線移動の最大速度 [m/s]
# 直線移動の最大加速度 [m/s^2]
self._max_linear_acceleration = max_linear_acceleration
# 関節移動の最大速度 [rad/s]
self._max_joint_speed = max_joint_speed
# 関節移動の最大加速度 [rad/s^2]
self._max_joint_acceleration = max_joint_acceleration
# デフォルトのブレンド半径 [m]
self._default_blend_radius = default_blend_radius
# 保護停止後の安全再起動を許可するかどうか
self._allow_restart_safety = allow_restart_safety
# 直近のエラーコード
self._last_error = None
# 直近のエラー詳細情報
self._last_error_info = None
# 非同期命令の実行後エラーを後から関連付けるための情報
self._async_error_capture = None
# 直近の接続処理の成否
self._last_connect_result = False
def __getattr__(self, name: str) -> Any:
"""P1/F1/I1/IO1 形式の属性参照を仮想変数アクセスへ変換する。"""
match = self._VAR_RE.fullmatch(name)
if match:
return self.get_var(match.group(1), int(match.group(2)))
raise AttributeError(f"{type(self).__name__!s} has no attribute {name!r}")
def __setattr__(self, name: str, value: Any) -> None:
"""P1/F1/I1/IO1 形式の属性代入を仮想変数アクセスへ変換する。"""
if name.startswith("_") or "_var_manager" not in self.__dict__:
object.__setattr__(self, name, value)
return
match = self._VAR_RE.fullmatch(name)
if match:
self.set_var(match.group(1), int(match.group(2)), value)
return
object.__setattr__(self, name, value)
# connection -------------------------------------------------------
[docs]
def connect_dashboard(self) -> bool:
"""Low-level API: Dashboard Server へ接続する。
Dashboard は UR の管理系インターフェースである。TP/PolyScope の
safety popup、Power On、Brake Release、Safety Restart を扱う。
この接続が失敗する場合は、IP、ネットワーク、Remote 設定、Dashboard
Server の到達性に問題がある。
"""
return self._connect_dashboard("connect_dashboard")
[docs]
def connect_error_receiver(self) -> bool:
"""Low-level API: Primary Interface のエラー受信経路を接続する。"""
return self._primary_error_receiver.connect()
[docs]
def disconnect_error_receiver(self) -> bool:
"""Low-level API: Primary Interface のエラー受信経路を切断する。"""
return self._primary_error_receiver.disconnect()
[docs]
def is_error_receiver_connected(self) -> bool:
"""Primary Interface のエラー受信状態を返す。"""
return self._primary_error_receiver.is_connected()
[docs]
def connect_receive(self) -> bool:
"""Low-level API: RTDEReceive の状態監視経路を接続する。"""
if self._rtde_r is not None:
if self.is_monitor_connected():
return True
try:
self._logger.info(
"<UR RTDE ACTION> connect_receive: RTDEReceiveInterface()"
)
# RTDE によるロボット状態受信用インターフェース
self._rtde_r = RTDEReceiveInterface(
self._robot_ip,
frequency=-1.0, # ロボットから状態データを何 Hz で受信するか
variables=[],
)
return self.is_monitor_connected()
except Exception as exc:
self._set_last_error(exc, description=str(exc))
self._logger.error("*** UR RTDE ERROR *** connect_receive failed: %s", exc)
self._rtde_r = None
return False
[docs]
def connect_io(self) -> bool:
"""Low-level API: RTDEIO の IO 経路を接続する。"""
if self._rtde_io is not None:
if self.is_io_connected():
return True
try:
self._logger.info("<UR RTDE ACTION> connect_io: RTDEIOInterface()")
self._rtde_io = RTDEIOInterface(self._robot_ip)
return self.is_io_connected()
except Exception as exc:
self._set_last_error(exc, description=str(exc))
self._logger.error("*** UR RTDE ERROR *** connect_io failed: %s", exc)
self._rtde_io = None
return False
[docs]
def connect_control(self) -> bool:
"""Low-level API: RTDEControl のモーション制御経路を接続する。
共通ロボット I/F では take_arm() から呼ばれる。connect() では呼ばない。
"""
return self._open_rtde_control("connect_control")
[docs]
def disconnect_control(self) -> bool:
"""Low-level API: RTDEControl のモーション制御経路だけを切断する。"""
self._cleanup_control_interface()
return not self.is_control_connected()
[docs]
def disconnect(self) -> bool:
self.disconnect_error_receiver()
self._cleanup_interfaces()
return (
self._rtde_c is None
and self._rtde_r is None
and self._rtde_io is None
and self._dashboard is None
)
[docs]
def _connect_dashboard(self, caller: str) -> bool:
"""Dashboard Server へ接続する。
Dashboard は UR の管理系インターフェースであり、TP に相当する
safety popup、Power On、Brake Release、Safety Restart を扱う。
RTDEControl より先に接続する。
"""
if self._dashboard is not None:
return True
try:
self._logger.info(
"<UR DASHBOARD ACTION> %s: DashboardClient.connect()", caller
)
dashboard = DashboardClient(self._robot_ip)
dashboard.connect()
self._dashboard = dashboard
return True
except Exception as exc:
self._set_last_error(exc, description=str(exc))
self._logger.error(
"*** UR DASHBOARD ERROR *** %s: DashboardClient.connect() failed: %s",
caller,
exc,
)
self._dashboard = None
return False
[docs]
def _open_rtde_monitor(self, caller: str) -> bool:
"""RTDEReceive/RTDEIO を開く。
状態監視系を先に確立する。RTDEControl はここでは作らない。
"""
try:
if self._rtde_r is None:
self._logger.info("<UR RTDE ACTION> %s: RTDEReceiveInterface()", caller)
self._rtde_r = RTDEReceiveInterface(self._robot_ip)
if self._rtde_io is None:
self._logger.info("<UR RTDE ACTION> %s: RTDEIOInterface()", caller)
self._rtde_io = RTDEIOInterface(self._robot_ip)
if self._rtde_r is None:
return False
if self._rtde_io is None:
return False
return True
except Exception as exc:
self._set_last_error(exc, description=str(exc))
self._logger.error(
"*** UR RTDE ERROR *** %s: monitor connection failed: %s", caller, exc
)
self._cleanup_rtde_interfaces(include_control=False)
return False
[docs]
def _open_rtde_control(self, caller: str) -> bool:
"""RTDEControl を開く。
Dashboard 復帰と状態監視系の確立後、最後に制御スクリプト開始を試す。
"""
if self._rtde_c is not None:
try:
if self._rtde_c.isConnected():
return True
except Exception as e:
self._logger.error(f"**Error**: {e}")
self._cleanup_control_interface()
try:
self._logger.info("<UR RTDE ACTION> %s: RTDEControlInterface()", caller)
self._rtde_c = RTDEControlInterface(self._robot_ip)
if self._rtde_c is None:
return False
if not self._rtde_c.isConnected():
return False
return True
except Exception as exc:
self._set_last_error(exc, description=str(exc))
self._logger.error(
"*** UR RTDE ERROR *** %s: control connection failed: %s",
caller,
exc,
)
self._cleanup_control_interface()
return False
[docs]
def _interface_is_connected(self, interface: Any) -> bool:
"""ur_rtde / Dashboard interface の接続状態を安全に確認する。"""
if interface is None:
return False
try:
return bool(interface.isConnected())
except AttributeError:
# DashboardClient には isConnected() がない版がある。
# その場合は軽い問い合わせが通るかで判定する。
try:
interface.robotmode()
return True
except Exception as e:
self._logger.error(f"**Error**: {e}")
return False
except Exception as e:
self._logger.error(f"**Error**: {e}")
return False
[docs]
def set_check_ik(self, enabled: bool) -> bool:
"""P座標移動前の逆運動学事前確認を設定する。"""
self._check_ik = bool(enabled)
return True
[docs]
def get_check_ik(self) -> bool:
"""P座標移動前の逆運動学事前確認設定を返す。"""
return bool(self._check_ik)
[docs]
def is_dashboard_connected(self) -> bool:
"""Dashboard Server の通信経路が生きているかを返す。
Dashboard は TP/PolyScope 相当の管理系であり、powerOn(),
brakeRelease(), closeSafetyPopup(), restartSafety() などの
復帰処理の主経路である。
"""
return self._interface_is_connected(self._dashboard)
[docs]
def is_monitor_connected(self) -> bool:
"""RTDEReceive の状態監視経路が生きているかを返す。"""
return self._interface_is_connected(self._rtde_r)
[docs]
def is_io_connected(self) -> bool:
"""RTDEIO の IO 経路が生きているかを返す。"""
return self._interface_is_connected(self._rtde_io)
[docs]
def is_control_connected(self) -> bool:
"""RTDEControl のモーション制御経路が生きているかを返す。"""
return self._interface_is_connected(self._rtde_c)
[docs]
def has_control_interface(self) -> bool:
"""RTDEControl インスタンスを保持しているかを返す。
接続が切れていても、エラー発生前に制御権を取得していたかを
上位層が判断するために使用する。
"""
return self._rtde_c is not None
[docs]
def is_connected(self) -> bool:
"""管理・監視・IO の下位通信経路がすべて接続済みかを返す。"""
return (
self.is_dashboard_connected()
and self.is_monitor_connected()
and self.is_io_connected()
)
[docs]
def is_safety_normal(self) -> bool:
"""安全状態が motion を妨げない状態かを返す。
RTDEReceive が有効な場合は safety_status_bits を優先する。
取得不能時は False とし、未接続を安全側に倒す。
"""
if not self.is_monitor_connected():
return False
try:
safety_mode = self._rtde_r.getSafetyMode()
safety_status_bits = self._rtde_r.getSafetyStatusBits()
flags = self._decode_safety_status_bits(safety_status_bits)
except Exception as e:
self._logger.error(f"**Error**: {e}")
return False
if self._safety_mode_name(safety_mode) in (
"SAFETY_MODE_PROTECTIVE_STOP",
"SAFETY_MODE_RECOVERY",
"SAFETY_MODE_SAFEGUARD_STOP",
"SAFETY_MODE_SYSTEM_EMERGENCY_STOP",
"SAFETY_MODE_ROBOT_EMERGENCY_STOP",
"SAFETY_MODE_VIOLATION",
"SAFETY_MODE_FAULT",
):
return False
for name in UrError._SAFETY_ERROR_PRIORITIES:
if flags.get(name, False):
return False
return True
[docs]
def is_power_on(self) -> bool:
"""ロボット駆動系（サーボ電源）が ON かを返す。
TP/コントローラ本体の電源ではなく、Dashboard powerOn() の対象に
なるロボット駆動系状態を表す。
"""
if not self.is_monitor_connected():
return False
try:
robot_status = self._rtde_r.getRobotStatus()
flags = self._decode_robot_status_bits(robot_status)
if flags.get("power_on", False):
return True
robot_mode_name = self._robot_mode_name(self._rtde_r.getRobotMode())
return robot_mode_name in ("POWER_ON", "IDLE", "RUNNING")
except Exception as e:
self._logger.error(f"**Error**: {e}")
return False
[docs]
def is_motion_ready(self) -> bool:
"""moveJ/moveL などの主要制御を送れる状態かを返す。
通信が一部成功しているだけでは True にしない。安全状態が正常で、
ロボット駆動系が ON で、RTDEControl が接続済みのとき True とする。
"""
return (
self.is_safety_normal()
and self.is_power_on()
and self.is_control_connected()
)
[docs]
def get_connection_status(self) -> Dict[str, bool]:
"""接続・準備状態をまとめて返す。
サーバー側やログ出力で、どの層が失敗しているかを切り分けるために使う。
"""
return {
"dashboard_connected": self.is_dashboard_connected(),
"monitor_connected": self.is_monitor_connected(),
"io_connected": self.is_io_connected(),
"control_connected": self.is_control_connected(),
"connected": self.is_connected(),
"safety_normal": self.is_safety_normal(),
"power_on": self.is_power_on(),
"motion_ready": self.is_motion_ready(),
}
[docs]
def get_version(
self,
rel_toml_path: Optional[Union[str, Path]] = None,
) -> str:
"""実行中の ur_rtde / Polyscope 情報を文字列で返す。
Args:
rel_toml_path: CobottaCtrl 互換用の引数。UR では使用しない。
Returns:
ur_rtde または Polyscope のバージョン情報。取得不能時は空文字列。
"""
del rel_toml_path
try:
if self._dashboard is not None:
return str(self._dashboard.polyscopeVersion())
except Exception as exc:
self._record_error(exc)
try:
import importlib.metadata
return importlib.metadata.version("ur-rtde")
except Exception as e:
self._logger.error(f"**Error**: {e}")
return ""
[docs]
def stop_script(self) -> bool:
"""RTDE制御スクリプトを終了する。通常終了には disconnect() を使う。"""
try:
self._control().stopScript()
return True
except Exception as exc:
return self._fail(exc)
# variables --------------------------------------------------------
[docs]
def _load_variables(self) -> None:
"""RobotVarManager へ委譲して仮想ロボット変数を読み込む。
互換用の薄いラッパーである。UrRtde 自身は JSON を直接解釈しない。
"""
try:
self._var_manager.load(str(self._variable_file))
except FileNotFoundError:
return
except Exception as exc:
self._record_error(exc)
[docs]
def _save_variables(self) -> None:
"""RobotVarManager へ委譲して仮想ロボット変数を保存する。"""
try:
self._var_manager.save(str(self._variable_file))
except Exception as exc:
self._record_error(exc)
[docs]
def _var_name(
self,
kind_or_combined: str,
id_num: Optional[int],
) -> Union[Tuple[str, int], bool]:
"""P1/J2/I3/F4/IO5 形式、または kind と番号の分離指定を正規化する。
内部では COBOTTA 互換の仮想変数名を `(kind, index)` に分解して扱う。"""
if id_num is not None:
kind = kind_or_combined.upper()
if kind not in ("P", "J", "I", "F", "IO"):
raise ValueError(f"unknown variable kind: {kind}")
return kind, int(id_num)
match = self._POSITION_VAR_RE.fullmatch(kind_or_combined)
if not match:
# raise ValueError(f"invalid variable name: {kind_or_combined}")
self._logger.error(f"invalid variable name: {kind_or_combined}")
return False
return match.group(1).upper(), int(match.group(2))
[docs]
def get_var(self, kind_or_combined: str, id_num: Optional[int] = None) -> Any:
"""COBOTTA 互換の仮想変数を取得する。
P/J/I/F は RobotVarManager が保持する仮想変数であり、IO だけは実 IO へ転送する。"""
kind, index = self._var_name(kind_or_combined, id_num)
if kind == "IO":
return self.get_IO(index)
return self._var_manager.get_var(kind, index, default=False)
[docs]
def set_var(
self, kind_or_combined: str, id_or_value: Any, value: Any = None
) -> bool:
"""COBOTTA 互換の仮想変数を設定する。
P/J は 6 成分、I は整数、F は浮動小数点へ正規化してから保存する。"""
try:
if value is None:
kind, index = self._var_name(kind_or_combined, None)
resolved = id_or_value
else:
kind, index = self._var_name(kind_or_combined, int(id_or_value))
resolved = value
if kind == "IO":
return self.set_IO(index, resolved)
if kind in ("P", "J"):
resolved = self._six(resolved, kind)
elif kind == "I":
resolved = int(resolved)
else:
resolved = float(resolved)
self._var_manager.set_var(kind, index, resolved)
return True
except Exception as exc:
return self._fail(exc)
[docs]
def get_P(
self,
id_num: int,
) -> Union[List[float], bool]:
"""P 型仮想変数を取得する。
戻り値の単位は公開 I/F と同じく mm/degree である。"""
return self.get_var("P", id_num)
[docs]
def set_P(
self,
id_num: int,
value: Sequence[Union[int, float]],
) -> bool:
"""P 型仮想変数を設定する。
値は 6 成分の公開 I/F 座標として扱う。"""
return self.set_var("P", id_num, value)
[docs]
def get_J(
self,
id_num: int,
) -> Union[List[float], bool]:
"""J 型仮想変数を取得する。
戻り値の単位は degree である。"""
return self.get_var("J", id_num)
[docs]
def set_J(
self,
id_num: int,
value: Sequence[Union[int, float]],
) -> bool:
"""J 型仮想変数を設定する。
値は 6 軸の degree 指定として扱う。"""
return self.set_var("J", id_num, value)
[docs]
def teach(
self,
kind_or_combined: str = "P",
id_num: Optional[int] = None,
*,
save: bool = True,
) -> bool:
"""現在のロボット状態を指定変数へ教示する。
CobottaCtrl.teach() と同じ公開 I/F である。`set_P()` は
呼び出し側が与えた値を代入する通常の setter であり、`teach()` は
実機から現在状態を読み取って `P10` や `J1` に記録する操作である。
UR には RC8/FANUC のようなコントローラ内部位置レジスタがない。
そのため、教示値は RobotVarManager の仮想変数として保持し、
JSON ファイルへ保存する。
Args:
kind_or_combined: `"P"`, `"J"`, `"P10"`, `"J1"` 形式の変数指定。
id_num: 変数番号。結合済み名の場合は省略できる。
save: `True` の場合、教示後に JSON へ即保存する。
Returns:
成功時 True、失敗時 False。
"""
try:
if id_num is None:
match = re.fullmatch(r"(P|J)(\d+)$", str(kind_or_combined).upper())
if not match:
raise ValueError("teach() requires 'P', 'J', 'P10', or 'J1'")
kind, index = match.group(1), int(match.group(2))
else:
kind, index = str(kind_or_combined).upper(), int(id_num)
if kind == "P":
value = self.current_position
elif kind == "J":
value = self.current_angle
else:
raise ValueError("teach() supports only P or J variables")
if value is False:
return False
# set_var() は CobottaCtrl と同じ通常の代入経路である。
# teach() では「実機現在値を読み取って代入する」点だけが異なる。
ok = self.set_var(kind, index, value)
if ok and save:
self._save_variables()
return bool(ok)
except Exception as exc:
return self._fail(exc)
[docs]
def get_I(
self,
id_num: int,
) -> Union[int, bool]:
"""I 型仮想変数を取得する。"""
return self.get_var("I", id_num)
[docs]
def set_I(
self,
id_num: int,
value: int,
) -> bool:
"""I 型仮想変数を設定する。"""
return self.set_var("I", id_num, value)
[docs]
def get_F(
self,
id_num: int,
) -> Union[float, bool]:
"""F 型仮想変数を取得する。"""
return self.get_var("F", id_num)
[docs]
def set_F(
self,
id_num: int,
value: float,
) -> bool:
"""F 型仮想変数を設定する。"""
return self.set_var("F", id_num, value)
[docs]
def get_all_var(self) -> Dict[str, Any]:
"""保持中の仮想変数をまとめて取得する。
主にデバッグ、保存内容確認、server I/F 確認用である。"""
return dict(self._var_manager._vars)
[docs]
def is_empty_position(
self,
id_num: Union[int, str],
coord_type: Optional[str] = None,
tol: Optional[float] = None,
) -> bool:
"""P/J 変数が未登録かどうかを返す。
Cobotta ではコントローラ変数の空状態を調べる用途で使う。UR では
ローカル JSON/メモリ上の P/J 変数登録状態を確認する。
"""
if isinstance(id_num, str):
kind, index = self._var_name(id_num, None)
else:
kind = (coord_type or "P").upper()
index = int(id_num)
if kind not in ("P", "J"):
return True
return not self._var_manager.has_var(kind, index)
# conversion -------------------------------------------------------
[docs]
@staticmethod
def _six(
values: Sequence[Union[int, float]],
name: str,
) -> List[float]:
"""6 成分の数値列を検証し、float list に変換する。
P/J 座標の内部正規化で使う。offset の 3 成分補完とは別の厳密検証である。"""
if not isinstance(values, (list, tuple)) or len(values) < 6:
raise ValueError(f"{name} must contain six numeric values")
result = [float(value) for value in values[:6]]
if not all(math.isfinite(value) for value in result):
raise ValueError(f"{name} contains non-finite values")
return result
[docs]
@classmethod
def _pose_to_ur(
cls,
pose: Sequence[Union[int, float]],
) -> List[float]:
"""公開 I/F の P 座標を UR RTDE の pose 表現へ変換する。
位置は mm から m、rotation-vector 成分は degree から rad へ変換する。"""
x, y, z, rx, ry, rz = cls._six(pose, "pose")
return [
x / 1000.0,
y / 1000.0,
z / 1000.0,
math.radians(rx),
math.radians(ry),
math.radians(rz),
]
[docs]
@classmethod
def _pose_from_ur(
cls,
pose: Sequence[Union[int, float]],
) -> List[float]:
"""UR RTDE の pose 表現を公開 I/F の P 座標へ変換する。
位置は m から mm、rotation-vector 成分は rad から degree へ変換する。"""
x, y, z, rx, ry, rz = cls._six(pose, "UR pose")
return [
x * 1000.0,
y * 1000.0,
z * 1000.0,
math.degrees(rx),
math.degrees(ry),
math.degrees(rz),
]
[docs]
@classmethod
def _joints_to_ur(
cls,
joints: Sequence[Union[int, float]],
) -> List[float]:
"""公開 I/F の J 座標を UR RTDE の関節角へ変換する。
degree から rad へ変換する。"""
return [math.radians(value) for value in cls._six(joints, "joints")]
[docs]
@classmethod
def _joints_from_ur(
cls,
joints: Sequence[Union[int, float]],
) -> List[float]:
"""UR RTDE の関節角を公開 I/F の J 座標へ変換する。
rad から degree へ変換する。"""
return [math.degrees(value) for value in cls._six(joints, "UR joints")]
# configuration ----------------------------------------------------
[docs]
@staticmethod
def _percent(
value: Union[int, float],
) -> float:
"""COBOTTA 互換の速度・加速度百分率を検証する。
0 より大きく 100 以下の値だけを許可する。"""
result = float(value)
if not 0.0 < result <= 100.0:
raise ValueError("percentage must be > 0 and <= 100")
return result
[docs]
@staticmethod
def _mode(
mode: Union[int, str, MotionMode],
) -> MotionMode:
"""COBOTTA 互換の motion mode 指定を MotionMode へ正規化する。
整数、文字列、MotionMode インスタンスを受け付ける。"""
if isinstance(mode, MotionMode):
return mode
if isinstance(mode, int):
return {
1: MotionMode.PTP,
2: MotionMode.LINE,
3: MotionMode.CIRCLE,
4: MotionMode.SPLINE,
}[mode]
return MotionMode(str(mode).upper())
@property
def motion_mode(self) -> MotionMode:
"""現在の既定 motion mode を返す。"""
return self._motion_mode
@motion_mode.setter
def motion_mode(
self,
value: Union[int, str, MotionMode],
) -> None:
"""motion_mode プロパティ setter である。"""
self.set_motion_mode(value)
[docs]
def set_motion_mode(
self,
mode: Union[int, str, MotionMode],
) -> bool:
"""既定の motion mode を設定する。"""
try:
self._motion_mode = self._mode(mode)
return True
except Exception as exc:
return self._fail(exc)
[docs]
def get_motion_mode(self) -> MotionMode:
"""現在の既定 motion mode を返す。"""
return self._motion_mode
@property
def coord_type(self) -> str:
"""現在の既定座標種別を返す。"""
return self._coord_type
@coord_type.setter
def coord_type(
self,
value: str,
) -> None:
"""coord_type プロパティ setter である。"""
self.set_coord_type(value)
[docs]
def set_coord_type(
self,
coord_type: str,
) -> bool:
"""既定の座標種別を設定する。
`P` は TCP pose、`J` は関節角である。"""
value = str(coord_type).upper()
if value not in ("P", "J"):
return self._fail(ValueError("coord_type must be P or J"))
self._coord_type = value
return True
[docs]
def get_coord_type(self) -> str:
"""現在の既定座標種別を返す。"""
return self._coord_type
@property
def path_blend(self) -> str:
"""現在のパスブレンド指定を返す。"""
return self._path_blend
@path_blend.setter
def path_blend(
self,
value: str,
) -> None:
"""path_blend プロパティ setter である。"""
self.set_path_blend(value)
[docs]
def set_path_blend(
self,
path_blend: str,
) -> bool:
"""パスブレンド指定を保存する。
`@0` はブレンドなし、`@P` は既定ブレンド半径を使う互換指定である。"""
self._path_blend = str(path_blend)
return True
[docs]
def get_path_blend(self) -> str:
"""現在のパスブレンド指定を返す。"""
return self._path_blend
[docs]
def set_speed(
self,
speed: Union[int, float],
) -> bool:
"""直交移動系の速度百分率を設定する。"""
try:
self._speed = self._percent(speed)
return True
except Exception as exc:
return self._fail(exc)
[docs]
def get_speed(self) -> float:
"""直交移動系の速度百分率を返す。"""
return self._speed
[docs]
def set_jspeed(
self,
speed: Union[int, float],
) -> bool:
"""関節移動系の速度百分率を設定する。"""
try:
self._jspeed = self._percent(speed)
return True
except Exception as exc:
return self._fail(exc)
[docs]
def get_jspeed(self) -> float:
"""関節移動系の速度百分率を返す。"""
return self._jspeed
[docs]
def set_accel(
self,
accel: Union[int, float],
) -> bool:
"""直交移動系の加速度百分率を設定する。"""
try:
self._accel = self._percent(accel)
return True
except Exception as exc:
return self._fail(exc)
[docs]
def get_accel(self) -> float:
"""直交移動系の加速度百分率を返す。"""
return self._accel
[docs]
def set_decel(
self,
decel: Union[int, float],
) -> bool:
"""停止時の減速度百分率を設定する。"""
try:
self._decel = self._percent(decel)
return True
except Exception as exc:
return self._fail(exc)
[docs]
def get_decel(self) -> float:
"""停止時の減速度百分率を返す。"""
return self._decel
@property
def speed(self) -> float:
"""直交移動系の速度百分率を返す。"""
return self._speed
@property
def jspeed(self) -> float:
"""関節移動系の速度百分率を返す。"""
return self._jspeed
@property
def accel(self) -> float:
"""直交移動系の加速度百分率を返す。"""
return self._accel
@property
def decel(self) -> float:
"""停止時の減速度百分率を返す。"""
return self._decel
@property
def extspeed(self) -> float:
"""外部軸速度。UR 単体では speed と同義の互換プロパティ。"""
return self._speed
@property
def extaccel(self) -> float:
"""外部軸加速度。UR 単体では accel と同義の互換プロパティ。"""
return self._accel
@property
def extdecel(self) -> float:
"""外部軸減速度。UR 単体では decel と同義の互換プロパティ。"""
return self._decel
[docs]
def set_extspeed(
self,
speed: Union[int, float],
) -> bool:
"""外部軸速度の互換 setter である。
UR 単体では通常速度へ委譲する。"""
return self.set_speed(speed)
[docs]
def get_extspeed(self) -> float:
"""外部軸速度の互換 getter である。
UR 単体では通常速度を返す。"""
return self._speed
[docs]
def set_extaccel(
self,
extaccel: Union[int, float],
) -> bool:
"""外部軸加速度の互換 setter である。
UR 単体では通常加速度へ委譲する。"""
return self.set_accel(extaccel)
[docs]
def get_extaccel(self) -> float:
"""外部軸加速度の互換 getter である。
UR 単体では通常加速度を返す。"""
return self._accel
[docs]
def set_extdecel(
self,
extdecel: Union[int, float],
) -> bool:
"""外部軸減速度の互換 setter である。
UR 単体では通常減速度へ委譲する。"""
return self.set_decel(extdecel)
[docs]
def get_extdecel(self) -> float:
"""外部軸減速度の互換 getter である。
UR 単体では通常減速度を返す。"""
return self._decel
@property
def lock(self) -> bool:
"""Cobotta 互換名。UR では明示的なロック状態を持たない。"""
return False
@property
def servo_on(self) -> bool:
"""サーボ/電源が動作可能かを推定して返す。"""
return self.is_connected() and not bool(
self.state().get("emergency_stopped", True)
)
@property
def direct_mode(self) -> bool:
"""UR は常時 direct mode 相当として扱う。"""
return True
[docs]
def get_direct_mode(self) -> bool:
"""direct_mode 互換プロパティを返す。"""
return self.direct_mode
[docs]
def _linear_params(
self,
speed: Optional[Union[int, float]],
accel: Optional[Union[int, float]],
) -> Tuple[float, float]:
"""百分率指定から UR RTDE の moveL 用速度・加速度へ変換する。
戻り値の単位は m/s と m/s^2 である。"""
if speed is None:
speed_pct = self._speed
else:
speed_pct = self._percent(speed)
if accel is None:
accel_pct = self._accel
else:
accel_pct = self._percent(accel)
return (
self._max_linear_speed * speed_pct / 100.0,
self._max_linear_acceleration * accel_pct / 100.0,
)
[docs]
def _joint_params(
self,
speed: Optional[Union[int, float]],
accel: Optional[Union[int, float]],
) -> Tuple[float, float]:
"""百分率指定から UR RTDE の moveJ 用速度・加速度へ変換する。
戻り値の単位は rad/s と rad/s^2 である。"""
if speed is None:
speed_pct = self._jspeed
else:
speed_pct = self._percent(speed)
if accel is None:
accel_pct = self._accel
else:
accel_pct = self._percent(accel)
return (
self._max_joint_speed * speed_pct / 100.0,
self._max_joint_acceleration * accel_pct / 100.0,
)
# state ------------------------------------------------------------
@property
def current_position(self) -> Union[List[float], bool]:
"""現在 TCP pose を公開 I/F 単位で返す。"""
return self.get_current_position()
@property
def current_angle(self) -> Union[List[float], bool]:
"""現在関節角を公開 I/F 単位で返す。"""
return self.get_current_angle()
[docs]
def get_current_position(self) -> Union[List[float], bool]:
"""現在 TCP pose を取得する。
公開 I/F として mm/degree rotation-vector の 6 成分を返す。"""
try:
return self._pose_from_ur(self._receive().getActualTCPPose())
except Exception as exc:
return self._fail(exc)
[docs]
def get_current_pose(self) -> Union[List[float], bool]:
"""get_current_position() の互換別名である。"""
return self.get_current_position()
[docs]
def get_current_angle(self) -> Union[List[float], bool]:
"""現在関節角を取得する。
公開 I/F として degree の 6 成分を返す。"""
try:
return self._joints_from_ur(self._receive().getActualQ())
except Exception as exc:
return self._fail(exc)
@property
def task(self) -> str:
"""現在の制御タスク名を返す。
UR では RTDE として扱う。"""
return "RTDE"
@property
def task_state(self) -> Dict[str, Any]:
"""現在のタスク状態を返す。
UR では state() の結果を返す。"""
return self.state()
[docs]
def get_task_status(self) -> Dict[str, Any]:
"""タスク状態の互換 I/F である。
UR では RTDE/Dashboard から集約した state() を返す。"""
return self.state()
[docs]
def is_running(self) -> bool:
"""UR 制御プログラムが実行中かを返す。
RTDEControlInterface の isProgramRunning() を用いる。"""
try:
return bool(self._control().isProgramRunning())
except Exception as e:
self._logger.error(f"**Error**: {e}")
return False
@property
def busy_status(self) -> bool:
"""動作中状態を返す互換プロパティである。"""
return self.is_running()
[docs]
def motion_complete(self) -> bool:
"""モーション完了状態を返す。
UR RTDE の isSteady() を優先し、失敗時は is_running() の否定で補完する。"""
try:
return bool(self._control().isSteady())
except Exception as e:
self._logger.error(f"**Error**: {e}")
return not self.is_running()
[docs]
def get_motion_complete(self) -> bool:
"""motion_complete() の互換 getter である。"""
return self.motion_complete()
[docs]
def wait_for_complete(
self, timeout: Optional[float] = None, interval: float = 0.02
) -> bool:
"""モーション完了まで待機する。
server から長時間呼び出す場合は timeout 指定を推奨する。"""
started = time.monotonic()
while not self.motion_complete():
if timeout is not None and time.monotonic() - started >= timeout:
return False
time.sleep(interval)
return True
[docs]
def state(self) -> Dict[str, Any]:
"""UR の現在状態を RTDE / Dashboard から取得する。
Cobotta/RC8 の `error_code` と異なり、UR RTDE から取得できるのは
主に robot mode、safety mode、safety status bits である。PolyScope の
ログ画面に表示される詳細なエラー本文を RTDE から直接取得する標準 API は
ないため、このメソッドでは CLI 側で判断できる状態情報を集約する。
"""
try:
receiver = self._receive()
robot_mode = self._safe_call(receiver.getRobotMode)
robot_status = self._safe_call(receiver.getRobotStatus)
safety_mode = self._safe_call(receiver.getSafetyMode)
safety_status_bits = self._safe_call(receiver.getSafetyStatusBits)
safety_flags = self._decode_safety_status_bits(safety_status_bits)
robot_status_flags = self._decode_robot_status_bits(robot_status)
state_diagnosis = self._diagnose_robot_state(
robot_mode,
robot_status_flags,
safety_mode,
safety_flags,
)
dashboard = self._dashboard_status()
connection_status = self.get_connection_status()
return {
"connected": self.is_connected(),
"ready": connection_status.get("ready", False),
"motion_ready": connection_status.get("motion_ready", False),
"connection_status": connection_status,
"running": self.is_running(),
"motion_complete": self.motion_complete(),
"robot_mode": robot_mode,
"robot_mode_name": self._robot_mode_name(robot_mode),
"robot_mode_description": self._robot_mode_description(robot_mode),
"robot_status_bits": robot_status,
"robot_status_flags": robot_status_flags,
"safety_mode": safety_mode,
"safety_mode_name": self._safety_mode_name(safety_mode),
"safety_mode_description": self._safety_mode_description(safety_mode),
"safety_status_bits": safety_status_bits,
"safety_status_flags": safety_flags,
"state_diagnosis": state_diagnosis,
"protective_stopped": bool(
safety_flags.get("protective_stopped", False)
),
"emergency_stopped": bool(safety_flags.get("emergency_stopped", False)),
"dashboard": dashboard,
}
except Exception as exc:
return {"connected": self.is_connected(), "error": str(exc)}
[docs]
def _safe_call(self, func: Any, default: Any = None) -> Any:
"""状態取得用の補助関数。
エラー表示中にさらに例外を連鎖させないため、個別の RTDE/Dashboard
問い合わせ失敗は `default` に落とす。
"""
try:
return func()
except Exception as e:
self._logger.error(f"**Error**: {e}")
return default
[docs]
@staticmethod
def _robot_mode_name(robot_mode: Any) -> str:
"""RTDE robot mode の整数値を可読名へ変換する。"""
if robot_mode is None:
return "UNKNOWN"
try:
return UrError._ROBOT_MODE_NAMES.get(
int(robot_mode), f"UNKNOWN({robot_mode})"
)
except Exception:
return f"UNKNOWN({robot_mode})"
[docs]
@staticmethod
def _robot_mode_description(robot_mode: Any) -> str:
"""RTDE robot mode の意味を返す。"""
if robot_mode is None:
return "Robot mode is unknown"
try:
value = int(robot_mode)
except Exception:
return f"Robot mode is unknown: {robot_mode}"
return UrError._ROBOT_MODE_DESCRIPTIONS.get(
value,
f"Unknown robot mode value: {robot_mode}",
)
[docs]
@staticmethod
def _safety_mode_name(safety_mode: Any) -> str:
"""RTDE safety mode の整数値を可読名へ変換する。"""
if safety_mode is None:
return "UNKNOWN"
try:
value = int(safety_mode)
except Exception:
return f"UNKNOWN({safety_mode})"
return UrError._SAFETY_MODE_NAMES.get(
value,
f"UNKNOWN({safety_mode})",
)
[docs]
@staticmethod
def _safety_mode_description(safety_mode: Any) -> str:
"""RTDE safety mode の意味を返す。"""
if safety_mode is None:
return "Safety mode is unknown"
try:
value = int(safety_mode)
except Exception:
return f"Safety mode is unknown: {safety_mode}"
return UrError._SAFETY_MODE_DESCRIPTIONS.get(
value,
f"Unknown safety mode value: {safety_mode}",
)
[docs]
def _decode_robot_status_bits(self, status_bits: Any) -> Dict[str, bool]:
"""RTDE robot status bit を dict へ展開する。"""
try:
value = int(status_bits)
except Exception as e:
self._logger.error(f"**Error**: {e}")
value = 0
return {
"power_on": bool(value & (1 << 0)),
"program_running": bool(value & (1 << 1)),
"teach_button_pressed": bool(value & (1 << 2)),
"power_button_pressed": bool(value & (1 << 3)),
}
[docs]
def _decode_safety_status_bits(self, status_bits: Any) -> Dict[str, bool]:
"""RTDE safety status bit を dict へ展開する。"""
try:
value = int(status_bits)
except Exception as e:
self._logger.error(f"**Error**: {e}")
value = 0
return {
name: bool(value & (1 << bit))
for bit, name in UrError._SAFETY_STATUS_BIT_NAMES.items()
}
[docs]
@classmethod
def _describe_flags(
cls,
flags: Dict[str, bool],
descriptions: Dict[str, str],
) -> List[str]:
"""bit flag を name=value(日本語説明) の list へ変換する。"""
result = []
for name, enabled in flags.items():
description = descriptions.get(name, "説明未定義")
result.append(f"{name}={enabled}({description})")
return result
[docs]
@classmethod
def _active_flag_names(cls, flags: Dict[str, bool]) -> List[str]:
"""有効な flag 名だけを取り出す。"""
result = []
for name, enabled in flags.items():
if enabled:
result.append(name)
return result
[docs]
@classmethod
def _diagnose_robot_state(
cls,
robot_mode: Any,
robot_status_flags: Dict[str, bool],
safety_mode: Any,
safety_status_flags: Dict[str, bool],
) -> str:
"""robot/safety 状態を組み合わせ、次に見るべき点を返す。
単独の数値名だけでは誤読しやすいため、robot_mode、robot_status、
safety_mode、safety_status を組み合わせる。
"""
robot_mode_name = cls._robot_mode_name(robot_mode)
safety_mode_name = cls._safety_mode_name(safety_mode)
summary = cls._format_state_summary(
robot_mode,
robot_status_flags,
safety_mode,
safety_status_flags,
)
judgment = (
"状態遷移中または通常外状態。robot_mode と safety_mode を継続監視する"
)
action = "Dashboard/TP の状態を確認する"
if safety_status_flags.get("system_emergency_stopped", False):
judgment = "System Emergency Stop が有効"
action = "非常停止系を物理的に解除する。ソフトだけでは復帰不可"
elif safety_status_flags.get("robot_emergency_stopped", False):
judgment = "Robot Emergency Stop が有効"
action = "非常停止系を物理的に解除する。ソフトだけでは復帰不可"
elif safety_status_flags.get("emergency_stopped", False):
judgment = "Emergency Stop が有効"
action = "非常停止系を物理的に解除する。ソフトだけでは復帰不可"
elif (
safety_status_flags.get("fault", False)
or safety_mode_name == "SAFETY_MODE_FAULT"
):
judgment = "Safety Fault が残っている。TP に表示された安全系エラーが主原因"
action = "TP のエラー内容を確認し、原因除去後に closeSafetyPopup() と restartSafety() を行う"
elif (
safety_status_flags.get("violation", False)
or safety_mode_name == "SAFETY_MODE_VIOLATION"
):
judgment = "Safety Violation が残っている。安全制限違反が主原因"
action = "原因除去後に closeSafetyPopup() と restartSafety() を行う"
elif (
safety_status_flags.get("protective_stopped", False)
or safety_mode_name == "SAFETY_MODE_PROTECTIVE_STOP"
):
judgment = "Protective Stop 中"
action = "原因確認後に closeSafetyPopup() と unlockProtectiveStop() を行う"
elif (
safety_status_flags.get("safeguard_stopped", False)
or safety_mode_name == "SAFETY_MODE_SAFEGUARD_STOP"
):
judgment = "Safeguard Stop 中。外部安全入力が停止要求中"
action = (
"安全扉、ライトカーテン、外部安全入力を解除する。ソフトだけでは復帰不可"
)
elif robot_mode_name == "POWER_OFF":
if robot_status_flags.get("power_on", False):
judgment = "状態取得タイミング差の可能性。robot_mode は POWER_OFF だが robot_status power_on は True"
action = "短時間待って再取得し、Dashboard robotmode も確認する"
else:
judgment = "安全系は通常だがロボット駆動系が Power Off。TP/コントローラ電源 OFF ではない"
action = "Dashboard powerOn() を実行し、robotmode が IDLE などへ遷移するまで待つ"
elif robot_mode_name == "POWER_ON":
judgment = "ロボット駆動系は Power On。ブレーキ解除または状態遷移待ちが必要"
action = "Dashboard brakeRelease() を実行し、robotmode が RUNNING/IDLE 側へ遷移するまで待つ"
elif robot_mode_name == "IDLE":
judgment = "ロボットは待機状態。安全系は制御開始を妨げていない可能性が高い"
action = "RTDEControlInterface 作成または制御スクリプト開始を試す"
elif robot_mode_name == "RUNNING":
judgment = (
"ロボットは実行状態。UR プログラムまたは RTDE control script が動作中"
)
action = "既存プログラム/RTDE セッションの競合がないか確認する"
elif robot_mode_name == "BOOTING":
judgment = "ロボット制御系が起動中"
action = "状態遷移完了まで待って再取得する"
elif robot_mode_name == "CONFIRM_SAFETY":
judgment = "TP 側で安全確認待ち"
action = "TP 上の安全確認を完了する"
return "%s; judgment=%s; action=%s" % (summary, judgment, action)
[docs]
@classmethod
def _is_error_state(cls, state: Dict[str, Any]) -> bool:
"""state() の結果からエラー状態かどうかを判定する。"""
if state.get("error"):
return True
flags = state.get("safety_status_flags") or {}
return any(
bool(flags.get(name, False)) for name in UrError._SAFETY_ERROR_PRIORITIES
)
[docs]
@classmethod
def _state_error_code(cls, state: Dict[str, Any]) -> int:
"""UR 状態から互換用の疑似エラーコードを返す。
UR RTDE では RC8 のようなエラー番号列を取得できないため、CLI 側で
使えるように負数の疑似コードへ正規化する。
"""
if state.get("error"):
return -9000
flags = state.get("safety_status_flags") or {}
for offset, name in enumerate(UrError._SAFETY_ERROR_PRIORITIES, start=1):
if flags.get(name, False):
return -9000 - offset
return 0
[docs]
@classmethod
def _state_error_description(cls, state: Dict[str, Any]) -> str:
"""state() の結果から人間が読めるエラー説明文字列を生成する。"""
if state.get("error"):
return str(state.get("error"))
flags = state.get("safety_status_flags") or {}
active = [name for name, enabled in flags.items() if enabled]
dashboard = state.get("dashboard") or {}
parts = []
code = cls._state_error_code(state)
if code != 0:
if active:
active_text = ", ".join(active)
else:
active_text = "unknown"
parts.append(f"UR safety state: {active_text}")
else:
parts.append("No UR safety error detected")
robot_mode_name = state.get("robot_mode_name")
if robot_mode_name:
robot_mode_description = state.get("robot_mode_description")
parts.append(
"robot_mode=%s(%s)"
% (
robot_mode_name,
robot_mode_description,
)
)
if state.get("safety_mode") is not None:
safety_mode_name = state.get("safety_mode_name")
safety_mode_description = state.get("safety_mode_description")
parts.append(
"safety_mode=%s(%s: %s)"
% (
state.get("safety_mode"),
safety_mode_name,
safety_mode_description,
)
)
if state.get("state_diagnosis"):
parts.append("state_diagnosis=%s" % state.get("state_diagnosis"))
if dashboard.get("safetystatus"):
parts.append(f"dashboard_safetystatus={dashboard.get('safetystatus')}")
if dashboard.get("robotmode"):
parts.append(f"dashboard_robotmode={dashboard.get('robotmode')}")
return "; ".join(parts)
[docs]
def _dashboard_status(self) -> Dict[str, Any]:
"""DashboardClient から補助的な状態文字列を取得する。
Dashboard を使っていない場合は空 dict を返す。Dashboard の safetystatus は
safetymode より詳細な問い合わせであるため、利用できる場合はエラー表示へ
含める。
"""
dashboard = self._dashboard
if dashboard is None:
return {}
result: Dict[str, Any] = {}
for name in (
"robotmode",
"safetystatus",
"safetymode",
"programState",
"getLoadedProgram",
"isInRemoteControl",
):
method = getattr(dashboard, name, None)
if callable(method):
result[name] = self._safe_call(method)
return result
# motion -----------------------------------------------------------
[docs]
@staticmethod
def _normalize_offset(
offset: Union[list, tuple],
) -> list:
"""COBOTTA 側と同じ規則で offset を 6 成分へ補完する。
(0, 0, 10) は (0, 0, 10, 0, 0, 0) として扱う。
(0, 0, 0, 10) は (0, 0, 0, 10, 0, 0) として扱う。
"""
# COBOTTA は 3 成分 offset を許容するため、UR 側でもここで姿勢成分を 0 補完する。
# 厳密な 6 成分検証が必要な P/J 座標本体とは分離して扱う。
normalized_offset = list(offset)
while len(normalized_offset) < 6:
normalized_offset.append(0)
return normalized_offset
[docs]
def _to_p_string(
self,
offset: Union[str, list, tuple],
) -> Union[str, bool]:
"""COBOTTA 側と同じ規則で offset を P 文字列へ整形する。
str はそのまま返し、list/tuple は不足分を 0 補完して P(...) へ変換する。
"""
if isinstance(offset, str):
return offset
if isinstance(offset, (list, tuple)):
vals = list(offset)[:6]
vals = self._normalize_offset(vals)
for x in vals:
if not isinstance(x, (int, float)):
return False
return "P(" + ",".join(str(x) for x in vals) + ")"
return False
[docs]
@staticmethod
def _strip_path_prefix(
base: str,
) -> str:
"""@P P1 や @0 P14 から @ ステートメントを除去する。"""
if isinstance(base, str):
return re.sub(r"^@\w+\s*", "", base.strip())
return base
[docs]
def _pose_add(
self,
base: Sequence[Union[int, float]],
# offset: Union[str, list, tuple],
offset: Union[str, List[Union[int, float]], Tuple[Union[int, float], ...]],
) -> List[float]:
"""P 型ベクトルを成分ごとに加算する。
COBOTTA 側の dev()/move(offset=...) と同じく、offset は str/list/tuple
を受け付ける。3 成分指定は後続の姿勢成分を 0 として補完する。
"""
b = self._six(base, "base")
offset_str = self._to_p_string(offset)
if not isinstance(offset_str, str):
raise ValueError("offset must be str, list, or tuple")
s = offset_str.strip()
if s.upper().startswith("P(") and s.endswith(")"):
s = s[2:-1]
elif s.startswith("(") and s.endswith(")"):
s = s[1:-1]
values = [v.strip() for v in s.split(",") if v.strip()]
if len(values) == 0:
raise ValueError("offset must contain numeric values")
nums = [float(v) for v in values[:6]]
nums = self._normalize_offset(nums)
if not all(math.isfinite(value) for value in nums):
raise ValueError("offset contains non-finite values")
return [x + dx for x, dx in zip(b, nums)]
[docs]
def _target(
self,
target: Union[str, Sequence[Union[int, float]]],
coord_type: Optional[str] = None,
) -> Tuple[List[float], str]:
"""move 系に渡された目標値を P/J ベクトルへ解決する。
対応する文字列は `P1`, `@P P1`, `@0 P1`,
`P1+(0,0,10)`, `@P P1+(0,0,10,0,0,0)` である。
`@P`/`@0` はパスブレンド指定としての名前だけ受け付け、
単点 move では軌道生成に影響させない。
"""
resolved_coord = (coord_type or self._coord_type).upper()
if isinstance(target, str):
text = target.strip()
offset = None
m_offset = re.match(
r"^(.+?)\s*([+-])\s*(P?\s*\(.*\)|[-+0-9.,\s]+)\s*$",
text,
)
if m_offset:
text = m_offset.group(1).strip()
offset = m_offset.group(3).strip()
if m_offset.group(2) == "-":
offset = self._pose_add(
[0, 0, 0, 0, 0, 0],
offset,
)
offset = [-v for v in offset]
text = self._strip_path_prefix(text)
match = self._POSITION_VAR_RE.fullmatch(text)
if not match:
raise ValueError(f"unsupported target expression: {target}")
kind = match.group(1).upper()
index = int(match.group(2))
if kind == "P":
value = self.get_P(index)
else:
value = self.get_J(index)
if value is False:
raise KeyError(f"position is not registered: {kind}{index}")
result = list(value)
if offset is not None:
if kind != "P":
raise ValueError(
"offset expression is supported only for P variables"
)
result = self._pose_add(result, offset)
return result, kind
if resolved_coord not in ("P", "J"):
raise ValueError("coord_type must be P or J")
return self._six(target, "target"), resolved_coord
[docs]
def _begin_api_error_capture(
self,
operation: str,
source_class: str,
source_method: str,
details: Optional[Dict[str, Any]] = None,
) -> Dict[str, Any]:
"""API実行前のPrimaryメッセージ位置と呼出情報を記録する。"""
self._last_error = None
self._last_error_info = None
mark = None
if self.is_error_receiver_connected():
mark = self._primary_error_receiver.mark()
return {
"operation": operation,
"source_class": source_class,
"source_method": source_method,
"primary_mark": mark,
"details": dict(details or {}),
}
[docs]
def _get_primary_errors_after(
self,
mark: Optional[int],
timeout: float = 0.25,
) -> List[Dict[str, Any]]:
"""指定位置以後のPrimaryエラーメッセージを返す。"""
if mark is None:
return []
messages = self._primary_error_receiver.get_messages_after(
mark,
timeout=timeout,
)
return [
message
for message in messages
if message.get("message_type")
in ("RUNTIME_EXCEPTION", "ERROR_CODE", "TEXT")
]
[docs]
def _read_error_state(self) -> Dict[str, Any]:
"""エラー判定用状態を取得する。取得失敗は状態内に保存する。"""
try:
return self.state()
except Exception as exc:
return {
"connected": self.is_connected(),
"state_error": str(exc),
}
[docs]
def _set_api_failure(
self,
capture: Dict[str, Any],
*,
result: Any = None,
exception: Optional[BaseException] = None,
state: Optional[Dict[str, Any]] = None,
description: Optional[str] = None,
stage: str = "api_result",
primary_timeout: float = 0.25,
) -> bool:
"""各インターフェースの情報から失敗原因を一度だけ確定する。"""
if state is None:
state = self._read_error_state()
primary_errors = self._get_primary_errors_after(
capture.get("primary_mark"),
timeout=primary_timeout,
)
runtime_error = None
error_code_message = None
for message in primary_errors:
if message.get("message_type") == "RUNTIME_EXCEPTION":
runtime_error = message
elif message.get("message_type") == "ERROR_CODE":
error_code_message = message
safety_error = UrError.decode_safety_state(state)
decoded_runtime = None
if safety_error is not None:
error_type = safety_error["error_type"]
description_ja = safety_error["description_ja"]
description_en = safety_error["description_en"]
error_origin = "UR safety system"
detected_by = (
"RTDEReceiveInterface.getSafetyMode()/" "getSafetyStatusBits()"
)
elif runtime_error is not None:
decoded_runtime = UrError.decode_runtime_exception(
runtime_error.get("message", "")
)
error_type = decoded_runtime["error_type"]
description_ja = decoded_runtime["description_ja"]
description_en = decoded_runtime["description_en"]
error_origin = "UR controller / URScript runtime"
detected_by = "UrPrimaryErrorReceiver"
elif error_code_message is not None:
error_type = "UR_ERROR_CODE"
description_ja = "URコントローラがエラーコードを通知しました。"
description_en = "The UR controller reported an error code."
error_origin = "UR controller"
detected_by = "UrPrimaryErrorReceiver"
elif exception is not None:
error_type = "API_EXCEPTION"
description_ja = "UR API実行中に例外が発生しました。"
description_en = "An exception occurred while executing the UR API."
error_origin = capture.get("source_class")
detected_by = (
f"{capture.get('source_class')}." f"{capture.get('source_method')}()"
)
else:
error_type = "API_FAILURE"
description_ja = "UR APIが失敗を返しました。"
description_en = "The UR API returned a failure result."
error_origin = capture.get("source_class")
detected_by = (
f"{capture.get('source_class')}." f"{capture.get('source_method')}()"
)
if description is None:
lines = [description_ja, description_en]
lines.extend(
[
"",
"エラー分類 / Error type:",
f" {error_type}",
"",
"実行API / Executed API:",
(
f" {capture.get('source_class')}."
f"{capture.get('source_method')}()"
),
]
)
if decoded_runtime is not None:
lines.extend(
[
"",
"URエラー識別子 / UR error identifier:",
f" {decoded_runtime.get('error_id')}",
"",
"発生ルーチン / Routine:",
f" {decoded_runtime.get('routine')}",
]
)
if error_code_message is not None:
lines.extend(
[
"",
"URエラーコード / UR error code:",
(
" code=%s argument=%s"
% (
error_code_message.get("message_code"),
error_code_message.get("message_argument"),
)
),
]
)
if exception is not None:
lines.extend(
[
"",
"例外 / Exception:",
f" {exception}",
]
)
description = "\n".join(lines)
error_value = exception
if error_value is None:
error_value = RuntimeError(description)
self._set_last_error(
error_value,
description=description,
source_class=capture.get("source_class"),
source_method=capture.get("source_method"),
operation=capture.get("operation"),
stage=stage,
state=state,
error_origin=error_origin,
detected_by=detected_by,
)
info = self._last_error_info
if info is not None:
info["error_type"] = error_type
info["details"] = capture.get("details", {})
info["layers"] = {
"high_level": {
"class": self.__class__.__name__,
"method": capture.get("operation"),
},
"low_level": {
"class": capture.get("source_class"),
"method": capture.get("source_method"),
"result": result,
"exception": None if exception is None else str(exception),
},
"receive": {
"class": "RTDEReceiveInterface",
"state": state,
"safety_error": safety_error,
},
"primary": {
"runtime_exception": runtime_error,
"error_code": error_code_message,
"messages": primary_errors,
},
}
if decoded_runtime is not None:
info["controller_error"] = decoded_runtime
if error_code_message is not None:
info["controller_error_code"] = error_code_message
self._logger.error(
"%s() failed / %s() 失敗\n%s",
capture.get("operation"),
capture.get("operation"),
description,
)
return False
[docs]
def _finish_api_call(
self,
capture: Dict[str, Any],
result: Any,
*,
is_async: bool = False,
false_is_failure: bool = True,
) -> bool:
"""API戻り値、Primaryメッセージ、RTDE安全状態を共通判定する。"""
if is_async:
self._async_error_capture = capture
if false_is_failure and result is False:
return self._set_api_failure(
capture,
result=result,
)
return True
state = self._read_error_state()
safety_error = UrError.decode_safety_state(state)
primary_errors = self._get_primary_errors_after(
capture.get("primary_mark"),
timeout=0.25,
)
has_controller_error = any(
message.get("message_type") in ("RUNTIME_EXCEPTION", "ERROR_CODE")
for message in primary_errors
)
if false_is_failure and result is False:
return self._set_api_failure(
capture,
result=result,
state=state,
primary_timeout=0.0,
)
if safety_error is not None or has_controller_error:
return self._set_api_failure(
capture,
result=result,
state=state,
primary_timeout=0.0,
stage="post_api_state",
)
return True
[docs]
def check_error(self) -> bool:
"""現在または直前の非同期命令に関連するURエラーを確認する。"""
capture = self._async_error_capture
if capture is None:
capture = self._begin_api_error_capture(
"check_error",
"RTDEReceiveInterface",
"getSafetyMode/getSafetyStatusBits",
)
state = self._read_error_state()
safety_error = UrError.decode_safety_state(state)
primary_errors = self._get_primary_errors_after(
capture.get("primary_mark"),
timeout=0.0,
)
has_controller_error = any(
message.get("message_type") in ("RUNTIME_EXCEPTION", "ERROR_CODE")
for message in primary_errors
)
if safety_error is None and not has_controller_error:
return True
return self._set_api_failure(
capture,
result=False,
state=state,
primary_timeout=0.0,
stage="asynchronous_error_check",
)
[docs]
def move(
self,
target: Union[str, Sequence[Union[int, float]]],
*,
motion_mode: Union[int, str, MotionMode, None] = None,
coord_type: Optional[str] = None,
path_blend: Optional[str] = None,
offset: Optional[Sequence[Union[int, float]]] = None,
pose: Optional[int] = None,
speed: Optional[Union[int, float]] = None,
accel: Optional[Union[int, float]] = None,
decel: Optional[Union[int, float]] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
check_ik: Optional[bool] = None,
) -> bool:
"""単一点移動を実行し、同期時は最終状態まで判定する。"""
del path_blend, pose, decel, bstrOpt
capture = self._begin_api_error_capture(
"move",
"RTDEControlInterface",
"unknown",
{
"target": target,
"motion_mode": str(motion_mode),
"coord_type": coord_type,
"offset": offset,
},
)
try:
if motion_mode is None:
mode = self._motion_mode
else:
mode = self._mode(motion_mode)
value, coord = self._target(target, coord_type)
if offset is not None:
if coord != "P":
raise ValueError(
"offset はP座標でのみ使用できます。 / "
"offset is supported only for P coordinates."
)
value = self._pose_add(value, offset)
capture["details"]["resolved_target"] = list(value)
target_pose_ur = None
if coord == "P":
target_pose_ur = self._pose_to_ur(value)
if check_ik is None:
run_ik_check = self._check_ik
else:
run_ik_check = bool(check_ik)
if run_ik_check:
capture["source_method"] = "getInverseKinematicsHasSolution"
current_q = self._receive().getActualQ()
has_solution = bool(
self._control().getInverseKinematicsHasSolution(
target_pose_ur,
current_q,
)
)
if not has_solution:
return self._set_api_failure(
capture,
result=False,
description=(
"移動前の到達可能性確認で、目標姿勢に対する解が"
"見つかりませんでした。\n"
"The pre-motion reachability check found no "
"solution for the target pose."
),
stage="precheck",
)
with self._lock:
if coord == "J":
capture["source_method"] = "moveJ"
v, a = self._joint_params(speed, accel)
result = self._control().moveJ(
self._joints_to_ur(value),
v,
a,
bool(is_async),
)
self._last_motion_mode = MotionMode.PTP
elif mode == MotionMode.PTP:
capture["source_method"] = "moveJ_IK"
v, a = self._joint_params(speed, accel)
result = self._control().moveJ_IK(
target_pose_ur,
v,
a,
bool(is_async),
)
self._last_motion_mode = MotionMode.PTP
elif mode == MotionMode.LINE:
capture["source_method"] = "moveL"
v, a = self._linear_params(speed, accel)
result = self._control().moveL(
target_pose_ur,
v,
a,
bool(is_async),
)
self._last_motion_mode = MotionMode.LINE
else:
raise NotImplementedError(
f"Unsupported single-point motion mode: {mode.value}"
)
return self._finish_api_call(
capture,
result,
is_async=bool(is_async),
)
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def move_path(
self,
targets: Sequence[Union[str, Sequence[Union[int, float]]]],
*,
motion_mode: Union[int, str, MotionMode, None] = None,
coord_type: Optional[str] = None,
speed: Optional[Union[int, float]] = None,
accel: Optional[Union[int, float]] = None,
blend: Optional[float] = None,
is_async: bool = False,
) -> bool:
"""複数点をURのパスとして移動する。blend の単位は m。"""
capture = self._begin_api_error_capture(
"move_path",
"RTDEControlInterface",
"moveJ/moveL",
{
"targets": list(targets),
"motion_mode": str(motion_mode),
},
)
try:
if not targets:
raise ValueError("targets is empty")
if motion_mode is None:
mode = self._motion_mode
else:
mode = self._mode(motion_mode)
if self._path_blend.upper() == "@0":
radius = 0.0
else:
radius = self._default_blend_radius
if blend is not None:
radius = float(blend)
if radius < 0:
raise ValueError("blend must be >= 0")
with self._lock:
if mode == MotionMode.PTP:
capture["source_method"] = "moveJ"
v, a = self._joint_params(speed, accel)
path = []
for target in targets:
value, coord = self._target(
target,
coord_type,
)
if coord == "J":
joints = self._joints_to_ur(value)
else:
joints = self._control().getInverseKinematics(
self._pose_to_ur(value)
)
if not joints:
return self._set_api_failure(
capture,
result=False,
description=(
"パス中の目標位置を関節座標へ"
"変換できませんでした。\n"
"A path target could not be "
"converted to joint coordinates.\n"
"\n"
f"目標 / Target:\n {target}"
),
stage="path_preparation",
)
path.append(list(joints) + [v, a, radius])
result = self._control().moveJ(
path,
bool(is_async),
)
self._last_motion_mode = MotionMode.PTP
elif mode in (
MotionMode.LINE,
MotionMode.SPLINE,
):
capture["source_method"] = "moveL"
v, a = self._linear_params(speed, accel)
path = []
for target in targets:
value, coord = self._target(
target,
coord_type,
)
if coord != "P":
raise ValueError("LINE path requires P coordinates")
path.append(self._pose_to_ur(value) + [v, a, radius])
result = self._control().moveL(
path,
bool(is_async),
)
self._last_motion_mode = MotionMode.LINE
else:
raise NotImplementedError("use move_circle() for CIRCLE")
return self._finish_api_call(
capture,
result,
is_async=bool(is_async),
)
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def move_circle(
self,
via: Union[str, Sequence[Union[int, float]]],
target: Union[str, Sequence[Union[int, float]]],
*,
speed: Optional[Union[int, float]] = None,
accel: Optional[Union[int, float]] = None,
is_async: bool = True,
mode: int = 1,
) -> bool:
"""円弧移動の互換 I/F である。
現在使用している ur-rtde Python binding では moveC が公開されないため未対応である。"""
del via, target, speed, accel, is_async, mode
return self._fail(
NotImplementedError(
"installed ur-rtde Python binding does not expose moveC"
)
)
[docs]
def dev(
self,
base: Union[str, Sequence[Union[int, float]]],
offset: Union[str, list, tuple],
) -> Union[List[float], bool]:
"""base に offset を加えた P 型ベクトルを返す。
CobottaCtrl の `dev()`/`move(offset=...)` 利用に合わせ、公開 I/F 上の
6 成分を単純加算する。UR の厳密な pose 合成が必要な場合は
`pose_trans()` を使う。
"""
try:
base_value, coord = self._target(base, "P")
if coord != "P":
raise ValueError("dev requires P coordinates")
return self._pose_add(base_value, offset)
except Exception as exc:
return self._fail(exc)
[docs]
def pose_trans(
self,
base: Union[str, Sequence[Union[int, float]]],
offset: Union[str, list, tuple],
) -> Union[List[float], bool]:
"""URScript pose_trans() 相当の姿勢合成を行う。"""
capture = self._begin_api_error_capture(
"pose_trans",
"RTDEControlInterface",
"poseTrans",
{
"base": base,
"offset": offset,
},
)
try:
base_value, coord = self._target(base, "P")
if coord != "P":
raise ValueError("pose_trans requires P coordinates")
offset_value = self._pose_add(
[0, 0, 0, 0, 0, 0],
offset,
)
result = self._control().poseTrans(
self._pose_to_ur(base_value),
self._pose_to_ur(offset_value),
)
if not result:
return self._set_api_failure(
capture,
result=result,
)
return self._pose_from_ur(result)
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def p2j(
self,
p_type_pos: Sequence[Union[int, float]],
) -> Union[List[float], bool]:
"""P 座標から J 座標へ変換する。"""
capture = self._begin_api_error_capture(
"p2j",
"RTDEControlInterface",
"getInverseKinematics",
{
"pose": list(p_type_pos),
},
)
try:
result = self._control().getInverseKinematics(self._pose_to_ur(p_type_pos))
if not result:
return self._set_api_failure(
capture,
result=result,
)
return self._joints_from_ur(result)
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def j2p(
self,
j_type_pos: Sequence[Union[int, float]],
) -> Union[List[float], bool]:
"""J 座標から P 座標へ変換する。"""
capture = self._begin_api_error_capture(
"j2p",
"RTDEControlInterface",
"getForwardKinematics",
{
"joints": list(j_type_pos),
},
)
try:
result = self._control().getForwardKinematics(
self._joints_to_ur(j_type_pos)
)
if not result:
return self._set_api_failure(
capture,
result=result,
)
return self._pose_from_ur(result)
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def _drive_core(
self,
target: Union[str, list, tuple],
*,
relative: bool = False,
path_blend: Optional[str] = None,
speed: Optional[Union[int, float]] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
) -> Union[List[float], bool]:
"""COBOTTA 側の Drive 系 I/F に合わせて J 座標を作る。
UR には COBOTTA の DriveEX/DriveAEX 相当の命令がないため、公開 I/F
と入力解釈を合わせ、最終的には J 座標の moveJ へ変換する。
target は "(1,10),(2,20)"、(1, 10)、[(1, 10), (2, 20)] を受ける。
"""
del path_blend, bstrOpt
if isinstance(target, str):
pairs = []
for match in re.finditer(
r"\(\s*([+-]?\d+)\s*,\s*([+-]?(?:\d+(?:\.\d*)?|\.\d+))\s*\)",
target,
):
pairs.append((int(match.group(1)), float(match.group(2))))
if len(pairs) == 0:
raise ValueError("target must contain Drive-style axis pairs")
elif isinstance(target, (list, tuple)):
if len(target) == 2:
if isinstance(target[0], (int, float)):
if isinstance(target[1], (int, float)):
pairs = [(int(target[0]), float(target[1]))]
else:
raise ValueError("target must be an axis/value pair")
else:
pairs = []
for item in target:
if not isinstance(item, (list, tuple)):
raise ValueError("target items must be axis/value pairs")
if len(item) != 2:
raise ValueError("target items must be axis/value pairs")
pairs.append((int(item[0]), float(item[1])))
else:
pairs = []
for item in target:
if not isinstance(item, (list, tuple)):
raise ValueError("target items must be axis/value pairs")
if len(item) != 2:
raise ValueError("target items must be axis/value pairs")
pairs.append((int(item[0]), float(item[1])))
else:
raise ValueError("target must be str, tuple, or list of tuple")
# Drive 系は COBOTTA の軸指定 I/F を UR の現在 J 座標に反映してから moveJ へ渡す。
# relative=True の場合だけ現在角へ加算する。
current = self.get_current_angle()
if current is False:
return False
joints = list(current)
for axis, value in pairs:
index = int(axis) - 1
if not 0 <= index < 6:
raise ValueError(f"axis must be 1..6: {axis}")
if relative:
joints[index] = joints[index] + float(value)
else:
joints[index] = float(value)
del speed, is_async
return joints
[docs]
def drive(
self,
target: Union[str, list, tuple, None],
*,
relative: bool = False,
path_blend: Optional[str] = None,
speed: Optional[Union[int, float]] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""COBOTTA 互換の Drive 統合 I/F。
relative=True で相対移動、relative=False で絶対移動として扱う。
"""
self._logger.info("== drive()")
try:
joints = self._drive_core(
target,
relative=relative,
path_blend=path_blend,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
if joints is False:
return False
return self.move(
joints,
coord_type="J",
motion_mode=MotionMode.PTP,
speed=speed,
is_async=is_async,
)
except Exception as exc:
return self._fail(exc)
[docs]
def drive_ex(
self,
target: Union[str, list, tuple],
*,
path_blend: Optional[str] = None,
speed: Optional[Union[int, float]] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""COBOTTA 互換の相対 Drive。"""
self._logger.info("== driveEX()")
return self.drive(
target,
relative=True,
path_blend=path_blend,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
[docs]
def drive_aex(
self,
target: Union[str, list, tuple, None],
*,
path_blend: Optional[str] = None,
speed: Optional[Union[int, float]] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""COBOTTA 互換の絶対 Drive。"""
self._logger.info("== driveAEX()")
return self.drive(
target,
relative=False,
path_blend=path_blend,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
[docs]
def drive_a(
self,
target: Union[str, list, tuple],
*,
path_blend: Optional[str] = None,
speed: Optional[Union[int, float]] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""COBOTTA 互換の絶対 Drive 別名である。
実処理は drive_AEX() に委譲する。"""
return self.drive_aex(
target,
path_blend=path_blend,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
[docs]
def approach(
self,
lComp: Union[int, str, MotionMode],
vntPoseBase: Union[str, Sequence[Union[int, float]]],
vntPoseLen: Sequence[Union[int, float]],
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""COBOTTA 互換の Approach I/F である。
基準 P 座標へ offset を加えた単点 move として扱う。"""
return self.move(
vntPoseBase,
motion_mode=lComp,
offset=vntPoseLen,
bstrOpt=bstrOpt,
)
[docs]
def stop(self) -> bool:
"""現在の移動種別に応じて停止命令を送る。"""
method_name = "stopJ"
if self._last_motion_mode == MotionMode.LINE:
method_name = "stopL"
capture = self._begin_api_error_capture(
"stop",
"RTDEControlInterface",
method_name,
)
try:
with self._lock:
if self._last_motion_mode == MotionMode.LINE:
result = self._control().stopL(
self._max_linear_acceleration * self._decel / 100.0
)
else:
result = self._control().stopJ(
self._max_joint_acceleration * self._decel / 100.0
)
if result is False:
return self._set_api_failure(
capture,
result=result,
)
return True
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def halt(self) -> bool:
"""stop() の互換別名である。"""
return self.stop()
[docs]
def task_stop(self) -> bool:
"""タスク停止の互換 I/F である。
UR では stop() へ委譲する。"""
return self.stop()
[docs]
def hand_stop(self) -> bool:
"""ハンド停止の互換 I/F である。
UR 単体では専用ハンドを持たないため stop() へ委譲する。"""
return self.stop()
# IO ---------------------------------------------------------------
[docs]
def get_IO(self, id_num: int) -> bool:
"""デジタル入力を取得する。
COBOTTA 互換の IO 番号を UR の入力番号として扱う。"""
try:
return bool(self._receive().getDigitalOutState(int(id_num)))
except Exception as exc:
return self._fail(exc)
[docs]
def set_IO(
self,
id_num: int,
value: Union[int, float],
) -> bool:
"""0..7=standard、8..15=configurable、16..17=tool output。"""
capture = self._begin_api_error_capture(
"set_IO",
"RTDEIOInterface",
"setDigitalOut",
{
"id_num": id_num,
"value": value,
},
)
try:
index = int(id_num)
state = bool(value)
if 0 <= index <= 7:
capture["source_method"] = "setStandardDigitalOut"
result = self._io().setStandardDigitalOut(
index,
state,
)
elif 8 <= index <= 15:
capture["source_method"] = "setConfigurableDigitalOut"
result = self._io().setConfigurableDigitalOut(
index - 8,
state,
)
elif 16 <= index <= 17:
capture["source_method"] = "setToolDigitalOut"
result = self._io().setToolDigitalOut(
index - 16,
state,
)
else:
raise ValueError("IO index must be 0..17")
if not bool(result):
return self._set_api_failure(
capture,
result=result,
)
return True
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
# tool / safety ----------------------------------------------------
[docs]
def set_tcp(
self,
tcp: Sequence[Union[int, float]],
) -> bool:
"""現在 TCP を設定する。"""
capture = self._begin_api_error_capture(
"set_tcp",
"RTDEControlInterface",
"setTcp",
{
"tcp": list(tcp),
},
)
try:
result = self._control().setTcp(self._pose_to_ur(tcp))
if not bool(result):
return self._set_api_failure(
capture,
result=result,
)
return True
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def is_pose_within_safety_limits(
self,
pose: Sequence[Union[int, float]],
) -> bool:
"""指定 P 座標が UR の安全範囲内かを確認する。"""
capture = self._begin_api_error_capture(
"is_pose_within_safety_limits",
"RTDEControlInterface",
"isPoseWithinSafetyLimits",
{
"pose": list(pose),
},
)
try:
result = self._control().isPoseWithinSafetyLimits(self._pose_to_ur(pose))
if not bool(result):
return self._set_api_failure(
capture,
result=result,
description=(
"指定姿勢は安全範囲外です。\n"
"The specified pose is outside "
"the safety limits."
),
)
return True
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def is_joints_within_safety_limits(
self,
joints: Sequence[Union[int, float]],
) -> bool:
"""指定 J 座標が UR の安全範囲内かを確認する。"""
capture = self._begin_api_error_capture(
"is_joints_within_safety_limits",
"RTDEControlInterface",
"isJointsWithinSafetyLimits",
{
"joints": list(joints),
},
)
try:
result = self._control().isJointsWithinSafetyLimits(
self._joints_to_ur(joints)
)
if not bool(result):
return self._set_api_failure(
capture,
result=result,
description=(
"指定関節角は安全範囲外です。\n"
"The specified joint values are "
"outside the safety limits."
),
)
return True
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def acquire_control(self) -> bool:
"""共通ロボット I/F: モーション制御権を取得する。
UR では RTDEControlInterface の確立に対応する。
"""
if not self.is_connected():
self._logger.error(
"*** UR API ERROR *** take_arm: connect() is required before take_arm()"
)
return False
if not self.is_safety_normal():
self._logger.error(
"*** UR API ERROR *** take_arm: safety state is not normal: %s",
self.error_description,
)
return False
if not self.is_power_on():
self._logger.error(
"*** UR API ERROR *** take_arm: robot drive power is not on: %s",
self.error_description,
)
return False
return self.connect_control()
[docs]
def release_control(self) -> bool:
"""共通ロボット I/F: モーション制御権を解放する。
UR では RTDEControlInterface の切断に対応する。
"""
return self.disconnect_control()
[docs]
def release_control_alias(self) -> bool:
"""共通ロボット I/F: give_arm() の別名。"""
return self.release_control()
[docs]
def turn_on_motor(self) -> bool:
"""モーターをONにする。"""
capture = self._begin_api_error_capture(
"turn_on_motor",
"DashboardClient",
"powerOn/brakeRelease",
)
try:
dashboard = self._dashboard_client()
capture["source_method"] = "powerOn"
power_result = dashboard.powerOn()
capture["source_method"] = "brakeRelease"
brake_result = dashboard.brakeRelease()
if power_result is False:
return self._set_api_failure(
capture,
result=power_result,
stage="power_on",
)
if brake_result is False:
return self._set_api_failure(
capture,
result=brake_result,
stage="brake_release",
)
return True
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def turn_off_motor(self) -> bool:
"""モーターをOFFにする。"""
capture = self._begin_api_error_capture(
"turn_off_motor",
"DashboardClient",
"powerOff",
)
try:
result = self._dashboard_client().powerOff()
if result is False:
return self._set_api_failure(
capture,
result=result,
)
return True
except Exception as exc:
return self._set_api_failure(
capture,
exception=exc,
stage="exception",
)
[docs]
def _create_dashboard_client(self) -> Tuple[Any, bool]:
"""DashboardClient を取得する。
既に接続済みの DashboardClient がある場合はそれを返す。
未接続の場合は reset_error() 用に一時接続を作成する。
Returns:
(dashboard, temporary): temporary=True の場合、呼び出し側で disconnect する。
"""
if self._dashboard is not None:
return self._dashboard, False
if DashboardClient is None:
raise RuntimeError("DashboardClient is unavailable in this ur-rtde")
dashboard = DashboardClient(self._robot_ip)
dashboard.connect()
return dashboard, True
[docs]
@staticmethod
def _dashboard_execute(dashboard: Any, method_name: str) -> Tuple[bool, Any]:
"""DashboardClient のメソッドを存在確認して実行する。
DashboardClient の戻り値は ur_rtde の版により str/None などが混在する。
例外を投げなければ「送信は成功」と扱う。UR 側で受理されたかどうかは、
直後の robotmode()/safetystatus() を読み直して判定する。
"""
method = getattr(dashboard, method_name, None)
if not callable(method):
return False, "DashboardClient has no method: %s" % method_name
return True, method()
[docs]
@staticmethod
def _dashboard_text(value: Any) -> str:
"""DashboardClient の応答を比較しやすい大文字文字列へ変換する。"""
if value is None:
return ""
return str(value).upper()
[docs]
@staticmethod
def _dashboard_has(
snapshot: Dict[str, Any], key: str, words: Sequence[str]
) -> bool:
"""Dashboard snapshot の指定 key に word 群のいずれかが含まれるかを返す。"""
text = UrRtde._dashboard_text(snapshot.get(key))
for word in words:
if str(word).upper() in text:
return True
return False
[docs]
def _dashboard_snapshot(self, dashboard: Any) -> Dict[str, Any]:
"""Dashboard Server から現在状態を取得する。
TP に表示される popup 本文そのものは標準 Dashboard API から読めない。
そのため、robotmode/safetystatus/safetymode/programState/isInRemoteControl を
まとめて読み、復帰判断とログ表示に使う。
"""
result: Dict[str, Any] = {}
for name in (
"robotmode",
"safetystatus",
"safetymode",
"programState",
"isInRemoteControl",
):
method = getattr(dashboard, name, None)
if callable(method):
try:
result[name] = method()
except Exception as exc:
result[name] = "ERROR: %s" % exc
return result
[docs]
def _log_dashboard_snapshot(self, caller: str, snapshot: Dict[str, Any]) -> None:
"""Dashboard snapshot を x_logger へ出す。"""
self._logger.info(
"*** UR DASHBOARD STATE *** %s: robotmode=%s; safetystatus=%s; safetymode=%s; programState=%s; isInRemoteControl=%s",
caller,
snapshot.get("robotmode"),
snapshot.get("safetystatus"),
snapshot.get("safetymode"),
snapshot.get("programState"),
snapshot.get("isInRemoteControl"),
)
[docs]
def _dashboard_call(self, dashboard: Any, caller: str, method_name: str) -> bool:
"""Dashboard コマンドを 1 つ実行してログに残す。"""
try:
self._logger.info(
"*** UR DASHBOARD ACTION *** %s: %s()",
caller,
method_name,
)
executed, response = self._dashboard_execute(dashboard, method_name)
if not executed:
self._logger.warning(
"*** UR DASHBOARD ACTION *** %s: %s() skipped: %s",
caller,
method_name,
response,
)
return False
self._logger.info(
"*** UR DASHBOARD ACTION *** %s: %s() response=%s",
caller,
method_name,
response,
)
return True
except Exception as exc:
self._logger.error(
"*** UR DASHBOARD ERROR *** %s: %s() failed: %s",
caller,
method_name,
exc,
)
self._set_last_error(exc, description=str(exc))
return False
[docs]
def _wait_dashboard_state(
self,
dashboard: Any,
caller: str,
label: str,
predicate: Any,
timeout: float = 10.0,
interval: float = 0.25,
) -> bool:
"""Dashboard 状態が predicate を満たすまで待つ。"""
started = time.monotonic()
last_snapshot: Dict[str, Any] = {}
while True:
snapshot = self._dashboard_snapshot(dashboard)
last_snapshot = snapshot
if predicate(snapshot):
self._logger.info(
"*** UR DASHBOARD WAIT *** %s: %s satisfied",
caller,
label,
)
self._log_dashboard_snapshot(caller, snapshot)
return True
if time.monotonic() - started >= timeout:
self._logger.error(
"*** UR DASHBOARD WAIT *** %s: %s timeout",
caller,
label,
)
self._log_dashboard_snapshot(caller, last_snapshot)
return False
time.sleep(interval)
[docs]
def _dashboard_safety_needs_fault_reset(self, snapshot: Dict[str, Any]) -> bool:
"""restartSafety() 系の復帰が必要かを Dashboard 文字列から判定する。"""
if self._dashboard_has(snapshot, "safetystatus", ("FAULT", "VIOLATION")):
return True
if self._dashboard_has(snapshot, "safetymode", ("FAULT", "VIOLATION")):
return True
return False
[docs]
def _dashboard_safety_needs_protective_unlock(
self, snapshot: Dict[str, Any]
) -> bool:
"""unlockProtectiveStop() 系の復帰が必要かを Dashboard 文字列から判定する。"""
if self._dashboard_has(snapshot, "safetystatus", ("PROTECTIVE",)):
return True
if self._dashboard_has(snapshot, "safetymode", ("PROTECTIVE",)):
return True
return False
[docs]
def _dashboard_safety_blocks_soft_reset(self, snapshot: Dict[str, Any]) -> bool:
"""ソフトだけでは解除できない安全停止かを判定する。"""
words = (
"EMERGENCY",
"SAFEGUARD",
"SYSTEM_EMERGENCY",
"ROBOT_EMERGENCY",
)
if self._dashboard_has(snapshot, "safetystatus", words):
return True
if self._dashboard_has(snapshot, "safetymode", words):
return True
return False
[docs]
def _dashboard_robot_power_off(self, snapshot: Dict[str, Any]) -> bool:
"""Dashboard robotmode が Power Off 系かを判定する。"""
return self._dashboard_has(snapshot, "robotmode", ("POWER_OFF", "POWER OFF"))
[docs]
def _dashboard_robot_booting(self, snapshot: Dict[str, Any]) -> bool:
"""Dashboard robotmode が起動遷移中かを判定する。"""
return self._dashboard_has(
snapshot, "robotmode", ("BOOT", "POWER_ON", "POWER ON")
)
[docs]
def _dashboard_robot_ready_for_brake_release(
self, snapshot: Dict[str, Any]
) -> bool:
"""brakeRelease() を試せる状態かを判定する。"""
if self._dashboard_robot_power_off(snapshot):
return False
if self._dashboard_robot_booting(snapshot):
return False
return True
[docs]
def _dashboard_robot_ready_for_rtde(self, snapshot: Dict[str, Any]) -> bool:
"""RTDE control script 開始を試せる候補状態かを判定する。"""
if self._dashboard_robot_power_off(snapshot):
return False
if self._dashboard_robot_booting(snapshot):
return False
if self._dashboard_safety_needs_fault_reset(snapshot):
return False
if self._dashboard_safety_needs_protective_unlock(snapshot):
return False
if self._dashboard_safety_blocks_soft_reset(snapshot):
return False
return True
[docs]
def _dashboard_recovery(self, caller: str = "reset_error") -> bool:
"""Dashboard Server 経由で UR のエラー解除と起動復帰を試みる。
COBOTTA の reset_error() が ManualResetPreparation / ClearError /
MotionPreparation をまとめて行うのと同じ位置付けである。UR では Dashboard
Server で TP popup と safety 状態を確認し、必要な解除操作を選択する。
ここでは RTDEControlInterface に触らない。RTDE control script を開始できない
状態から復帰するための前処理であり、壊れた RTDEControlInterface に対する
reuploadScript() は行わない。
"""
dashboard = None
temporary = False
try:
dashboard, temporary = self._create_dashboard_client()
# get_inverse_kin() などの通常の URScript 実行エラーは、
# safety popup ではなく通常 popup として TP に表示される。
self._dashboard_call(
dashboard,
caller,
"closePopup",
)
self._dashboard_call(
dashboard,
caller,
"closeSafetyPopup",
)
snapshot = self._dashboard_snapshot(dashboard)
self._log_dashboard_snapshot(caller + "/before", snapshot)
if self._dashboard_safety_blocks_soft_reset(snapshot):
message = (
"Dashboard safety state requires physical release: "
"safetystatus=%s; safetymode=%s"
) % (snapshot.get("safetystatus"), snapshot.get("safetymode"))
self._logger.error("*** UR DASHBOARD ERROR *** %s: %s", caller, message)
self._set_last_error(RuntimeError(message), description=message)
return False
if self._dashboard_safety_needs_fault_reset(snapshot):
self._dashboard_call(dashboard, caller, "closeSafetyPopup")
self._dashboard_call(dashboard, caller, "restartSafety")
self._wait_dashboard_state(
dashboard,
caller,
"safety fault/violation cleared",
lambda s: not self._dashboard_safety_needs_fault_reset(s),
timeout=15.0,
)
snapshot = self._dashboard_snapshot(dashboard)
self._log_dashboard_snapshot(caller + "/after_restartSafety", snapshot)
if self._dashboard_safety_needs_protective_unlock(snapshot):
self._dashboard_call(dashboard, caller, "closeSafetyPopup")
self._dashboard_call(dashboard, caller, "unlockProtectiveStop")
self._wait_dashboard_state(
dashboard,
caller,
"protective stop cleared",
lambda s: not self._dashboard_safety_needs_protective_unlock(s),
timeout=15.0,
)
snapshot = self._dashboard_snapshot(dashboard)
self._log_dashboard_snapshot(
caller + "/after_unlockProtectiveStop", snapshot
)
if self._dashboard_robot_power_off(snapshot):
self._dashboard_call(dashboard, caller, "powerOn")
self._wait_dashboard_state(
dashboard,
caller,
"robotmode leaves POWER_OFF/BOOTING",
self._dashboard_robot_ready_for_brake_release,
timeout=20.0,
)
snapshot = self._dashboard_snapshot(dashboard)
self._log_dashboard_snapshot(caller + "/after_powerOn", snapshot)
if self._dashboard_robot_ready_for_brake_release(snapshot):
self._dashboard_call(dashboard, caller, "brakeRelease")
ok = self._wait_dashboard_state(
dashboard,
caller,
"robot ready for RTDE control",
self._dashboard_robot_ready_for_rtde,
timeout=20.0,
)
snapshot = self._dashboard_snapshot(dashboard)
self._log_dashboard_snapshot(caller + "/after_brakeRelease", snapshot)
if not ok:
message = (
"Dashboard recovery did not reach RTDE-ready state: "
"robotmode=%s; safetystatus=%s; safetymode=%s"
) % (
snapshot.get("robotmode"),
snapshot.get("safetystatus"),
snapshot.get("safetymode"),
)
self._set_last_error(RuntimeError(message), description=message)
return False
return True
except Exception as e:
return self.print_error_info(e)
finally:
if temporary and dashboard is not None:
try:
dashboard.disconnect()
except Exception as e:
self._logger.error(f"**Error**: {e}")
pass
[docs]
def dashboard_recovery(self) -> bool:
"""UR のエラー解除と動作準備を行う。
COBOTTA 側の reset_error() と同じく、単なる状態クリアではなく、
エラー解除に必要な一連の復帰操作をまとめる API である。
UR では Dashboard Server 経由で TP popup と safety 状態を確認し、
safety fault/violation、protective stop、Power Off、Brake Release 待ちを
順に解消する。RTDEControlInterface が未接続・接続失敗状態でも使えるよう、
復帰処理は Dashboard 経路に限定する。
"""
self._logger.debug("== reset_error()")
self._last_error = None
self._last_error_info = None
try:
return bool(self._dashboard_recovery())
except Exception as e:
return self.print_error_info(e)
[docs]
def change_work(self, work_no: int = 0) -> bool:
"""work 番号切替の互換 I/F である。
UR では work 座標系を持たないため番号だけ保持する。"""
del work_no
return True
[docs]
def get_current_work_no(self) -> int:
"""現在 work 番号を返す互換 I/F である。"""
return 0
[docs]
def get_work(self, work_no: int = 0) -> List[float]:
"""work 定義取得の互換 I/F である。
UR では未定義のため False を返す。"""
del work_no
return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
[docs]
def set_work(
self,
work_no: int,
work_define: Sequence[Union[int, float]],
work_attribute: Optional[Any] = None,
) -> bool:
"""work 定義設定の互換 I/F である。
UR では番号と値だけを互換用に受け付ける。"""
del work_no, work_define, work_attribute
return True
[docs]
def start_log(self) -> bool:
"""COBOTTA ログ開始の互換 I/F である。
UR 実装では専用ログ機能を持たないため True を返す。"""
return True
[docs]
def stop_log(self) -> bool:
"""COBOTTA ログ停止の互換 I/F である。"""
return True
[docs]
def get_log_record(self, index: int) -> Dict[str, Any]:
"""互換ログレコードを返す。
UR では専用ログを保持しないため False を返す。"""
del index
return {}
[docs]
def get_motion_preparation(self) -> Dict[str, Any]:
"""モーション準備状態を返す互換 I/F である。"""
return {"connected": self.is_connected(), "servo_on": self.servo_on}
[docs]
def rotate(
self,
plane: str,
angle: float,
center: Sequence[Union[int, float]],
*,
path_blend: Optional[str] = None,
speed: Optional[Union[int, float]] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""現在 pose から姿勢成分だけを相対回転させる互換 I/F である。"""
del plane, angle, center, path_blend, speed, is_async, bstrOpt
return self._fail(NotImplementedError("rotate is not implemented for UrRtde"))
[docs]
def take_camera(
self,
evp_script_name: str = "take_camera",
pac_script_name: str = "take_camera.pac",
) -> bool:
"""カメラ占有の互換 I/F である。
UR 単体ではカメラを持たないため False を返す。"""
del evp_script_name, pac_script_name
return self._fail(NotImplementedError("take_camera is Cobotta-specific"))
[docs]
def take_script(self, script: str = "pro1") -> bool:
"""スクリプト占有の互換 I/F である。
UR では専用スクリプト占有を持たないため True を返す。"""
del script
return self._fail(NotImplementedError("take_script is Cobotta-specific"))
[docs]
def get_cobotta_file_names(self) -> List[str]:
"""COBOTTA ファイル一覧取得の互換 I/F である。
UR では対象ファイル機構がないため空 list を返す。"""
return []
[docs]
def get_file_object(self, filename: str) -> bool:
"""COBOTTA ファイルオブジェクト取得の互換 I/F である。"""
del filename
return False
[docs]
def download_file_from_cobotta(
self,
cobotta_filename: str,
output_dir: str = ".",
) -> bool:
"""COBOTTA ファイル取得の互換 I/F である。
UR では未対応である。"""
del cobotta_filename, output_dir
return False
[docs]
def download_all_files_from_cobotta(
self,
output_dir: str = ".",
skip_filenames: Optional[Sequence[str]] = None,
) -> bool:
"""COBOTTA 全ファイル取得の互換 I/F である。
UR では未対応である。"""
del output_dir, skip_filenames
return False
[docs]
def upload_content_to_cobotta(
self,
file_content: str,
cobotta_filename: str,
) -> bool:
"""COBOTTA ファイル内容アップロードの互換 I/F である。
UR では未対応である。"""
del file_content, cobotta_filename
return False
[docs]
def upload_file_to_cobotta(
self,
upload_filename: str,
cobotta_filename: Optional[str] = None,
) -> bool:
"""COBOTTA ファイルアップロードの互換 I/F である。
UR では未対応である。"""
del upload_filename, cobotta_filename
return False
[docs]
def get_cobotta_file_content(self, cobotta_filename: str) -> str:
"""COBOTTA ファイル内容取得の互換 I/F である。
UR では未対応である。"""
del cobotta_filename
return ""
[docs]
def get_cobotta_all_files_content(
self,
skip_filenames: Optional[Sequence[str]] = None,
) -> Dict[str, str]:
"""COBOTTA 全ファイル内容取得の互換 I/F である。
UR では未対応である。"""
del skip_filenames
return {}
# error / helpers --------------------------------------------------
@property
def error_code(self) -> int:
"""現在の互換エラーコードを返す。"""
return self.get_error_code()
@property
def error_description(self) -> str | bool:
"""最後の例外または現在の UR safety state を説明文字列として返す。"""
return self.get_error_description(self.error_code)
[docs]
def get_error(self) -> List[Dict[str, Any]]:
"""現在確認できる UR エラー情報を返す。
COBOTTA では controller の GetCurErrorInfo を返す。UR には同等の蓄積
エラー一覧がないため、RTDE / Dashboard から取得可能な現在状態を 1 件の
互換エラー情報として返す。
"""
info = self.get_current_error_info()
try:
code = int(info.get("code", 0))
except Exception as e:
self._logger.error(f"**Error**: {e}")
code = -1
if code == 0:
return []
return [info]
[docs]
def get_error_code(self) -> int:
"""現在の互換エラーコードを返す。"""
if self._last_error_info is not None:
return int(self._last_error_info.get("code", -1))
return self._state_error_code(self.state())
[docs]
def get_error_count(self) -> int:
"""現在確認できる互換エラー数を返す。"""
return len(self.get_error())
[docs]
def get_error_info(self, index: int) -> Dict[str, Any]:
"""指定 index の互換エラー情報を返す。"""
errors = self.get_error()
if index < 0 or index >= len(errors):
return {}
return errors[index]
[docs]
def get_error_description(self, error_code: int) -> Union[str, bool]:
"""error_code に対応する説明文字列を返す。
COBOTTA 側と同じく、外部からエラー原因を確認するときはこの description
を見る。UR では RC8 の GetErrorDescription 相当がないため、最後に記録した
例外情報または現在の safety / robot state から説明文を生成する。
"""
try:
code = int(error_code)
except Exception as e:
return self.print_error_info(e)
if self._last_error_info is not None:
try:
last_code = int(self._last_error_info.get("code", -1))
except Exception as e:
self._logger.error(f"**Error**: {e}")
last_code = -1
if code == last_code:
return str(self._last_error_info.get("description", ""))
if code == 0:
return ""
return self._state_error_description(self.state())
[docs]
def get_current_error_info(self) -> Dict[str, Any]:
"""現在発生しているエラー情報を 1 件返す。
COBOTTA の get_current_error_info() に相当する。UR では現在の RTDE / Dashboard
状態から code / description / state を生成する。
"""
if self._last_error_info is not None:
return dict(self._last_error_info)
if self.is_connected():
state = self.state()
else:
state = {"connected": False}
code = self._state_error_code(state)
description = ""
if code != 0:
description = self._state_error_description(state)
return {
"code": code,
"description": description,
"state": state,
}
[docs]
def print_error(self, e: Any) -> bool:
"""例外、文字列、互換エラーコードをログ出力する。"""
return self._error_handle(e)
[docs]
def print_error_info(self, e: Any) -> bool:
"""COBOTTA 互換のエラー表示エイリアス。"""
return self.print_error(e)
[docs]
def _error_handle(self, e: Any) -> bool:
"""COBOTTA 型の内部エラー処理。
外部から呼ぶ API ではない。各メソッドの except 節から呼び、
最後の error_description を更新して False を返す。
ここでは state() や error_description を再帰的に呼ばない。
"""
description = (
"UR API 実行中にエラーが発生しました。\n"
"An error occurred while executing the UR API.\n"
"\n"
f"詳細 / Details:\n {e}"
)
self._set_last_error(
e,
description=description,
error_origin=self.__class__.__name__,
detected_by="_error_handle()",
)
self._logger.error(
"UR API error / UR API エラー\n" "description:\n%s",
description,
)
return False
[docs]
def _set_last_error(
self,
error: Any,
description: Optional[str] = None,
source_class: Optional[str] = None,
source_method: Optional[str] = None,
operation: Optional[str] = None,
stage: Optional[str] = None,
state: Optional[Dict[str, Any]] = None,
error_origin: Optional[str] = None,
detected_by: Optional[str] = None,
) -> None:
"""最後のエラー情報を保存する。
ログは出さない。エラー表示は print_error()/print_error_info() 側で行う。
"""
if description is None:
description = str(error)
code = -1
try:
code = int(str(error))
except (TypeError, ValueError):
code = -1
if isinstance(error, BaseException):
last_error = error
else:
last_error = RuntimeError(str(error))
self._last_error = last_error
self._last_error_info = {
"code": code,
"description": str(description),
"exception_type": type(error).__name__,
"source_class": source_class or self.__class__.__name__,
"source_method": source_method or "unknown",
"operation": operation or "unknown",
"stage": stage or "unknown",
"error_origin": error_origin or source_class or self.__class__.__name__,
"detected_by": detected_by or source_method or "unknown",
}
if state is not None:
self._last_error_info["state"] = state
[docs]
def _record_error(self, error: Any) -> None:
"""後方互換用。内部では最後のエラー情報だけを保存する。"""
self._set_last_error(error, description=str(error))
[docs]
def _fail(self, error: Any) -> bool:
"""例外を COBOTTA 型の失敗値 False に変換する。"""
return self.print_error_info(error)
[docs]
def _cleanup_control_interface(self) -> None:
"""RTDEControl だけを破棄する。"""
interface = self._rtde_c
if interface is not None:
try:
interface.disconnect()
except Exception as exc:
self._logger.warning(
"*** UR RTDE CLEANUP *** _rtde_c cleanup failed: %s",
exc,
)
self._rtde_c = None
[docs]
def _cleanup_rtde_interfaces(
self,
include_control: bool = True,
) -> None:
"""RTDE interface を破棄する。Dashboard は閉じない。"""
names = ["_rtde_io", "_rtde_r"]
if include_control:
names.insert(0, "_rtde_c")
for name in names:
interface = getattr(self, name, None)
if interface is not None:
try:
interface.disconnect()
except Exception as exc:
self._logger.warning(
"*** UR RTDE CLEANUP *** %s cleanup failed: %s",
name,
exc,
)
setattr(self, name, None)
[docs]
def _cleanup_interfaces(self) -> None:
"""接続失敗時の掃除専用処理。
cleanup 中の失敗は最後のエラー原因を上書きしない。
"""
self._cleanup_rtde_interfaces(include_control=True)
interface = self._dashboard
if interface is not None:
try:
interface.disconnect()
except Exception as exc:
self._logger.warning(
"*** UR RTDE CLEANUP *** _dashboard cleanup failed: %s",
exc,
)
self._dashboard = None
[docs]
def _control(self) -> Any:
"""RTDEControlInterface を取得する。
未接続の場合は例外を送出する。"""
if self._rtde_c is None:
raise RuntimeError("RTDE control is not connected")
return self._rtde_c
[docs]
def _receive(self) -> Any:
"""RTDEReceiveInterface を取得する。
未接続の場合は例外を送出する。"""
if self._rtde_r is None:
raise RuntimeError("RTDE receive is not connected")
return self._rtde_r
[docs]
def _io(self) -> Any:
"""RTDEIOInterface を取得する。
未接続の場合は例外を送出する。"""
if self._rtde_io is None:
raise RuntimeError("RTDE IO is not connected")
return self._rtde_io
[docs]
def _dashboard_client(self) -> Any:
"""DashboardClient を取得する。
Dashboard 無効時または未接続時は例外を送出する。"""
if self._dashboard is None:
raise RuntimeError("Dashboard is disabled; set use_dashboard=True")
return self._dashboard
[docs]
def _unsupported(
self,
name: str,
) -> bool:
"""未対応機能を Cobotta 互換の False 戻り値へ変換する。"""
return self._fail(NotImplementedError(f"{name} is specific to Cobotta/RC8"))
[docs]
def init_hand(self) -> bool:
"""COBOTTA ハンド初期化の互換 I/F である。
UR 単体では未対応である。"""
return self._unsupported("init_hand")
[docs]
def open_hand(
self,
speed: Optional[int] = None,
) -> bool:
"""COBOTTA ハンド開操作の互換 I/F である。
UR 単体では未対応である。"""
del speed
return self._unsupported("open_hand")
[docs]
def hand_open(
self,
speed: Optional[int] = None,
) -> bool:
"""open_hand() の互換別名である。"""
return self.open_hand(speed)
[docs]
def hand(
self,
*args: Any,
**kwargs: Any,
) -> bool:
"""COBOTTA ハンド制御の互換 I/F である。
UR 単体では未対応である。"""
del args, kwargs
return self._unsupported("hand")
[docs]
def run_script(self) -> bool:
"""COBOTTA スクリプト実行の互換 I/F である。
UR では専用スクリプト管理を持たないため未対応である。"""
return self._unsupported(
"run_script; use RTDEControlInterface.sendCustomScript"
)
if __name__ == "__main__":
ROBOT_IP = "10.170.10.56"
logger = XLogger(log_level="INFO", logger_name=__name__)
robot = UrRtde(
ROBOT_IP,
logger=logger,
)
robot.set_check_ik(False)
try:
if not robot.connect_dashboard():
raise SystemExit(1)
# ---- Rest Error(1)
if not robot.connect_error_receiver():
raise SystemExit(1)
if not robot.connect_receive():
raise SystemExit(1)
if not robot.connect_io():
raise SystemExit(1)
# ---- Rest Error(2)
if not robot.dashboard_recovery():
raise SystemExit(1)
if not robot.connect_control():
raise SystemExit(1)
current_position = robot.current_position
if current_position is False:
raise SystemExit(1)
logger.info("current_position=%s", current_position)
if not robot.move(
current_position,
offset=(0.0, 0.0, 5.0),
):
logger.error(
"move failed: code=%s description=%s",
robot.error_code,
robot.error_description,
)
raise SystemExit(1)
logger.info("current_position=%s", robot.current_position)
finally:
robot.disconnect()