【Python】2リンクマニピュレータの順運動学シミュレーション

この記事では、Python言語を用いて、2リンクマニピュレータ(2自由度アーム)の順運動学をシミュレーションする方法をソースコード付きで解説します。

2リンクアームの順運動学シミュレーション

Python言語を用いて、2リンクマニピュレータの順運動学を計算し、その姿勢をグラフ上に描いてみます。

詳細:2リンクマニピュレータの順運動学

今回は、それをPythonで計算してみました。

ソースコード

サンプルプログラムのソースコードです。

# -*- coding: utf-8 -*-
import numpy as np
from numpy import sin,cos
import matplotlib.pyplot as plt

# 並進行列(x軸方向に並進)
def L(l):
    Li = np.matrix([[ 1., 0., l ],
                    [ 0., 1., 0.],
                    [ 0., 0., 1.]])
    return Li

# 回転行列(z軸周りに回転)
def R(th):
    Ri = np.matrix([[cos(th), -sin(th), 0.],
                    [sin(th),  cos(th), 0.],
                    [0.,            0., 1.]])
    return Ri

# 順運動学の計算
def fk(l1, l2, th1, th2):
    # 原点座標(縦ベクトル)
    vec = np.matrix([[0.], [0.], [1.]])

    # 順運動学の計算
    (x1, y1, z1) = R(th1)*L(l1)*vec;                # 第1関節の位置
    (x2, y2, z2) = R(th1)*L(l1)*R(th2)*L(l2)*vec;   # 第2関節の位置
    
    return x1, y1, x2, y2


# グラフの描画
def plot(x, y):
    fig = plt.figure()
    ax = fig.add_subplot(111)
    # リンクの描画
    plt.plot(x, y,"-g",lw=5,label="link")
    # 関節のびょうg
    plt.plot(x, y,"or",lw=5, ms=10,label="joint")
    plt.xlim(-0.2,1.0)
    plt.ylim(-0.2,1.0)
    plt.grid()
    plt.show()


def main():
    # リンク1と2の長さ
    l1, l2 = 0.5, 0.5

    # 第1, 2の関節角度
    [th1, th2] = np.radians([30, 60])

    # 順運動学の計算
    x1, y1, x2, y2 = fk(l1, l2, th1, th2)

    # ロボットアームのx座標, y座標をリストに格納
    x = [0, x1, x2]
    y = [0, y1, y2]

    # ロボットアームの姿勢をグラフにプロット
    plot(x, y)


if __name__ == '__main__':
    main()
必要なライブラリ インストール方法
NumPy 【NumPy】インストール方法
Matplolib 【Matplolib】インストール方法

実行結果

サンプルプログラムの実行結果です。


リンク1、2の長さが0.5で第1関節角度を30[deg]、第2関節角度を60[deg]で計算しています。

おすすめ記事

Pythonでロボットシミュレーション
ロボット工学入門 基礎編
Python入門 サンプル集
NumPy入門 サンプル集

シェア&フォローお願いします!