手臂逆向控制推導

此篇為使用python3給末端點座標，求轉軸的兩個角度

上一篇推導的補充

由上一篇可以得知手臂的逆向推導公式，可是存在一個問題，並沒有將方程式列出來，最後證明出的面積是由順向方程式的角度去限制角度將每個角度所會經過的路徑會畫出來。

今天從新將方程式從新推導，因為不確定自己推導是否正確，我們幾個人將工具備齊，利用順向法確認算出來的角度是否有包含於在面積裡面，使用Vrep確定轉軸的角度是否正確，前面幾次因為設錯角度，造成轉軸的角度都不是正確的轉軸，後來利用餘弦定理的方法，終於推導出方程式。

接下知道點座標後，就可以利用點得到正確的角度，轉軸轉向正確的角度夾取物件，也可以同時控制V-rep和實體的部分，是否要配合影像辨識取點座標，目前還在考慮當中，還未執行，可能先用Vrep模擬取下列印件的方式為優先，手臂的控制介面目前進度到可以同時在兩個頁面控制printer和手臂。

$$\theta_T = cos^{-1}(\frac{x^2+y^2-L1^2-L2^2}{2\times L1\times L2})$$

$$\theta_n = cos^{-1}(\frac{x}{\sqrt{x^2+y^2}}))$$

$$\theta_\alpha = cos^{-1}(\frac{x^2+y^2+L1^2-L2^2}{2\times L1\times \sqrt{x^2+y^2}})$$

大軸旋轉的角度

$$\theta_2 = 90 - (\theta_n + \theta_\alpha)$$

小軸旋轉的角度

$$\theta_3 = \theta_T - \theta_2$$

python 方程式推導

import math
from math import *

L1 = 135.0
L2 = 145.0

#X ,Y= 0,0

x =120.0
y = 10.0

theate = float((x*x+y*y-L1*L1-L2*L2)/(2*L1*L2))

print(theate)
rad = math.acos(theate)
print(rad)
tha = math.degrees(rad)
#tha為算出來的角度
print("算出來的",tha)

###
##test==================

#thn = arcos(x/(sqrt(x^2+y^2)))
#tha = (x^2+y^2+L1^2+L2^2)/(2(sqrt(x^2+y^2)*L1))

theatanr = math.acos(x/(math.sqrt(x*x+y*y)))
theatan = math.degrees(theatanr)
#theataN的角度
print("theatan角度",theatan)
theatalphar = math.acos((x*x+y*y+L1*L1-L2*L2)/(2*math.sqrt(x*x+y*y)*L1))

theatalpha = math.degrees(theatalphar)
#theatAlpha的角度
print("theatalpha角度",theatalpha)

#theata2 = theatan + theatAlpha
#theata2大軸轉的角度
test = theatan+theatalpha
theata2 = 90- test
print("theata2",theata2)
#theata3 = tha - theata2
#theata3小軸轉的角度
theata3 = tha - test
print("theata3",theata3)