ラムダの足の順運動学計算
股関節を原点と置く。
胴体姿勢を設定する。

  m = Rot(y,-j1)* Rot(x,-j2)*Rot(z,-j3)*(Rot(x,-j4)*l2+l1);
  l1:大腿(< 0 股関節が原点なので、l1,l2はﾏｲﾅｽの値をとる。)
  l2:脛(< 0)
　脛(l2)の先端が足首関節。
  膝関節についた脛(l2)が-j4で曲がり、大腿(l1)が足される。
　その後、j1,j2,j3により足首関節の座標が決まる。
　※j1〜j6は足首関節を原点とした角度になっているので股関節を原点とする場合は
　符号を反転させて与えなければならないため、-j1などと表記している。


mxyz =
-c1s3s4l2-s1s2c3s4l2+s1c2c4l2+s1c2l1    0       0
-c2c3s4l2-s2c4l2-s2l1   		0       0
-s1s3s4l2+c1s2c3s4l2-c1c2c4l2-c1c2l1    0       0

x = -c1s3s4l2-s1s2c3s4l2+s1c2c4l2+s1c2l1
y = -c2c3s4l2-s2c4l2-s2l1
z = -s1s3s4l2+c1s2c3s4l2-c1c2c4l2-c1c2l1

mxyztp = Rot(y,bp) * Rot(x,bt) * mxyz

x_tp = cpx+spsty+spctz
y_tp = cty-stz
z_tp = -spx+cpsty+cpctz

足首関節の姿勢を算出する。
Rot(x,-bt)*Rot(y,-bp)*Rot(z,r)*Rot(x,t)*Rot(y,p) = Rot(y,-j1)*Rot(x,-j2)*Rot(z,-j3)*Rot(x,-j4)*Rot(x,-j5)*Rot(y,-j6)
↓
Rot(y,bp)*Rot(x,bt)*Rot(x,-bt)*Rot(y,-bp)*Rot(z,r)*Rot(x,t)*Rot(y,p) = Rot(y,bp)*Rot(x,bt)*Rot(y,-j1)*Rot(x,-j2)*Rot(z,-j3)*Rot(x,-j4)*Rot(x,-j5)*Rot(y,-j6)
↓
Rot(z,r)*Rot(x,t)*Rot(y,p) = Rot(y,bp)*Rot(x,bt)*Rot(y,-j1)*Rot(x,-j2)*Rot(z,-j3)*Rot(x,-j4)*Rot(x,-j5)*Rot(y,-j6)


mrtp =
crcp-srstsp     -srct   crsp+srstcp
srcp+crstsp     crct    srsp-crstcp
-ctsp   	st      ctcp

mpt1234 =
[cpc1c3+spcts1c3-spstc2s3-cps1s2s3+spctc1s2s3]  	[cpc1s3c4+spcts1s3c4+spstc2c3c4+cps1s2c3c4-spctc1s2c3c4-spsts2s4+cps1c2s4-spctc1c2s4]   [cpc1s3s4+spcts1s3s4+spstc2c3s4+cps1s2c3s4-spctc1s2c3s4+spsts2c4-cps1c2c4+spctc1c2c4]
[-sts1c3-ctc2s3-stc1s2s3]       			[-sts1s3c4+ctc2c3c4+stc1s2c3c4-cts2s4+stc1c2s4]        					[-sts1s3s4+ctc2c3s4+stc1s2c3s4+cts2c4-stc1c2c4]
[-spc1c3+cpcts1c3-cpstc2s3+sps1s2s3+cpctc1s2s3]         [-spc1s3c4+cpcts1s3c4+cpstc2c3c4-sps1s2c3c4-cpctc1s2c3c4-cpsts2s4-sps1c2s4-cpctc1c2s4]  [-spc1s3s4+cpcts1s3s4+cpstc2c3s4-sps1s2c3s4-cpctc1s2c3s4+cpsts2c4+sps1c2c4+cpctc1c2c4]


= (mpt1234をannと置く)
a11     a12     a13
a21     a22     a23
a31     a32     a33

mpt123456 = ma56 =
a11c6+a12s5s6+a13c5s6   a12c5-a13s5     -a11s6+a12s5c6+a13c5c6
a21c6+a22s5s6+a23c5s6   a22c5-a23s5     -a21s6+a22s5c6+a23c5c6
a31c6+a32s5s6+a33c5s6   a32c5-a33s5     -a31s6+a32s5c6+a33c5c6

mrtp=m123456 として、

-tan(r) = -srct / crct = a12c5-a13s5 / a22c5-a23s5
-tan(p) = -ctsp / ctcp = a31c6+a32s5s6+a33c5s6 / -a31s6+a32s5c6+a33c5c6
sin(t) = st = a32c5-a33s5
tan(t) = cp * st / ctcp = cp(a32c5-a33s5) / -a31s6+a32s5c6+a33c5c6

t =0 , p = 0　の時のj5,j6を求める。

sin(t) = 0 = a32c5-a33s5

a32c5 = a33s5

s5 / c5 = a32 / a33 = tan(j5_0)

tan(p) = 0 = a31c6+a32s5s6+a33c5s6 / -a31s6+a32s5c6+a33c5c6

a31c6+a32s5s6+a33c5s6 = 0

j5_0を代入して、

a31c6+a32s5_0s6+a33c5_0s6 = a31c6+(a32s5_0+a33c5_0)s6 = 0

s6 / c6 = -a31 / (a32s5_0+a33c5_0) = tan(j6_0)

fp = j6_0 - j6
ft = j5_0 - j5

rは上の式にj5_0を代入して求める。