はじめに
この記事はロボティクスアドベントカレンダーの7日目の記事です。
以前PythonのTkinterを使って仮想ジョイスティックのようなものを作ってみました。
今回はこれを現在開発しているロボットのシミュレータに導入していきたいと思います。
実際に動いている様子はこのようになります。
ソースコードはこのリポジトリのadd-virtual-joy-stickブランチにあります。
環境
以下の環境で動作を確認しています。
また、Pythonのライブラリについてはプロジェクト内にpyproject.tomlファイルがあります。
| 項目 | バージョン |
|---|---|
| Ubuntu | 24.04 |
| Python | 3.12.3 |
仮想ジョイスティックの実装
ほぼ前回の記事から変わっていませんが、実装を以下に載せます。
マウスの動きとクリックを検出してジョイスティックの操作を検出、get_position()関数で現在のジョイスティックの位置を取得することができます。
クリックでON/OFFを切り替えるために、OFFの際は0,0を返すようにしています。
import dataclasses
import tkinter as tk
@dataclasses.dataclass
class WindowSize:
width: int
height: int
@dataclasses.dataclass
class Position:
x: float
y: float
_WINDOS_SIZE = WindowSize(200, 300)
_CANVAS_SIZE = WindowSize(200, 200)
_CIRCLE_SIZE = 20
class MousePosition(tk.Frame):
def __init__(self):
self._root = tk.Tk()
super().__init__(self._root, height=_WINDOS_SIZE.height, width=_WINDOS_SIZE.width)
self._root.title("Tk JoyStick")
canvas = tk.Canvas(
self._root,
background="white",
height=_CANVAS_SIZE.height,
width=_CANVAS_SIZE.width,
)
self._canvas = canvas
self._virtual_stick_circle = canvas.create_oval(
_CANVAS_SIZE.width / 2 - _CIRCLE_SIZE,
_CANVAS_SIZE.height / 2 - _CIRCLE_SIZE,
_CANVAS_SIZE.width / 2 + _CIRCLE_SIZE,
_CANVAS_SIZE.height / 2 + _CIRCLE_SIZE,
tag="oval",
)
canvas.create_line(_CANVAS_SIZE.width / 2, 0, _CANVAS_SIZE.width / 2, _CANVAS_SIZE.height)
canvas.create_line(0, _CANVAS_SIZE.height / 2, _CANVAS_SIZE.width, _CANVAS_SIZE.height / 2)
canvas.grid(row=0, column=0)
canvas.bind("<Motion>", self._mouse_callback)
canvas.bind("<Button-1>", self._click_callback)
self._position_label = tk.Label(self._root)
self._position_label.grid(row=1, column=0)
self._last_position = Position(0.0, 0.0)
self._is_clicked = False
def _mouse_callback(self, event):
position_from_center = Position(
event.x - 1 / 2 * _CANVAS_SIZE.width,
1 / 2 * _CANVAS_SIZE.height - event.y,
)
if self._is_clicked:
self._position_label["text"] = (
str(position_from_center.x) + ", " + str(position_from_center.y)
)
self._canvas.move(
self._virtual_stick_circle,
-1 * (self._last_position.x - position_from_center.x),
self._last_position.y - position_from_center.y,
)
self._last_position.x = position_from_center.x
self._last_position.y = position_from_center.y
def _click_callback(self, event: tk.Event) -> None:
del event # unused
if self._is_clicked:
self._canvas.config(background="red")
self._position_label["text"] = "Stop"
self._is_clicked = False
else:
self._canvas.config(background="white")
self._is_clicked = True
def get_position(self) -> tuple[float, float]:
if self._is_clicked:
return (self._last_position.x, self._last_position.y)
else:
return (0.0, 0.0)
def update(self) -> None:
self._root.update()
if __name__ == "__main__":
import time
controller = MousePosition()
for _ in range(10000):
controller.update()
time.sleep(0.01)
ジョイスティックからの速度指令
今回は2軸（X, Y入力）を備えた1つのジョイスティックからロボットへ速度指令を送ります。
イメージとしては以下の図のように、中心から右斜めの場所だと前進と右旋回の指令が送られます。
そもそも1つのインターフェイスで2方向の処理をしようとしているので2つのジョイスティックで前進方向、旋回方向のように割り当てると操作性は上がるかと思います。
この処理には座標変換と旋回指令のための角度の算出が必要になります。
座標の変換
この仮想ジョイスティックはよく数学とかであるような横方向がX軸（右が正）、縦方向がY軸（上が正）になっています。
シミュレータはロボットのローカル座標系（ロボット前方がX軸）に対して速度を設定します。
下の図の左側が仮想ジョイスティック、右側がシミュレータです。
このシミュレータのロボットは横方向の移動はせず、前後と旋回方向の移動のみなので、見た目が分かりやすいように、仮想ジョイスティックのY軸方向をロボットの前後方向、ジョイスティックの位置(Y軸とのなす角度)をロボットの旋回の方向としましょう。
ジョイスティックの座標 $\mathbf{J} = (\text{Jx}, \text{Jy})$ から、ロボットの速度指令 $\mathbf{U} = (V, \omega)$ への変換は以下の通りです。
$$V = \frac{\text{Jy}}{\text{Scale}_{V}}$$
$$\omega = \text{atan2}(\text{Jx}, \text{Jy})$$
補足1
通常の数学関数 $\text{atan2}(\text{y}, \text{x})$ は、X軸正方向を 0 rad としますが、ここで $\text{atan2}(\text{Jx}, \text{Jy})$ と引数を逆にすることで、Y軸正方向（前方）を 0 rad とした角度が得られます。
補足2
$\text{atan2}$ の戻り値はラジアン（$-\pi$ ～ $+\pi$）です。このωの計算結果になにかゲインをかけることで操作性の向上が見込まれるかもしれません。
シミュレータへ統合
simulator/main.pyへ統合しましょう。
仮装ジョイスティック上の現在の値を取得してsimulatorへset_velocityする箇所は以下になります。
これを起動すると以下の動画のようにロボットを操作することができます。
まとめ
今回の記事ではロボットを任意の方向に操作できるようにしました。
ビジュアライザー、プランナーなどなど、今後も追加していきます！