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

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

スポンサーリンク

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

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

関連記事
詳細 2リンクマニピュレータの順運動学
スポンサーリンク

ソースコード

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

# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider
import math

# 順運動学の計算
def fk(L, th):
    # 各リンクの長さと関節角度の取得
    l1, l2 = L
    th1, th2 = th

    # リンク1の手先
    x1 = l1 * math.cos(th1)
    y1 = l1 * math.sin(th1)

    # リンク2の手先
    x2 = x1 + l2 * math.cos(th1 + th2)
    y2 = y1 + l2 * math.sin(th1 + th2)

    # 手先位置をNumPy配列に格納して返す
    return np.array([[0, 0], [x1, y1], [x2, y2]])


def main():
    # リンク1, 2の長さ
    L = [0.5, 0.5]

    # 第1, 2の関節角度
    th = np.radians([90, 0])

    # 順運動学の計算
    p = fk(L, th)

    # グラフ描画位置の設定
    fig, ax = plt.subplots()
    plt.axis('equal')
    plt.subplots_adjust(left=0.1, bottom=0.15)
    plt.xlim([-1, 1])
    plt.ylim([-0.3, 1.3])
    # グラフ描画
    plt.grid()
    graph, = plt.plot(p.T[0], p.T[1])

    def update_th1(slider_val):
        # 関節1の角度を更新
        th[0] = np.radians([slider_val])

        # 順運動学の計算
        p = fk(L, th)

        # 手先位置を更新
        graph.set_data(p.T[0], p.T[1])
        graph.set_linestyle('-')
        graph.set_linewidth(5)
        graph.set_marker('o')
        graph.set_markerfacecolor('g')
        graph.set_markeredgecolor('g')
        graph.set_markersize(15)

        # グラフの再描画
        fig.canvas.draw_idle()

    def update_th2(slider_val):
        # 関節2の角度を更新
        th[1] = np.radians([slider_val])

        # 順運動学の計算
        p = fk(L, th)

        # 手先位置を更新
        graph.set_data(p.T[0], p.T[1])
        graph.set_linestyle('-')
        graph.set_linewidth(5)
        graph.set_marker('o')
        graph.set_markerfacecolor('g')
        graph.set_markeredgecolor('g')
        graph.set_markersize(15)

        # グラフの再描画
        fig.canvas.draw_idle()

    # スライダーの表示位置
    slider1_pos = plt.axes([0.1, 0.05, 0.8, 0.03])
    slider2_pos = plt.axes([0.1, 0.01, 0.8, 0.03])

    # Sliderオブジェクトのインスタンス作成
    threshold_slider1 = Slider(slider1_pos, 'th1', 0, 180)
    threshold_slider2 = Slider(slider2_pos, 'th2', 0, 180)

    # スライダーの値が変更された場合の処理を呼び出し
    threshold_slider1.on_changed(update_th1)
    threshold_slider2.on_changed(update_th2)
    graph.set_linestyle('-')
    graph.set_linewidth(5)
    graph.set_marker('o')
    graph.set_markerfacecolor('g')
    graph.set_markeredgecolor('g')
    graph.set_markersize(15)
    plt.grid()
    plt.show()

if __name__ == '__main__':
    main()

サンプルプログラムの実行結果です。
2つのスライダーで関節角度1・2を制御できます。

【Python】ロボットシミュレーション
この記事では、Python言語を用いて、ロボットシミュレーションする方法をソースコード付きで解説します。
404 NOT FOUND | 西住工房(改)
【Python超入門】使い方とサンプル集
Pythonとは、統計処理や機械学習、ディープラーニングといった数値計算分野を中心に幅広い用途で利用されている人気なプログラミング言語です。 主な特徴として「効率のよい、短くて読みやすいコードを書きやすい」、「ライブラリが豊富なのでサクッと...
Python
スポンサーリンク

コメント