kachaka-api でカチャカを動かす

こんにちは。Preferred Roboticsで「カチャカ」を開発している岸です。

カチャカは、ロボットと家具が一体となって機能する、スマートファニチャープラットフォームです。 カチャカは専用のアプリや音声コマンドで操作可能で、家具を目的の場所まで自動で運ぶことができます。 例えば、読書の時間に本が載った家具をリビングに運んだり、食器や調味料が載った家具をキッチンとリビングの間で運んだりすることができます。 さらに、家具を片付けたい位置に移動させることで、部屋を常にきれいに保つために使うこともできます。

自社EC, Amazon, 楽天などのECサイトで購入できるほか、アールティさん でもお取り扱いいただいています。

そんなカチャカですが、2023年8月に、開発者向けAPIを公開しました。 このAPIを活用することで、カチャカの機能を自由にカスタマイズし、外部サービスとの連携が可能になります。
このカチャカAPIの使い方をご説明する カチャカAPIワークショップ も2023/9/15に開催されます。

カチャカAPIのインターフェースにはgRPCが採用されています。 gRPCサーバがカチャカの本体で動作しているので、PCなどのカチャカ外部にgRPCクライントを実装して、カチャカを操作することができます。

さらに、カチャカ本体に内蔵されたサンドボックス環境も用意されており、その中で自作のプログラムを実行することも可能です。 これにより、カチャカの移動指示や家具とのドッキング、解除などの操作が自由にプログラムで制御できます。 スマートスピーカーでの音声操作、GPS連携による位置情報ベースの動作、大規模言語モデルとの連携など、多様なシナリオでのカチャカの活用ができるようになりました。

この記事では、最も手軽にAPIを使う方法である、サンドボックス環境内で動作するJupyter Notebookを使って、カチャカを動かす方法を紹介します。 またロボティクス分野の教材として活用していただくために、座標変換や経路追従制御の実装方法も紹介します。

事前準備

カチャカAPIはデフォルトでは無効になっているので、スマートフォンアプリから有効にする必要があります。
まず、カチャカのスマートフォンアプリを以下の手順に従って操作し、カチャカAPIを有効にしてください。 設定タブ → ロボットを選択 → 「カチャカAPI 」→ 「カチャカAPIを有効化する」をONにします。

ダイアログが表示されるので、「利用規約」を確認の上、「カチャカAPI利用規約に同意する」をチェックして「設定する」を押して下さい。

Jupyter Notebookへのアクセス

つぎに、スマートフォンアプリの「設定」→「アプリ情報」→「IPアドレス」からカチャカのIPアドレスを確認してください。 google chromeなどのwebブラウザから http://<カチャカのIPアドレス>:26501/ にアクセスすると、Jupyter Notebookの画面が表示されます。

サンプルコードとkachaka-apiのダウンロード

つぎに、pythonで実装された、カチャカAPIを使うための便利なライブラリをダウンロードします。 Jupyter Notebookの画面の左側のファイル一覧から README.ipynb をダブルクリックしてください。 上部メニューの「▶▶」をクリックしてください。サンプルコードとkachaka-apiがダウンロードされます。

ダウンロードが完了すると、左側のファイル一覧に kachaka-apiフォルダが作成されます。ダウンロードしたkachaka-apiは以下のようにimportして使うことができます。

import kachaka_api

client = kachaka_api.KachakaApiClient()

アプリのマップ上の目的地に移動させる

それではkachaka-apiを使って、カチャカに移動をさせてみます。 以下のコードで、カチャカがシェルフをダイニングに移動します。 カチャカアプリで、「シェルフ」という名前の家具と、「ダイニング」という名前の目的地が設定してある必要があります。

client.update_resolver()
client.move_shelf("シェルフ", "ダイニング")

カチャカが載せる家具や移動する目的地はアプリから名前を変更することがあるため、プログラムを書いた後に変わってしまうことがあります。 名前が変わっても動作するようにするには、以下のようにIDを指定します。

client.move_shelf("S01", "L01")

名前とIDの対応づけは予め知っておく必要があるので、以下のAPIでIDを確認します。

client.get_shelves()

client.get_locations()

move_shel("シェルフ", "ダイニング") をしたときに用いた、client.update_resolver() というAPIは、この名前とIDの対応づけを更新するためのAPIだったわけです。 clientのインスタンスを作った後に、アプリから名前を変更していない場合は、update_resolver() は一度呼べば十分です。
その他にも特定の目的地に移動するためのAPIは色々あります。

API名	説明
move_shelf	指定した家具を指定した目的地に移動する
return_shelf	指定した家具をホームに戻す
move_to_location	指定した目的地に移動する
return_home	充電ドックに戻る
undock_shelf	載せている家具を下ろす
dock_shelf	カチャカの目の前にある家具を載せる

外部APIの利用例: 天気に応じて動かす、スクリプトの常時実行

次に、 move_shelf APIを使って、便利な機能を実装してみます。 ある時点の天気を取得して、それに応じて動作を変えます。 晴れの場合は観葉植物を載せている家具を窓際に移動させ、雨の場合は観葉植物を載せている家具をダイニングに移動させます。

天気を取得するには、OpenWeatherMapのAPIを使います。 sign_upからアカウントを作成し、api_keysでAPIキーを取得する必要があります。 以下のコードの API_KEY="" の部分には取得したAPIキーを入力してください。 ZIP_CODE="" には、天気を取得したい地域の日本の郵便番号をXXX-XXXXの形式で入力してください。

import datetime
import json
import requests
import time
from typing import Tuple

import kachaka_api

client = kachaka_api.KachakaApiClient()
client.update_resolver()

API_KEY = ""
ZIP_CODE = ""
TRIGGER_HOUR = 12

client.speak("今日もいい天気だったらいいなー")

def get_weather_and_description() -> Tuple[str, str]:
    response = requests.get(f"https://api.openweathermap.org/data/2.5/weather?zip={ZIP_CODE},jp&appid={API_KEY}&lang=ja")
    ret = json.loads(response.text)
    weather = ret["weather"][0]
    return weather["main"], weather["description"]

last_triggered_hour = -1
while True:
    date_time_hour = datetime.datetime.now().hour
    if date_time_hour != TRIGGER_HOUR or date_time_hour == last_triggered_hour:
        time.sleep(60)
        continue
    weather, description = get_weather_and_description()
    if weather == "Clear":
        target_location = "窓際"
    else:
        target_location = "リビング"
    client.speak(f"今日の天気は{description}です。")
    client.move_shelf("シェルフ", target_location, cancel_all=False)
    last_triggered_hour = date_time_hour

このscriptを move_shelf_by_weather.py という名前で /home/kachaka/kachaka-api/python に保存してください。 /home/kachaka/kachaka_startup.sh に以下の行を追加すると、カチャカ起動時に自動で実行されるようになります。 毎日12時に天気を取得するようになっています。

python3 -u /home/kachaka/kachaka-api/python/move_shelf_by_weather.py &

すぐに動作を試したい場合は 設定タブ → ロボットを選択 → 「カチャカAPI 」→ 「カチャカAPIを有効化する」をON => OFF => ON と切り替えてください。 サンドボックス環境が再起動して、すぐにスクリプトが実行され、「今日もいい天気だったらいいなー」と話します。

窓際に着いてからは、その場旋回して植木の全方向から日を当てたいですよね。その場で旋回させる方法は、後続の章で紹介します。窓際に移動してからその場旋回する様子はつぎの動画のようになります。

マップ上の自由な位置に移動させる

これまではアプリから設定した目的地にカチャカを移動させていましたが、マップ上の自由な位置に移動させることもできます。

client.move_to_pose(0.5, 0.0, 0.0)

x, y, yaw の順に、移動先の座標を指定できます。 この座標は、おおよそ充電ドックの位置を原点とした座標系になっています。 アプリで見られるマップの画像の中のどの位置が原点なのかというのを知るには、マップを取得するAPIを使う必要があります。

from IPython.display import Image, display

map = client.get_png_map()
print(map.origin)
display(Image(data=map.data))

ここで、map.origin の中に入っている値が、マップの原点とグリッドの原点の間の位置関係を表しています。 グリッドの原点は、マップ画像の左下の角に相当する位置にあります。そしてマップの画像の原点は画像の左上の角にあるわけです。

また、map.resolution は、マップのグリッドの一つが実際には何mなのかを表しています。 これら複数の座標系の間の位置関係と、スケールの違いを考えることによって、マップの画像上にロボットの位置を描画することや、画像上の位置から逆にロボットの位置を求めることができます。座標系間の変換はAffine変換として表現されます。実際にどのように計算するかは https://github.com/pf-robotics/kachaka-api/blob/ba5905eb506c7cf3cf4973f537bce9d5ccaeb3d4/python/kachaka_api/geometry_util.py#L28 をご覧になってください。
ピクセルと実際の座標の間を相互変換することで、マップ上のクリックした位置に移動するカチャカを描画するサンプルプログラムが https://github.com/pf-robotics/kachaka-api/blob/ba5905eb506c7cf3cf4973f537bce9d5ccaeb3d4/python/demos/plot_map_robot_lidar.ipynb にあります。実行すると、動画のように好きな位置にカチャカを動かすことができます。

速度を指定して移動させる

ここまでは座標への移動を指示していましたが、座標は関係なく自分でロボットの移動速度を指定して、自由に移動させることもできます。速度を指令してカチャカを動かす場合の注意点として、手動操縦モードに変更する必要があるということがあります。 以下のコマンドで60秒間は速度指令をすることができます。

client.set_manual_control_enabled(True)

時間が経つと自動でオフになるため、必要に応じて定期的に有効にする必要があります。 また、充電ドック上にいる場合は前に進んで充電ドックから出ます。
つぎのAPIで、直進させたり、その場旋回させたり、それらを合成した動作をさせたりできます。

client.set_robot_velocity(0.1, 0.0)  # 前進
client.set_robot_velocity(0.0, 0.3)  # その場旋回
client.set_robot_velocity(0.1, 0.3)  # 曲がりながら前進

時間ごとに指令速度を変えて、面白い動きをさせてみましょう。

import time

velocity_duration_sequence = [
    (0.0, -0.3, 3.0),
    (0.2, 0.6, 3.0),
    (-0.2, -0.9, 3.0),
]
try:
    client.set_manual_control_enabled(True)
    for linear_speed, angular_speed, duration in velocity_duration_sequence:
        start_time = time.time()
        while time.time() - start_time < duration:
            client.set_robot_velocity(linear_speed, angular_speed)
            time.sleep(0.1)
    client.set_robot_velocity(0.0, 0.0)
    client.return_home()
finally:
    client.set_manual_control_enabled(False)

これで、動画のような一人ジェンガゲームをすることができます 😂

経路に沿って動かしてみる

速度を指定して動かせはしましたが、予めどの経路をロボットが通るか分かった上で動かしたい場合もあります。 そのために、Stanley Control をベースにした、経路追従制御を実装してみます。 これは、DARPA Grand Challenge 2005で優秀な成績を収めたStanford大学のチームが開発した制御アルゴリズムで、Stanleyという自動運転車に使われました。
まず、ロボットが通る経路を表す、経由点を生成します。 ここでは、楕円形の経路を作ってみます。

import numpy as np

def generate_ellipse_way_points(
    radius_x: float, radius_y: float, point_num: int
) -> np.ndarray:
    theta = np.linspace(0, 2 * np.pi, point_num) - np.pi / 2
    x = radius_x * np.cos(theta)
    y = radius_y * np.sin(theta) + radius_y
    return np.vstack((x, y))

描画してみます。

import matplotlib.pyplot as plt

fig, ax = plt.subplots(figsize=(10, 10))
ax.set_aspect("equal")
ax.set_xlabel("x")
ax.set_ylabel("y")

local_way_points = generate_ellipse_way_points(
    0.5, 0.25, 30
)
ax.scatter(*local_way_points)

これらの点を、プログラム開始時点のカチャカの姿勢を原点とした位置に変換します。 座標系としては、odometryを使います。 オドメトリはロボットの車輪の回転数とIMUを使って推定した姿勢です。 マップを使ったlocalizationに比べて、値が飛ぶことが少なく滑らかに変化するため、経路追従制御をするのに向いています。 一方で、繰り返し動作するうちに誤差が蓄積していくため、長時間の移動には向きません。 そういった場合はlocalizationと組み合わせて、滑らかさと精度を両立させる必要がありますが、更に複雑なプログラムになっていきます。
ここではオドメトリのみを用いた、誤差の累積を無視した実装をしていきます。 次のコードで、経由点を生成し、オドメトリ座標系での値に変換されます。

from kachaka_api import geometry_util

# 現在の姿勢を始点として、楕円形の軌道を生成
xy_yaw, _ = get_xy_yaw_and_linear_speed(await client.get_ros_odometry())
trans_mat = geometry_util.calculate_2d_transform_matrix(*xy_yaw)[:2]
local_way_points = generate_ellipse_way_points(
    ELLIPSE_RADIUS_X, ELLIPSE_RADIUS_Y, NUM_WAY_POINTS
)
local_way_points = np.vstack((local_way_points, np.ones((local_way_points.shape[1]))))
# オドメトリ座標系に変換
odom_based_way_points = (trans_mat @ local_way_points).T

Stanley Controlでは追従したい経路があったときに、ロボットからその経路までの距離$e$と、経路との角度のズレ$\phi$を使って、指令速度を決めます。

そのために、経由点を複数の線分に変換し、線分とロボットの距離と角度を計算することで、経路との距離等を求めることにします。 線分を表すclassを以下のように実装しました。

from typing import Tuple

class LineSegment(NamedTuple):
    start: np.ndarray
    end: np.ndarray

    def calc_distance(self, point: np.ndarray) -> Tuple[float, float]:
        # y 軸は線分と直角の方向
        y_from_line = np.cross(
            point - self.start, self.end - self.start
        ) / np.linalg.norm(self.end - self.start)
        if np.dot(self.end - self.start, point - self.start) < 0:
            distance_from_line = np.linalg.norm(point - self.start)
        elif np.dot(self.start - self.end, point - self.end) < 0:
            distance_from_line = np.linalg.norm(point - self.end)
        else:
            distance_from_line = abs(y_from_line)
        return distance_from_line, y_from_line

calc_distance(...) が線分と点の間の距離を求めている実装です。

線分と点があったとき、点からの垂線は$e=|p-s| \sin \alpha$で表されます。このとき、$p$が線の右側にあれば$e$は正の値、左側にあれば負の値になります。線分の上にロボットが近づくように制御したいので、単純な距離でなく、正負を持つ値を使います。$\sin \alpha$は$(p-s)\times(g-s)=|p-s||g-s|\sin \alpha$ という外積の定義より求められるので、結局$e$は外積を使って次のように求めています。

e = \frac{(p-s)\times(g-s)}{\|g-s\|}

ただ、複数の線分のうち最も近い線分を求めるために、最短距離そのものも求める必要があるので、$p$の垂線が線分の外側にある場合は特別に場合分けして、線分の端点との距離を最短距離として計算しています。
そして、複数の経由点から線分のリストを作ります。

line_segments = [
    LineSegment(start=p0, end=p1)
    for p0, p1 in zip(odom_based_way_points[:-1], odom_based_way_points[1:])
]

あとは複数の線分の中から、最も近いものを探して、その線分との距離と角度を計算します。 すべての線分への距離を計算して最も近いものを探しています。

distances_and_segments = [
    (segment.calc_distance(current_position), segment) for segment in line_segments
]
(_, y_from_line), nearest_segment = min(
    distances_and_segments, key=lambda x: x[0][0]
)
angle_to_line = nearest_segment.angle - xy_yaw[2]

そして、距離と角度を使って、ロボットの旋回角速度を計算し、指令することで、ロボットを経路に沿って動かすことができます。

\begin{align}
\theta &= \phi + \tan^{-1} \frac{K_e e}{v} \\
\omega &= K \theta
\end{align}

angle = angle_to_line + math.atan2(K_e * y_from_line, current_linear_speed)
angular_speed = K * angle

サンプルコードの全体は https://github.com/pf-robotics/kachaka-api/blob/ba5905eb506c7cf3cf4973f537bce9d5ccaeb3d4/python/demos/follow_path.ipynb にありますので、ご確認ください。
私はこのような動きを応用して、家で回転寿司をしてみました 😂

楕円形に沿って動くので、家の中を広くダイナミックに動き回ることができます。

まとめ

カチャカを操作する、pythonで実装された便利なライブラリ、kachaka-apiの紹介をしました。 それを使って、天気に応じて家具を移動させたり、地図上の任意の位置に移動させたり、ジェンガを動かしたり、寿司を回したりする例をご紹介しました。

kachaka-apiを使うと、他にも色々実用的なものも作れると思います。 例えば人が帰宅したときに玄関にシェルフを自動で運搬して荷物を受け取ってもらうようにしたり、スマートホーム機器と連携させたり、色々と応用が考えられます。 そういった便利なapiの使い方も、別の機会にご紹介できればと思います。

興味のある方は2023/9/15に開催される、カチャカAPIワークショップ へのご参加をおすすめします。
読んでくださりありがとうございました。