ラムダの足の順運動学計算
足首関節を原点と置く。

  m = Rot(y,j6)* Rot(x,j5)*(Rot(x,j4)*l1+l2);
  l1:大腿
  l2:脛
　大腿(l1)の先端が股関節。
  膝関節についた大腿(l1)がj4で曲がり、脛(l2)が足される。
　その後、j5,j6により股関節の座標が決まる。

m:
-s6s5s4l1+s6c5c4l1+s6c5l2       0       0
-c5s4l1-s5c4l1-s5l2     	0       0
-c6s5s4l1+c6c5c4l1+c6c5l2       0       0


x = -s6s5s4l1+s6c5c4l1+s6c5l2
y = -c5s4l1-s5c4l1-s5l2
z = -c6s5s4l1+c6c5c4l1+c6c5l2

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

mrpt=
crcp    -srct+crspst    srst+crspct
srcp    crct+srspst     -crst+srspct
-sp     cpst    cpct

m6543=
c6c3+s6s5c4s3+s6c5s4s3  -c6s3+s6s5c4c3+s6c5s4c3 -s6s5s4+s6c5c4
c5c4s3-s5s4s3   	c5c4c3-s5s4c3   	-c5s4-s5c4
-s6c3+c6s5c4s3+c6c5s4s3 s6s3+c6s5c4c3+c6c5s4c3  -c6s5s4+c6c5c4
= (m6543をannと置く)

a11     a12     a13
a21     a22     a23
a31     a32     a33


m654321=
a11c1+a12s2s1-a13c2s1   a12c2+a13s2     a11s1-a12s2c1+a13c2c1
a21c1+a22s2s1-a23c2s1   a22c2+a23s2     a21s1-a22s2c1+a23c2c1
a31c1+a32s2s1-a33c2s1   a32c2+a33s2     a31s1-a32s2c1+a33c2c1

mrpt=m654321 として、

tan(t) = cpst / cpct = a32c2+a33s2 / a31s1-a32s2c1+a33c2c1
tan(r) = srcp / crcp = a21c1+a22s2s1-a23c2s1 / a11c1+a12s2s1-a13c2s1
- sin(p) = -sp = a31c1+a32s2s1-a33c2s1

- tan(p) = st * -sp / cpst = st(a31c1+a32s2s1-a33c2s1) / a32c2+a33s2

tan()にした方が守備範囲が広くなるのでは。