/*
 * 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;