pythonで3リンクの逆運動学を解いて表示する
pythonで3リンク逆運動学を計算して2次元で表示します。
プログラム
・3リンク問題を逆運動学で算出してmatplotlibで表示しています
・Px,Pyが手先目標座標なので、使用するときは変更してください
・L1,L2,L3のリンクの長さは直接指定しているので、使用するときは変更してください
・tht3は直接指定しているので、使用するときは変更してください
・エラー系は実装していません
# -*- coding: utf-8 -*- import math import numpy import matplotlib.pyplot as plt fig = plt.figure() #アーム手先座標 Px = 15 Py = 20 #Linkの長さ L1 = 10 L2 = 10 L3 = 10 #逆運動学計算↓ #手先座標に対し # x =L1cos(tht1)+L2cos(tht1+th2)+L3cos(tht1+tht2+tht3) # y =L1sin(tht1)+L2sin(tht1+th2)+L3sin(tht1+tht2+tht3) # tht = tht1+tht2+tht3 #が成り立つ # L3の角度指定。(x軸とのなす角) tht = math.radians(90) #tht1の算出 A = Py - (L3*math.sin(tht)) B = Px - (L3*math.cos(tht)) C = (pow((Py - (L3*math.sin(tht))),2) + pow((Px - (L3*math.cos(tht))),2) + pow(L1,2) - pow(L2,2)) / (2*L1) tht1= math.atan2(A,B) - math.atan2(math.sqrt((pow(A,2)+pow(B,2)-pow(C,2))),C) #tht2の算出 D = (pow((Py - L3*math.sin(tht)),2) + pow((Px - L3*math.cos(tht)),2) - pow(L1,2) + pow(L2,2)) / (2*L2) tht2 = math.atan2(math.sqrt((pow(A,2)+pow(B,2)-pow(C,2))),C) - math.atan2(math.sqrt((pow(A,2)+pow(B,2)-pow(D,2)))*-1,D) #tht3の算出 tht3 = tht-tht2-tht1 #表示のために順運動学計算する #リンク１の座標算出 #数式↓ #x =L1cos(tht1) #y =L2sin(tht1) #プログラム↓ x1 = L1 * numpy.cos(tht1) y1 = L1 * numpy.sin(tht1) #リンク2の座標算出 #数式↓ #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))) #リンク3の座標算出 #数式↓ #x3 =L1cos(tht1)+L2cos(tht1+th2)+L3cos(tht1+tht2+tht3) #y3 =L1sin(tht1)+L2sin(tht1+th2)+L3sin(tht1+tht2+tht3) #プログラム↓ x3 = x2 + (L3 * numpy.cos((tht1+tht2+tht3))) y3 = y2 + (L3 * numpy.sin((tht1+tht2+tht3))) #算出結果を格納 #リンクは[x,y]=[0,0]から表示するため最初の要素に0を代入 x = [0, x1, x2, x3] y = [0, y1, y2, y3] #表示 #リンクの座標表示↓ 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.text(x3, y3, x3,verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8) plt.text(x3, y3-0.8, y3, verticalalignment='bottom', horizontalalignment='right',color='green', fontsize=8) #線分表示↓ plt.plot(x,y,"r-") #表示実行 plt.show()
