pythonで2リンク順運動学を解いて表示する
pythonで2リンク順運動学を計算して2次元で表示します。
プログラム
・2リンク問題を順運動学で算出してmatplotlibで表示しています。
・L1,L2のリンクの長さは直接指定しているので使用するときは変更してください
・deg1,deg2はリンクの角度なので使用するときは変更してください
・計算結果はprint文とグラフ上に表示されます
・エラー系は実装していません
import math import numpy import matplotlib.pyplot as plt fig = plt.figure() #Linkの長さ L1 = 10 L2 = 10 #各リンクの角度指定 deg1 = 30 deg2 = 50 #リンク1の座標算出 #数式↓ #x =L1cos(tht1) #y =L2sin(tht1) #プログラム↓ x1 = L1 * numpy.cos(math.radians(deg1)) y1 = L1 * numpy.sin(math.radians(deg1)) #リンク1の座標算出 #数式↓ #x2 =L1cos(tht1)+L2cos(tht1+th2) #y2 =L1sin(tht1)+L2sin(tht1+th2) #プログラム↓ 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] #表示 #リンクの座標表示↓ plt.text(x1, y1, x1, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8) plt.text(x1, y1-0.8, y1,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8) plt.text(x2, y2, x2,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8) plt.text(x2, y2-0.8, y2, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8) #線分表示↓ plt.plot(x,y,"r-") #表示実行 plt.show()