pythonで2リンクの逆運動学を解いて表示する
pythonで2リンク逆運動学を計算して2次元で表示します。
プログラム
・2リンク問題を逆運動学で算出してmatplotlibで表示しています
・Px,Pyが手先目標座標なので、使用するときは変更してください
・L1,L2のリンクの長さは直接指定しているので、使用するときは変更してください
・計算結果はprint文とグラフ上に表示されます
・エラー系は実装していません
import math import numpy import matplotlib.pyplot as plt fig = plt.figure() #アーム手先座標 Px = 6 Py = 7.5 #Linkの長さ L1 = 10 L2 = 10 #逆運動学計算↓ L3 = math.sqrt((Px*Px) + (Py*Py)) fai2 = math.acos(((L1*L1) + (L2*L2) - ( L3 * L3)) / (2*L1*L2)) tht2 = math.pi - fai2 fai1 = math.acos(((L1*L1) + (L3 * L3) - (L2*L2)) / (2*L1*L3)) fai0 = math.atan(Py / Px) tht1 = math.atan2(Py , Px) - fai1 #角度計算↓ #deg1 = math.degrees(tht1) #deg2 = math.degrees(tht2) #表示のために順運動学計算する #リンク1の座標算出 #数式↓ #x =L1cos(tht1) #y =L2sin(tht1) #プログラム↓ x1 = L1 * numpy.cos(tht1) y1 = L1 * numpy.sin(tht1) #リンク1の座標算出 #数式↓ #x2 =L1cos(tht1)+L2cos(tht1+th2) #y2 =L1sin(tht1)+L2sin(tht1+th2) #プログラム↓ x2 = x1 + L2 * numpy.cos((tht1+tht2)) y2 = y1 + L2 * numpy.sin((tht1+tht2)) #計算結果表示↓ 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()