#!/usr/bin/env python
# coding:utf-8
"""
スピードの設定が１００になってないと
動いていないように見えるので注意
Kengo NAKADA:
https://github.com/shimane-dev, https://github.com/kengo-nakada
kengo.nakada@mat.shimane-u.ac.jp, kengo.nakada@gmail.com
"""
# print(version("cobotta2-system"))
import sys
import os
from tokenize import cookie_re
from typing import Any, Optional, Set, Dict, List, Union, Tuple, Type, TypeVar
import gc
import uuid
from pathlib import Path
import json
import re
# import tomllib
try:
# python 3.11
import tomllib
except ModuleNotFoundError:
# python 3.7
import tomli as tomllib
import time
# import threading
# error Handling
try:
import win32com.client
import pythoncom
except Exception as e:
# クライアントモジュールとして Linux から使う時にエラーを無視する
pass
# from cobotta_name_resolver import VariableNameResolver
from cobotta2.util.cobotta_motion_mode import MotionMode
from cobotta2.config import Config
[docs]
class CobottaOrin2:
"""
ORiN2 の COM を用いて、python から制御する
そのための簡単なラッパークラス
"""
Provider = "CaoProv.DENSO.RC8"
def __init__(
self,
cobotta_ip: str = "192.168.0.1",
arm_name: str = "Arm",
instance_name: str = "Ctrl",
*,
logger: Optional[Any] = None,
log_level: Optional[str] = None,
init_com: Optional[bool] = True,
config: Union[Config, Any, None] = None,
):
"""
Args:
cobotta_ip (str): Cobotta IP Address
arm_name (str): Arm Name
instance_name (str): instance name(ログでの確認・区別用)
logger (Any): logger
"""
if logger is None:
import logging
log_level = log_level or "INFO"
logging.basicConfig(level=log_level.upper())
logger = logging.getLogger(__name__)
# logger = get_silent_logger()
self._logger = logger
if config is None:
self._cobotta_ip = cobotta_ip
else:
self._cobotta_ip = config.COBOTTA_IP
self._logger.info(f"Config[cobottaip]: {config.COBOTTA_IP}")
self._arm_name = arm_name
self._instance_name = instance_name
# COM 初期化するかどうか？初期化したかどうか？のフラグ
self._init_com = bool(init_com)
self._com_initialized = False
self._jspeed: int = 100
self._speed: int = 100
self._extspeed: int = 100
self._accel: float = 100.0
self._decel: float = 100.0
self._extaccel: float = 100.0
self._extdecel: float = 100.0
# self.version = self.get_version("../../pyproject.toml")
# self.version = self.get_version(Path(__file__).parent.parent / "pyproject.toml")
# self._coord_type = "J"
# self._coord_type = None
self._coord_type = "P"
# パス変異量、次のパスへつなぐ動作をいつ開始するのか？
# self._ = "@P"
# self._path_blend = None
self._path_blend = "@P"
self._motion_mode: MotionMode = MotionMode.PTP
# self._resolver = VariableNameResolver()
self._task = None
self._task_state = None
self._caoEng = None # CAOEngine
self._caoCtrl = None # caoEng.Workspaces(0).AddController(cobotta_ip)
self._caoRob = None # caoCtrl.AddRobot(arm_name, "")
# --- ハンドルを release する方式にするため削除 ---
# caoCtrl.AddFile() で一度作成したインスタンを必ず覚える
# self._caoFile: Union[Dict[str, object], None] = {}
# cobotta 内部のファイルをオープンするハンドラの保存先
self._open_files = {}
self._var_IO = None
self._var_P = None
self._var_I = None
self._var_F = None
# for CobottaProcess
# manager = multiprocessing.Manager()
# self._share_data = manager.dict()
# self._stop_event = manager.Event()
self._initialize_com()
[docs]
@staticmethod
def connect():
"""dummy"""
pass
return True
[docs]
def _initialize_com(self) -> None:
"""
主にgRPCの自動スレッド化対策
gRPCが制御のインスタンスとインスから呼び出されるメソッドを別スレッドで動かすセンスのために
windows の COM モデルだと __init__ で COM 初期化すると崩壊する。
COMは同一スレッドである必要がある
そのためにCOM init は外部化するつ必要がある。
ローカルで使うときは、この _initialize_com を動かす。
それ以外は COM ロードを外部のものを使う
Returns:
"""
self._logger.info(f"==== Initialize COM ====")
try:
# Create CAOEngine
if self._init_com:
pythoncom.CoInitialize()
self._com_initialized = True
# init_com=False の場合は、呼び出し側（runner等）が
# 初期化している前提なので initialized を変更しない
self._logger.info(f"== Instance Name: {self._instance_name}")
self._logger.info(f"== Create CAO Engine")
self._caoEng = win32com.client.Dispatch("CAO.CaoEngine")
self._logger.info(f"== CobottaCtrl")
# self._logger.info(f" Version: {self.version}")
self._logger.info(f" Cobotta IP: {self._cobotta_ip}")
# Connect To R8
self._logger.info(f"== Create CAO Ctrl")
self._caoCtrl = self._caoEng.Workspaces(0).AddController(
"",
self.Provider,
"",
f"Server={self._cobotta_ip}",
)
# Connect To VRC
# caoCtrl = caoEng.Workspaces(0).AddController("","CaoProv.DENSO.RC8", "", "WPJ=*")
# AddRobot CaoRob オブジェクト生成とともに作る
self._logger.info(f"== Create CAO Robo")
self._logger.info(f"Arm_Name = {self._arm_name}")
self._caoRob = self._caoCtrl.AddRobot(self._arm_name, "")
except Exception as e:
self.delete()
self._logger.info(f"***Error***: __init__, {e}")
sys.exit(-1)
try:
#
# @var
#
state = self._caoRob.AddVariable("@STATE")
servo_on = self._caoRob.AddVariable("@SERVO_ON")
rc8_type = self._caoRob.AddVariable("@TYPE")
rc8_type_name = self._caoRob.AddVariable("@TYPE_NAME")
manual_move_mode = self._caoRob.AddVariable("@MANUAL_MOVE_MODE")
asp_mode = self._caoRob.AddVariable("@ASP_MODE")
lock = self._caoRob.AddVariable("@LOCK") # LOCK
synth_speed = self._caoRob.AddVariable("@SYNTH_SPEED")
motion_time = self._caoRob.AddVariable("@MOTION_TIME")
speed = self._caoRob.AddVariable("@SPEED")
jspeed = self._caoRob.AddVariable("@JSPEED")
accel = self._caoRob.AddVariable("@ACCEL")
decel = self._caoRob.AddVariable("@DECEL")
extspeed = self._caoRob.AddVariable("@EXTSPEED")
extaccel = self._caoRob.AddVariable("@EXTACCEL")
extdecel = self._caoRob.AddVariable("@EXTDECEL")
current_angle = self._caoRob.AddVariable("@CURRENT_ANGLE") # J-type
current_position = self._caoRob.AddVariable("@CURRENT_POSITION") # P-type
busy_status = self._caoRob.AddVariable("@BUSY_STATUS") # busy state
error_code = self._caoRob.AddVariable("@ERROR_CODE")
error_description = self._caoRob.AddVariable("@ERROR_DESCRIPTION")
self._var_IO = self._caoCtrl.AddVariable(f"IO*", "")
self._var_P = self._caoCtrl.AddVariable(f"P*", "")
self._var_I = self._caoCtrl.AddVariable(f"I*", "")
self._var_F = self._caoCtrl.AddVariable(f"F*", "")
self._var_data = {
"lock": lambda: lock.Value,
"state": lambda: state.Value,
"servo_on": lambda: servo_on.Value,
"rc8_type": lambda: rc8_type.Value,
"rc8_type_name": lambda: rc8_type_name.Value,
"asp_mode": lambda: asp_mode.Value,
"manual_move_mode": lambda: manual_move_mode.Value,
"synth_speed": lambda: synth_speed.Value,
"motion_time": lambda: motion_time.Value,
"speed": lambda: speed.Value,
"jspeed": lambda: jspeed.Value,
"accel": lambda: accel.Value,
"decel": lambda: decel.Value,
"extspeed": lambda: extspeed.Value,
"extaccel": lambda: extaccel.Value,
"extdecel": lambda: extdecel.Value,
"current_angle": lambda: current_angle.Value,
"current_position": lambda: current_position.Value,
"busy_status": lambda: busy_status.Value,
"error_code": lambda: error_code.Value,
"error_description": lambda: error_description.Value,
}
self._logger.info("")
except pythoncom.com_error as e:
self.delete()
self._logger.info(f"***Error***: __init__, {e}")
sys.exit(-1)
#
# system (Disconnect)
#
[docs]
def delete(self) -> bool:
"""GiveArm"""
try:
# halt
# self.halt()
# Disconnect
self.give_arm()
if self._caoRob is not None:
del self._caoRob
if self._caoCtrl is not None:
del self._caoCtrl
if self._caoEng is not None:
del self._caoEng
except Exception:
pass
if self._com_initialized:
try:
pythoncom.CoUninitialize()
except Exception:
pass
self._com_initialized = False
gc.collect()
return True
[docs]
def disconnect(self) -> bool:
try:
self.delete()
except Exception:
pass
return True
def __delete__(self):
try:
print("DELETE connect,,")
self.delete()
except Exception:
pass
[docs]
@staticmethod
def get_version(rel_toml_path):
"""
Args:
rel_toml_path:
Returns:
"""
base_dir = Path(__file__).parent
toml_path = (base_dir / rel_toml_path).resolve()
with open(toml_path, "rb") as f:
data = tomllib.load(f)
return data["project"]["version"]
def __getattr__(self, name):
"""
instance.P1, instance.P2, ..., F1, F2... の形式にマッチ
Args:
name:
Returns:
"""
# m = re.match(r"([PIF])(\d+)$", name)
m = re.match(r"(P|F|I|IO)(\d+)$", name)
if m:
kind, id_num = m.group(1), int(m.group(2))
var = getattr(self, f"_var_{kind}")
var.ID = id_num
return var.Value
raise AttributeError(
f"'{type(self).__name__}' object has no attribute '{name}'"
)
def __setattr__(self, name, value):
"""
instance.P1, instance.P2, ..., F1, F2... の形式にマッチ
Args:
name:
value:
Returns:
"""
if isinstance(name, str):
# m = re.match(r"([PIF])(\d+)$", name)
m = re.match(r"(P|F|I|IO)(\d+)$", name)
if m:
kind, id_num = m.group(1), int(m.group(2))
var = object.__getattribute__(self, f"_var_{kind}")
var.ID = id_num
var.Value = value
return
# それ以外は通常通り
super().__setattr__(name, value)
[docs]
def get_var(
self,
kind_or_combined: str,
id_num: Optional[int] = None,
) -> Any:
"""
kind_or_combined: 'P', 'I', 'F', 'IO' または 'P40', 'IO0' のような形式
id_num: 数値ID（省略可能）
get_var('P', 40)
get_var('IO', 0)
get_var('P40')
get_var('IO0')
"""
valid_kinds = (
"IO",
"P",
"I",
"F",
)
if id_num is None:
kind = None
for kind_name in valid_kinds:
if kind_or_combined.startswith(kind_name):
try:
id_part = kind_or_combined[len(kind_name) :]
id_num = int(id_part)
kind = kind_name
break
except ValueError:
self._logger.error(f"Invalid id number in {kind_or_combined!r}")
return False
if kind is None:
self._logger.error(f"Unknown variable kind in {kind_or_combined!r}")
return False
else:
kind = kind_or_combined
if kind not in valid_kinds:
self._logger.error(f"Invalid kind: {kind!r} (must be one of {valid_kinds})")
return False
var = getattr(self, f"_var_{kind}")
var.ID = id_num
return var.Value
[docs]
def set_var(
self,
kind_or_combined: str,
# id_or_value: int,
id_or_value: Any,
value: Any = None,
) -> bool:
"""
kind_or_combined: 'P', 'I', 'F', 'IO' または 'P40', 'IO0' のような形式
id_or_value: ID番号または（combined形式のときは）value
value: 実際の値（combined形式のときに指定）
"""
# valid_kinds = ("P", "I", "F", "IO")
valid_kinds = ("IO", "P", "I", "F")
if value is None:
kind = None
id_num = None
for kind_name in valid_kinds:
if kind_or_combined.startswith(kind_name):
try:
id_part = kind_or_combined[len(kind_name) :]
id_num = int(id_part)
kind = kind_name
break
except ValueError:
self._logger.error(f"Invalid id number in {kind_or_combined!r}")
return False
if kind is None:
self._logger.error(f"Unknown variable kind in {kind_or_combined!r}")
return False
value = id_or_value
else:
kind = kind_or_combined
id_num = id_or_value
if kind not in valid_kinds:
self._logger.error(f"Invalid kind: {kind!r} (must be one of {valid_kinds})")
return False
var = getattr(self, f"_var_{kind}")
var.ID = id_num
var.Value = value
return True
[docs]
def get_P(
self,
id_num: int,
) -> Union[List[float], bool]:
return self.get_var("P", id_num)
[docs]
def set_P(
self,
id_num: int,
value: Union[List[float], Tuple[float, ...]],
) -> bool:
try:
self.set_var("P", id_num, value)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_I(
self,
id_num: int,
) -> Union[int, bool]:
return self.get_var("I", id_num)
[docs]
def set_I(
self,
id_num: int,
value: int,
# value: float,
) -> bool:
try:
self.set_var("I", id_num, value)
return True
except Exception as e:
return self.print_error(e)
[docs]
def get_F(
self,
id_num: int,
) -> Union[float, bool]:
return self.get_var("F", id_num)
[docs]
def set_F(
self,
id_num: int,
value: float,
) -> bool:
try:
self.set_var("F", id_num, value)
return True
except Exception as e:
return self.print_error(e)
[docs]
def get_IO(self, id_num: int) -> Any:
return self.get_var("IO", id_num)
[docs]
def set_IO(
self,
id_num: int,
value: float,
) -> bool:
try:
self.set_var("IO", id_num, value)
return True
except Exception as e:
return self.print_error(e)
#
# Error Print
#
[docs]
def print_error(self, e) -> bool:
# self._logger.error(e)
# return False
return self._error_handle(e)
# def print_error_info(self, e) -> bool:
# self._logger.error(e)
# self._logger.error(f"error_code: {self.error_code}")
# self._logger.error(f"error_description: {self.error_description}")
# return False
[docs]
def print_error_info(self, e) -> bool:
return self.print_error(e)
[docs]
def _error_handle(self, e) -> bool:
"""
b-cap制御側と仕組みを合わせる
Args:
e: エラーコード番号
Returns:
"""
if isinstance(e, str):
self._logger.error(e)
return False
self._logger.error(e)
error_code = None
error_description = None
try:
error_code = int(str(e))
except (TypeError, ValueError):
error_code = self.error_code
if error_code is not None:
try:
if int(error_code) < 0:
error_code_hex = format(int(error_code) & 0xFFFFFFFF, "x")
else:
error_code_hex = hex(int(error_code))
self._logger.error(f"error_code: {error_code} (0x{error_code_hex})")
except Exception:
self._logger.error(f"error_code: {error_code}")
else:
self._logger.error("error_code: None")
if error_code is not None:
try:
error_description = self.get_error_description(int(error_code))
except Exception:
error_description = None
if error_description in (None, ""):
error_description = self.error_description
self._logger.error(f"error_description: {error_description}")
return False
#
# system (STOP)
#
[docs]
def halt(self) -> bool:
"""
Robo 停止(moveとかと止めるとき)
Returns:
bool: True/False
"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("Halt")
return True
except Exception as e:
# self._logger.critical(f"Exception Halt: {e}")
# return self.print_error_info(e)
return False
@property
def task(self):
return self._task
[docs]
def _release_loaded_task(self) -> None:
if self._task is not None:
try:
self._task.Variables.Remove("@STATUS")
except Exception:
pass
self._task_state = None
if self._caoCtrl is not None and self._task is not None:
try:
self._caoCtrl.Tasks.Remove(self._task.Name)
except Exception:
pass
self._task = None
[docs]
def _release_loaded_file(self, cobotta_filename: str) -> None:
if self._caoCtrl is None:
return
try:
self._caoCtrl.Files.Remove(cobotta_filename)
except Exception:
pass
@property
def task_state(self):
return self._task_state
[docs]
def get_task_status(self):
"""task_state のbool値とかぶるので、None のままだと None, 例外でも None にする"""
if self._task_state is None:
return None
try:
return self._task_state.Value
except Exception as e:
self._logger.error(f"get_task_status: {e}")
# return False
return None
[docs]
def task_stop(self) -> bool:
"""
Task 停止
個別の motor の停止とはことなって、task run を止める
Returns:
bool: True/False
"""
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
self._caoCtrl.Execute("Stop")
return True
except Exception as e:
# self._logger.critical(f"Exception Task Stop: {e}")
return False
# return self.print_error_info(e)
[docs]
def hand_stop(self) -> bool:
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
self._caoCtrl.Execute("HandStop")
return True
except Exception as e:
# self._logger.critical(f"Exception HandStop: {e}")
return False
# return self.print_error_info(e)
[docs]
def stop(self):
"""
それぞれの 停止命令(ハンド,move,タスク)を実行する
Returns:
"""
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
# hand_stop
self.hand_stop()
except Exception as e:
pass
try:
# move 停止
self.halt()
except Exception as e:
pass
try:
# task 停止
self.task_stop()
except Exception as e:
pass
try:
# motor off
self.turn_off_motor()
except Exception as e:
pass
#
# system (Rest)
#
[docs]
def reset_error(self) -> bool:
"""
Returns:
bool: 成功か失敗か？
"""
self._logger.debug("== reset_error()")
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
# 1. RC8 が要求するリセット準備
self._caoRob.Execute("ManualResetPreparation")
self._logger.info("[reset_error] ManualResetPreparation executed")
# 2. エラー消去（Controller 操作）
self._caoCtrl.Execute("ClearError")
self._caoCtrl.Execute("GiveArm")
# 3. MotionPreparation
self._caoRob.Execute("MotionPreparation", "")
self._logger.info("[reset_error] MotionPreparation executed")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def release_arm(self) -> bool:
"""
リリースアーム
Returns:
bool: 成功か失敗か？
"""
self._logger.debug("== release_arm()")
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
self._caoCtrl.Execute("GiveArm")
return True
except Exception as e:
return self.print_error_info(e)
#
# state
#
[docs]
def motion_complete(
self,
) -> Union[str, bool]:
"""
アームが停止しているのならば inactive
"""
self._logger.debug("== state()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
if not self._caoRob.Execute("MotionComplete"):
return "active"
return "inactive"
#
# 座標系: work
#
[docs]
def change_work(
self,
work_no: int,
) -> bool:
"""
Args:
work_no (int): 0=ベース座標系
Returns:
"""
self._logger.debug("== change_work()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._logger.info(f"change: Work{work_no}")
self._caoRob.change(f"Work{work_no}")
return True
except Exception as e:
return self.print_error(e)
[docs]
def get_current_work_no(
self,
) -> Union[int, bool]:
"""
CurWork
現在の work(座標軸) 番号を取得
Change で work 版ををそもそも定義してないと例外が出る
Returns:
"""
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
return self._caoCtrl.Execute("CurWork")
except Exception as e:
# return self.print_error_info(e)
self._logger.error(f"**Error**: おそらくワーク設定がまだ存在してない. {e}")
return False
[docs]
def get_work(
self,
work_no: Optional[int] = None,
) -> Union[list, tuple, bool]:
"""
GetWorkDef
Args:
work_no (int | None):
Returns:
"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
if work_no is None:
work_no = self.get_current_work_no()
try:
self._logger.info(f"GetWorkDef, {work_no}")
return self._caoRob.Execute("GetWorkDef", work_no)
except Exception as e:
return self.print_error(e)
[docs]
def set_work(
self,
work_no: int,
work_define: Union[tuple, list, str],
work_attribute: int = 0,
) -> bool:
"""
1, "P2"
1, "P(100, 200, 300, 180, 0, 180)"
Args:
work_no (int):
work_define (tuple | list | str): [X,Y,Z,RX,RY,RZ]
work_attribute (int):
Returns:
bool:
"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
if isinstance(work_define, (list, tuple)):
base = list(work_define)[:6] # dev は str でも list でも 6要素まで
work_define = "P(" + ",".join(str(x) for x in base) + ")"
self._logger.info(f"set_work_define: {work_no} {work_define} {work_attribute}")
try:
self._caoRob.Execute("SetWorkDef", (work_no, work_define))
return True
except Exception as e:
return self.print_error(e)
#
# 座標系: Tool
#
#
# take
#
[docs]
@staticmethod
def _make_file_content(
evp_script_name: str,
) -> str:
""""""
file_content = f"""
#Include "Variant.h"
Dim ctrl As Object
Sub Main
ctrl = Cao.AddController("Runner", "CaoProv.DENSO.EVP2", "", "@IfnotMember")
ctrl.LoadFile "{evp_script_name}"
ctrl.Run
End Sub
"""
return file_content
[docs]
def take_camera(
self,
evp_script_name: str,
# pac_script_name: str,
pac_script_name: Optional[str] = None,
) -> bool:
"""
一連の作業を省略するためだけのメソッド
1) EVP2用のスクリプト名を入れる(事前にカメラ側で作っておく)
2) 撮影用のPacScript自動作成(PacScript名だけを設定する)
3) take_script
Args:
evp_script_name (str):
pac_script_name (str): 内部でこっそり使うスクリプト名（なんでもいい)
Returns:
bool:
"""
# pac_script_name = "EVP2_p3.pcs"
# evp_script_name = "try_camera"
try:
# 実は名前はなんでもいい
# 内容は content から自動で作りアップロードするので名前は必要
if pac_script_name is None:
pac_script_name: str = f"EVP2_{uuid.uuid4().hex[:8]}.pcs"
else:
pac_script_name: str = str(pac_script_name)
file_content = self._make_file_content(evp_script_name)
self.upload_content_to_cobotta(file_content, pac_script_name)
self.take_script(pac_script_name)
return True
except Exception as e:
return self.print_error(e)
[docs]
def take_script(self, script: str = "pro1") -> bool:
"""
AddTask() and AddVariable()
PacScript を追加して、スクリプト動作する
Args:
script (str): PacScript Name (拡張しありを入れる)
Returns:
bool:
"""
# スクリプトに拡張子がある時はそれをはずす
script = self.remove_extension(script)
self._logger.info(f"== take_script({script})")
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
# print(f"{self._caoEng}")
# print(f"{self._caoRob}")
# print(f"{self._caoCtrl}")
# AddTask
try:
self._release_loaded_task()
# スクリプトがローカルにない場合など
self._logger.info(f"== Addtask({script})")
self._task = self._caoCtrl.Addtask(f"{script}", "")
# AddVariable (TaskObject)
self._logger.info(f"== AddVariable('@STATUS')")
self._task_state = self._task.AddVariable("@STATUS", "")
self._logger.info(f"status: {self._task_state}")
return True
except Exception as e:
self._logger.error(
f"スクリプトがcobotta側にない場合などで、Addtask が失敗する"
)
return self.print_error_info(e)
[docs]
def take_arm(self) -> bool:
"""
AddRbot() and Take Arm()
cobotta を追加して、手動動作する (PacScript動作ではない)
Returns:
bool:
"""
self._logger.debug("== take_arm()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
# Take Arm (制御権取得/速度ど)
# アームグループ0 で、初期化する take arm keep = 0
try:
self._caoRob.Execute("TakeArm", [0, 0])
self._logger.info("Complete Take arm")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def give_arm(self) -> bool:
"""
Arm の解放(サーバー側で cobotta 側を解放)
サーバーを使うマルチユーザーはまだ考慮してない
Returns:
bool: 可否(例外が出たら否)
"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("GiveArm")
self._logger.info("Complete Give Arm")
return True
except Exception as e:
self._logger.error(f"Exception give_arm: {e}")
return False
#
# @var
#
# @VarData("P", 1)
# def P1(self):
# pass
# @VarData("P", 2)
# def P2(self):
# pass
#
# property: coord_type
#
@property
def coord_type(self):
return self.get_coord_type()
@coord_type.setter
def coord_type(self, value):
self.set_coord_type(value)
[docs]
def set_coord_type(
self,
coord_type: str,
) -> bool:
"""
self._coord_type = "J"
Args:
coord_type:
Returns:
"""
self._coord_type = coord_type
return True
[docs]
def get_coord_type(self) -> str:
return self._coord_type
#
# property: pose format
#
@property
def path_blend(self):
return self.get_path_blend()
@path_blend.setter
def path_blend(self, value):
self.set_path_blend(value)
[docs]
def set_path_blend(
self,
path_blend: str,
) -> bool:
"""
self._path_blend = "@P"
パス変異量、次のパスへつなぐ動作をいつ開始するのか？
Args:
path_blend:
Returns:
None:
"""
self._path_blend = path_blend
return True
[docs]
def get_path_blend(self) -> str:
return self._path_blend
#
# property: motion_mode
#
@property
def motion_mode(self):
return self.get_motion_mode()
@motion_mode.setter
def motion_mode(self, value):
self.set_motion_mode(value)
[docs]
def get_motion_mode(self):
return self._motion_mode
[docs]
def set_motion_mode(
self,
mode: Union[int, str, MotionMode],
) -> bool:
int_to_mode = {
1: MotionMode.PTP,
2: MotionMode.LINE,
3: MotionMode.CIRCLE,
4: MotionMode.SPLINE,
}
if isinstance(mode, MotionMode):
self._motion_mode = mode
return True
if isinstance(mode, int):
if mode in int_to_mode:
self._motion_mode = int_to_mode[mode]
return True
self._logger.error(f"Invalid motion mode int: {mode}")
return False
if isinstance(mode, str):
mode_text = mode.lower()
for candidate_motion_mode in MotionMode:
if candidate_motion_mode.value.lower() == mode_text:
self._motion_mode = candidate_motion_mode
return True
if candidate_motion_mode.name.lower() == mode_text:
self._motion_mode = candidate_motion_mode
return True
self._logger.error(f"Invalid motion mode string: {mode}")
return False
self._logger.error(f"Invalid type for motion mode: {type(mode)}")
return False
#
# bstrOpt
#
[docs]
@staticmethod
def _to_bstrOpt_string(
opt: Union[str, int, None],
) -> Optional[str]:
if opt is None:
return None
if isinstance(opt, str):
return opt
if isinstance(opt, int):
return f"SPEED={opt},NEXT"
return None
[docs]
def _resolve_bstrOpt(
self,
bstrOpt: Union[str, int, None],
speed: Optional[int] = None,
is_async: Optional[bool] = True,
accel: Optional[float] = None,
decel: Optional[float] = None,
) -> Union[str, bool, None]:
""""""
if speed is not None:
use_speed = speed
else:
use_speed = self._speed
if accel is not None:
use_accel = accel
else:
use_accel = self._accel
if decel is not None:
use_decel = decel
else:
use_decel = self._decel
if bstrOpt is not None:
use_bstrOpt = self._to_bstrOpt_string(bstrOpt)
if use_bstrOpt is None:
self._logger.error("Invalid bstrOpt type")
return False
return use_bstrOpt
opts: List[str] = []
if use_speed is not None:
opts.append(f"SPEED={use_speed}")
if use_accel is not None:
opts.append(f"ACCEL={use_accel}")
if use_decel is not None:
opts.append(f"DECEL={use_decel}")
if is_async:
opts.append("NEXT")
if len(opts) == 0:
return None
return ",".join(opts)
[docs]
def _resolve_hand_bstrOpt(
self,
bstrOpt: Union[str, int, None] = None,
is_async: Optional[bool] = False,
) -> Union[str, bool, None]:
""""""
if isinstance(bstrOpt, str):
opt = bstrOpt
elif isinstance(bstrOpt, int):
opt = None
elif bstrOpt is None:
opt = None
else:
self._logger.error("Invalid hand bstrOpt type")
return False
if opt is None and is_async:
return "NEXT"
return opt
[docs]
def _resolve_motion_mode(
self,
mode: Union[int, str, MotionMode, None],
) -> MotionMode:
int_to_mode = {
1: MotionMode.PTP,
2: MotionMode.LINE,
3: MotionMode.CIRCLE,
4: MotionMode.SPLINE,
}
if mode is None:
return self._motion_mode
if isinstance(mode, MotionMode):
return mode
if isinstance(mode, int):
if mode in int_to_mode:
return int_to_mode[mode]
self.print_error(f"**Error**: Invalid motion_mode int: {mode}")
return self._motion_mode
if isinstance(mode, str):
mode_text = mode.lower()
for candidate_motion_mode in MotionMode:
if candidate_motion_mode.value.lower() == mode_text:
return candidate_motion_mode
if candidate_motion_mode.name.lower() == mode_text:
return candidate_motion_mode
self.print_error(f"**Error**: Unknown motion_mode string: '{mode}'")
return self._motion_mode
self.print_error(f"**Error**: Invalid type for motion_mode: {type(mode)}")
return self._motion_mode
#
# filename
#
[docs]
def get_cobotta_file_names(
self,
) -> Union[List[str], Tuple[str, ...], None]:
"""
cobotta 内部のファイル一覧(cao.fileName)
Returns:
"""
if not self._caoCtrl:
self.print_error("**Error**: caoCtrl is None")
return None
# cobotta 内部のファイル名一覧
return self._caoCtrl.FileNames
@property
def open_files(self):
"""コボッタ内部のファイルを開いたりするのに使う"""
return self._open_files
[docs]
def get_open_files(self):
"""コボッタ内部のファイルを開いたりするのに使う"""
return self._open_files
[docs]
def get_file_object(self, filename):
"""
同じファイル名のAddFileは一度きり。
Args:
filename:
Returns:
"""
try:
if filename not in self._open_files:
# self._open_files[filename] = self._caoCtrl.AddFile(filename + ".pcs")
self._open_files[filename] = self._caoCtrl.AddFile(filename)
return self._open_files[filename]
except Exception as e:
self.print_error_info(e)
return None
[docs]
def get_cobotta_file_content(
self,
cobotta_filename: str,
) -> Optional[Dict[str, str]]:
"""
Args:
cobotta_filename (str):
Returns:
Optional[Dict[str, str]]:
"""
# 同名のファイルオブジェクトは一度きりだけ取得
file = self.get_file_object(cobotta_filename)
if not file:
self._logger.error(
f"**Error**: Failed to get file object: {cobotta_filename}"
)
return None
try:
content = file.Value
if isinstance(content, str):
return {"filename": cobotta_filename, "content": content}
else:
pass
# self.print_error_info(
# f"Warning: Not string for {cobotta_filename}, skipped"
# )
except Exception as e:
self.print_error_info(
f"Error: Failed to read file: {cobotta_filename}: {e}"
)
return None
[docs]
def get_cobotta_all_files_content(
self,
skip_filenames: Union[List[str], Set[str], Tuple[str, ...], str, None] = None,
) -> Optional[List[Dict[str, str]]]:
"""
COBOTTA内全ファイル内容を [{'filename':..., 'content':...}, ...] で返す（保存しない）
クラス内に保存するだけ
Args:
skip_filenames (set or list): 除外したいファイル名
Returns:
list: [{'filename':..., 'content':...}, ...]
"""
if not self._caoCtrl:
self.print_error_info("**Error**: caoCtrl is None")
# return []
return None
# cobotta 内のファイル名一覧
filenames = self.get_cobotta_file_names()
self._logger.info(f"filenames: {filenames}")
if filenames is None:
self.print_error_info("**Error**: No file names retrieved from COBOTTA.")
return None
if not isinstance(filenames, (list, tuple)):
self.print_error_info(
"**Error**: Invalid file names retrieved from COBOTTA."
)
return None
result = []
# skip_filenames = set(skip_filenames or [])
if skip_filenames is None:
skip_filenames = ["PacAttri.lst"]
elif isinstance(skip_filenames, str):
skip_filenames = [skip_filenames]
else:
skip_filenames = list(skip_filenames)
for name in filenames:
if name in skip_filenames:
# self.print_error_info(f"**Warning**: Skipped file (by request): {name}")
continue
# ファイル名一つ一つに対して中身を取得する
item = self.get_cobotta_file_content(name)
if item is not None:
result.append(item)
else:
pass
# self.print_error_info(
# f"**Warning**: Skipped file (failed read): {name}"
# )
return result
[docs]
def download_file_from_cobotta(
self,
cobotta_filename: str,
output_dir: Union[str, Path, None] = None,
) -> bool:
"""
COBOTTA内部の指定ファイルをローカルに保存
Args:
cobotta_filename (str): COBOTTA内部ファイル名( without .pcs)
output_dir (str | Path | None): 保存先ディレクトリ
Returns:
bool: 成功すればTrue、失敗すればFalse
"""
item = self.get_cobotta_file_content(cobotta_filename)
if not item:
return False
if not output_dir:
output_dir = Path.cwd()
else:
output_dir = Path(output_dir)
output_dir.mkdir(parents=True, exist_ok=True)
file_path = output_dir / cobotta_filename
if file_path.exists():
# self.print_error_info(
# f"**Warning**: File already exists, skipped: {file_path}"
# )
return False
try:
with open(file_path, "w", encoding="utf-8", newline="") as f:
f.write(item["content"])
return True
except Exception as e:
pass
self.print_error_info(
f"**Error**: Failed to save file '{cobotta_filename}': {e}"
)
return False
[docs]
def download_all_files_from_cobotta(
self,
output_dir: Union[str, Path, None] = None,
# skip_filenames: Optional[Set[str]] = "PacAttri.lst",
skip_filenames: Union[List[str], Set[str], Tuple[str, ...], str, None] = None,
) -> bool:
"""
COBOTTA内全ファイルをローカル保存
Args:
output_dir (str | Path | None): 保存先ディレクトリ
skip_filenames (set or list): 除外したいファイル名
Returns:
bool: 全ファイル保存成功でTrue、失敗が1つでもあればFalse
"""
items = self.get_cobotta_all_files_content(skip_filenames=skip_filenames)
self._logger.debug(f"{items}")
if not isinstance(items, list):
self._logger.debug(f"**Error**: No file content in COBOTTA")
return False
if not output_dir:
# None
output_dir = Path.cwd()
else:
output_dir = Path(output_dir)
self._logger.debug(f"output_dir = {output_dir}")
output_dir.mkdir(parents=True, exist_ok=True)
success = True
for item in items:
file_path = output_dir / item["filename"]
if file_path.exists():
# self.print_error_info(
# f"**Warning**: File already exists, skipped: {file_path}"
# )
continue
try:
with open(file_path, "w", encoding="utf-8", newline="") as f:
f.write(item["content"])
except Exception as e:
# self.print_error_info(
# f"**Error**: Failed to save file '{item['filename']}': {e}"
# )
success = False
return success
[docs]
def upload_content_to_cobotta(
self, file_content: str, cobotta_filename: str
) -> bool:
"""
Args:
file_content:
cobotta_filename:
Returns:
"""
self._logger.info(f"== upload_content_to_cobotta")
self._logger.info(f"\n{file_content}")
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
file = None
try:
# # 1. 既に登録済みなら再利用
# if cobotta_filename in self._caoFile:
# self._logger.info(f"use existing caoFile: {cobotta_filename}")
# file = self._caoFile[cobotta_filename]
# else:
# self._logger.info(f"create caoFile: {cobotta_filename}")
# self._caoFile[cobotta_filename] = self._caoCtrl.AddFile(
# cobotta_filename, "@Create=1"
# )
# self._caoFile[cobotta_filename].Value = file_content
self._release_loaded_file(cobotta_filename)
file = self._caoCtrl.AddFile(cobotta_filename, "@Create=1")
file.Value = file_content
return True
except Exception as e:
self._logger.error(f"**Error**: Failed to upload file: {str(e)}")
return False
[docs]
def upload_file_to_cobotta(
self,
upload_filename: Union[str, Path],
cobotta_filename: str,
) -> bool:
"""
Args:
upload_filename (str|Path): アップロードするローカルのファイル
cobotta_filename (str): cobotta 内部でのファイル名
Returns:
"""
self._logger.info(f"== upload_file_to_cobotta: {upload_filename}")
try:
# ローカルファイルの内容をすべて読み込む
with open(upload_filename, "r", encoding="utf-8") as f:
file_content = f.read()
return self.upload_content_to_cobotta(file_content, cobotta_filename)
except Exception as e:
return self.print_error(f"**Error**: Failed to upload file: {e}")
#
# @var
#
[docs]
def _get_var_data(
self,
message,
default=None,
) -> Any:
"""
@current_positon などの @型内部変数を取得する
Args:
message:
default:
Returns:
"""
func = self._var_data.get(f"{message}", None)
if func is not None:
ret = func()
if ret is None:
return default
return ret
return default
@property
def current_position(self) -> Union[list, tuple, bool]:
"""P型で取得, @Current_Position と同等"""
# return self._share_data.get("current_angle", None)
# return self._get_var_data("current_position", None)
return self._get_var_data("current_position", False)
@property
def current_angle(self) -> Union[list, tuple, bool]:
"""J型で取得, @Current_Position と同等"""
# return self._share_data.get("current_angle", None)
# return self._get_var_data("current_angle", None)
return self._get_var_data("current_angle", False)
@property
def speed(self) -> Optional[int]:
"""内部スピード(100パーセント), take_arm で100に初期化, 実速度=内部x外部"""
return self._get_var_data("speed", None)
@property
def jspeed(self):
return self._get_var_data("jspeed", None)
@property
def accel(self):
return self._get_var_data("accel", None)
@property
def decel(self):
return self._get_var_data("decel", None)
@property
def extspeed(self):
return self._get_var_data("extspeed", None)
@property
def extaccel(self):
return self._get_var_data("extaccel", None)
@property
def extdecel(self):
return self._get_var_data("extdecel", None)
#
# running state
#
@property
def lock(self):
return self._get_var_data("lock", None)
# return self._share_data.get("lock", None)
@property
def busy_status(self):
"""アーム動作中のならば True"""
return self._get_var_data("busy_status", None)
# return self._share_data.get("busy_status", None)
[docs]
def is_running(self):
return self.busy_status
[docs]
def state(self):
return self._get_var_data("state", None)
@property
def servo_on(self):
return self._get_var_data("servo_on", None)
#
#
#
@property
def rc8_type(self):
return self._get_var_data("rc8_type", None)
@property
def rc8_type_name(self):
return self._get_var_data("rc8_type_name", None)
@property
def asp_mode(self):
return self._get_var_data("asp_mode", None)
@property
def manual_move_mode(self):
return self._get_var_data("manual_move_mode", None)
#
# Error
#
@property
def error_code(self):
return self._get_var_data("error_code", None)
# return self._share_data.get("error_code", None)
@property
def error_description(self):
"""最後のエラーの内容表示"""
return self._get_var_data("error_description", None)
# return self._share_data.get("error_description", None)
[docs]
def get_error(self):
"""
現在 RC8 に蓄積しているエラー情報をすべて取得して返す。
戻り値:
[
[error_code, message, subcode, file_line, progname, line, file_id],
...
]
"""
self._logger.debug("== get_error() ==")
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
info_list = []
try:
count = int(self.get_error_count())
except Exception as e:
self.print_error_info(e)
return info_list
for i in range(count):
try:
info_list.append(self.get_error_info(i))
except Exception as e:
self._logger.error(f"get_error(): GetCurErrorInfo({i}) failed: {e}")
return info_list
[docs]
def get_error_code(self):
return self.error_code
[docs]
def get_error_count(self):
"""
b-cap制御からの逆輸入
現在 RC8 に蓄積しているエラー件数を返す。
"""
self._logger.debug("== get_error_count() ==")
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
return self._caoCtrl.Execute("GetCurErrorCount")
except Exception as e:
return self.print_error_info(e)
[docs]
def get_error_info(self, index: int):
"""
b-cap制御からの逆輸入
index 番目のエラー詳細を返す。
Args:
index: index 番目
Returns:
str: index 番目のエラー詳細
"""
self._logger.debug(f"== get_error_info({index}) ==")
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
return self._caoCtrl.Execute("GetCurErrorInfo", index)
except Exception as e:
return self.print_error_info(e)
[docs]
def get_error_description(
self,
error_code: int,
) -> Union[str, bool]:
"""
b-cap制御からの逆輸入
error_code から RC8 の説明文字列を取得する。
"""
self._logger.debug(f"== get_error_description({error_code}) ==")
if not self._caoCtrl:
self.print_error("**Error**: caoCtrl is None")
# return ""
return False
try:
error_code = int(error_code)
desc = self._caoCtrl.Execute("GetErrorDescription", error_code)
if error_code < 0:
error_code_hex = format(error_code & 0xFFFFFFFF, "x")
else:
error_code_hex = hex(error_code)
self._logger.error("Error Code : 0x" + str(error_code_hex))
return desc
except Exception as e:
self._logger.error(f"GetErrorDescription failed: {e}")
# return ""
return False
#
# info
#
@property
def direct_mode(self):
return self.get_direct_mode()
[docs]
def get_direct_mode(self):
"""
True:Move() など、False: PacScript
"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._logger.debug("== get_direct_mode()")
ret = self._caoRob.Execute("GetDirectMode")
return ret
except Exception as e:
return self.print_error_info(e)
# try:
# return self._caoRob.Execute("GetDirectMode")
# except Exception as e:
# return self.print_error_info(e)
[docs]
def get_motion_complete(self):
"""アームが停止しているのならば True (MotonComplete)"""
self._logger.debug("== get_motion_complete()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
return self._caoRob.Execute("MotionComplete")
except Exception as e:
return self.print_error_info(e)
[docs]
def get_all_var(self):
"""変数名の入手(@var)一覧"""
self._logger.debug("== get_all_var()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
vars_ = self._caoRob.GetVariableNames()
# print(json.dumps(vars, indent=4))
except Exception as e:
return self.print_error_info(e)
return vars_
[docs]
def get_current_error_info(self):
"""現在発生しているエラーの情報を返す(現在のエラー1件)"""
self._logger.debug("== get_current_error_info()")
if not self._caoCtrl:
return self.print_error("**Error**: caoCtrl is None")
try:
return self._caoCtrl.Execute("GetCurErrorInfo")
except Exception as e:
return self.print_error_info(e)
[docs]
def get_current_pose(
self,
) -> Union[int, bool]:
"""
現在の P座標の fig(pose) を取得する。
"""
cur = self.get_current_position()
if cur is False:
return False
# 通常 7要素:
# [x, y, z, rx, ry, rz, fig]
if not isinstance(cur, (list, tuple)):
return False
if len(cur) < 7:
return False
fig = cur[6]
if not isinstance(fig, (int, float)):
return False
return int(fig)
[docs]
def get_current_position(
self,
) -> Union[list, tuple, bool]:
"""現在位置 P型で取得, @Current_Position と同等"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
return self._caoRob.Execute("CurPos") # output Current Position
except Exception as e:
return self.print_error_info(e)
[docs]
def get_current_jnt(
self,
) -> Union[list, tuple, bool]:
"""@CURRENT_ANGLE と同等"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
return self._caoRob.Execute("CurJnt") # Angle
except Exception as e:
return self.print_error_info(e)
[docs]
def get_current_angle(self) -> Union[list, tuple, bool]:
return self.get_current_jnt()
#
# motor
#
[docs]
def turn_on_motor(self) -> bool:
"""
モーターON
Returns:
bool: 可否
"""
self._logger.debug("== turn_on_motor()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
# Motor On: 1
try:
self._caoRob.Execute("Motor", 1)
except Exception as e:
return self.print_error_info(e)
return True
[docs]
def turn_off_motor(self) -> bool:
"""
モーター off
Returns:
bool: 可否
"""
self._logger.debug("== turn_off_motor()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
# Motor Off: 0
try:
self._caoRob.Execute("Motor", 0)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def wait_for_complete(self) -> bool:
self._logger.debug("== wait_for_complete()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
MotionCompValue = self._caoRob.Execute("MotionComplete", [-1, 1])
while not MotionCompValue:
time.sleep(0.5)
MotionCompValue = self._caoRob.Execute("MotionComplete", [-1, 1])
# self._logger.debug(f"MotionComplete: {MotionCompValue}")
return True
except Exception as e:
return self.print_error_info(e)
#
# log:command
#
[docs]
def start_log(self):
"""StartLog"""
self._logger.info("== start_log()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("StartLog")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def stop_log(self):
"""StopLog"""
self._logger.info("== stop_log()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("StopLog")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def clear_log(self):
"""ClearLog (clear and start ctrl log)"""
self._logger.info("== clear_log()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("ClearLog")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def start_servo_log(self):
"""StartServoLog"""
self._logger.info("== start_servo_log()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("StartServoLog")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def stop_servo_log(self):
"""StopServoLog"""
self._logger.info("== stop_servo_log()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("StopServoLog")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def clear_servo_log(self):
"""ClearServoLog"""
self._logger.info("== clear_servo_log()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("ClearServoLog")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_log_count(self) -> int:
"""GetLogCount"""
self._logger.info("== get_log_count()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
return self._caoRob.Execute("GetLogCount")
except Exception as e:
return self.print_error_info(e)
[docs]
def get_log_record(self, index: int) -> int:
"""GetLogRecord"""
self._logger.info("== get_log_coord()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
return self._caoRob.Execute("GetLogRecord", index)
except Exception as e:
return self.print_error_info(e)
#
# Command
#
[docs]
def autocal(
self,
origin: bool = False,
) -> bool:
"""起動時のCALSETが有効になっていると動かない"""
self._logger.info(f"== autocal(origin={origin})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
pos: Union[list, tuple, bool] = self.get_current_position()
if not isinstance(pos, (list, tuple)):
return False
try:
self._caoRob.Execute("AutoCal")
self.move((0, 0, 90, 0, 90, 0), coord_type="J")
if origin:
return True
self.wait_for_complete()
self.take_arm()
self.set_extspeed(100)
return self.move(pos, coord_type="P")
except Exception as e:
return self.print_error_info(e)
[docs]
def get_motion_preparation(self):
"""
動作準備完了状態を取得します．
動作準備完了状態でない場合，AutoCal 以外でロボット動作できません．
動作準備完了状態は，セーフティのエラー発生，非常停止 ON，防護停止 ON，
セーフティパラメータの送信で，未完了状態になりま
Returns:
"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("GetMotionPreparationState")
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def run_script(self) -> bool:
self._logger.info("== run_script()")
if self._task is None:
return self.print_error("**Error**: task is None")
try:
# Task Start
# 1 = Run 1 cycle
# 2 = Run continuously
# 3 = 1 Step
# 5 = Retry
self._task.Start(1)
# Read Task State
# task_state_temp = self._task_state.Value
task_state_temp = self.get_task_status()
if task_state_temp is None:
return self.print_error("**Error**: task_state is None")
# Loop Until a task is completed
while task_state_temp != 2:
# task_state_temp = self._task_state.Value
task_state_temp = self.get_task_status()
if task_state_temp is None:
return self.print_error("**Error**: task_state is None")
self._logger.info("TaskState = " + str(task_state_temp))
# End while
self._logger.info("Task Finish")
return True
except Exception as e:
return self.print_error_info(e)
#
# low_level (ORiN2-speed)
#
[docs]
def set_speed(
self,
speed: int,
) -> bool:
"""軸毎のセットはなく、0:全軸(JSPEED)、-1:合成速度(SPEED)のみ"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Speed(-1, speed)
self._speed = speed
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_speed(self) -> Union[int, bool]:
# return self._speed
# return self._get_var_data("speed", None)
return self._get_var_data("speed", False)
[docs]
def set_accel(
self,
accel: float,
) -> bool:
"""軸毎のセットはなく、0:全軸(JSPEED)、-1:合成速度(SPEED)のみ"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Accelerate(-1, accel, -1)
self._accel = accel
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_accel(self) -> Union[float, bool]:
# return self._accel
# return self._get_var_data("accel", None)
return self._get_var_data("accel", False)
[docs]
def set_decel(
self,
decel: float,
) -> bool:
"""軸毎のセットはなく、0:全軸(JSPEED)、-1:合成速度(SPEED)のみ"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Accelerate(-1, -1, decel)
self._decel = decel
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_decel(self) -> Union[float, bool]:
# return self._decel
# return self._get_var_data("decel", None)
return self._get_var_data("decel", False)
[docs]
def set_jspeed(
self,
speed: int,
) -> bool:
"""軸毎のセットはなく、0:全軸(JSPEED)、-1:合成速度(SPEED)のみ"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Speed(0, speed)
self._jspeed = speed
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_jspeed(self) -> Union[int, bool]:
try:
# return self._jspeed
# return self._get_var_data("jspeed", None)
return self._get_var_data("jspeed", False)
except Exception as e:
return self.print_error_info(e)
#
# low_level (TP8-speed)
#
[docs]
def set_extspeed(
self,
speed: int,
) -> bool:
"""軸毎のセットはなく、0:全軸(JSPEED)、-1:合成速度(SPEED)のみ"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("ExtSpeed", (speed, -1, -1))
self._extspeed = speed
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_extspeed(self) -> Union[int, bool]:
# return self._extspeed
# return self._get_var_data("extspeed", None)
return self._get_var_data("extspeed", False)
[docs]
def set_extaccel(
self,
extaccel: float,
) -> bool:
""""""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("ExtSpeed", (-1, extaccel, -1))
self._extaccel = extaccel
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_extaccel(self) -> Union[float, bool]:
# return self._extaccel
# return self._get_var_data("extaccel", None)
return self._get_var_data("extaccel", False)
[docs]
def set_extdecel(
self,
extdecel: float,
) -> bool:
""""""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
self._caoRob.Execute("ExtSpeed", (-1, -1, extdecel))
self._extdecel = extdecel
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def get_extdecel(self) -> Union[float, bool]:
# return self._extdecel
# return self._get_var_data("extdecel", None)
return self._get_var_data("extdecel", False)
#
# Motor
#
#
# Motor:Hand
#
[docs]
def init_hand(self) -> bool:
"""hand の回転だけ初期位置へ"""
self._logger.info("== init_hand()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
pos_j = self.get_current_jnt()
if not isinstance(pos_j, (list, tuple)):
return False
pos_j_new_list = []
for i, x in enumerate(pos_j):
if i == 5:
pos_j_new_list.append(0)
else:
pos_j_new_list.append(x)
pos_j_new = tuple(pos_j_new_list)
pos_new = self.j2p(pos_j_new)
if not isinstance(pos_new, list):
return False
self._logger.debug(f"{json.dumps(pos_new, indent=4)}")
try:
self.move(pos_new)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def open_hand(
self,
speed: Optional[int] = None,
) -> bool:
return self.hand_move_A(30, speed=speed)
[docs]
def hand_open(
self,
speed: Optional[int] = None,
) -> bool:
return self.hand_move_A(30, speed=speed)
[docs]
def hand(
self,
pos: Optional[int] = None,
*,
position: Optional[int] = None, # mm
force: Optional[int] = None,
direction: Union[bool, int] = True,
relative: bool = False,
speed: Optional[int] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
) -> bool:
""""""
if force is not None:
return self.hand_move_H(
force=force,
direction=direction,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
if position is not None:
hand_position = position
elif pos is not None:
hand_position = pos
else:
self._logger.error("hand(): position or pos is required")
return False
if relative:
return self.hand_move_R(
position=hand_position,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
return self.hand_move_A(
position=hand_position,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
[docs]
def hand_move_H(
self,
force: int,
direction: Union[bool, int] = True,
*,
speed: Optional[int] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
) -> bool:
""""""
self._logger.info(f"== hand_move_H(force={force}N, direction={direction})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
hand_bstrOpt = self._resolve_hand_bstrOpt(
bstrOpt=bstrOpt,
is_async=is_async,
)
if hand_bstrOpt is False:
return False
params: List[Union[str, int, float]] = [force, direction]
if isinstance(hand_bstrOpt, str):
params.append(hand_bstrOpt)
try:
self._caoCtrl.Execute("HandMoveH", params)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def hand_move_AH(
self,
pos: Optional[Union[int, float]] = None,
*,
position: Optional[Union[int, float]] = None,
force: Union[int, float],
speed: Optional[int] = None,
is_async: Optional[bool] = False,
) -> bool:
""""""
if position is None:
position = pos
self._logger.info(f"== hand_move_AH(position={position}mm, force={force}N)")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
if position is None:
return self.print_error("hand_move_AH(): position or pos is required")
if speed is None:
speed = self._speed
if is_async:
hand_bstrOpt = "NEXT"
else:
hand_bstrOpt = "DetectON"
params: List[Union[str, int, float]] = [
position,
speed,
force,
hand_bstrOpt,
]
try:
self._caoCtrl.Execute("HandMoveAH", params)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def hand_move_A(
self,
pos: Optional[int] = None,
*,
position: Optional[int] = None, # mm
speed: Optional[int] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
) -> bool:
""""""
if position is not None:
hand_position = position
elif pos is not None:
hand_position = pos
else:
self._logger.error("hand_move_A(): position or pos is required")
return False
self._logger.info(f"== hand_move_A(pos={hand_position}, speed={speed})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
hand_bstrOpt = self._resolve_hand_bstrOpt(
bstrOpt=bstrOpt,
is_async=is_async,
)
if hand_bstrOpt is False:
return False
if speed is None:
speed = self._speed
params: List[Union[str, int, float]] = [hand_position, speed]
if isinstance(hand_bstrOpt, str):
params.append(hand_bstrOpt)
try:
self._caoCtrl.Execute("HandMoveA", params)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def hand_move_R(
self,
pos: Optional[int] = None,
*,
position: Optional[int] = None, # mm
speed: Optional[int] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
) -> bool:
""""""
if position is not None:
hand_position = position
elif pos is not None:
hand_position = pos
else:
self._logger.error("hand_move_R(): position or pos is required")
return False
self._logger.info(f"== hand_move_R(pos={hand_position}, speed={speed})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
hand_bstrOpt = self._resolve_hand_bstrOpt(
bstrOpt=bstrOpt,
is_async=is_async,
)
if hand_bstrOpt is False:
return False
if speed is None:
speed = self._speed
params: List[Union[str, int, float]] = [hand_position, speed]
if isinstance(hand_bstrOpt, str):
params.append(hand_bstrOpt)
try:
self._caoCtrl.Execute("HandMoveR", params)
return True
except Exception as e:
return self.print_error_info(e)
# def hand_chuck(self, val: bool = True):
# self._logger.debug("== hand_chuck()")
# if not self._caoRob:
# return self.print_error("**Error**: caoRob is None")
# try:
# if val:
# return self._caoRob.Execute("HandChuck")
# else:
# return self._caoRob.Execute("HandUnChuck")
# except Exception as e:
# return self.print_error_info(e)
#
# Tool
#
[docs]
def p2j(
self,
p_type_pos: Union[list, tuple],
) -> Union[list, tuple, bool]:
"""P型をJ型に変換する"""
self._logger.info("== p2j()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
target = "P(" + ",".join(str(v) for v in p_type_pos) + ")"
self._logger.info(f"target = {target}")
try:
return self._caoRob.Execute("P2J", target)
except Exception as e:
return self.print_error_info(e)
[docs]
def j2p(
self,
j_type_pos: Union[list, tuple],
) -> Union[list, tuple, bool]:
"""j型をp型に変換する"""
self._logger.info("== j2p()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
target = "J(" + ",".join(str(v) for v in j_type_pos) + ")"
self._logger.info(f"target = {target}")
try:
return self._caoRob.Execute("J2P", target)
except Exception as e:
return self.print_error_info(e)
#
# Motor: Move系 /util
#
[docs]
@staticmethod
def remove_extension(
filename: str,
) -> str:
"""拡張子を除く"""
return os.path.splitext(filename)[0]
[docs]
def _move(
self,
lComp,
vntPose,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""
LOW level API.
ポーズ: P型, J型(6軸指定), T型。
@から始まるのはパス開始変位:
"@0", "@P", "@E", "@<数値>", "@A<数値>" のいずれか。
@P Pose（登録済みポーズ名）
@E External pose（外部ポーズ）
@L ローカルポーズ
@T Tool座標系
@W Work座標系
これが "@P P2" とあると、P2 がポーズなのか何なのかわからないので必要。
"<変数型><番号>" または "[<変数型>](<要素1>,<要素2>,…)" の形式。
lComp1,2: PTP/Line
[<@パス開始変位>]<ポーズ> [<付加軸オプション>]
例: "P1", "@P T100", "@E J520"
lComp3: Circle 円弧補完
<ポーズ1>, [<@パス開始変位>] <ポーズ2> [<付加軸オプション>]
※ ポーズ1,2は必ず同じ変数型でなくてはなりません。
例: "P1, @E P2", "T100, @P T101"
lComp4: Spline
[<@パス開始変位>]<自由曲線軌道番号> [<付加軸オプション>]
例: "1", "@P 20", "@E 5"
Args:
lComp (int): 保管方法。
vntPose: ポーズ列。
bstrOpt (str | int): 動作オプション。
Returns:
None: 戻り値はありません。
"""
self._logger.info("== move()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._to_bstrOpt_string(bstrOpt)
args = [lComp, vntPose]
if bstrOpt is not None:
args.append(bstrOpt)
try:
self._caoRob.Move(*args)
# self._caoRob.Move(lComp, vntPose, bstrOpt)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
@staticmethod
def _normalize_offset(
offset: Union[list, tuple],
) -> list:
"""
(0,0,10) のときに (0,0,10,0,0,0)
として、rx,ry,yzを暗黙にしないで加える
(0,0,0,10) のときに (0,0,0,10,0,0)
となる
"""
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]:
"""6軸表示
str ならそのまま、list ならば offset 値の書式(P書式)にする
"""
if isinstance(offset, str):
# そのまま
return offset
if isinstance(offset, (list, tuple)):
# P-type
vals = list(offset)[:6]
vals = self._normalize_offset(vals)
for x in vals:
if not isinstance(x, (int, float)):
return False
return f"P({','.join(str(x) for x in vals)})"
# return f"P({','.join(str(int(x)) for x in val)})"
return False
[docs]
@staticmethod
def _strip_path_prefix(
base: str,
) -> str:
"""@ステートメントを消す: @P P1 → P1, @0 P14 → P14 などに変換"""
if isinstance(base, str):
b_clean = re.sub(r"^@\w+\s*", "", base.strip())
return b_clean
return base
[docs]
def _make_pacscript_offset_target(
self,
target: str,
path_blend: Optional[str] = None,
) -> Union[list, bool]:
"""
target が str のとき
PacScript風 + オフセット構文を処理する。
PacScript風のオフセット書式が move 命令に一緒に来た時に、VntPose を作る
例:
"P10+(10,0,0,0,0,0)"
"@P P10+(10,0,0,0,0,0)"
文字列構文では内部 P 番号に保持された fig 情報を使う経路を維持するため、
pose(fig) 解釈を追加しない。
"""
base_str, offset_str = map(str.strip, target.split("+", 1))
# path_blend (@P 等) を分離
path_prefix_match = re.match(r"^(@\w+)\s+", base_str)
if path_prefix_match:
path_prefix = path_prefix_match.group(1).strip()
else:
if path_blend is not None:
path_prefix = path_blend
else:
path_prefix = self.path_blend
# dev は path_prefix を含まない基準座標を必要とする
stripped_base = self._strip_path_prefix(base_str)
s = offset_str.strip()
if s.upper().startswith("P("):
offset_pose = s
else:
if s.startswith("("):
if s.endswith(")"):
offset_pose = f"P{s}"
else:
offset_pose = f"P({s})"
else:
offset_pose = f"P({s})"
self._logger.debug(f'base_str: "{base_str}"')
self._logger.debug(f'base_stripped: "{stripped_base}"')
self._logger.debug(f'offset_pose: "{offset_pose}"')
offset_target = self.dev(stripped_base, offset_pose)
if offset_target is False:
return False
offset_target = list(offset_target[:6])
if path_prefix is not None:
return [offset_target, "P", path_prefix]
return [offset_target, "P"]
[docs]
def _make_move_offset_target(
self,
target: Union[str, list, tuple],
offset: Union[str, list, tuple],
coord_type: Optional[str] = None,
path_blend: Optional[str] = None,
pose: Optional[int] = None,
) -> Union[list, bool]:
"""
move(..., offset=...) 用 target を作る。
dev は常に P座標 6軸のみを扱う。
pose(fig) は dev の外側で扱う。
規則:
move("P10", offset=...)
-> dev("P10", offset)
move([6軸], offset=...)
-> 6軸 dev
move([6軸], pose=3, offset=...)
-> dev 後に 7要素目=3 を追加
move([7軸], offset=...)
-> 7要素目 fig を保持
move([7軸], pose=5, offset=...)
-> 7要素目を 5 に置換
"""
# -------------------------
# offset move は P座標専用
# -------------------------
if coord_type is not None:
if coord_type != "P":
self._logger.error("offset move は P 座標のみ対応")
return False
# -------------------------
# 実際に使う path_blend を決定
# -------------------------
if path_blend is not None:
use_path_blend = path_blend
else:
use_path_blend = self.path_blend
# -------------------------
# coord_type は P 固定
# -------------------------
use_coord_type = "P"
# ==========================================================
# str target
# ==========================================================
if isinstance(target, str):
stripped_target = target.strip()
# -------------------------
# Dev 実行
# -------------------------
dev_result = self.dev(
stripped_target,
offset,
)
if dev_result is False:
return False
# pose 情報を無視する
new_target = list(dev_result[:6])
# -------------------------
# path_blend 追加
# -------------------------
if use_path_blend is not None:
return [
new_target,
use_coord_type,
use_path_blend,
]
return [new_target, use_coord_type]
# ==========================================================
# tuple を list に変換
# ==========================================================
if isinstance(target, tuple):
target = list(target)
# ==========================================================
# list target
# ==========================================================
if isinstance(target, list):
if len(target) != 6 and len(target) != 7:
self._logger.error(f"Invalid move target list format: {target}")
return False
for x in target[:6]:
if not isinstance(x, (int, float)):
self._logger.error(f"Invalid move target list format: {target}")
return False
# -------------------------
# dev は 6軸のみ
# -------------------------
base6 = list(target[:6])
# fig の決定
if pose is not None:
fig = pose
else:
if len(target) == 7:
fig = target[6]
else:
fig = None
# -------------------------
# Dev 実行
# -------------------------
dev_result = self.dev(
base6,
offset,
)
if dev_result is False:
return False
new_target = list(dev_result[:6])
# -------------------------
# fig を戻す
# -------------------------
if fig is not None:
new_target.append(fig)
# -------------------------
# path_blend 追加
# -------------------------
if use_path_blend is not None:
return [
new_target,
use_coord_type,
use_path_blend,
]
return [
new_target,
use_coord_type,
]
self._logger.error(f"Unsupported move target: {target}")
return False
[docs]
def _make_move_target(
self,
target: Union[str, list, tuple],
coord_type: Optional[str] = None,
path_blend: Optional[str] = None,
pose: Optional[int] = None,
) -> Union[str, list, bool]:
"""target: str, list, tuple
move 用の target を ORiN2 Move API に渡す形式へ正規化する
ここでは offset 演算は扱わない。
offset 付き move は _move_core() 側で dev を通してから、
その結果を move target に組み立てる。
pose(fig) を解釈するのは list / tuple target のみである。
str target は既存の文字列構文として扱い、
Args:
target:
coord_type:
path_blend: オプションのパスブレンド値。明示指定があればこれを最優先で使う。
pose:
Returns:
"""
# tuple を list に変換
if isinstance(target, tuple):
target = list(target)
# 実際に使う path_blend を決定
if path_blend is not None:
use_path_blend = path_blend
else:
use_path_blend = self.path_blend
# 実際に使う coord_type を決定
if coord_type is not None:
use_coord_type = coord_type
else:
if self.coord_type is not None:
use_coord_type = self.coord_type
else:
use_coord_type = "P"
# str のとき
if isinstance(target, str):
stripped_target = target.strip()
# PacScript 風 + オフセット構文を処理
if "+" in stripped_target:
# + の中にも path_blend 指定がありうる場合は更に拡張可
return self._make_pacscript_offset_target(
stripped_target,
path_blend=use_path_blend,
)
# @ ステートメント(path_blend) があるときはすべてを受け入れる
if stripped_target.startswith("@"):
motion_target = stripped_target
# "P*" のときに path_blend を登録していたら @ステートメント(path_blend) を入れる
elif (
stripped_target.upper().startswith("P")
and stripped_target[1:].isdigit()
):
if use_path_blend is not None:
motion_target = f"{use_path_blend} {stripped_target}"
else:
motion_target = stripped_target
# motion_target = f"{self.path_blend} {stripped_target}"
else:
# 一番誤解なく書いた string 全て
motion_target = stripped_target
return motion_target
if isinstance(target, list):
# すでに coord_type と path_blend 入り vntPose 形式ならそのまま返す
if len(target) == 3:
if isinstance(target[0], list):
return target
validated_target = list(target)
if use_coord_type == "P":
# P 座標だけ 6軸または 7軸を許可する
if len(validated_target) != 6:
if len(validated_target) != 7:
self._logger.error(f"Invalid move target list format: {target}")
return False
# 先頭 6 要素が P 座標である
for x in validated_target[:6]:
if not isinstance(x, (int, float)):
self._logger.error(f"Invalid move target list format: {target}")
return False
# pose(fig) の決定は P 座標だけで行う
#
# move([6軸]) -> 6軸 P 座標
# move([6軸], pose=x) -> 7要素目に pose を追加
# move([7軸]) -> 7要素目をそのまま使う
# move([7軸], pose=x) -> 7要素目を pose で上書き
if pose is not None:
# move([6軸], pose=xxx)
if len(validated_target) == 6:
validated_target.append(pose)
# move([7軸], pose=xxx)
else:
validated_target[6] = pose
else:
# P 以外はロボット側の座標種別ごとの要素数をそのまま使う
# pose(fig) は P 座標専用なので、P 以外では解釈しない
for x in validated_target:
if not isinstance(x, (int, float)):
self._logger.error(f"Invalid move target list format: {target}")
return False
motion_target = [validated_target, use_coord_type]
if use_path_blend is not None:
motion_target.append(use_path_blend)
return motion_target
self._logger.error(f"Unsupported move target: {target}")
return False
[docs]
def dev(
self,
base: Union[str, list, tuple],
offset: Union[str, list, tuple],
) -> Union[list, tuple, bool]:
"""
基準位置にたいするオフセットで、座標を作る
P型が対象
そして、P型の座標だが、+1要素を返すので、move系とかに食わせるときは注意
# NG
# ret = self._caoRob.Execute("Dev", ["@P P1222", "P(-50,0,20)"])
# ret = self._caoRob.Execute("Dev", ("P1", (1, 1, 1)))
# OK (省略時はお尻が全部ゼロ)
# ret = self._caoRob.Execute("Dev", ("P1", "P(0, 0, 0)"))
# ret = self._caoRob.Execute("Dev", ("P10", "P(100, 200, 300, 180, 0, 180)"))
# ret = self._caoRob.Execute("Dev", ("P10", "P(100, 200, 300, 180, 0, 180)"))
# ret = self._caoRob.Execute(
# "Dev", ("P(100, 200, 300, 180, 0, 180)", "P(100, 200, 300, 180, 0, 180)")
# )
cobotta.dev("P1", [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.0])
cobotta.dev("P1", "P(0,0,0)")
Args:
base (str | list | tulpe)
offset (str | list | tuple)
Returns:
"""
self._logger.info(f"== dev(base={base}, offset={offset})")
# 計算座標
if isinstance(base, (list, tuple)):
base_values = list(base)[:6]
for x in base_values:
if not isinstance(x, (int, float)):
self._logger.error("base は str または 6軸 list/tuple")
return False
base_str = "P(" + ",".join(str(x) for x in base_values) + ")"
elif isinstance(base, str):
base_str = self._strip_path_prefix(base)
else:
return self.print_error("base は str または 6軸 list/tuple")
offset_str = self._to_p_string(offset)
if not isinstance(offset_str, str):
self._logger.error("offset は str または list/tuple で")
return False
self._logger.info(f" base={base_str}")
self._logger.info(f" offset={offset_str}")
try:
ret = self._caoRob.Execute(
"Dev",
# [stripped_base, offset_str],
[base_str, offset_str],
)
self._logger.info(f" new={ret}")
# P型が対称なので、6パラメーター
return ret
except Exception as e:
return self.print_error_info(e)
# def move_offset(self, base: Union[str, list], offset: str | list):
# pose = self.dev(base, offset)
# return self._caoRob.Move(1, [pose, "P", "@P"], "Next")
[docs]
def _move_core(
self,
lComp: int,
target: Union[str, list, tuple],
coord_type: Optional[str],
path_blend: Optional[str],
offset: Union[str, tuple, list, None],
pose: Optional[int],
bstrOpt: Union[str, int, None],
) -> bool:
"""
move 実行の分岐のみ担当。
target の解釈は make_* 系へ委譲する。
"""
self._logger.info("== _move_core()")
if coord_type is not None:
use_coord_type = coord_type
else:
if self.coord_type is not None:
use_coord_type = self.coord_type
else:
use_coord_type = "P"
if path_blend is not None:
use_path_blend = path_blend
else:
use_path_blend = self.path_blend
# str target
if isinstance(target, str):
stripped_target = target.strip()
if "+" in stripped_target:
vntPose = self._make_pacscript_offset_target(
stripped_target,
path_blend=use_path_blend,
)
else:
if offset is None:
vntPose = self._make_move_target(
target=stripped_target,
coord_type=use_coord_type,
path_blend=use_path_blend,
pose=None,
)
else:
vntPose = self._make_move_offset_target(
target=stripped_target,
offset=offset,
coord_type=use_coord_type,
path_blend=use_path_blend,
pose=None,
)
# list target
else:
if offset is None:
vntPose = self._make_move_target(
target=target,
coord_type=use_coord_type,
path_blend=use_path_blend,
pose=pose,
)
else:
vntPose = self._make_move_offset_target(
target=target,
offset=offset,
coord_type=use_coord_type,
path_blend=use_path_blend,
pose=pose,
)
if vntPose is False:
return False
self._logger.info(f" lComp = {lComp}")
self._logger.info(f" vntPose = {vntPose}")
self._logger.info(f" bstrOpt = {bstrOpt}")
args = [lComp, vntPose]
if bstrOpt is not None:
args.append(bstrOpt)
try:
self._caoRob.Move(*args)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def move(
self,
target: Union[str, list, tuple],
*,
motion_mode: Union[int, str, MotionMode, None] = None,
coord_type: Optional[str] = None,
path_blend: Optional[str] = None,
offset: Union[tuple, list, None] = None,
pose: Optional[int] = None,
speed: Optional[int] = None,
accel: Optional[float] = None,
decel: Optional[float] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""
Next は途中で止めるためというよりは、move() 事態を非同期にすることで
move()
move()
がならんだときに、最初の move が終わってないときに次の move を読み込むことで
モーションをつなげる意図がある。つまり、そのための非同期実行のオプション
デフォルトで True としておく
つまり、FINAL にするためには、MotionComplete が必要
というのをデフォルト動作にする
--> Next を一般的ではない気がするので、async かな。
target:
- list(6/7) → vntPose形式 [Pos, coord_type, path_blend] へ自動変換
※7要素なら先頭6要素だけ使う
- list(3) → 既にvntPose形式
- str → "@P P2"等はそのまま
"""
self._logger.info(f"== move({target})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
# LINE/PTP/...
mm = self._resolve_motion_mode(motion_mode)
lComp = mm.to_lComp()
bstrOpt = self._resolve_bstrOpt(
bstrOpt=bstrOpt,
speed=speed,
accel=accel,
decel=decel,
is_async=is_async,
)
if bstrOpt is False:
self._logger.error("Invalid bstrOpt type")
return False
try:
# lComp = {
# MotionMode.PTP: 1,
# MotionMode.LINE: 2,
# MotionMode.CIRCLE: 3,
# MotionMode.SPLINE: 4,
# }.get(self.motion_mode, 1)
ret = self._move_core(
lComp,
target=target,
coord_type=coord_type,
path_blend=path_blend,
offset=offset,
pose=pose,
bstrOpt=bstrOpt,
)
if not ret:
return False
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def move_P(
self,
target: Union[str, list, tuple],
*,
coord_type: Optional[str] = None,
path_blend: Optional[str] = None,
offset: Union[tuple, list, None] = None,
pose: Optional[int] = None,
speed: Optional[int] = None,
accel: Optional[float] = None,
decel: Optional[float] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""PTP"""
self._logger.info(f"== move_P({target})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._resolve_bstrOpt(
bstrOpt=bstrOpt,
speed=speed,
accel=accel,
decel=decel,
is_async=is_async,
)
if bstrOpt is False:
self._logger.error("Invalid bstrOpt type")
return False
try:
ret = self._move_core(
1,
target=target,
coord_type=coord_type,
path_blend=path_blend,
offset=offset,
pose=pose,
bstrOpt=bstrOpt,
)
if not ret:
return False
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def move_L(
self,
target: Union[str, list, tuple],
*,
coord_type: Optional[str] = None,
path_blend: Optional[str] = None,
offset: Union[tuple, list, None] = None,
pose: Optional[int] = None,
speed: Optional[int] = None,
accel: Optional[float] = None,
decel: Optional[float] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""Line 直線補完"""
self._logger.info(f"== move_L({target})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._resolve_bstrOpt(
bstrOpt=bstrOpt,
speed=speed,
accel=accel,
decel=decel,
is_async=is_async,
)
if bstrOpt is False:
self._logger.error("Invalid bstrOpt type")
return False
try:
ret = self._move_core(
2,
target=target,
coord_type=coord_type,
path_blend=path_blend,
offset=offset,
pose=pose,
bstrOpt=bstrOpt,
)
if not ret:
return False
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def move_C(
self,
target: Union[str, list, tuple],
*,
coord_type: Optional[str] = None,
path_blend: Optional[str] = None,
offset: Union[tuple, list, None] = None,
pose: Optional[int] = None,
speed: Optional[int] = None,
accel: Optional[float] = None,
decel: Optional[float] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""Circle 円弧補完"""
self._logger.info(f"== move_C({target})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._resolve_bstrOpt(
bstrOpt=bstrOpt,
speed=speed,
accel=accel,
decel=decel,
is_async=is_async,
)
if bstrOpt is False:
self._logger.error("Invalid bstrOpt type")
return False
try:
ret = self._move_core(
3,
target=target,
coord_type=coord_type,
path_blend=path_blend,
offset=offset,
pose=pose,
bstrOpt=bstrOpt,
)
if not ret:
return False
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def move_S(
self,
target: Union[str, list, tuple],
*,
coord_type: Optional[str] = None,
path_blend: Optional[str] = None,
offset: Union[tuple, list, None] = None,
pose: Optional[int] = None,
speed: Optional[int] = None,
accel: Optional[float] = None,
decel: Optional[float] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""Spline 自由曲線補完"""
self._logger.info(f"== move_S({target})")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._resolve_bstrOpt(
bstrOpt=bstrOpt,
speed=speed,
accel=accel,
decel=decel,
is_async=is_async,
)
if bstrOpt is False:
self._logger.error("Invalid bstrOpt type")
return False
try:
ret = self._move_core(
4,
target=target,
coord_type=coord_type,
path_blend=path_blend,
offset=offset,
pose=pose,
bstrOpt=bstrOpt,
)
if not ret:
return False
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def approach(
self,
lComp,
vntPoseBase,
vntPoseLen,
bstrOpt,
):
self._logger.debug("== approach()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
orig_bstrOpt = bstrOpt
bstrOpt = self._to_bstrOpt_string(bstrOpt)
if orig_bstrOpt is not None and bstrOpt is None:
self._logger.error("Invalid bstrOpt type")
return False
params = [lComp, vntPoseBase, vntPoseLen]
if bstrOpt is not None:
params.append(bstrOpt)
try:
self._caoRob.Execute("Approach", params)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def _drive_core(
self,
target: Union[str, list, tuple],
*,
path_blend: Optional[str] = None,
speed: Optional[int] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
) -> Union[list, str, bool]:
"""
Drive は存在せずに DriveEX, DriveAEX を使う
(Ex 拡張)
６軸指定は、J で指定
target: list of (target, pos) or string "(1,100),(3,200)"
v1 = [(1,10), -1, "@P"]
v2 = [(2,10), -1]
vntPose = [v1, v2]
Execute("DriveAEX", [vntPose, "Speed=10, Next])
Execute("DriveAEX", ["@0 (1,10), (2,10), "Speed=10, Next])
PacScript:
DriveA(1,60)
DriveA(2,20),(3,135),(4,-10),(5, 25)
"""
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._resolve_bstrOpt(
bstrOpt=bstrOpt,
speed=speed,
is_async=is_async,
)
if bstrOpt is False:
return False
if path_blend is not None:
use_path_blend = path_blend
else:
use_path_blend = self.path_blend
# PacScript書式 str ならそのまま
if isinstance(target, str):
args = target.strip()
# tuple or list
elif isinstance(target, (list, tuple)):
# 「(n, val)」単一ペア対応
if len(target) == 2 and all(isinstance(x, (int, float)) for x in target):
args = f"({target[0]},{target[1]})"
# 複数ペア対応 list, tuple は (n1,val1), (n2,val2)" の連結文字列 (*) 単一ペアだとこれが正しく動かない)
elif all(isinstance(t, (list, tuple)) and len(t) == 2 for t in target):
args = ", ".join(f"({t[0]},{t[1]})" for t in target)
else:
return self.print_error(
"**Error**: targets内は(軸,値) tuple or list のみ許可"
)
else:
return self.print_error(
"**Error**: targets must be str, tuple, or list of tuple"
)
if use_path_blend is not None:
args = f"{use_path_blend} {args}"
# 送信
self._logger.info(f" targets={target}")
self._logger.info(f" args={args}")
self._logger.info(f" bstrOpt={bstrOpt}")
params = [args]
if bstrOpt is not None:
params.append(bstrOpt)
else:
params = args
# params = "(0,0),(6,0)" などが入る
return params
[docs]
def drive(
self,
target: Union[str, list, tuple, None],
*,
relative: bool = False,
path_blend: Optional[str] = None,
speed: Optional[int] = None,
is_async: Optional[bool] = False,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""
単一APIで DriveEX（相対） / DriveAEX（絶対）を切り替えて実行する統合メソッド。
Args:
target: 文字列（PacScript書式）または `(軸, 値)` のペア、もしくはそのリスト / タプル。
例:
"(1,10),(2,20)"
[(1,10), (2,20)]
(1, 10)
relative (bool): `True` のとき相対移動（DriveEX）、`False` のとき絶対移動（DriveAEX）。
path_blend (str | None): 先頭に付与する経路ブレンド指定（例: `"@0"`, `"Next"` など）。
`None` の場合は `self.path_blend` を踏襲。
speed (int | None): 速度指定（`_resolve_bstrOpt` に委譲して `bstrOpt` を構成）。
is_async (bool | None): 非同期指定（`_resolve_bstrOpt` に委譲）。
bstrOpt (str | int | None): 追加オプション指定。`speed` / `is_async` とマージされる。
Returns:
bool: 実行成功時 `True`、失敗時 `False`。
"""
self._logger.info("== drive()")
if relative:
cmd = "DriveEX"
mode = "relative"
else:
cmd = "DriveAEX"
mode = "absolute"
self._logger.info(f" mode={mode} -> {cmd}")
# 共通のパラメータ整形（エラー時は _drive_core が False 等を返す想定）
params = self._drive_core(
target,
path_blend=path_blend,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
# _drive_core 側でエラーを print_error 済みのときは中断
if params is False or params is None:
return False
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
try:
# 相対/絶対を切り替えて実行
self._caoRob.Execute(cmd, params)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def drive_EX(
self,
target: Union[str, list, tuple],
*,
path_blend: Optional[str] = None,
speed: Optional[int] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""
相対移動
POSEDATA 6軸
"""
self._logger.info("== driveEX()")
params = self._drive_core(
target,
path_blend=path_blend,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
if params is False or params is None:
return False
try:
# DriveEX <---> Drive (PacScript)
# 動作がおかしく感じるのは PacScript の仕様(PacScriptで動かすと正しく一致)
# 相対移動
self._caoRob.Execute("DriveEX", params)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def drive_AEX(
self,
target: Union[str, list, tuple, None],
*,
path_blend: Optional[str] = None,
speed: Optional[int] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""
絶対移動
POSEDATA 6軸
"""
self._logger.info("== driveAEX()")
params = self._drive_core(
target,
path_blend=path_blend,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
# _drive_core 側でエラーを print_error 済みのときは中断
if params is False or params is None:
return False
try:
# DriveAEX <---> Drive A (PacScript)
# 動作がおかしく感じるのは PacScript の仕様(PacScriptで動かすと正しく一致)
# 絶対移動
self._caoRob.Execute("DriveAEX", params)
return True
# return self._caoRob.Execute("DriveAEX", [args])
# return self._caoRob.Execute("DriveAEX", ["@0 (6,90)"])
except Exception as e:
return self.print_error_info(e)
[docs]
def drive_A(
self,
target: Union[str, list],
*,
path_blend: Optional[str] = None,
speed: Optional[int] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
return self.drive_AEX(
target,
path_blend=path_blend,
speed=speed,
is_async=is_async,
bstrOpt=bstrOpt,
)
[docs]
def rotate(
self,
plane: str,
angle: float,
center: list,
*,
path_blend: Optional[str] = None,
speed: Optional[int] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""
cobotta.rotate("XY", 25, [200, -20, 197])
Args:
plane (str): "XY", "XZ", "YZ"
angle (float): 回転角(deg)
center (list): 回転中心(V(xx,yy,zz)))
path_blend (str):
speed (int): 速度%
is_async:
bstrOpt:
Returns:
"""
self._logger.debug("== rotate()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
if speed is None:
speed = self._speed
# 実際に使う path_blend を決定
if path_blend is not None:
use_path_blend = path_blend
else:
use_path_blend = self.path_blend
param = []
bstrOpt = self._resolve_bstrOpt(
bstrOpt=bstrOpt,
speed=speed,
is_async=is_async,
)
if bstrOpt is False:
return False
try:
result = f'V({", ".join(map(str, center))})'
self._logger.info(f" plane: {plane}")
self._logger.info(f" angle: {angle}")
self._logger.info(f" center: {result}")
param.append(plane)
param.append(angle)
param.append(result)
if use_path_blend is not None:
param.append(use_path_blend)
if bstrOpt is not None:
param.append(bstrOpt)
self._logger.info(f" param: {param}")
self._caoRob.Rotate(*param)
return True
except Exception as e:
return self.print_error_info(e)
[docs]
def rotate_H(
self,
angle: Union[str, float, int],
*,
path_blend: Optional[str] = None,
speed: Optional[int] = None,
is_async: Optional[bool] = True,
bstrOpt: Union[str, int, None] = None,
) -> bool:
"""
相対角度
(*)
相対なので、今いる角度によっては動かないことがある
今のポジションから目標ポジションで、ツールポイントの速度が決まるのでそれがハード限界を超えることがある
アプローチベクトルを軸とした回転動作
ふつうはエンドエフェクタの中心面に垂直がアプローチベクトル
つまりJ座標で見れば J6 となる。のが普通
caoRob.Execute"RotateH", Array("@P 32.5" ,"S=50" )
"""
self._logger.debug("== rotate_H()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
if speed is None:
speed = self._speed
bstrOpt = self._resolve_bstrOpt(
bstrOpt=bstrOpt,
speed=speed,
is_async=is_async,
)
if bstrOpt is False:
return False
# 実際に使う path_blend を決定
if path_blend is not None:
use_path_blend = path_blend
else:
use_path_blend = self.path_blend
if not isinstance(angle, (float, int, str)):
return self.print_error("**Error**: targets must be float, int, or str")
# 文字列化（float/int は文字列に、str はそのまま）
angle_str = str(angle)
# angle_str が "@" で始まらない場合のみ path_blend を付与
if use_path_blend and not angle_str.strip().startswith("@"):
angle_str = f"{use_path_blend} {angle_str}"
self._logger.info(f" angle={angle_str}")
self._logger.info(f" bstrOpt={bstrOpt}")
params = [angle_str]
if bstrOpt is not None:
params.append(bstrOpt)
else:
params = angle_str
try:
# return self._caoRob.Execute("RotateH", [f"{target_str}", bstrOpt])
# return self._caoRob.Execute("RotateH", "@P 30.0")
self._caoRob.Execute("RotateH", params)
return True
except Exception as e:
return self.print_error_info(e)
#
# test
#
[docs]
def test(
self,
lComp: int = 1,
bstrOpt: Union[int, str, None] = None,
) -> bool:
self._logger.debug("== test()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._to_bstrOpt(bstrOpt)
# vntPose=""
# Move P, @P P[1] Format POSEDATA(VARIANT) C0 VT_BSTR
Pos = [10.0, 0.0, 90.0, 0.0, 90.0, 0.0]
vntPose = [Pos, "J", "@P"]
try:
self._caoRob.Move(lComp, vntPose, bstrOpt)
except Exception as e:
return self.print_error_info(e)
self._logger.debug(f"Complete Move {vntPose}")
return True
[docs]
def test1(
self,
lComp: int = 1,
bstrOpt: Union[str, int, None] = None,
) -> bool:
self._logger.debug("== test1()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._to_bstrOpt(bstrOpt)
# vntPose=""
# Move P, @P P[1] Format POSEDATA(VARIANT) C0 VT_BSTR
Pos = [10.0, 0.0, 90.0, 0.0, 90.0, 0.0]
vntPose = [Pos, "J", "@P"]
self._caoRob.Move(lComp, vntPose, bstrOpt)
self._logger.debug(f"Complete Move {vntPose}")
# Move P,@P P[2] Format POSE DATA(VARIANT) C0 VT_VARIANT|VT_ARRAY
Pos = [-10.0, 0.0, 90.0, 0.0, 90.0, 0.0]
vntPose = [Pos, "J", "@P"]
self._caoRob.Move(lComp, vntPose, bstrOpt)
self._logger.debug(f"Complete Move {vntPose}")
# Move P,@P P(200,0,245,180,0,180,5) Format POSEDATA(VARIANT) C0 VT_VARIANT|VT_ARRAY
Pos = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0]
vntPose = [Pos, "J", "@P"]
self._caoRob.Move(lComp, vntPose)
self._logger.debug(f"Complete Move {vntPose}")
# Not recommend using POSE DATA VT_R4|VT_ARRAY Because it can not use @P,@E,J(),T(),
return True
[docs]
def test2(self, bstrOpt: Union[int, str, None] = None) -> bool:
self._logger.debug("== test2()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
bstrOpt = self._to_bstrOpt_string(bstrOpt)
# Move P, @P P[1] Format POSEDATA(VARIANT) C0 VT_BSTR
# Next option:
# The next line (statement) can be executed without waiting for motion completion.
# Move時にNEXTオプションをつけることによって，CaoRobot::Haltメソッドでロボ
# ットの動作を停止させることができる
# self._caoRob.Move(1, "@P P1", bstrOpt)
# # self.wait_for_complete()
# self._logger.debug("Complete Move P1")
# Move P,@P P[2] Format POSE DATA(VARIANT) C0 VT_VARIANT|VT_ARRAY
# caoRob.Move(1,[2,"P","@P"],bstr_Opt)
self._caoRob.Move(1, "@P P2", bstrOpt)
self.wait_for_complete()
self._logger.debug("Complete Move P2")
# Move P,@P P(200,0,245,180,0,180,5) Format POS EDATA(VARIANT) C0 VT_VARIANT|VT_ARRAY
# Pos = [200.0,0.0,245.0,180.0,0.0,180.0,5]
# caoRob.Move(1,[Pos,"P","@P"],bstr_Opt)
self._caoRob.Move(1, "@P P3", bstrOpt)
self.wait_for_complete()
self._logger.debug("Complete Move P(***,***,,,,))")
# Not recommend using POSE DATA VT_R4|VT_ARRAY Because it can not use @P,@E,J(),T(),
return True
[docs]
def test3(self, bstrOpt: Union[str, int, None] = None) -> bool:
self._logger.debug("== test2()")
self._logger.info("create process()")
if not self._caoRob:
return self.print_error("**Error**: caoRob is None")
if bstrOpt is None:
bstrOpt = f"Next Speed={self._speed}"
# wait_time_ = 0.5
# from cobotta_test_process import CobottaTestProcess
# _proc = multiprocessing.Process(
# target=CobottaTestProcess.state_process,
# args=(self._stop_event, self._share_data, wait_time_),
# )
# self._logger.info("process.start()")
# _proc.start()
lComp = 1
Pos = [10.0, 0.0, 90.0, 0.0, 90.0, 0.0]
vntPose = [Pos, "J", "@P"]
self._caoRob.Move(lComp, vntPose, bstrOpt)
self._logger.info(f"Complete Move {vntPose}")
print("busy_state (ctrl): ", self.busy_status)
Pos = [-10.0, 0.0, 90.0, 0.0, 90.0, 0.0]
vntPose = [Pos, "J", "@P"]
self._caoRob.Move(lComp, vntPose, bstrOpt)
self._logger.info(f"Complete Move {vntPose}")
print("busy_state (ctrl): ", self.busy_status)
Pos = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0]
vntPose = [Pos, "J", "@P"]
self._caoRob.Move(lComp, vntPose, bstrOpt)
self._logger.info(f"Complete Move {vntPose}")
print("busy_state (ctrl): ", self.busy_status)
# self._caoRob.Move(1, "@P P1", bstrOpt)
# self._logger.info("Complete Move P1")
# print("busy_state (ctrl): ", self.busy_status)
# print("P1")
self._caoRob.Move(1, "@P P2", bstrOpt)
self._logger.info("Complete Move P2")
print("busy_state (ctrl): ", self.busy_status)
print("P2")
self._caoRob.Move(1, "@P P3", bstrOpt)
self._logger.info("Complete Move P3")
print("busy_state (ctrl): ", self.busy_status)
print("P3")
self.wait_for_complete()
print("busy_state (ctrl): ", self.busy_status)
# self._stop_event.set()
# _proc.join()
return True
if __name__ == "__main__":
from x_logger.x_logger import XLogger
from x_logger.util import *
logger_ = XLogger(
log_level="debug",
logger_name=Config.CLIENT_LOGGER_NAME,
)
# 各スレッドでCAOEngine作成や接続処理が必要になる
# pythoncom.CoInitialize()
HERE = Path(__file__).parent
# Config.load_yaml(HERE / "config_cobotta2.yaml")
# Config.load_yaml(HERE / "config_cobotta2_server2.yaml")
Config.load_yaml(HERE / "config_cobotta4_server3.yaml")
cobotta = CobottaOrin2(
cobotta_ip=Config.COBOTTA_IP,
arm_name="Arm",
logger=logger_,
)
# cobotta = CobottaCtrl(cobotta_ip="10.170.10.36", arm_name="Arm", logger=logger_)
# cobotta.reset_error()
try:
if not cobotta.take_arm():
logger_.info(cobotta.error_code)
logger_.info(cobotta.error_description)
cobotta.disconnect()
sys.exit(-1)
if not cobotta.turn_on_motor():
logger_.info(cobotta.error_code)
logger_.info(cobotta.error_description)
cobotta.disconnect()
sys.exit(-1)
# motion_preparation = cobotta.get_motion_preparation()
# logger_.info(f"motion_preparation = {motion_preparation}")
# if not motion_preparation:
# logger_.info("**Error**: state is not True")
# sys.exit(-1)
# cobotta.reset_error()
print(cobotta.get_current_position())
# cobotta._caoRob.Execute("Rotate")
# cobotta.move("P124")
# cobotta.move("P123")
# cobotta.rotate("XY", 45, [240, 13, 197])
# print(cobotta.get_current_work())
# print(cobotta.get_current_error_info())
# print(cobotta.error_code)
# print(cobotta.error_description)
# cobotta.reset_error()
# cobotta.release_arm()
# cobotta.take_arm()
# cobotta.init_hand()
# cobotta.set_work_define(1, "P(1,2,3,4,5,6)")
# # upload
# print(cobotta.get_cobotta_file_names())
# cobotta.upload_file_to_cobotta("P62.pcs", "P64.pcs")
# cobotta.save_all_internal_files()
# Position/Angle
# cobo1: (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.0)
# logger_.info(f"P2: {cobotta.P2}")
# ret = cobotta.dev("P1", [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.0])
# ret = cobotta.dev("P1", "P(0,0,0)")
# cobotta.move("@E P2+(0,0,0)", bstrOpt=100)
# cobotta._caoRob.Move(1, cobotta._caoRob.Execute("Dev", ("P2", "P(0,0,0)")))
##########################
# OK (テスト用で原始的に)
# ret = cobotta._caoRob.Execute("Dev", ("P2", "P(0,0,10)"))
# pose = list(ret[:6]) # ORiN2は配列型を期待する
# cobotta._caoRob.Move(1, [pose, "P", "@P"], "Next")
# cobotta._caoRob.Move(1, [pose, "P", "@E"], "Next")
# cobotta._caoRob.Move(1, [pose, "P"], "Next")
##########################
# OK
# cobotta.move_offset("P2", "P(0,0,0)")
# cobotta.move("@E P2+(0,0,10)")
# ボタン位置
# cobo1:(-29.045351028442383, 343.1440734863281, 115.3128433227539, -178.28956604003906, -16.619848251342773, 141.1028594970703, 1.0)
# logger_.info(f"P2: {cobotta.P2}")
# Start 位置
# cobo1:(115.07630157470703, -272.94757080078125, 240.98133850097656, 171.91989135742188, -21.261798858642578, -19.010150909423828
# logger_.info(f"P3: {cobotta.P3}")
# ###########
# logger_.info(f"get_current_work: {cobotta.get_current_work()}")
# logger_.info(f"get_work_define: {cobotta.get_work_define(1)}")
# ###########
# start 位置
# cobotta.move("P2")
# cobotta.wait_for_complete()
# ret = cobotta.get_direct_mode()
# logger_.info(f"main:get_direct_mode: {ret}")
# print(cobotta.get_pos())
# print(cobotta.state())
# cobotta.test3()
# print("ErrorCode:", cobotta.error_code)
# print("ErrorDescription:", cobotta.error_description)
# print("EmergencyStop:", cobotta.emergency_stop)
# print("ProtectiveSTOP:", cobotta.protective_stop)
#################
# 基本テスト
#
# cobotta.test(bstrOpt=100)
# # cobotta.test1()
# cobotta.test2(bstrOpt=100)
# cobotta.wait_for_complete()
# cobotta.aaa(100)
## print("")
## cobotta.wait_for_complete()
except KeyboardInterrupt as ee:
logger_.info(f"KeyboardInterrupt: {ee}")
except Exception as ee:
logger_.info(f"{ee}")
finally:
cobotta.disconnect()