Robotics: Simple Geometric Inverse Kinematics

จากรูป
แขนสองแขนมีความยาวเป็น          l₁, l₂
ตำแหน่งปลายทางที่ต้องการคือ     (x,y)
แขน l₁ ทำมุมกับแกน X ที่              $\mathrm{\gamma <!--\; \gamma \; -->}+\mathrm{\beta <!--\; \beta \; -->}$
แขน l₂ ทำมุมกับแขน l₁ ที่               $\mathrm{\theta <!--\; \theta \; -->}$

เป้าหมายในการใช้ inverse kinematics ในครั้งนี้คือ การสร้างระบบที่รับค่าตำแหน่ง (x,y) แล้วหาผลลัพธ์ที่เป็นค่าของมุม $\mathrm{\theta <!--\; \theta \; -->},\mathrm{\gamma <!--\; \gamma \; -->}+\mathrm{\beta <!--\; \beta \; -->}$ ออกมา

วิธีคิด

1. จาก Pythagorian theorem , $$h=\sqrt[2]{x^2+y^2}$$

2.  $$tan(\mathrm{\alpha <!--\; \alpha \; -->})=\frac{y}{x}$$
$$\mathrm{ \; \; \; \alpha <!--\; \alpha \; -->}=ta{n}^{-<!--\; -\; -->1}(\frac{y}{x})$$

3. จาก Law of cosine [4] , $$cos(\mathrm{\beta <!--\; \beta \; -->})=\frac{l_1^2+h^2-<!--\; -\; -->l_2^2}{2l_{1}h}$$
$$\mathrm{\beta <!--\; \beta \; -->}=co{s}^{-<!--\; -\; -->1}(\frac{l_1^2+h^2-<!--\; -\; -->l_2^2}{2l_{1}h})$$

$$cos(\mathrm{\theta <!--\; \theta \; -->})=\frac{l_1^2+l_2^2-<!--\; -\; -->h^2}{2l_1{l}_2}$$
$$\mathrm{\theta <!--\; \theta \; -->}=co{s}^{-<!--\; -\; -->1}(\frac{l_1^2+l_2^2-<!--\; -\; -->h^2}{2l_1{l}_2})$$
แทนค่า h ที่ได้จากข้อ 1 ก็จะได้ค่าของมุมที่ต้องการ

Python Script

import math

class MiSGIK():#stands for Mini Simple Geometry Inverse Kinematics :-)
 def __init__(self,l1,l2,unit='degrees'):
  self.l1 = l1
  self.l2 = l2
  self.unit=unit

 def _distance(self,target=(1,1)):
  # compute h value
  return math.sqrt(target[0]**2+target[1]**2)
 
 def _law_of_cosine(self,a,b,c):
  cos = (a**2+b**2-c**2)/(2*a*b)
  #cos must be in range of [-1,1]
  if cos < -1 : cos = -1
  if cos > 1 : cos = 1
  ang = math.acos(cos)
  return ang
 
 def solve(self,target=(1,1)):
  # parameters : x,y = coordinate of target, l1= lower arm length, l2 = upper arm lenght
  #to find theta1, theta2 for any given (x,y) position in term of A1,A2
  # where A1  = D1 + D2 , the angle of lower arm and A2 = angle of  upper arm
  # find distance between origin and target x,y
  dist = self._distance(target)
  alpha = math.atan2(target[1],target[0]) # in radian
  beta = self._law_of_cosine(dist,self.l1,self.l2)
  theta1 = alpha + beta
  theta2 = self._law_of_cosine(self.l1,self.l2,dist)
  if self.unit == 'degrees':
   ang_a1 = math.degrees(theta1)
   ang_a2 = math.degrees(theta2)  
   return (int(round(ang_a1)),int(round(ang_a2))) 
  else :
   return (theta1,theta2) 

#--- End of class ---
 
def test():
   myik = MiSGIK(l1=10,l2=10,unit='degrees')
   x = [1,2,3,4,5,6,7,8,9,10]
   y = [1,2,3,4,5,6,7,8,9,10]
   for i in range(len(x)):
       a1,a2 = myik.solve((x[i],y[i]))
       print("Target at [{0},{1}], Angles are {2} {3}".format(x[i],y[i],a1,180-a2))
 
if __name__ == "__main__":
 test()

---

เอกสารอ้างอิง
[1] https://ed.iitm.ac.in/~sandipan/files/serialkinematicsv2.pdf
[2] https://math.stackexchange.com/questions/1589549/geometric-solution-for-the-inverse-kinematics-problem-posted-this-in-robotics-b
[3] https://robotacademy.net.au/lesson/inverse-kinematics-for-a-2-joint-robot-arm-using-algebra/
[4] https://somchaisom.blogspot.com/2018/03/trigonometry-cosine.html