まじかる☆マリオネット資料室 - dev/memo Diff

!MSPインタプリタの仕様
*[[20070119時点|http://marionette.mtlab.jp/diary/2007.01-03.html#20070119-1]]
**インタプリタコマンドのXが「サーボPWM反映」なのは間違いだと思う

![[持っていくものリスト|dev/memo/持っていくものリスト]]
!C言語関連
*http://www.nurs.or.jp/~sug/soft/super/
!PIC関連
*[[時間とステップ数の対応表]]
!L-Card関連
*[[L-Card:GPIOドライバの注意点(20050804-1)|http://marionette.witchstyle.jp/diary/?200508#20050804-1]]
**/dev/led[0-9]は負論理なので注意(echo 0 > /dev/led で点灯、echo 1 > で消灯)
*[[L-Card:GPIO関連のコネクタピン配置(20050807-2)|http://marionette.witchstyle.jp/diary/?200508#20050807-2]]
!無線関連
*[[RS232-STR(野村エンジニアリングの無線モジュール)の可変長パケット通信]]
!運動学まわり
*基礎
**Radian(ラジアン角) vs Degree (度)
*** X(Rad) = X(Deg) * PI / 180
**三角関数の定理
***[[Wikipediaの「三角関数」|http://ja.wikipedia.org/wiki/%E4%B8%89%E8%A7%92%E9%96%A2%E6%95%B0]]参照



**[[脚のピッチ軸の関係式(20040226の日記)|http://marionette.witchstyle.jp/diary/?200402#20040226]]
***(Rubyの式は最後のRad/Deg変換が抜けているので注意)

{{attach_view(leg_angle.png)}}

図から
θ4 = θ3 + θ5
L34 * sinθ3 = L45 * sinθ5
(θ4の回転方向が逆だが符号は気にしないことにする)

式1)
∴ θ4 = θ3 + asin( sin(θ3) * L34 / L45 )

また、L34を延長し、∠J3,J4',J5が直角をなす点 J4'を仮定すると
L35^2 = L34'^2 + L4'5^2
L44' = L45 * cosθ4
L4'5 = L45 * sinθ4

式2)
∴  L35^2 = ( L34 + L45 * cosθ4 )^2 + ( L45 * sinθ4 )^2

これをJ4に対する式に変換する

右辺第１項を展開すると
L35^2 = L34^2 + 2 * (L34 * L45 * cosθ4) + (L45 * cosθ4)^2 +  (L45 * sinθ4)^2
       = L34^2 + 2 * (L34 * L45 * cosθ4) + L45^2  ∵ cosθ^2 + sinθ^2 = 1

左辺がθ4 になるように変換すると
2*(L34*L45*cosθ4) = L35^2 - L34^2 - L45^2
             cosθ4 = ( L35^2 - L34^2 - L45^2 )/(2 * L34 * L45)
式3)
                θ4 = acos ( ( L35^2 - L34^2 - L45^2 )/( 2 * L34 * L45) )

さらにθ4の値からθ3を求めるため
θ4 = θ3 + asin(sinθ3 * L34/L45)
をθ3を左辺とした式に変換する。
  
sin(θ4-θ3) = sinθ3 * L34/L45
加法定理より
( sinθ4 * cosθ3 - cosθ4 * sinθ3 ) = sinθ3 * L34/L45
両辺をsinθ3で割ると
  sinθ4/tanθ3 - cosθ4 = L34/L45
           sinθ4/tanθ3 = L34/L45 + cosθ4
                  tanθ3 = sinθ4/(L34/L45 + cosθ4)
式4)
                     θ3 = atan( sinθ4 / (L34/L45 + cosθ4) )


Ruby で計算する場合 (Jx は θx のDegree値を持つ変数とする)

式1')
J4 = J3 + Math.asin(Math.sin(J3 * Math::PI/180) * L34/L45 ) / Math::PI*180

式2')
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) を返すモジュール関数

式3')
J4 = Math.acos( ( L35**2 - L34**2 - L45**2 ) / ( 2 * L34 * L45) ) / Math::PI*180

式4')
J3  = Math.atan( Math.sin(J4 * Math::PI/180) / (L34/L45 + Math.cos(J4 * Math::PI/180)) ) / Math::PI*180

ServoCreationのDXFデータからサイズを拾うと
L34 = 39.0 mm + 30.0 mm = 69.0 mm
L45 = 39.0 mm + 39.0 mm = 78.0 mm

腰と足首を同一平面上にする J3,J4,J5 を求める式

J5 = Math.asin(Math.sin(J3 * Math::PI/180) * L34/L45 ) / Math::PI*180
J4 = J3 + J5

L35が与えられたとき、J3,J4,J5 を求める式
J4  = Math.acos( ( L35**2 - 10845.0 ) / 10764.0 ) / Math::PI*180
J3  = Math.atan( Math.sin(J4 * Math::PI/180) / (L34/L45 + Math.cos(J4 * Math::PI/180) )


**行列演算ソフト
***[[20040804の日記|http://marionette.witchstyle.jp/diary/?200408#20040804]] と cyber205さんのコメント
*** もっと最近にもあった気がする。

**[[DH法による順・逆運動学|http://www02.so-net.ne.jp/~ogawa_t/dh/dh.html]]

-----
{{attach_form}}
{{attach_map}}