逆運動学でロボット制御 - Shinichi0713/LLM-fundamental-study GitHub Wiki

2次元平面上の3自由度ロボットアーム（各関節はz軸回り回転、リンク長固定）について、
逆運動学（ヤコビアン反復法）でエンドエフェクタを目標位置に到達させ、
その動作をMatplotlibでアニメーション表示するサンプルコードを示します。

1. 2次元版の順運動学・逆運動学

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

# 2次元の順運動学
def forward_kinematics_2d(thetas, link_lengths):
    x = [0]
    y = [0]
    angle = 0
    for i in range(len(thetas)):
        angle += thetas[i]
        x.append(x[-1] + link_lengths[i] * np.cos(angle))
        y.append(y[-1] + link_lengths[i] * np.sin(angle))
    return np.array(x), np.array(y)

# 2次元の逆運動学（ヤコビアン反復法）
def inverse_kinematics_2d(target, link_lengths, initial_thetas=None, max_iter=100, tol=1e-3):
    n = len(link_lengths)
    if initial_thetas is None:
        thetas = np.zeros(n)
    else:
        thetas = np.array(initial_thetas)
    trajectory = [thetas.copy()]
    for _ in range(max_iter):
        x, y = forward_kinematics_2d(thetas, link_lengths)
        end = np.array([x[-1], y[-1]])
        error = target - end
        if np.linalg.norm(error) < tol:
            break
        # 数値ヤコビアン
        J = np.zeros((2, n))
        delta = 1e-5
        for i in range(n):
            dtheta = thetas.copy()
            dtheta[i] += delta
            dx, dy = forward_kinematics_2d(dtheta, link_lengths)
            diff = np.array([dx[-1], dy[-1]]) - end
            J[:, i] = diff / delta
        dthetas = np.linalg.pinv(J) @ error
        thetas += dthetas
        trajectory.append(thetas.copy())
    return np.array(trajectory)

2. アニメーション表示

# パラメータ
link_lengths = [1.0, 1.0, 0.7]
target = np.array([1.5, 1.2])
initial_thetas = [0.0, 0.0, 0.0]

# 逆運動学で経路を計算
trajectory = inverse_kinematics_2d(target, link_lengths, initial_thetas, max_iter=80)

# アニメーション準備
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set_xlim(-sum(link_lengths)-0.1, sum(link_lengths)+0.1)
ax.set_ylim(-sum(link_lengths)-0.1, sum(link_lengths)+0.1)
line, = ax.plot([], [], 'o-', lw=4, color='blue')
target_dot, = ax.plot(target[0], target[1], 'ro', markersize=10)

def update(frame):
    thetas = trajectory[frame]
    x, y = forward_kinematics_2d(thetas, link_lengths)
    line.set_data(x, y)
    return line, target_dot

ani = FuncAnimation(fig, update, frames=len(trajectory), interval=80, blit=True)
plt.title('2D 3-DOF Robot Arm Inverse Kinematics')
plt.show()

補足

inverse_kinematics_2d関数が、各反復ごとの関節角度をtrajectoryとして返すので、それをアニメーションで再生します。
目標点は赤丸で表示されます。
3自由度ですが、リンク数や目標点は変更可能です。
Python環境にmatplotlibとnumpyが必要です。

はい、ご要望は「重力・質量・慣性モーメントも考慮した多関節ロボットの動力学シミュレーション（2次元）をmatplotlibのアニメーションで可視化したい」ということですね。

これは「運動方程式（Euler-Lagrange方程式やニュートン-オイラー法）」を用いて、
各関節トルクからロボットの運動（角加速度→角速度→角度）を時系列で計算し、
その動作をアニメーション表示する、というものです。

ポイント

運動方程式：
[ M(\theta)\ddot{\theta} + C(\theta, \dot{\theta})\dot{\theta} + G(\theta) = \tau ]
- (M)：慣性行列
- (C)：コリオリ・遠心力項
- (G)：重力項
- (\tau)：関節トルク
数値積分：
オイラー法やRunge-Kutta法で角加速度→角速度→角度を時間発展
アニメーション：
毎ステップの関節角度からロボット形状を描画

サンプル：2自由度ロボットアーム（平面、各リンクに質量・慣性あり）

（3自由度も同じ考え方で拡張できますが、まず2自由度で示します）

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

# ロボットパラメータ
l1, l2 = 1.0, 1.0      # リンク長
m1, m2 = 1.0, 1.0      # 質量
I1, I2 = 0.02, 0.02    # 慣性モーメント
g = 9.81               # 重力加速度

def M_matrix(theta):
    c2 = np.cos(theta[1])
    M11 = I1 + I2 + m2 * l1**2 + 2 * m2 * l1 * l2 * c2
    M12 = I2 + m2 * l1 * l2 * c2
    M21 = M12
    M22 = I2
    return np.array([M11, M12], [M21, M22](/Shinichi0713/LLM-fundamental-study/wiki/M11,-M12],-[M21,-M22))

def C_matrix(theta, dtheta):
    s2 = np.sin(theta[1])
    C11 = -m2 * l1 * l2 * s2 * dtheta[1]
    C12 = -m2 * l1 * l2 * s2 * (dtheta[0] + dtheta[1])
    C21 = m2 * l1 * l2 * s2 * dtheta[0]
    C22 = 0
    return np.array([C11, C12], [C21, C22](/Shinichi0713/LLM-fundamental-study/wiki/C11,-C12],-[C21,-C22))

def G_vector(theta):
    g1 = (m1 * l1 / 2 + m2 * l1) * g * np.cos(theta[0]) + m2 * l2 / 2 * g * np.cos(theta[0] + theta[1])
    g2 = m2 * l2 / 2 * g * np.cos(theta[0] + theta[1])
    return np.array([g1, g2])

def forward_kinematics(theta):
    x0, y0 = 0, 0
    x1 = l1 * np.cos(theta[0])
    y1 = l1 * np.sin(theta[0])
    x2 = x1 + l2 * np.cos(theta[0] + theta[1])
    y2 = y1 + l2 * np.sin(theta[0] + theta[1])
    return np.array([x0, x1, x2]), np.array([y0, y1, y2])

# シミュレーション設定
dt = 0.02
steps = 200
theta = np.array([np.pi/4, np.pi/4])      # 初期角度
dtheta = np.array([0.0, 0.0])             # 初期角速度

# トルク設定（例：第2関節だけ一定トルク、他は0）
tau = np.zeros((steps, 2))
tau[:, 1] = 1.0  # 第2関節に一定トルク

# 各ステップの角度を記録
theta_hist = []
for t in range(steps):
    M = M_matrix(theta)
    C = C_matrix(theta, dtheta)
    G = G_vector(theta)
    ddtheta = np.linalg.inv(M) @ (tau[t] - C @ dtheta - G)
    dtheta += ddtheta * dt
    theta += dtheta * dt
    theta_hist.append(theta.copy())
theta_hist = np.array(theta_hist)

# アニメーション
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set_xlim(-2.2, 2.2)
ax.set_ylim(-2.2, 2.2)
line, = ax.plot([], [], 'o-', lw=4, color='blue')

def update(frame):
    x, y = forward_kinematics(theta_hist[frame])
    line.set_data(x, y)
    return line,

ani = FuncAnimation(fig, update, frames=steps, interval=30, blit=True)
plt.title('2D Robot Arm Dynamics Simulation')
plt.show()

説明

M_matrix, C_matrix, G_vector で運動方程式の各項を定義
tau で各関節のトルクを与える（例では第2関節に一定トルク）
オイラー法で数値積分し、各時刻の関節角度を計算
アニメーションでロボットの動作を可視化

拡張・カスタマイズ

トルクを時間変化させたり、制御則を入れることも可能です
3自由度や他のリンクパラメータも同様に拡張できます
各リンクごとに異なる質量・慣性もOK

承知しました。
2自由度平面ロボットアーム（二連リンク）の運動方程式（Euler-Lagrange法による）は、
以下のように表されます。

2自由度平面ロボットの運動方程式

[ M(\theta)\ddot{\theta} + C(\theta, \dot{\theta})\dot{\theta} + G(\theta) = \tau ]

ここで、

(\theta = \begin{bmatrix} \theta_1 \ \theta_2 \end{bmatrix})：各関節角度
(\dot{\theta} = \begin{bmatrix} \dot{\theta}_1 \ \dot{\theta}_2 \end{bmatrix})：各関節角速度
(\ddot{\theta} = \begin{bmatrix} \ddot{\theta}_1 \ \ddot{\theta}_2 \end{bmatrix})：各関節角加速度
(\tau = \begin{bmatrix} \tau_1 \ \tau_2 \end{bmatrix})：各関節トルク

慣性行列 (M(\theta))

[ M(\theta) = \begin{bmatrix} M_{11} & M_{12} \ M_{21} & M_{22} \end{bmatrix} ]

[ \begin{align*} M_{11} &= I_1 + I_2 + m_2 l_1^2 + 2 m_2 l_1 l_2 \cos\theta_2 \ M_{12} &= I_2 + m_2 l_1 l_2 \cos\theta_2 \ M_{21} &= M_{12} \ M_{22} &= I_2 \end{align*} ]

(l_1, l_2)：各リンク長
(m_1, m_2)：各リンク質量
(I_1, I_2)：各リンクの慣性モーメント（回転中心周り）

コリオリ・遠心力項 (C(\theta, \dot{\theta})\dot{\theta})

[ C(\theta, \dot{\theta})\dot{\theta} = \begin{bmatrix}

m_2 l_1 l_2 \sin\theta_2 (2\dot{\theta}_1\dot{\theta}_2 + \dot{\theta}_2^2) \ m_2 l_1 l_2 \sin\theta_2 \dot{\theta}_1^2 \end{bmatrix} ]

または、行列表記で [ C(\theta, \dot{\theta}) = \begin{bmatrix}

m_2 l_1 l_2 \sin\theta_2 \dot{\theta}_2 & - m_2 l_1 l_2 \sin\theta_2 (\dot{\theta}_1 + \dot{\theta}_2) \ m_2 l_1 l_2 \sin\theta_2 \dot{\theta}_1 & 0 \end{bmatrix} ]

重力項 (G(\theta))

[ G(\theta) = \begin{bmatrix} (m_1 l_1/2 + m_2 l_1)g \cos\theta_1 + m_2 l_2/2 , g \cos(\theta_1 + \theta_2) \ m_2 l_2/2 , g \cos(\theta_1 + \theta_2) \end{bmatrix} ]

(g)：重力加速度

全体の運動方程式

[ M(\theta)\ddot{\theta} + C(\theta, \dot{\theta})\dot{\theta} + G(\theta) = \tau ]

この式を使って、関節の角加速度（(\ddot{\theta})）を数値的に計算し、
角速度・角度へと時間積分していくことで、ロボットの運動をシミュレーションできます。

「キーボード入力でリアルタイムに各関節のトルクを操作できる2自由度ロボットアームの動力学アニメーション」

ポイント

キー操作でトルク値を増減できます（例：q/aで第1関節、w/sで第2関節）。
現在のトルク値を画面に表示します。
アニメーションはMatplotlibで行い、FuncAnimationのイベントハンドラでキーボード入力を受け付けます。

サンプルコード

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

# ロボットパラメータ
l1, l2 = 1.0, 1.0
m1, m2 = 1.0, 1.0
I1, I2 = 0.02, 0.02
g = 9.81

def M_matrix(theta):
    c2 = np.cos(theta[1])
    M11 = I1 + I2 + m2 * l1**2 + 2 * m2 * l1 * l2 * c2
    M12 = I2 + m2 * l1 * l2 * c2
    M21 = M12
    M22 = I2
    return np.array([M11, M12], [M21, M22](/Shinichi0713/LLM-fundamental-study/wiki/M11,-M12],-[M21,-M22))

def C_matrix(theta, dtheta):
    s2 = np.sin(theta[1])
    C11 = -m2 * l1 * l2 * s2 * dtheta[1]
    C12 = -m2 * l1 * l2 * s2 * (dtheta[0] + dtheta[1])
    C21 = m2 * l1 * l2 * s2 * dtheta[0]
    C22 = 0
    return np.array([C11, C12], [C21, C22](/Shinichi0713/LLM-fundamental-study/wiki/C11,-C12],-[C21,-C22))

def G_vector(theta):
    g1 = (m1 * l1 / 2 + m2 * l1) * g * np.cos(theta[0]) + m2 * l2 / 2 * g * np.cos(theta[0] + theta[1])
    g2 = m2 * l2 / 2 * g * np.cos(theta[0] + theta[1])
    return np.array([g1, g2])

def forward_kinematics(theta):
    x0, y0 = 0, 0
    x1 = l1 * np.cos(theta[0])
    y1 = l1 * np.sin(theta[0])
    x2 = x1 + l2 * np.cos(theta[0] + theta[1])
    y2 = y1 + l2 * np.sin(theta[0] + theta[1])
    return np.array([x0, x1, x2]), np.array([y0, y1, y2])

# シミュレーション初期値
dt = 0.02
theta = np.array([np.pi/4, np.pi/4])
dtheta = np.array([0.0, 0.0])
tau = np.array([0.0, 0.0])

# matplotlibアニメーションセットアップ
fig, ax = plt.subplots()
ax.set_aspect('equal')
ax.set_xlim(-2.2, 2.2)
ax.set_ylim(-2.2, 2.2)
line, = ax.plot([], [], 'o-', lw=4, color='blue')
txt = ax.text(-2, 2, '', fontsize=12, color='red')

# キーボードイベントハンドラ
def on_key(event):
    global tau
    step = 0.5
    if event.key == 'q':
        tau[0] += step
    elif event.key == 'a':
        tau[0] -= step
    elif event.key == 'w':
        tau[1] += step
    elif event.key == 's':
        tau[1] -= step
    elif event.key == 'z':  # リセット
        tau[:] = 0.0

fig.canvas.mpl_connect('key_press_event', on_key)

def update(frame):
    global theta, dtheta, tau
    # 動力学計算
    M = M_matrix(theta)
    C = C_matrix(theta, dtheta)
    G = G_vector(theta)
    ddtheta = np.linalg.inv(M) @ (tau - C @ dtheta - G)
    dtheta += ddtheta * dt
    theta += dtheta * dt
    # 描画
    x, y = forward_kinematics(theta)
    line.set_data(x, y)
    txt.set_text(f'tau1: {tau[0]:.2f}, tau2: {tau[1]:.2f}')
    return line, txt

ani = FuncAnimation(fig, update, frames=3000, interval=20, blit=True)
plt.title('2D Robot Arm Dynamics with Keyboard Torque Control\n[q/a]:tau1 +/-  [w/s]:tau2 +/-  [z]:reset')
plt.show()

操作方法

q：第1関節トルク増加
a：第1関節トルク減少
w：第2関節トルク増加
s：第2関節トルク減少
z：両トルクリセット