順運動学について興味を持ったので2軸ではありますが，先行して実装した方を参考にして実装をしてみました(パクったともいう)．

スライダーで腕の角度を変えたいなと思ったのでそこの部分を追加した感じとなります．今後は，以前に線分同士の交差判定を行ったので，これをもとに衝突検知もしたいと思います．

余談ですが，colabで上げてやりたかったのですがcolabは基本的にgui機能を封じているので上げませんでした．Foamの機能を使ってもよかったのかもしれませんが，matplotlibのアニメーションを動的に生成することができなさそうだと判断したので断念しました．(実装よりもcolab関連の方が時間使ったことは内緒)
以下実装です．

arm.py

import math
import numpy
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider


fig, ax = plt.subplots(figsize=(8,8))
plt.subplots_adjust(left=0.25, bottom=0.25)
plt.xlim(-20,20)
plt.ylim(-20,20)
ax.axis('equal')
ax.set_xlim(-11,11)
ax.set_ylim(-11,11)

#Linkの長さ 
L1 = 5
L2 = 5

#各リンクの角度指定
deg1 = 30
deg2 = 50

theta1 = Slider(plt.axes([0.25,0.0,0.65,0.03]), 'theta1', -180, 180, valinit=deg1)
theta2 = Slider(plt.axes([0.25,0.05,0.65,0.03]), 'theta2', -180, 180, valinit=deg2)

#リンク１の座標算出
x1 = L1 * numpy.cos(math.radians(deg1))
y1 = L1 * numpy.sin(math.radians(deg1))

#リンク2の座標算出
x2 = x1 + L2 * numpy.cos(math.radians(deg1+deg2))
y2 = y1 + L2 * numpy.sin(math.radians(deg1+deg2))

#計算結果表示↓
print(x1)
print(x2)
print(y1)
print(y2)


#算出結果を格納
#リンクは[x,y]=[0,0]から表示するため最初の要素に0を代入
x = [0, x1, x2]
y = [0, y1, y2]

#表示
#線分表示↓
plots, = ax.plot(x,y,"r-")
ax.plot([0,3],[0,3])

def update(val):
    deg1 = theta1.val
    deg2 = theta2.val
    x1 = L1 * numpy.cos(math.radians(deg1))
    y1 = L1 * numpy.sin(math.radians(deg1))
    x2 = x1 + L2 * numpy.cos(math.radians(deg1+deg2))
    y2 = y1 + L2 * numpy.sin(math.radians(deg1+deg2))
    x = [0, x1, x2]
    y = [0, y1, y2]
    ax.axis('equal')
    ax.set_xlim(-11,11)
    ax.set_ylim(-11,11)
    plots.set_data(x,y)
    fig.canvas.draw_idle()

theta1.on_changed(update)
theta2.on_changed(update)


#表示実行
plt.show()

pythonで2軸アームの順運動学

参考