腕の逆運動計算(2007.8.27修正）

肩関節の計算


手先を基準として考える。

x,y,z は手先原点の数値となっている。
ただし、角度J1〜J4,rは肩原点となっているので注意。
rは肩原点と手先を結んだ線を軸とした角度となっている。

上腕の長さ：al1
下腕の長さ：al2

cos(J4) = (l^2-al1^2-al2^2) / (2*al1*al2)

l^2 = x^2+y^2+z^2

手先原点の時の、肩関節姿勢は

Rot(y,a)･Rot(y,-J4)･Rot(z,-J3)･Rot(y,-J2)･Rot(x,-J1)

sin(a) = al1 * sin(J4) / l;
角度aは仮想の角度。
手先が原点とし、肩座標が(0,0,l)(ただし、l = sqrt(x^2+y^2+z^2))にあるとした時の手先のY軸周りの角度。

一方肩関節姿勢は

Rot(z,-r)･Rot(y,-p0)･Rot(x,-t0)

                 x
tan(p0) = ----------------
           sqrt(y^2+z^2)

           -y
tan(t0) = ---
            z

t0,p0は手先を原点としたときの肩座標へのベクトル方向をRot(x,t0)･Rot(y,p0)で表した角度。

これより、

Rot(z,-J3)･Rot(y,-J2)･Rot(x,-J1) = Rot(y,-(a-J4))･Rot(z,-r)･Rot(y,-p0)･Rot(x,-t0)


Rot(z,-r)･Rot(y,-p0)･Rot(x,-t0)=

crcp0   srct0+crsp0st0  srst0-crsp0ct0
-srcp0  crct0-srsp0st0  crst0+srsp0ct0
sp0     -cp0st0         cp0ct0

=
a11     a12     a13
a21     a22     a23
a31     a32     a33
と置く。

Rot(y,-(a-J4))･Rot(z,-r)･Rot(y,-p0)･Rot(x,-t0)=

c4a11-s4a31     c4a12-s4a32     c4a13-s4a33
a21     a22     a23
s4a11+c4a31     s4a12+c4a32     s4a13+c4a33


Rot(z,-J3)･Rot(y,-J2)･Rot(x,-J1)=

c3c2    s3c1+c3s2s1     s3s1-c3s2c1
-s3c2   c3c1-s3s2s1     c3s1+s3s2c1
s2      -c2s1   	c2c1


             -c2s1         s4a12+c4a32
tan(J1) = - --------- = - ---------------
              c2c1         s4a13+c4a33

             -s3c2             a21
tan(J3) = - --------- = - --------------
              c3c2         c4a11-s4a31

                     s2                       s4a11+c4a31
tan(J2) = --------------------- = ------------------------------------
           -s1*-c2s1 + c1*c2c1     -s1(s4a12+c4a32) + c1(s4a13+c4a33)