#!/usr/bin/env python
"""
K.NAKADA, kengo.nakada@gmail.com, kengo.nakada@mat.shimane-u.ac.jp
"""
from typing import Any, Union, List, Dict, Tuple, Callable, Optional, Sequence
# grpc
from cobotta2.server_grpc import JoyPadClient
# local
from cobotta_bcap_arm import CobottaBcapArm
from x_logger import XLogger
[ドキュメント]
class CobottaBcapCtrl:
"""
JoyPad GUI から b-CAP 制御を行うための中枢コントローラクラス。
本クラスの役割は以下に限定される。
- JoyPadClient を通じて b-CAP コマンドを送信する
- CobottaBcapArm と連携し、アームの Take / Give 状態を管理する
- GUI（Qt）からの操作をスロットとして受け取り、適切な RPC を呼び出す
実際の制御ロジックは JoyPadClient およびサーバ側 ctrl に存在し、
本クラスは UI と通信層の橋渡しのみを行う。
"""
def __init__(
self,
parent: Any, # main GUI
client: JoyPadClient, # JoyPadClient
arm_ctrl: CobottaBcapArm, # CobottaBcapArm
logger: Optional[XLogger] = None,
):
"""
CobottaBcapCtrl を初期化する。
Args:
parent (Any):
メイン GUI オブジェクト。
ui 属性を持ち、Qt Designer で生成された各種ウィジェットに
アクセスできることを前提とする。
client (JoyPadClient):
gRPC 経由でサーバ側の ctrl に接続するクライアント。
すべての b-CAP 操作要求はこのクライアントを通じて送信される。
arm_ctrl (CobottaBcapArm):
アームの Take / Give 状態を管理する補助コントローラ。
本クラスはアーム制御を直接行わず、必ず arm_ctrl に委譲する。
logger (XLogger, optional):
ログ出力用ロガー。
指定されない場合は本クラス専用の logger を生成する。
Notes:
- ハンド速度のデフォルト値は内部パラメータとして保持される
- UI 上の各ボタン・入力欄とスロットの接続もここで行う
"""
self._parent = parent
self._client = client
self._arm_ctrl = arm_ctrl
# default parameter
self._hand_speed = 100
if logger is None:
logger = XLogger(log_level="info", logger_name="CobottaBcapCtrl")
self._logger = logger
# ------------------
# UI
# ------------------
self.ui = parent.ui
# Hand
self.ui.JoyPad_button_hand_open.clicked.connect(self._on_hand_open)
self.ui.JoyPad_button_hand_move.clicked.connect(self._on_hand_move)
# エンターで移動
self.ui.JoyPad_lineEdit_read_hand_pos.returnPressed.connect(
self.ui.JoyPad_button_hand_move.click
)
self.ui.JoyPad_lineEdit_read_hand_force.returnPressed.connect(
self.ui.JoyPad_button_hand_move.click
)
# Arm Move (for JoyPad)
self.ui.JoyPad_button_move.clicked.connect(self._on_move)
# P 読み込みボタン
self.ui.JoyPad_button_read_pos.clicked.connect(self._on_read_p_clicked)
# P 書き込みボタン
self.ui.JoyPad_button_save_pos.clicked.connect(self._on_save_p_clicked)
# -----------------------------------------------
# JoyPad Motor on/off
# -----------------------------------------------
[ドキュメント]
def motor_on(self) -> None:
"""
サーボモータを ON にする。
Behavior:
- JoyPadClient.motor_on() を呼び出す
- RPC が False を返した場合や例外が発生した場合はログに記録する
Notes:
- client が未初期化の場合は何も行わずに終了する
"""
self._logger.info("== motor_on()")
if self._client is None:
self._logger.error("JoyPadClient is not initialized")
return
try:
ok = self._client.motor_on()
if not ok:
self._logger.error("[BCAP] motor_on() returned False")
except Exception as e:
self._logger.error(f"[BCAP] motor_on() failed: {e}")
[ドキュメント]
def motor_off(self) -> None:
"""
サーボモータを OFF にする。
Behavior:
- JoyPadClient.motor_off() を呼び出す
- RPC が失敗した場合はログにエラーを出力する
Notes:
- client が未初期化の場合は処理を行わない
"""
self._logger.info("== motor_off()")
if self._client is None:
self._logger.error("JoyPadClient is not initialized")
return
try:
ok = self._client.motor_off()
if not ok:
self._logger.error("motor_off() returned False")
except Exception as e:
self._logger.error(f"motor_off() failed: {e}")
# -----------------------------------------------
# JoyPad Take Arm/Give Arm
# -----------------------------------------------
[ドキュメント]
def take_arm(self) -> bool:
"""
アームの取得（Take Arm）を要求する。
Returns:
bool:
True : アーム取得に成功、または既に取得済み
False : 取得に失敗した場合
Notes:
- 実処理は CobottaBcapArm に委譲される
- 本メソッドはログ出力と委譲のみを行う
"""
self._logger.info("== take_arm()")
return self._arm_ctrl.take_arm()
[ドキュメント]
def give_arm(self) -> bool:
"""
アームの解放（Give Arm）を要求する。
Returns:
bool:
True : アーム解放に成功、または元々保持していない
False : 解放に失敗した場合
Notes:
- 実処理は CobottaBcapArm に委譲される
"""
self._logger.info("== give_arm()")
return self._arm_ctrl.give_arm()
# -----------------------------------------------
# JoyPad Hand
# -----------------------------------------------
[ドキュメント]
def _on_read_p_clicked(self) -> None:
"""
UI の「P 読み込み」ボタンが押されたときのスロット。
Behavior:
- UI から P インデックスを取得する
- JoyPadClient.get_value("P", index) を呼び出す
- 取得した姿勢値を UI 表示欄に反映する
Notes:
- P 値は [x, y, z, rx, ry, rz] の配列であることを前提とする
"""
if self._client is None:
self._logger.error("JoyPadClient is not initialized")
return
p_index_str = self.ui.JoyPad_lineEdit_read_p_index.text().strip()
if p_index_str == "":
self._logger.info("P index is empty. Skip read.")
return
try:
p_index = int(p_index_str)
except Exception:
self._logger.error("Invalid P index (not an integer)")
return
pose = self._client.get_value("P", p_index)
if pose is None or len(pose) < 6:
self._logger.error(f"Failed to read P{p_index}")
return
x, y, z, rx, ry, rz = pose[:6]
self.ui.JoyPad_lineEdit_view_pos_x_2.setText(f"{x:.3f}")
self.ui.JoyPad_lineEdit_view_pos_y_2.setText(f"{y:.3f}")
self.ui.JoyPad_lineEdit_view_pos_z_2.setText(f"{z:.3f}")
self.ui.JoyPad_lineEdit_view_pos_rx_2.setText(f"{rx:.3f}")
self.ui.JoyPad_lineEdit_view_pos_ry_2.setText(f"{ry:.3f}")
self.ui.JoyPad_lineEdit_view_pos_rz_2.setText(f"{rz:.3f}")
self._logger.info(f"Read P{p_index}: {pose}")
[ドキュメント]
def _on_save_p_clicked(self) -> None:
"""
UI の「P 書き込み」ボタンが押されたときのスロット。
Behavior:
- UI から P インデックスと姿勢値を取得する
- JoyPadClient.put_value("P", index, pose) を呼び出す
- 保存結果をログに出力する
"""
if self._client is None:
self._logger.error("JoyPadClient is not initialized")
return
p_index_str = self.ui.JoyPad_lineEdit_read_save_p_index.text().strip()
if p_index_str == "":
self._logger.info("P index is empty. Skip save.")
return
try:
p_index = int(p_index_str)
except Exception:
self._logger.error("Invalid P index (not an integer)")
return
try:
x = float(self.ui.JoyPad_lineEdit_view_pos_x.text())
y = float(self.ui.JoyPad_lineEdit_view_pos_y.text())
z = float(self.ui.JoyPad_lineEdit_view_pos_z.text())
rx = float(self.ui.JoyPad_lineEdit_view_pos_rx.text())
ry = float(self.ui.JoyPad_lineEdit_view_pos_ry.text())
rz = float(self.ui.JoyPad_lineEdit_view_pos_rz.text())
except Exception as e:
self._logger.error(f"Invalid pose values: {e}")
return
pose = [x, y, z, rx, ry, rz]
ok = self._client.put_value("P", p_index, pose)
if not ok:
self._logger.error(f"Failed to save P{p_index} = {pose}")
return
self._logger.info(f"Saved P{p_index} = {pose}")
# -----------------------------------------------
# Move
# -----------------------------------------------
[ドキュメント]
def _on_move(self) -> None:
"""
UI の「Move」ボタンが押されたときのスロット。
Behavior:
- P インデックス指定モードの場合:
JoyPadClient.move(p_index) を呼び出す
- 座標指定モードの場合:
JoyPadClient.move(pose_list) を呼び出す
Notes:
- UI のラジオボタン状態により動作分岐する
- pose は [x, y, z, rx, ry, rz] の list として送信される
"""
self._logger.info("== on_move()")
if self._client is None:
self._logger.error("JoyPadClient is not initialized")
return
if self.ui.JoyPad_select_cobotta_p_index.isChecked():
p_index_str = self.ui.JoyPad_lineEdit_read_p_index.text().strip()
if p_index_str == "":
self._logger.error("No P index given")
return
try:
p_index = int(p_index_str)
except Exception:
self._logger.error("Invalid P index")
return
ok = self._client.move(p_index)
if not ok:
self._logger.error("move(P-index) failed")
return
if self.ui.JoyPad_select_cobotta_pos.isChecked():
try:
x = float(self.ui.JoyPad_lineEdit_read_x.text())
y = float(self.ui.JoyPad_lineEdit_read_y.text())
z = float(self.ui.JoyPad_lineEdit_read_z.text())
rx = float(self.ui.JoyPad_lineEdit_read_rx.text())
ry = float(self.ui.JoyPad_lineEdit_read_ry.text())
rz = float(self.ui.JoyPad_lineEdit_read_rz.text())
except Exception:
self._logger.error("Invalid pose for move")
return
pose = [x, y, z, rx, ry, rz]
ok = self._client.move(pose)
if not ok:
self._logger.error("move(pose) failed")
return
# -----------------------------------------------
# Hand open/move
# -----------------------------------------------
[ドキュメント]
def _on_hand_open(self) -> None:
"""
UI の「Hand Open」ボタンが押されたときのスロット。
Behavior:
- あらかじめ決められた開放位置・速度で hand_move_A() を呼び出す
- 失敗時はログにエラーを出力する
"""
self._logger.info("== on_hand_open()")
if self._client is None:
return
pos_open = 30.0
speed_open = 100.0
try:
ok = self._client.hand_move_A(position=pos_open, speed=speed_open)
except Exception as e:
self._logger.error(f"hand_move_A(open) failed: {e}")
return
if not ok:
self._logger.error("hand_move_A(open) returned False")
[ドキュメント]
def _on_hand_move(self) -> None:
"""
UI の「Hand Move」ボタンが押されたときのスロット。
Behavior:
- 力制御モードが選択されている場合:
hand_move_H() を呼び出す
- 位置制御モードが選択されている場合:
hand_move_A() を呼び出す
Notes:
- UI のチェック状態により制御モードを切り替える
"""
self._logger.info("== on_hand_move()")
if self._client is None:
return
if self.ui.JoyPad_select_hand_force.isChecked():
try:
force = float(self.ui.JoyPad_lineEdit_read_hand_force.text())
except Exception:
self._logger.error("Invalid force value for hand move")
return
ok = self._client.hand_move_H(force=force, direction=1)
if not ok:
self._logger.error("hand_move_H(move) returned False")
return
if self.ui.JoyPad_select_hand_pos.isChecked():
try:
pos = float(self.ui.JoyPad_lineEdit_read_hand_pos.text())
except Exception:
self._logger.error("Invalid pos value for hand move")
return
ok = self._client.hand_move_A(position=pos, speed=50.0)
if not ok:
self._logger.error("hand_move_A(move) returned False")
return