/*
* Copyright (c) 1994-2000 FUJITSU LABORATORIES LIMITED
* All rights reserved.
*
* FUJITSU LABORATORIES LIMITED ("FLL") hereby grants you a limited,
* non-exclusive and royalty-free license to use, copy, modify and
* redistribute, solely for non-commercial and non-profit purposes, the
* computer program, "Risa/Asir" ("SOFTWARE"), subject to the terms and
* conditions of this Agreement. For the avoidance of doubt, you acquire
* only a limited right to use the SOFTWARE hereunder, and FLL or any
* third party developer retains all rights, including but not limited to
* copyrights, in and to the SOFTWARE.
*
* (1) FLL does not grant you a license in any way for commercial
* purposes. You may use the SOFTWARE only for non-commercial and
* non-profit purposes only, such as academic, research and internal
* business use.
* (2) The SOFTWARE is protected by the Copyright Law of Japan and
* international copyright treaties. If you make copies of the SOFTWARE,
* with or without modification, as permitted hereunder, you shall affix
* to all such copies of the SOFTWARE the above copyright notice.
* (3) An explicit reference to this SOFTWARE and its copyright owner
* shall be made on your publication or presentation in any form of the
* results obtained by use of the SOFTWARE.
* (4) In the event that you modify the SOFTWARE, you shall notify FLL by
* e-mail at risa-admin@sec.flab.fujitsu.co.jp of the detailed specification
* for such modification or the source code of the modified part of the
* SOFTWARE.
*
* THE SOFTWARE IS PROVIDED AS IS WITHOUT ANY WARRANTY OF ANY KIND. FLL
* MAKES ABSOLUTELY NO WARRANTIES, EXPRESSED, IMPLIED OR STATUTORY, AND
* EXPRESSLY DISCLAIMS ANY IMPLIED WARRANTY OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE OR NONINFRINGEMENT OF THIRD PARTIES'
* RIGHTS. NO FLL DEALER, AGENT, EMPLOYEES IS AUTHORIZED TO MAKE ANY
* MODIFICATIONS, EXTENSIONS, OR ADDITIONS TO THIS WARRANTY.
* UNDER NO CIRCUMSTANCES AND UNDER NO LEGAL THEORY, TORT, CONTRACT,
* OR OTHERWISE, SHALL FLL BE LIABLE TO YOU OR ANY OTHER PERSON FOR ANY
* DIRECT, INDIRECT, SPECIAL, INCIDENTAL, PUNITIVE OR CONSEQUENTIAL
* DAMAGES OF ANY CHARACTER, INCLUDING, WITHOUT LIMITATION, DAMAGES
* ARISING OUT OF OR RELATING TO THE SOFTWARE OR THIS AGREEMENT, DAMAGES
* FOR LOSS OF GOODWILL, WORK STOPPAGE, OR LOSS OF DATA, OR FOR ANY
* DAMAGES, EVEN IF FLL SHALL HAVE BEEN INFORMED OF THE POSSIBILITY OF
* SUCH DAMAGES, OR FOR ANY CLAIM BY ANY OTHER PARTY. EVEN IF A PART
* OF THE SOFTWARE HAS BEEN DEVELOPED BY A THIRD PARTY, THE THIRD PARTY
* DEVELOPER SHALL HAVE NO LIABILITY IN CONNECTION WITH THE USE,
* PERFORMANCE OR NON-PERFORMANCE OF THE SOFTWARE.
*
* $OpenXM: OpenXM_contrib2/asir2018/lib/robot,v 1.1 2018/09/19 05:45:08 noro Exp $
*/
def rot_x(T) {
return newmat(3,3,[[1,0,0],[0,cos(T),-sin(T)],[0,sin(T),cos(T)]]);
}
def rot_mx(T) {
return newmat(3,3,[[1,0,0],[0,cos(T),sin(T)],[0,-sin(T),cos(T)]]);
}
def rot_z(T) {
return newmat(3,3,[[cos(T),-sin(T),0],[sin(T),cos(T),0],[0,0,1]]);
}
E=rot_z(t1-r)*rot_mx(t2+t3)*rot_z(t4)*rot_mx(t5)*rot_z(t6-p)-rot_mx(t)$
F=newmat(3,3)$
for ( I = 0; I < 3; I++ )
for ( J = 0; J < 3; J++ )
F[I][J] = subst(E[I][J],
sin(t),st,cos(t),ct,
sin(r),sr,cos(r),cr,
sin(p),sp,cos(p),cp,
sin(-r+t1),srt1,cos(-r+t1),crt1,
sin(t2+t3),st23,cos(t2+t3),ct23,
sin(t4),st4,cos(t4),ct4,
sin(t5),st5,cos(t5),ct5,
sin(-p+t6),spt6,cos(-p+t6),cpt6
);
def mtol(F) {
for ( L = [st^2+ct^2-1,
srt1^2+crt1^2-1,st23^2+ct23^2-1,
st4^2+ct4^2-1,st5^2+ct5^2-1,spt6^2+cpt6^2-1]
, I = 0; I < 3; I++ )
for ( J = 0; J < 3; J++ )
L = cons(F[I][J],L);
return L;
}
L = mtol(F)$
V = [cpt6,spt6,ct5,st5,ct4,st4,ct23,st23,crt1,srt1,ct,st]$
Ord = 2;
end;