この記事の他にも，ロボティクスの辞書に様々なロボティクスに関する記事をまとめていますので、よろしければそちらもご覧ください！

事前に必要な知識（見ておいた方がいい記事）
- 【図解】逆運動学（解析的解法）【pythonコード付き】
- 【図解】ヤコビ行列を用いた数値解法による逆運動学【pythonコード付】

お疲れ様です。秋並です。

今回は、ロボットアームの動作計画の中でも

タスク空間における動作計画

について解説します。

本記事内のコードはすべてgoogle colabratory上でも動作します（2024/4/14現在）。

本記事に出てくるコードはnotebook上で動作させる（「ipynb」ファイル）ことを前提としているため「py」ファイルでは一部のコードは動作しないのでご注意ください（主に描画関連）

タスク空間とは

タスク空間(task space)とは、「ロボットが実際にタスクを実行する空間」のことで、要するに、我々が普段生活している空間のことになります。論文や書籍によってはワークスペース(work space)と呼ばれることもあります。

タスク空間における動作計画の実行手順

タスク空間における動作計画の実行手順は以下のようになります。

１. 「初期手先位置$\vec{P}_{\mathrm{start}}$」と「目標手先位置$\vec{P}_{\mathrm{goal}}$」を設定します。

２. 障害物と衝突しないような、「$\vec{P}_{\mathrm{start}}$ と $\vec{P}_{\mathrm{goal}}$ を結ぶ手先の軌道」を求めます。

タスク空間において、軌道を求める方法は以下の2種類が存在します。

人間が手動で軌道を設定する
「A*, RRT」などの動作計画法（経路計画法）と呼ばれるアルゴリズムによって自動的に求める

今回は、動作計画法の解説はしません（後日、別記事にて解説予定）。

３.求めた軌道を一定幅で分割し、それを「手先軌道 $\mathrm{P}= {\vec{P}_{\mathrm{start}}, \vec{P}_2, \vec{P}_3, ..., \vec{P}_{\mathrm{goal}-1}, \vec{P}_{\mathrm{goal}}}$」とします。

４.逆運動学により「手先軌道 $\mathrm{P}$」から「関節軌道 $\mathrm{Q}= {\vec{Q}_{\mathrm{start}}, \vec{Q}_2, \vec{Q}_3, ..., \vec{Q}_{\mathrm{goal}-1}, \vec{Q}_{\mathrm{goal}}}$」を求めます。

逆運動学については、以下記事を参考にしてください。
【図解】逆運動学（解析的解法）【pythonコード付き】
【図解】ヤコビ行列を用いた数値解法による逆運動学【pythonコード付】

５.「関節軌道 $\mathrm{Q}$」に従って順番に各関節角度を動かしていきます。

タスク空間による動作計画のメリットとデメリット

タスク空間による動作計画には以下のようなメリットとデメリットが存在します。

メリット
- タスク空間における軌道を求めるので、人間が任意の手先軌道を指定することが可能（動作計画法を用いることで、自動的に軌道を求めることも可能）
デメリット
- 逆運動学を何度も解く必要がある（手先軌道の長さ分）
  - このデメリットを解消するのが「コンフィギュレーション空間を用いた動作計画」になります（別記事にて解説予定）。

コード

上記の手順をpythonで記述した例が以下のようになります。

コードを実行すると下図のような「初期姿勢」から「目標姿勢」に至るロボットアームの動作が描画されます。

障害物がある空間における動作計画は実装が複雑になる（「動作計画法」や「衝突判定」といった、それ単体で解説が必要な概念が登場する）ため、ここでは 「障害物がない空間」における動作計画の実装例を記載します。

具体的には、「初期手先位置$\vec{P}_{\mathrm{start}}$」と「目標手先位置$\vec{P}_{\mathrm{goal}}$」を」を直線で結んだ「手先軌道$\mathrm{P}$」を実現するための「関節軌道$\mathrm{Q}$」を求めることとします。

「ここを変更すると結果が変わります」の箇所を変更すると結果が変わります

import math
import matplotlib.pyplot as plt
import numpy as np

def inverse_kinematics_2link(l1, l2, xe, ye):
    """
    2次元平面上の2リンクロボットアームの逆運動学を求める
    
    Parameters
    ----------
    l1 : float
        リンク1の長さ
    l2 : float
        リンク2の長さ
    xe : float
        手先のx座標
    ye : float
        手先のy座標

    Returns
    -------
    theta1 : float
        リンク1の関節角度(rad)
    theta2 : float
        リンク2の関節角度(rad)
    """
    try:
        # 代数的に求めた逆運動学の式
        theta1 = -math.acos((xe**2 + ye**2 + l1**2 - l2**2)/(2 * l1 * math.sqrt(xe**2 + ye**2))) + math.atan2(ye, xe)
        theta2 = math.acos((xe**2 + ye**2 - l1**2 - l2**2)/(2 * l1 * l2))

        # 幾何学的に求めた逆運動学の式（「幾何学的」な手法を試したい場合は、下記のコメントを外してください）
        # theta1 = math.atan2(ye,xe) - math.acos((-l2**2 + l1**2 + xe**2 + ye**2)/(2 * l1 * math.sqrt(xe**2 + ye**2)))
        # theta2 = math.pi - math.acos((-xe**2 - ye**2 + l2**2 + l1**2)/(2 * l2* l1))
    except: # 解が存在しない(xe, ye)を入力した場合、Noneを出力
        theta1 = None
        theta2 = None
    return theta1, theta2


# 描画には各リンクの先端の座標が必要なので、求めたtheta1, theta2を使って順運動学を解く（描画しないなら不要）
# 同次変換行列（順運動学）← 「同次変換行列による順運動学」の記事はこちら https://qiita.com/akinami/items/9e65389929cedb1c9551
def make_homogeneous_transformation_matrix(link_length, theta):
    """
    2次元平面における同次変換行列を求める
    
    Parameters
    ----------
    link_length : float
        リンクの長さ
    theta : float
        回転角度(rad)

    Returns
    -------
    T : numpy.ndarray
        同次変換行列
    """
    return np.array([[np.cos(theta), -np.sin(theta), link_length*np.cos(theta)],
                     [np.sin(theta),  np.cos(theta), link_length*np.sin(theta)],
                     [            0,              0,                        1]])

##### ここを変更すると結果が変わります #######
# 各リンクの長さ
l1 = 2
l2 = 2

# 初期手先位置（実現可能な手先位置でないと解が求まりません）
x_start = 1.0
y_start = 3.0
P_start = np.array([[x_start],
                    [y_start]])

# 目標手先位置（実現可能な手先位置でないと解が求まりません）
x_goal = -2.0
y_goal = 2.0
P_goal = np.array([[x_goal],
                   [y_goal]])

resolution = 0.2 # 手先軌道の分解能
############################################

# 「手先軌道P = [P_start, P_2, P_3, ..., P_goal-1, P_goal]」 を求める #############
# 手先軌道を描けるかを判定するための変数（実現できない手先位置が軌道上に存在しないか）
enable_path = True

# 今回は障害物がないため、P_startとP_goalを結ぶ直線を手先軌道とする。障害物がある環境では動作計画法（経路計画法）などを用いて障害物と衝突しないような軌道を求める。
d = math.sqrt((P_goal[0][0] - P_start[0][0])**2 + (P_goal[1][0] - P_start[1][0])**2) # P_start <-> P_goal 間の距離
divid_num = int(d/resolution) # resolutionの分解能で分割するための分割数
P_delta = np.array([[(P_goal[0][0] - P_start[0][0])/divid_num],
                   [(P_goal[1][0] - P_start[1][0])/divid_num]]) # 距離resolutionだけ移動するための、x,y方向の微小移動量

P = [P_start] # 手先軌道P
P_current = P_start.copy()
# P_start -> P_goal方向に resolutionの分解能 で手先位置を移動させていき、「手先軌道P」を作成
while (0.001 < abs(P_goal[0][0] - P_current[0][0])) and (0.001 < abs(P_goal[1][0] - P_current[1][0])):
    P_current += P_delta
    P.append(P_current.copy())
##################################################################################

# 「関節軌道 Q=[Q_start, Q_1, Q_2, Q_3, ..., Q_goal-1, Q_goal]」を求める ############
Q = [] # 関節軌道Q
for i in range(len(P)):
    # 逆運動学によって「i番目の関節角度Q_i」を求める
    theta1_current, theta2_current = inverse_kinematics_2link(l1, l2, P[i][0][0],P[i][1][0])
    # 実現不可能な手先位置でないか確認
    if theta1_current == None:
        enable_path = False
    Qcurrent = np.array([[theta1_current],
                         [theta2_current]])
    Q.append(Qcurrent)
###################################################################################

# 以下描画用 #######################################################################
fig = plt.figure(figsize=(5,5))
ax = fig.add_subplot(1,1,1)

# 軌道が求められた場合、描画する
if enable_path:
    # 描画用にlink2の原点座標をまとめるlistを用意
    o2_list =[]
    for i in range(len(Q)):
        x1, y1 = 0, 0

        # 同次変換行列による順運動学でlink2の原点座標を求める（この操作は描画しないのならば不要）
        H12 = make_homogeneous_transformation_matrix(l1, Q[i][0][0])
        o2 = H12@np.array([[x1],
                           [y1],
                           [1]])
        o2_list.append(o2)

    # x,y 方向の範囲をlink1, link2を伸ばしきった長さにする
    plt.xlim(-(l1+l2),(l1+l2))
    plt.ylim(-(l1+l2),(l1+l2))

    # 手先の軌道、アームの軌道を描画
    for i in range(len(P)):
        x2 = o2_list[i][0][0]
        y2 = o2_list[i][1][0]
        xe = P[i][0][0]
        ye = P[i][1][0]
        ax.plot([x1, x2], [y1, y2], color="tomato", lw=1) # link1の描画
        ax.plot([x2, xe], [y2, ye], color="lightgreen", lw=1) # link2の描画
        if i != 0:
            xe_pre = P[i-1][0][0]
            ye_pre = P[i-1][1][0]
            ax.plot([xe_pre, xe], [ye_pre, ye], color="blue", lw=1) # 手先の軌道を描画
        print(f"Q[{i}] = ({math.degrees(Q[i][0][0]):.2f}°, {math.degrees(Q[i][1][0]):.2f}°)")
    plt.show()
else:
    print("実現不可能な軌道です。")

（おまけ）アニメーション

以下コードを実行すると、アニメーションで初期姿勢→目標姿勢に推移する様子が可視化できます。

「ここを変更すると結果が変わります」の箇所を変更すると結果が変わります

from matplotlib.animation import FuncAnimation
from IPython.display import HTML
import math
import matplotlib.pyplot as plt
import numpy as np

def inverse_kinematics_2link(l1, l2, xe, ye):
    """
    2次元平面上の2リンクロボットアームの逆運動学を求める
    
    Parameters
    ----------
    l1 : float
        リンク1の長さ
    l2 : float
        リンク2の長さ
    xe : float
        手先のx座標
    ye : float
        手先のy座標

    Returns
    -------
    theta1 : float
        リンク1の関節角度(rad)
    theta2 : float
        リンク2の関節角度(rad)
    """
    try:
        # 代数的に求めた逆運動学の式
        theta1 = -math.acos((xe**2 + ye**2 + l1**2 - l2**2)/(2 * l1 * math.sqrt(xe**2 + ye**2))) + math.atan2(ye, xe)
        theta2 = math.acos((xe**2 + ye**2 - l1**2 - l2**2)/(2 * l1 * l2))

        # 幾何学的に求めた逆運動学の式（「幾何学的」な手法を試したい場合は、下記のコメントを外してください）
        # theta1 = math.atan2(ye,xe) - math.acos((-l2**2 + l1**2 + xe**2 + ye**2)/(2 * l1 * math.sqrt(xe**2 + ye**2)))
        # theta2 = math.pi - math.acos((-xe**2 - ye**2 + l2**2 + l1**2)/(2 * l2* l1))
    except: # 解が存在しない(xe, ye)を入力した場合、Noneを出力
        theta1 = None
        theta2 = None
    return theta1, theta2


# 描画には各リンクの先端の座標が必要なので、求めたtheta1, theta2を使って順運動学を解く（描画しないなら不要）
# 同次変換行列（順運動学）← 「同次変換行列による順運動学」の記事はこちら https://qiita.com/akinami/items/9e65389929cedb1c9551
def make_homogeneous_transformation_matrix(link_length, theta):
    """
    2次元平面における同次変換行列を求める
    
    Parameters
    ----------
    link_length : float
        リンクの長さ
    theta : float
        回転角度(rad)

    Returns
    -------
    T : numpy.ndarray
        同次変換行列
    """
    return np.array([[np.cos(theta), -np.sin(theta), link_length*np.cos(theta)],
                     [np.sin(theta),  np.cos(theta), link_length*np.sin(theta)],
                     [            0,              0,                        1]])

##### ここを変更すると結果が変わります #######
# 各リンクの長さ
l1 = 2
l2 = 2

# 初期手先位置（実現可能な手先位置でないと解が求まりません）
x_start = 1.0
y_start = 3.0
P_start = np.array([[x_start],
                    [y_start]])

# 目標手先位置（実現可能な手先位置でないと解が求まりません）
x_goal = -2.0
y_goal = 2.0
P_goal = np.array([[x_goal],
                   [y_goal]])

resolution = 0.2 # 手先軌道の分解能
############################################

# 「手先軌道P = [P_start, P_2, P_3, ..., P_goal-1, P_goal]」 を求める #############
# 手先軌道を描けるかを判定するための変数（実現できない手先位置が軌道上に存在しないか）
enable_path = True

# 今回は障害物がないため、P_startとP_goalを結ぶ直線を手先軌道とする。障害物がある環境では動作計画法（経路計画法）などを用いて障害物と衝突しないような軌道を求める。
d = math.sqrt((P_goal[0][0] - P_start[0][0])**2 + (P_goal[1][0] - P_start[1][0])**2) # P_start <-> P_goal 間の距離
divid_num = int(d/resolution) # resolutionの分解能で分割するための分割数
P_delta = np.array([[(P_goal[0][0] - P_start[0][0])/divid_num],
                   [(P_goal[1][0] - P_start[1][0])/divid_num]]) # 距離resolutionだけ移動するための、x,y方向の微小移動量

P = [P_start] # 手先軌道P
P_current = P_start.copy()
# P_start -> P_goal方向に resolutionの分解能 で手先位置を移動させていき、「手先軌道P」を作成
while (0.001 < abs(P_goal[0][0] - P_current[0][0])) and (0.001 < abs(P_goal[1][0] - P_current[1][0])):
    P_current += P_delta
    P.append(P_current.copy())
##################################################################################

# 「関節軌道 Q=[Q_start, Q_1, Q_2, Q_3, ..., Q_goal-1, Q_goal]」を求める ############
Q = [] # 関節軌道Q
for i in range(len(P)):
    # 逆運動学によって「i番目の関節角度Q_i」を求める
    theta1_current, theta2_current = inverse_kinematics_2link(l1, l2, P[i][0][0],P[i][1][0])
    # 実現不可能な手先位置でないか確認
    if theta1_current == None:
        enable_path = False
    Qcurrent = np.array([[theta1_current],
                         [theta2_current]])
    Q.append(Qcurrent)
###################################################################################

# 以下描画用 #######################################################################
fig = plt.figure(figsize=(5,5))
ax = fig.add_subplot(1,1,1)

# 軌道が求められた場合、描画する
if enable_path:
    # 描画用にlink2の原点座標をまとめるlistを用意
    o2_list =[]
    for i in range(len(Q)):
        x1, y1 = 0, 0

        # 同次変換行列による順運動学でlink2の原点座標を求める（この操作は描画しないのならば不要）
        H12 = make_homogeneous_transformation_matrix(l1, Q[i][0][0])
        o2 = H12@np.array([[x1],
                           [y1],
                           [1]])
        o2_list.append(o2)

# リンクが画面内に収まるように設定
fig = plt.figure(figsize=(5,5))
ax = fig.add_subplot(1, 1, 1)

# jupyter book内にアニメーションを表示
def update(i):
    ax.cla()
    # x,y 方向の範囲をlink1, link2を伸ばしきった長さにする
    ax.set_xlim(-(l1+l2),(l1+l2))
    ax.set_ylim(-(l1+l2),(l1+l2))
    x2 = o2_list[i][0][0]
    y2 = o2_list[i][1][0]
    xe = P[i][0][0]
    ye = P[i][1][0]
    ax.plot([x1, x2], [y1, y2], color="tomato", lw=1) # link1の描画
    ax.plot([x2, xe], [y2, ye], color="lightgreen", lw=1) # link2の描画
    if i != 0:
        for j in range(1, i+1):
            xe = P[j][0][0]
            ye = P[j][1][0]
            xe_pre = P[j-1][0][0]
            ye_pre = P[j-1][1][0]
            ax.plot([xe_pre, xe], [ye_pre, ye], color="blue", lw=1) # 手先の軌道を描画

ani = FuncAnimation(fig, update, interval=50, frames=len(P))
HTML(ani.to_jshtml()) # HTMLに
# ani.save('motion_plan_task_space.mp4', writer="ffmpeg") # mp4で保存．これを実行すると処理時間が増加します
# ani.save('motion_plan_task_space.gif', writer="imagemagick") # gifで保存．これを実行すると処理時間が増加します

まとめとコンフィギュレーション空間における動作計画について

今回は

タスク空間における動作計画

について解説しました。

タスク空間における動作計画は「人間が任意の手先軌道を指定することが可能」であり、直感的に軌道を設定することが出来ます。

一方で、逆運動学を何回も（軌道の長さ分）解く必要があるため、計算回数が多くなります。

上記のデメリットを解決する方法に「コンフィギュレーション空間」を用いた方法が存在します。

コンフィギュレーション空間を用いた動作計画では、「初期関節角度$Q_{\mathrm{start}}$」「目標関節角度$Q_{\mathrm{goal}}$」のみを逆運動学で求めればよいため、計算回数が少なくて済むといったメリットがあります。

コンフィギュレーション空間を用いた動作計画の詳細については、こちらの記事を参照ください。

以上で今回の解説を終わります。ロボティクスに興味のある方のお役に少しでも立てたら幸いです。