Return to robot CVS log

Up to [local] / OpenXM_contrib2 / asir2000 / lib

Annotation of OpenXM_contrib2/asir2000/lib/robot, Revision 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;