Return to robot CVS log

Up to [local] / OpenXM_contrib2 / asir2018 / lib

Annotation of OpenXM_contrib2/asir2018/lib/robot, Revision 1.1

1.1     ! noro        1: /*
        !             2:  * Copyright (c) 1994-2000 FUJITSU LABORATORIES LIMITED
        !             3:  * All rights reserved.
        !             4:  *
        !             5:  * FUJITSU LABORATORIES LIMITED ("FLL") hereby grants you a limited,
        !             6:  * non-exclusive and royalty-free license to use, copy, modify and
        !             7:  * redistribute, solely for non-commercial and non-profit purposes, the
        !             8:  * computer program, "Risa/Asir" ("SOFTWARE"), subject to the terms and
        !             9:  * conditions of this Agreement. For the avoidance of doubt, you acquire
        !            10:  * only a limited right to use the SOFTWARE hereunder, and FLL or any
        !            11:  * third party developer retains all rights, including but not limited to
        !            12:  * copyrights, in and to the SOFTWARE.
        !            13:  *
        !            14:  * (1) FLL does not grant you a license in any way for commercial
        !            15:  * purposes. You may use the SOFTWARE only for non-commercial and
        !            16:  * non-profit purposes only, such as academic, research and internal
        !            17:  * business use.
        !            18:  * (2) The SOFTWARE is protected by the Copyright Law of Japan and
        !            19:  * international copyright treaties. If you make copies of the SOFTWARE,
        !            20:  * with or without modification, as permitted hereunder, you shall affix
        !            21:  * to all such copies of the SOFTWARE the above copyright notice.
        !            22:  * (3) An explicit reference to this SOFTWARE and its copyright owner
        !            23:  * shall be made on your publication or presentation in any form of the
        !            24:  * results obtained by use of the SOFTWARE.
        !            25:  * (4) In the event that you modify the SOFTWARE, you shall notify FLL by
        !            26:  * e-mail at risa-admin@sec.flab.fujitsu.co.jp of the detailed specification
        !            27:  * for such modification or the source code of the modified part of the
        !            28:  * SOFTWARE.
        !            29:  *
        !            30:  * THE SOFTWARE IS PROVIDED AS IS WITHOUT ANY WARRANTY OF ANY KIND. FLL
        !            31:  * MAKES ABSOLUTELY NO WARRANTIES, EXPRESSED, IMPLIED OR STATUTORY, AND
        !            32:  * EXPRESSLY DISCLAIMS ANY IMPLIED WARRANTY OF MERCHANTABILITY, FITNESS
        !            33:  * FOR A PARTICULAR PURPOSE OR NONINFRINGEMENT OF THIRD PARTIES'
        !            34:  * RIGHTS. NO FLL DEALER, AGENT, EMPLOYEES IS AUTHORIZED TO MAKE ANY
        !            35:  * MODIFICATIONS, EXTENSIONS, OR ADDITIONS TO THIS WARRANTY.
        !            36:  * UNDER NO CIRCUMSTANCES AND UNDER NO LEGAL THEORY, TORT, CONTRACT,
        !            37:  * OR OTHERWISE, SHALL FLL BE LIABLE TO YOU OR ANY OTHER PERSON FOR ANY
        !            38:  * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, PUNITIVE OR CONSEQUENTIAL
        !            39:  * DAMAGES OF ANY CHARACTER, INCLUDING, WITHOUT LIMITATION, DAMAGES
        !            40:  * ARISING OUT OF OR RELATING TO THE SOFTWARE OR THIS AGREEMENT, DAMAGES
        !            41:  * FOR LOSS OF GOODWILL, WORK STOPPAGE, OR LOSS OF DATA, OR FOR ANY
        !            42:  * DAMAGES, EVEN IF FLL SHALL HAVE BEEN INFORMED OF THE POSSIBILITY OF
        !            43:  * SUCH DAMAGES, OR FOR ANY CLAIM BY ANY OTHER PARTY. EVEN IF A PART
        !            44:  * OF THE SOFTWARE HAS BEEN DEVELOPED BY A THIRD PARTY, THE THIRD PARTY
        !            45:  * DEVELOPER SHALL HAVE NO LIABILITY IN CONNECTION WITH THE USE,
        !            46:  * PERFORMANCE OR NON-PERFORMANCE OF THE SOFTWARE.
        !            47:  *
        !            48:  * $OpenXM$
        !            49: */
        !            50: def rot_x(T) {
        !            51:        return newmat(3,3,[[1,0,0],[0,cos(T),-sin(T)],[0,sin(T),cos(T)]]);
        !            52: }
        !            53:
        !            54: def rot_mx(T) {
        !            55:        return newmat(3,3,[[1,0,0],[0,cos(T),sin(T)],[0,-sin(T),cos(T)]]);
        !            56: }
        !            57:
        !            58: def rot_z(T) {
        !            59:        return newmat(3,3,[[cos(T),-sin(T),0],[sin(T),cos(T),0],[0,0,1]]);
        !            60: }
        !            61:
        !            62: E=rot_z(t1-r)*rot_mx(t2+t3)*rot_z(t4)*rot_mx(t5)*rot_z(t6-p)-rot_mx(t)$
        !            63: F=newmat(3,3)$
        !            64: for ( I = 0; I < 3; I++ )
        !            65:        for ( J = 0; J < 3; J++ )
        !            66:                F[I][J] = subst(E[I][J],
        !            67:                        sin(t),st,cos(t),ct,
        !            68:                        sin(r),sr,cos(r),cr,
        !            69:                        sin(p),sp,cos(p),cp,
        !            70:                        sin(-r+t1),srt1,cos(-r+t1),crt1,
        !            71:                        sin(t2+t3),st23,cos(t2+t3),ct23,
        !            72:                        sin(t4),st4,cos(t4),ct4,
        !            73:                        sin(t5),st5,cos(t5),ct5,
        !            74:                        sin(-p+t6),spt6,cos(-p+t6),cpt6
        !            75:                );
        !            76:
        !            77: def mtol(F) {
        !            78:        for ( L = [st^2+ct^2-1,
        !            79:                srt1^2+crt1^2-1,st23^2+ct23^2-1,
        !            80:                st4^2+ct4^2-1,st5^2+ct5^2-1,spt6^2+cpt6^2-1]
        !            81:                , I = 0; I < 3; I++ )
        !            82:                for ( J = 0; J < 3; J++ )
        !            83:                        L = cons(F[I][J],L);
        !            84:        return L;
        !            85: }
        !            86: L = mtol(F)$
        !            87: V = [cpt6,spt6,ct5,st5,ct4,st4,ct23,st23,crt1,srt1,ct,st]$
        !            88: Ord = 2;
        !            89: end;