Return to robot CVS log

Up to [local] / OpenXM_contrib2 / asir2000 / lib

Annotation of OpenXM_contrib2/asir2000/lib/robot, Revision 1.1.1.1

1.1       noro        1: /* $OpenXM: OpenXM/src/asir99/lib/robot,v 1.1.1.1 1999/11/10 08:12:30 noro Exp $ */
                      2: def rot_x(T) {
                      3:        return newmat(3,3,[[1,0,0],[0,cos(T),-sin(T)],[0,sin(T),cos(T)]]);
                      4: }
                      5:
                      6: def rot_mx(T) {
                      7:        return newmat(3,3,[[1,0,0],[0,cos(T),sin(T)],[0,-sin(T),cos(T)]]);
                      8: }
                      9:
                     10: def rot_z(T) {
                     11:        return newmat(3,3,[[cos(T),-sin(T),0],[sin(T),cos(T),0],[0,0,1]]);
                     12: }
                     13:
                     14: E=rot_z(t1-r)*rot_mx(t2+t3)*rot_z(t4)*rot_mx(t5)*rot_z(t6-p)-rot_mx(t)$
                     15: F=newmat(3,3)$
                     16: for ( I = 0; I < 3; I++ )
                     17:        for ( J = 0; J < 3; J++ )
                     18:                F[I][J] = subst(E[I][J],
                     19:                        sin(t),st,cos(t),ct,
                     20:                        sin(r),sr,cos(r),cr,
                     21:                        sin(p),sp,cos(p),cp,
                     22:                        sin(-r+t1),srt1,cos(-r+t1),crt1,
                     23:                        sin(t2+t3),st23,cos(t2+t3),ct23,
                     24:                        sin(t4),st4,cos(t4),ct4,
                     25:                        sin(t5),st5,cos(t5),ct5,
                     26:                        sin(-p+t6),spt6,cos(-p+t6),cpt6
                     27:                );
                     28:
                     29: def mtol(F) {
                     30:        for ( L = [st^2+ct^2-1,
                     31:                srt1^2+crt1^2-1,st23^2+ct23^2-1,
                     32:                st4^2+ct4^2-1,st5^2+ct5^2-1,spt6^2+cpt6^2-1]
                     33:                , I = 0; I < 3; I++ )
                     34:                for ( J = 0; J < 3; J++ )
                     35:                        L = cons(F[I][J],L);
                     36:        return L;
                     37: }
                     38: L = mtol(F)$
                     39: V = [cpt6,spt6,ct5,st5,ct4,st4,ct23,st23,crt1,srt1,ct,st]$
                     40: Ord = 2;
                     41: end;