pythonで2リンクの逆運動学を解いて表示する

pythonで2リンク逆運動学を計算して2次元で表示します。

f:id:weekendproject9:20171117013028p:plain

環境

win7 64bit
python 3.5.2
matplotlib 1.5.3

プログラム

・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()