witchの日記: (ROBO) X-2 脚のモーション作成式(その2)
日記 by
witch
1)式の左辺が θ5 になるように変換すると
L34 * sinθ3 = L45 * sinθ5
sinθ5 = sinθ3 * L34 / L45
θ5 = asin( sinθ3 * L34 / L45 )
これを 2)式に代入して
θ4 = θ3 + asin( sinθ3 * L34 / L45 )
Ruby で計算する場合 (Jx は θx の値を持つ変数とする)
J4 = J3 + Math.asin( Math.sin( J3 * Math::PI / 180 * L34 / L45 ) )
さらに
L35 = Math.hypot( L34 + L45 * Math.cos( J4 * Math::PI / 180 ),
L45 * Math.sin( J4 * Math::PI / 180 ) )
※) Math.hypot(x,y) は sqrt(x*x, y*y) を返すモジュール関数
ServoCreationのDXFデータからサイズを拾うと
L34 = 39.0 mm + 30.0 mm = 69.0 mm
L45 = 39.0 mm + 39.0 mm = 78.0 mm
参考:他のリンクの長さ
L12 = 0.0 mm (股ヨー、股ロール間は直交)
L23 = 60.0 mm
L56 = 30.0 mm + 14.9 mm = 44.9 mm
L67 = 30.0 mm + 1.2 mm + 3.8 mm = 35.0mm (足首から足裏まで)
次は、股ピッチ、股ロールを動かした場合の腰の高さの計算式作成
L34 * sinθ3 = L45 * sinθ5
sinθ5 = sinθ3 * L34 / L45
θ5 = asin( sinθ3 * L34 / L45 )
これを 2)式に代入して
θ4 = θ3 + asin( sinθ3 * L34 / L45 )
Ruby で計算する場合 (Jx は θx の値を持つ変数とする)
J4 = J3 + Math.asin( Math.sin( J3 * Math::PI / 180 * L34 / L45 ) )
さらに
L35 = Math.hypot( L34 + L45 * Math.cos( J4 * Math::PI / 180 ),
L45 * Math.sin( J4 * Math::PI / 180 ) )
※) Math.hypot(x,y) は sqrt(x*x, y*y) を返すモジュール関数
ServoCreationのDXFデータからサイズを拾うと
L34 = 39.0 mm + 30.0 mm = 69.0 mm
L45 = 39.0 mm + 39.0 mm = 78.0 mm
参考:他のリンクの長さ
L12 = 0.0 mm (股ヨー、股ロール間は直交)
L23 = 60.0 mm
L56 = 30.0 mm + 14.9 mm = 44.9 mm
L67 = 30.0 mm + 1.2 mm + 3.8 mm = 35.0mm (足首から足裏まで)
次は、股ピッチ、股ロールを動かした場合の腰の高さの計算式作成
(ROBO) X-2 脚のモーション作成式(その2) More ログイン