ラムダの足の順運動学計算 股関節を原点と置く。 m = Rot(y,-j1)* Rot(x,-j2)*Rot(x,-j3)*(Rot(x,-j4)*l2+l1); l1:大腿(< 0 股関節が原点なので、l1,l2はマイナスの値をとる。) l2:脛(< 0)  脛(l2)の先端が足首関節。 膝関節についた脛(l2)が-j4で曲がり、大腿(l1)が足される。  その後、j1,j2,j3により足首関節の座標が決まる。  ※j1〜j6は足首関節を原点とした角度になっているので股関節を原点とする場合は  符号を反転させて与えなければならないため、-j1などと表記している。 m: -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 足首関節の姿勢を算出する。 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) mrtp= crcp-srstsp -srct crsp+srstcp srcp+crstsp crct srsp-crstcp -ctsp st ctcp m1234= c1c3-s1s2s3 c1s3c4+s1s2c3c4+s1c2s4 c1s3s4+s1s2c3s4-s1c2c4 -c2s3 c2c3c4-s2s4 c2c3s4+s2c4 s1c3+c1s2s3 s1s3c4-c1s2c3c4-c1c2s4 s1s3s4-c1s2c3s4+c1c2c4 = (m1234をannと置く) a11 a12 a13 a21 a22 a23 a31 a32 a33 m123456 = 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