Pythonでマニピュレータの逆運動学解を導出するプログラムを作成
現在,ロボット関連のプログラムを作成する際にPythonでの記述が必要となり,参考サイトのLuaで書かれたプログラムをPythonで作成する作業をしています.
発生している問題・エラーメッセージ
参考サイトInverse Kinematics for Animation
以下に逆運動学解を導出するLuaベースのプログラムを記述します.
Lua
1function chain:constrain(calc, line, cf) 2 local scalar = calc:Dot(line) / line.magnitude; 3 local proj = scalar * line.unit; 4 5 -- get axis that are closest 6 local ups = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Top)), cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Bottom))}; 7 local rights = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Right)), cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Left))}; 8 table.sort(ups, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end); 9 table.sort(rights, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end); 10 11 local upvec = ups[1]; 12 local rightvec = rights[1]; 13 14 -- get the vector from the projection to the calculated vector 15 local adjust = calc - proj; 16 if scalar < 0 then 17 -- if we're below the cone flip the projection vector 18 proj = -proj; 19 end; 20 21 -- get the 2D components 22 local xaspect = adjust:Dot(rightvec); 23 local yaspect = adjust:Dot(upvec); 24 25 -- get the cross section of the cone 26 local left = -(proj.magnitude * math.tan(self.left)); 27 local right = proj.magnitude * math.tan(self.right); 28 local up = proj.magnitude * math.tan(self.up); 29 local down = -(proj.magnitude * math.tan(self.down)); 30 31 -- find the quadrant 32 local xbound = xaspect >= 0 and right or left; 33 local ybound = yaspect >= 0 and up or down; 34 35 local f = calc; 36 -- check if in 2D point lies in the ellipse 37 local ellipse = xaspect^2/xbound^2 + yaspect^2/ybound^2; 38 local inbounds = ellipse <= 1 and scalar >= 0; 39 40 if not inbounds then 41 -- get the angle of our out of ellipse point 42 local a = math.atan2(yaspect, xaspect); 43 -- find nearest point 44 local x = xbound * math.cos(a); 45 local y = ybound * math.sin(a); 46 -- convert back to 3D 47 f = (proj + rightvec * x + upvec * y).unit * calc.magnitude; 48 end; 49 50 -- return our final vector 51 return f; 52end;
が,以下の記述部分で躓いています.
Lua
1local ups = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Top)), cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Bottom))}; 2local rights = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Right)), cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Left))}; 3table.sort(ups, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end); 4table.sort(rights, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end); 5 6local upvec = ups[1]; 7local rightvec = rights[1];
ローカル変数ups
,rights
にはそれぞれ2つのベクトルTop
とBottom
,Right
とLeft
が入ると思われすが,その後の記述
Lua
1table.sort(ups, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end); 2table.sort(rights, function(a, b) return (a - calc).magnitude < (b - calc).magnitude end);
にてどのように処理するかがわかりません.
説明元が英語であるため,Google翻訳等で訳を出しながら作業をしていますが,関連部分の記述もあまり理解できていない状況です.
We can then use vectorToWorldSpace on that chrame to get the up. down, left, right, etc.
そのcframe上でvectorToWorldSpaceを使用して,回転を基準とした上下左右のベクトルを取得できます.
とありますが,この部分はどのようにすればPythonに実装できますでしょうか?
試したこと
その他の記述部分は以下のようにしました.
Python
1import math 2import numpy as np 3 4class Constrain: 5 def __init__(self, calc, line, cf): 6 scalar = np.dot(calc, line) # 1. point O 7 proj = self.scalar * (line / np.linalg.norm(line)) # vector O 8 9 ### ここに問題部分が入る ### 10 11 adjust = calc - proj 12 13 if scalar < 0: 14 proj = -proj 15 16 # get the 2D components 17 xaspect = np.dot(adjust, rightvec) 18 yaspect = np.dot(adjust, upvec) 19 20 # qj = Stan(theta j) 21 left = -(np.linalg.norm(proj) * math.tan(self.left)) 22 right = np.linalg.norm(proj) * math.tan(self.right) 23 up = np.linalg.norm(proj) * math.tan(self.up) 24 down = -(np.linalg.norm(proj) * math.tan(self.down)) 25 26 # find quadrant 27 if xaspect >= 0: 28 xbound = right 29 else 30 xbound = left 31 32 if yaspect >= 0: 33 ybound = up 34 else 35 ybound = down 36 37 f = calc 38 39 # check if in 2D point lies in the ellipse 40 ellipse = xaspect**2/xbound**2 + yaspect**2/ybound**2 41 inbounds = False 42 if elipse <= 1 && scalar >= 0: 43 inbounds = True 44 45 if inbounds == False: 46 # get the angle of our out of ellipse point 47 a = math.atan2(yaspect, xaspect) 48 # find nearest point 49 x = xbound * math.cos(a) 50 y = ybound * math.sin(a) 51 # convert back to 3D 52 f = ()(proj + rightvec * x + upvec * y) / np.linalg.norm(proj + rightvec * x + upvec * y)) * np.linalg.norm(calc) 53 54 return f
問題部分以外の記述は以上の通りになりますが,まだ試していない状況です.
Numpy
とmath
をインポートして計算をできるようにしました.
バッドをするには、ログインかつ
こちらの条件を満たす必要があります。