% (********************************************)
% (** symoro+ : symbolic modelling of robots **)
% (**========================================**)
% (**      irccyn-ecn - 1, rue de la noe     **)
% (**      b.p.92101                         **)
% (**      44321 nantes cedex 3, france      **)
% (**      www.irccyn.ec-nantes.fr           **)
% (********************************************)


%    name of file : e:/mes_documents_boulot/boulot/recherche/travaux/recherche_personnelle/identification_dynamique/dycaro_orthoglide/matlab/modeles_pr_simulation/orthoglide.inm




%      geometric parameters   


% j       ant     mu      sigma   gamma   b       alpha   d       theta   r


% 1       0       1       1       0       0       0       0       0       q1

%                                 pi              pi
% 2       0       1       1       --      a       --      0       0       -a + q2
%                                 2               2
%                                                 -pi             -pi
% 3       0       1       1       0       a       ---     0       ---     -a + q3
%                                                  2               2
%                                                 -pi
% 4       1       1       0       0       0       ---     0       q4      0
%                                                  2
%                                                 -pi
% 5       4       1       0       0       0       ---     0       q5      0
%                                                  2

% 6       5       1       0       0       0       0       d4      q6      0

%                                                 pi
% 7       6       1       0       0       0       --      0       q7      0
%                                                 2
%                                                 -pi
% 8       2       1       0       0       0       ---     0       q8      0
%                                                  2
%                                                 -pi
% 9       8       1       0       0       0       ---     0       q9      0
%                                                  2

% 10      9       1       0       0       0       0       d4      q10     0

%                                                 pi
% 11      10      1       0       0       0       --      0       q11     0
%                                                 2
%                                                 -pi
% 12      3       1       0       0       0       ---     0       q12     0
%                                                  2
%                                                 -pi
% 13      12      1       0       0       0       ---     0       q13     0
%                                                  2

% 14      13      1       0       0       0       0       d4      q14     0

%                                                 pi
% 15      14      1       0       0       0       --      0       q15     0
%                                                 2



%              inertial parameters

% j     xx    xy    xz    yy    yz    zz    mx    my    mz    m     ia

% 1     0     0     0     0     0     0     0     0     0     m1    0

% 2     0     0     0     0     0     0     0     0     0     m2    0

% 3     0     0     0     0     0     0     0     0     0     m3    ia3

% 4     0     0     0     0     0     zz4   0     0     0     0     0

% 5     0     0     0     0     0     zz5   mx5   0     0     0     0

% 6     0     0     0     0     0     0     0     0     0     0     0

% 7     0     0     0     0     0     0     0     0     0     0     0

% 8     0     0     0     0     0     0     0     0     0     0     0

% 9     0     0     0     0     0     0     0     0     0     0     0

% 10    0     0     0     0     0     0     0     0     0     0     0

% 11    0     0     0     0     0     0     0     0     0     0     0

% 12    0     0     0     0     0     0     0     0     0     0     0

% 13    0     0     0     0     0     0     0     0     0     0     0

% 14    0     0     0     0     0     0     0     0     0     0     0

% 15    0     0     0     0     0     0     0     0     0     0     0

%                       inertia matrix
% equations:

% declaration of the function
% function orthoglide_ddm()

% declaration of global input variables
global a g2 d4 d6
global m1 m2 m3 
global mx4 my4 m4 zz4 mx5 my5 mz5
global m5 yy5 zz5 xx5 xy5 xz5 yz5 
global mx6 my6 mz6 m6 yy6 zz6 xx6 xy6 xz6 yz6
global mx7 my7 mz7 m7 yy7 zz7 xx7 xy7
global xz7 yz7 mx8 my8 m8 zz8
global mx9 my9 mz9 m9 yy9 zz9 xx9 xy9 xz9 yz9
global mx10 my10 mz10 m10 yy10 zz10 xx10 xy10
global xz10 yz10 mx11 my11 mz11 m11 yy11 zz11
global xx11 xy11 xz11 yz11 mx12 my12 m12 zz12
global mx13 my13 mz13 m13 yy13 zz13 xx13 xy13
global xz13 yz13 mx14 my14 mz14 m14 yy14 zz14
global xx14 xy14 xz14 yz14 mx15 my15 mz15 m15
global yy15 zz15 xx15 xy15 xz15 yz15 fv1 ia1 fs1
global fv2 ia2 fs2 fv3 ia3 fs3 fv4 fs4
global fv5 fs5 fv6 fs6 fv7 fs7 fv8 fs8 fv9 fs9
global fv10 fs10 fv11 fs11 fv12 fs12 fv13 fs13 fv14 fs14
global fv15 fs15

% positions, vitesses et accélérations des moteurs
global q1 q2 q3
global qd1 qd2 qd3
global gam1 gam2 gam3

% declaration of global output variables
global qdd1 qdd2 qdd3

% Function description:
    un = ones(length(q1),1);
    zer = zeros(length(q1),1);
    
% forward geometric model
	rho1=q1;
	rho2=q2;
	rho3=q3;
	un=ones(length(rho1),1);
	termgd1=-rho1 - d6.*un;
	termgd2=-rho2 - (d6 - a).*un;
	termgd3=-rho3 - (d6 - a).*un;
	termgd4=(termgd1 + a.*un)./termgd2;
	termgd5=(termgd1.^2 - termgd2.^2 - a^2.*un)./(2.*termgd2);
	termgd6=(termgd1 + a.*un)./termgd3;
	termgd7=(termgd1.^2 - termgd3.^2 - a^2.*un)./(2.*termgd3);	
	termgd8=termgd4.^2 + termgd6.^2 + un;
	termgd9=2.*(termgd4.*termgd5 + termgd6.*termgd7 + termgd4.*termgd2 - a.*un);
	termgd10=termgd5.^2 + termgd7.^2 + termgd2.^2 + 2.*termgd5.*termgd2 + un.*(a^2-d4^2);
	posz=(-termgd9 + sqrt(termgd9.^2 - 4.*termgd8.*termgd10))./(2.*termgd8);	
	posx=termgd4.*posz + termgd5;
	posy=termgd6.*posz + termgd7;
    q4=atan(-(posz + termgd1)./posx);
	s4=sin(q4);
	c4=cos(q4);
    s5=-posy./d4;
    c5=posx./(d4.*c4);
	q5=atan2(s5,c5);
    s5=sin(q5);
    c5=cos(q5);
	q6=-q5;
	q7=-q4-pi/2.*un;
	s6=sin(q6);
	c6=cos(q6);
	s7=sin(q7);
	c7=cos(q7);
    q8=atan(-(posx + termgd2)./posy);
	s8=sin(q8);
	c8=cos(q8);
	s9=-(posz - a.*un)./d4;
	c9=posy./(d4.*c8);
    q9=atan2(s9,c9);
    s9=sin(q9);
    c9=cos(q9);
	q10=-q9;
	q11=-q8-pi/2.*un;
	s10=sin(q10);
	c10=cos(q10);
	s11=sin(q11);
	c11=cos(q11);
    q12=atan(-(posy + termgd3)./(posz - un.*a));
	s12=sin(q12);
	c12=cos(q12);
	s13=-posx./d4;
	c13=(posz - a.*un)./(d4.*c12);
    q13=atan2(s13,c13);
    s13=sin(q13);
    c13=cos(q13);
	q14=-q13;
	q15=-q12-pi/2.*un;
	s14=sin(q14);
	c14=cos(q14);
	s15=sin(q15);
	c15=cos(q15);
 
% forward kinematic model
	rhod1=qd1;
	rhod2=qd2;
	rhod3=qd3;
	a11=posx;
	a12=posy;
	a13=posz + termgd1;
	a21=posx + termgd2;
	a22=posy;
	a23=posz - a.*un;
	a31=posx;
	a32=posy + termgd3;
	a33=posz - a.*un;
	deta = a11.*(a22.*a33 - a23.*a32) - a21.*a12.*a33 + a21.*a13.*a32 + a31.*a12.*a23 - a31.*a13.*a22;
	j11=(a22.*a33 - a23.*a32).*a13./deta;
	j12=-a21.*(a12.*a33 - a13.*a32)./deta;
	j13=(a12.*a23 - a13.*a22).*a32./deta;
	j21=-(a21.*a33 - a23.*a31).*a13./deta;
	j22=(a11.*a33 - a13.*a31).*a21./deta;
	j23=-(a11.*a23 - a13.*a21).*a32./deta;
	j31=(a21.*a32 - a22.*a31).*a13./deta;
	j32=-(a11.*a32 - a12.*a31).*a21./deta;
	j33=(a11.*a22 - a21.*a12).*a32./deta;
	posdx=j11.*rhod1 + j12.*rhod2 + j13.*rhod3;
	posdy=j21.*rhod1 + j22.*rhod2 + j23.*rhod3;
	posdz=j31.*rhod1 + j32.*rhod2 + j33.*rhod3;
	
	jc112=-d4.*s4.*c5;
	jc113=-d4.*c4.*s5;
	jc123=-d4.*c5;
	jc131=un;
	jc132=-d4.*c4.*c5;
	jc133=d4.*s4.*s5;
	dejc1=jc112.*jc123;
	ijc111=-jc123.*jc132./dejc1;
	ijc112=(-jc112.*jc133 + jc113.*jc132)./dejc1;
	ijc113=jc112.*jc123./dejc1;
	ijc121=jc123./dejc1;
	ijc122=-jc113./dejc1;
	ijc132=jc112./dejc1;
	qd4=ijc121.*posdx + ijc122.*posdy;
	qd5=ijc132.*posdy;
	qd6=-qd5;
	qd7=-qd4;
 	
	jc211=un;
	jc212=-d4.*c8.*c9;
	jc213=d4.*s8.*s9;
	jc222=-d4.*s8.*c9;
	jc223=-d4.*c8.*s9;
	jc233=-d4.*c9;
	dejc2=jc211.*jc222.*jc233;
	ijc211=jc222.*jc233./dejc2;
	ijc212=-jc212.*jc233./dejc2;
	ijc213=(jc212.*jc223 - jc213.*jc222)./dejc2;
	ijc222=jc211.*jc233./dejc2;
	ijc223=-jc211.*jc223./dejc2;
	ijc233=jc211.*jc222./dejc2;
	qd8=ijc222.*posdy + ijc223.*posdz;
	qd9=ijc233.*posdz;
	qd10=-qd9;
	qd11=-qd8;
	
	jc313=-d4.*c13;
	jc321=un;
	jc322=-d4.*c12.*c13;
	jc323=d4.*s12.*s13;
	jc332=-d4.*s12.*c13;
	jc333=-d4.*c12.*s13;
	dejc3=jc321.*jc313.*jc332;
	ijc311=(jc322.*jc333 - jc323.*jc332)./dejc3;
	ijc312=jc313.*jc332./dejc3;
	ijc313=- jc313.*jc322./dejc3;
	ijc321=-jc321.*jc333./dejc3;
	ijc323=jc313.*jc321./dejc3;
	ijc331=jc321.*jc332./dejc3;
	qd12=ijc321.*posdx + ijc323.*posdz;
	qd13=ijc331.*posdx;
	qd14=-qd13;
	qd15=-qd12;	
    
% Second order forward kinematic model
	a11d=posdx;
	a12d=posdy;
	a13d=posdz - rhod1;
	a21d=posdx - rhod2;
	a22d=posdy;
	a23d=posdz;
	a31d=posdx;
	a32d=posdy - rhod3;
	a33d=posdz;
    detad = a11d.*(a22.*a33 - a23.*a32) + a11.*(a22d.*a33 - a23d.*a32 + a22.*a33d - a23.*a32d) - a21d.*a12.*a33 - a21.*a12d.*a33 - a21.*a12.*a33d +...
        a21d.*a13.*a32 + a21.*a13d.*a32 + a21.*a13.*a32d + a31d.*a12.*a23 + a31.*a12d.*a23 + a31.*a12.*a23d - a31d.*a13.*a22 - a31.*a13d.*a22 - a31.*a13.*a22d;
    deta2 = deta.^2;
    
    j11d=(((a22d.*a33 - a23d.*a32 + a22.*a33d - a23.*a32d).*a13 + (a22.*a33 - a23.*a32).*a13d).*deta - (a22.*a33 - a23.*a32).*a13.*detad)./deta2;
	j12d=((-(a12d.*a33 - a13d.*a32 + a12.*a33d - a13.*a32d).*a21 - (a12.*a33 - a13.*a32).*a21d).*deta + a21.*(a12.*a33 - a13.*a32).*detad)./deta2;
	j13d=(((a12d.*a23 - a13d.*a22 + a12.*a23d - a13.*a22d).*a32 + (a12.*a23 - a13.*a22).*a32d).*deta - (a12.*a23 - a13.*a22).*a32.*detad)./deta2;
	j21d=((-(a21d.*a33 - a23d.*a31 + a21.*a33d - a23.*a31d).*a13 - (a21.*a33 - a23.*a31).*a13d).*deta + (a21.*a33 - a23.*a31).*a13.*detad)./deta2;
	j22d=(((a11d.*a33 - a13d.*a31 + a11.*a33d - a13.*a31d).*a21 + (a11.*a33 - a13.*a31).*a21d).*deta - (a11.*a33 - a13.*a31).*a21.*detad)./deta2;
	j23d=((-(a11d.*a23 - a13d.*a21 + a11.*a23d - a13.*a21d).*a32 - (a11.*a23 - a13.*a21).*a32d).*deta + (a11.*a23 - a13.*a21).*a32.*detad)./deta2;
	j31d=(((a21d.*a32 - a22d.*a31 + a21.*a32d - a22.*a31d).*a13 + (a21.*a32 - a22.*a31).*a13d).*deta - (a21.*a32 - a22.*a31).*a13.*detad)./deta2;
	j32d=((-(a11d.*a32 - a12d.*a31 + a11.*a32d - a12.*a31d).*a21 - (a11.*a32 - a12.*a31).*a21d).*deta + (a11.*a32 - a12.*a31).*a21.*detad)./deta2;
	j33d=(((a11d.*a22 - a21d.*a12 + a11.*a22d - a21.*a12d).*a32 + (a11.*a22 - a21.*a12).*a32d).*deta - (a11.*a22 - a21.*a12).*a32.*detad)./deta2;

    jc112d=-d4.*(qd4.*c4.*c5 - qd5.*s4.*s5);
	jc113d=-d4.*(-qd4.*s4.*s5 + qd5.*c4.*c5);
	jc123d=d4.*qd5.*s5;
	jc131d=zer;
	jc132d=-d4.*(-qd4.*s4.*c5 - qd5.*c4.*s5);
	jc133d=d4.*(qd4.*c4.*s5 + qd5.*s4.*c5);
	dejc1d=jc112d.*jc123 + jc112.*jc123d;
 	
	jc211d=zer;
	jc212d=-d4.*(-qd8.*s8.*c9 - qd9.*c8.*s9);
	jc213d=d4.*(qd8.*c8.*s9 + qd9.*s8.*c9);
	jc222d=-d4.*(qd8.*c8.*c9 - qd9.*s8.*s9);
	jc223d=-d4.*(-qd8.*s8.*s9 + qd9.*c8.*c9);
	jc233d=-d4.*qd9.*s9;
	dejc2d=jc211d.*jc222.*jc233 + jc211.*jc222d.*jc233 + jc211.*jc222.*jc233d;

	jc313d=-d4.*qd13.*s13;
	jc321d=zer;
	jc322d=-d4.*(-qd12.*s12.*c13 - qd13.*c12.*s13);
	jc323d=d4.*(qd12.*c12.*s13 + qd13.*s12.*c13);
	jc332d=-d4.*(qd12.*c12.*c13 - qd13.*s12.*s13);
	jc333d=-d4.*(-qd12.*s12.*s13 + qd13.*c12.*c13);
	dejc3d=jc321d.*jc313.*jc332 + jc321.*jc313d.*jc332 + jc321.*jc313.*jc332d;

    
	
% Computation of the inertia matrix of the virtual tree structure
	s4=sin(q4);
	c4=cos(q4);
	s5=sin(q5);
	c5=cos(q5);
	s6=sin(q6);
	c6=cos(q6);
	s7=sin(q7);
	c7=cos(q7);
	s8=sin(q8);
	c8=cos(q8);
	s9=sin(q9);
	c9=cos(q9);
	s10=sin(q10);
	c10=cos(q10);
	s11=sin(q11);
	c11=cos(q11);
	s12=sin(q12);
	c12=cos(q12);
	s13=sin(q13);
	c13=cos(q13);
	s14=sin(q14);
	c14=cos(q14);
	s15=sin(q15);
	c15=cos(q15);
	as115=c15.*mx15 - my15.*s15;
	as315=c15.*my15 + mx15.*s15;
	aj1115=c15.*xx15 - s15.*xy15;
	aj1215=c15.*xy15 - s15.*yy15;
	aj1315=c15.*xz15 - s15.*yz15;
	aj3115=s15.*xx15 + c15.*xy15;
	aj3215=s15.*xy15 + c15.*yy15;
	aj3315=s15.*xz15 + c15.*yz15;
	aja1115=aj1115.*c15 - aj1215.*s15;
	aja2115=-(c15.*xz15) + s15.*yz15;
	aja3115=aj3115.*c15 - aj3215.*s15;
	aja3315=aj3215.*c15 + aj3115.*s15;
	xxp14=aja1115 + xx14;
	xyp14=aja2115 + xy14;
	xzp14=aja3115 + xz14;
	yyp14=yy14 + zz15;
	yzp14=-aj3315 + yz14;
	zzp14=aja3315 + zz14;
	mxp14=as115 + mx14;
	myp14=my14 - mz15;
	mzp14=as315 + mz14;
	mp14=m14 + m15;
	as114=c14.*mxp14 - myp14.*s14;
	as214=c14.*myp14 + mxp14.*s14;
	aj1114=c14.*xxp14 - s14.*(aja2115 + xy14);
	aj1214=c14.*xyp14 - s14.*yyp14;
	aj1314=c14.*xzp14 - s14.*yzp14;
	aj2114=s14.*xxp14 + c14.*(aja2115 + xy14);
	aj2214=s14.*xyp14 + c14.*yyp14;
	aj2314=s14.*xzp14 + c14.*yzp14;
	aj3114=aja3115 + xz14;
	aj3214=-aj3315 + yz14;
	aja1114=aj1114.*c14 - aj1214.*s14;
	aja2114=aj2114.*c14 - aj2214.*s14;
	aja3114=aj3114.*c14 - aj3214.*s14;
	aja2214=aj2214.*c14 + aj2114.*s14;
	aja3214=aj3214.*c14 + aj3114.*s14;
	pas2114=as214.*d4;
	pas2214=-(as114.*d4);
	pas3114=d4.*mzp14;
	pas3314=-(as114.*d4);
	xxp13=aja1114 + xx13;
	xyp13=aja2114 - pas2114 + xy13;
	xzp13=aja3114 - pas3114 + xz13;
	yyp13=aja2214 + d4.^2.*mp14 - 2.*pas2214 + yy13;
	yzp13=aja3214 + yz13;
	zzp13=d4.^2.*mp14 - 2.*pas3314 + zz13 + zzp14;
	mxp13=as114 + d4.*mp14 + mx13;
	myp13=as214 + my13;
	mzp13=mz13 + mzp14;
	mp13=m13 + mp14;
	as113=c13.*mxp13 - myp13.*s13;
	aj3113=-(s13.*xxp13) - c13.*(aja2114 - pas2114 + xy13);
	aj3213=-(s13.*xyp13) - c13.*yyp13;
	aj3313=-(s13.*xzp13) - c13.*yzp13;
	aja3313=-(aj3213.*c13) - aj3113.*s13;
	zzp12=aja3313 + zz12;
	mxp12=as113 + mx12;
	myp12=my12 + mzp13;
	mp12=m12 + mp13;
	mp3=m3 + mp12;
	as111=c11.*mx11 - my11.*s11;
	as311=c11.*my11 + mx11.*s11;
	aj1111=c11.*xx11 - s11.*xy11;
	aj1211=c11.*xy11 - s11.*yy11;
	aj1311=c11.*xz11 - s11.*yz11;
	aj3111=s11.*xx11 + c11.*xy11;
	aj3211=s11.*xy11 + c11.*yy11;
	aj3311=s11.*xz11 + c11.*yz11;
	aja1111=aj1111.*c11 - aj1211.*s11;
	aja2111=-(c11.*xz11) + s11.*yz11;
	aja3111=aj3111.*c11 - aj3211.*s11;
	aja3311=aj3211.*c11 + aj3111.*s11;
	xxp10=aja1111 + xx10;
	xyp10=aja2111 + xy10;
	xzp10=aja3111 + xz10;
	yyp10=yy10 + zz11;
	yzp10=-aj3311 + yz10;
	zzp10=aja3311 + zz10;
	mxp10=as111 + mx10;
	myp10=my10 - mz11;
	mzp10=as311 + mz10;
	mp10=m10 + m11;
	as110=c10.*mxp10 - myp10.*s10;
	as210=c10.*myp10 + mxp10.*s10;
	aj1110=c10.*xxp10 - s10.*(aja2111 + xy10);
	aj1210=c10.*xyp10 - s10.*yyp10;
	aj1310=c10.*xzp10 - s10.*yzp10;
	aj2110=s10.*xxp10 + c10.*(aja2111 + xy10);
	aj2210=s10.*xyp10 + c10.*yyp10;
	aj2310=s10.*xzp10 + c10.*yzp10;
	aj3110=aja3111 + xz10;
	aj3210=-aj3311 + yz10;
	aja1110=aj1110.*c10 - aj1210.*s10;
	aja2110=aj2110.*c10 - aj2210.*s10;
	aja3110=aj3110.*c10 - aj3210.*s10;
	aja2210=aj2210.*c10 + aj2110.*s10;
	aja3210=aj3210.*c10 + aj3110.*s10;
	pas2110=as210.*d4;
	pas2210=-(as110.*d4);
	pas3110=d4.*mzp10;
	pas3310=-(as110.*d4);
	xxp9=aja1110 + xx9;
	xyp9=aja2110 - pas2110 + xy9;
	xzp9=aja3110 - pas3110 + xz9;
	yyp9=aja2210 + d4.^2.*mp10 - 2.*pas2210 + yy9;
	yzp9=aja3210 + yz9;
	zzp9=d4.^2.*mp10 - 2.*pas3310 + zz9 + zzp10;
	mxp9=as110 + d4.*mp10 + mx9;
	myp9=as210 + my9;
	mzp9=mz9 + mzp10;
	mp9=m9 + mp10;
	as19=c9.*mxp9 - myp9.*s9;
	aj319=-(s9.*xxp9) - c9.*(aja2110 - pas2110 + xy9);
	aj329=-(s9.*xyp9) - c9.*yyp9;
	aj339=-(s9.*xzp9) - c9.*yzp9;
	aja339=-(aj329.*c9) - aj319.*s9;
	zzp8=aja339 + zz8;
	mxp8=as19 + mx8;
	myp8=my8 + mzp9;
	mp8=m8 + mp9;
	mp2=m2 + mp8;
	as17=c7.*mx7 - my7.*s7;
	as37=c7.*my7 + mx7.*s7;
	aj117=c7.*xx7 - s7.*xy7;
	aj127=c7.*xy7 - s7.*yy7;
	aj137=c7.*xz7 - s7.*yz7;
	aj317=s7.*xx7 + c7.*xy7;
	aj327=s7.*xy7 + c7.*yy7;
	aj337=s7.*xz7 + c7.*yz7;
	aja117=aj117.*c7 - aj127.*s7;
	aja217=-(c7.*xz7) + s7.*yz7;
	aja317=aj317.*c7 - aj327.*s7;
	aja337=aj327.*c7 + aj317.*s7;
	xxp6=aja117 + xx6;
	xyp6=aja217 + xy6;
	xzp6=aja317 + xz6;
	yyp6=yy6 + zz7;
	yzp6=-aj337 + yz6;
	zzp6=aja337 + zz6;
	mxp6=as17 + mx6;
	myp6=my6 - mz7;
	mzp6=as37 + mz6;
	mp6=m6 + m7;
	as16=c6.*mxp6 - myp6.*s6;
	as26=c6.*myp6 + mxp6.*s6;
	aj116=c6.*xxp6 - s6.*(aja217 + xy6);
	aj126=c6.*xyp6 - s6.*yyp6;
	aj136=c6.*xzp6 - s6.*yzp6;
	aj216=s6.*xxp6 + c6.*(aja217 + xy6);
	aj226=s6.*xyp6 + c6.*yyp6;
	aj236=s6.*xzp6 + c6.*yzp6;
	aj316=aja317 + xz6;
	aj326=-aj337 + yz6;
	aja116=aj116.*c6 - aj126.*s6;
	aja216=aj216.*c6 - aj226.*s6;
	aja316=aj316.*c6 - aj326.*s6;
	aja226=aj226.*c6 + aj216.*s6;
	aja326=aj326.*c6 + aj316.*s6;
	pas216=as26.*d4;
	pas226=-(as16.*d4);
	pas316=d4.*mzp6;
	pas336=-(as16.*d4);
	xxp5=aja116 + xx5;
	xyp5=aja216 - pas216 + xy5;
	xzp5=aja316 - pas316 + xz5;
	yyp5=aja226 + d4.^2.*mp6 - 2.*pas226 + yy5;
	yzp5=aja326 + yz5;
	zzp5=d4.^2.*mp6 - 2.*pas336 + zz5 + zzp6;
	mxp5=as16 + d4.*mp6 + mx5;
	myp5=as26 + my5;
	mzp5=mz5 + mzp6;
	mp5=m5 + mp6;
	as15=c5.*mxp5 - myp5.*s5;
	aj315=-(s5.*xxp5) - c5.*(aja216 - pas216 + xy5);
	aj325=-(s5.*xyp5) - c5.*yyp5;
	aj335=-(s5.*xzp5) - c5.*yzp5;
	aja335=-(aj325.*c5) - aj315.*s5;
	zzp4=aja335 + zz4;
	mxp4=as15 + mx4;
	myp4=my4 + mzp5;
	mp4=m4 + mp5;
	mp1=m1 + mp4;
	ed32=-(c4.*mxp4) + myp4.*s4;
	ee12=-(c5.*myp5) - mxp5.*s5;
	ee33=-(ee12.*s4);
	ef12=-(c6.*myp6) - mxp6.*s6;
	ef22=c6.*mxp6 - myp6.*s6;
	nf32=d4.*ef22 + zzp6;
	ef13=c5.*ef12 - ef22.*s5;
	nf33=-(aj236.*c5) - aj136.*s5;
	ef34=-(ef13.*s4);
	eg12=-(c7.*my7) - mx7.*s7;
	eg32=c7.*mx7 - my7.*s7;
	eg13=c6.*eg12;
	eg23=eg12.*s6;
	ng13=aj137.*c6 + s6.*zz7;
	ng23=-(d4.*eg32) + aj137.*s6 - c6.*zz7;
	ng33=aj337 + d4.*eg23;
	eg14=c5.*eg13 - eg23.*s5;
	ng34=-(c5.*ng23) - ng13.*s5;
	eg35=-(c4.*eg32) - eg14.*s4;
	eh32=-(c8.*mxp8) + myp8.*s8;
	ei12=-(c9.*myp9) - mxp9.*s9;
	ei33=-(ei12.*s8);
	ej12=-(c10.*myp10) - mxp10.*s10;
	ej22=c10.*mxp10 - myp10.*s10;
	nj32=d4.*ej22 + zzp10;
	ej13=c9.*ej12 - ej22.*s9;
	nj33=-(aj2310.*c9) - aj1310.*s9;
	ej34=-(ej13.*s8);
	ek12=-(c11.*my11) - mx11.*s11;
	ek32=c11.*mx11 - my11.*s11;
	ek13=c10.*ek12;
	ek23=ek12.*s10;
	nk13=aj1311.*c10 + s10.*zz11;
	nk23=-(d4.*ek32) + aj1311.*s10 - c10.*zz11;
	nk33=aj3311 + d4.*ek23;
	ek14=c9.*ek13 - ek23.*s9;
	nk34=-(c9.*nk23) - nk13.*s9;
	ek35=-(c8.*ek32) - ek14.*s8;
	el32=-(c12.*mxp12) + myp12.*s12;
	em12=-(c13.*myp13) - mxp13.*s13;
	em33=-(em12.*s12);
	en12=-(c14.*myp14) - mxp14.*s14;
	en22=c14.*mxp14 - myp14.*s14;
	nn32=d4.*en22 + zzp14;
	en13=c13.*en12 - en22.*s13;
	nn33=-(aj2314.*c13) - aj1314.*s13;
	en34=-(en13.*s12);
	eo12=-(c15.*my15) - mx15.*s15;
	eo32=c15.*mx15 - my15.*s15;
	eo13=c14.*eo12;
	eo23=eo12.*s14;
	no13=aj1315.*c14 + s14.*zz15;
	no23=-(d4.*eo32) + aj1315.*s14 - c14.*zz15;
	no33=aj3315 + d4.*eo23;
	eo14=c13.*eo13 - eo23.*s13;
	no34=-(c13.*no23) - no13.*s13;
	eo35=-(c12.*eo32) - eo14.*s12;
	atmp(1,1)=ia1 + mp1;
	atmp(2,1)=0;
	atmp(3,1)=0;
	atmp(4,1)=ed32;
	atmp(5,1)=ee33;
	atmp(6,1)=ef34;
	atmp(7,1)=eg35;
	atmp(8,1)=0;
	atmp(9,1)=0;
	atmp(10,1)=0;
	atmp(11,1)=0;
	atmp(12,1)=0;
	atmp(13,1)=0;
	atmp(14,1)=0;
	atmp(15,1)=0;
	atmp(2,2)=ia2 + mp2;
	atmp(3,2)=0;
	atmp(4,2)=0;
	atmp(5,2)=0;
	atmp(6,2)=0;
	atmp(7,2)=0;
	atmp(8,2)=eh32;
	atmp(9,2)=ei33;
	atmp(10,2)=ej34;
	atmp(11,2)=ek35;
	atmp(12,2)=0;
	atmp(13,2)=0;
	atmp(14,2)=0;
	atmp(15,2)=0;
	atmp(3,3)=ia3 + mp3;
	atmp(4,3)=0;
	atmp(5,3)=0;
	atmp(6,3)=0;
	atmp(7,3)=0;
	atmp(8,3)=0;
	atmp(9,3)=0;
	atmp(10,3)=0;
	atmp(11,3)=0;
	atmp(12,3)=el32;
	atmp(13,3)=em33;
	atmp(14,3)=en34;
	atmp(15,3)=eo35;
	atmp(4,4)=zzp4;
	atmp(5,4)=aj335;
	atmp(6,4)=nf33;
	atmp(7,4)=ng34;
	atmp(8,4)=0;
	atmp(9,4)=0;
	atmp(10,4)=0;
	atmp(11,4)=0;
	atmp(12,4)=0;
	atmp(13,4)=0;
	atmp(14,4)=0;
	atmp(15,4)=0;
	atmp(5,5)=zzp5;
	atmp(6,5)=nf32;
	atmp(7,5)=ng33;
	atmp(8,5)=0;
	atmp(9,5)=0;
	atmp(10,5)=0;
	atmp(11,5)=0;
	atmp(12,5)=0;
	atmp(13,5)=0;
	atmp(14,5)=0;
	atmp(15,5)=0;
	atmp(6,6)=zzp6;
	atmp(7,6)=aj337;
	atmp(8,6)=0;
	atmp(9,6)=0;
	atmp(10,6)=0;
	atmp(11,6)=0;
	atmp(12,6)=0;
	atmp(13,6)=0;
	atmp(14,6)=0;
	atmp(15,6)=0;
	atmp(7,7)=zz7;
	atmp(8,7)=0;
	atmp(9,7)=0;
	atmp(10,7)=0;
	atmp(11,7)=0;
	atmp(12,7)=0;
	atmp(13,7)=0;
	atmp(14,7)=0;
	atmp(15,7)=0;
	atmp(8,8)=zzp8;
	atmp(9,8)=aj339;
	atmp(10,8)=nj33;
	atmp(11,8)=nk34;
	atmp(12,8)=0;
	atmp(13,8)=0;
	atmp(14,8)=0;
	atmp(15,8)=0;
	atmp(9,9)=zzp9;
	atmp(10,9)=nj32;
	atmp(11,9)=nk33;
	atmp(12,9)=0;
	atmp(13,9)=0;
	atmp(14,9)=0;
	atmp(15,9)=0;
	atmp(10,10)=zzp10;
	atmp(11,10)=aj3311;
	atmp(12,10)=0;
	atmp(13,10)=0;
	atmp(14,10)=0;
	atmp(15,10)=0;
	atmp(11,11)=zz11;
	atmp(12,11)=0;
	atmp(13,11)=0;
	atmp(14,11)=0;
	atmp(15,11)=0;
	atmp(12,12)=zzp12;
	atmp(13,12)=aj3313;
	atmp(14,12)=nn33;
	atmp(15,12)=no34;
	atmp(13,13)=zzp13;
	atmp(14,13)=nn32;
	atmp(15,13)=no33;
	atmp(14,14)=zzp14;
	atmp(15,14)=aj3315;
	atmp(15,15)=zz15;
	
	atmp_1_2 = atmp_2_1;
	atmp_1_3 = atmp_3_1;
	atmp_1_4 = atmp_4_1;
	atmp_1_5 = atmp_5_1;
	atmp_1_6 = atmp_6_1;
	atmp_1_7 = atmp_7_1;
	atmp_1_8 = atmp_8_1;
	atmp_1_9 = atmp_9_1;
	atmp_1_10 = atmp_10_1;
	atmp_1_11 = atmp_11_1;
	atmp_1_12 = atmp_12_1;
	atmp_1_13 = atmp_13_1;
	atmp_1_14 = atmp_14_1;
	atmp_1_15 = atmp_15_1;
	
	atmp_2_3 = atmp_3_2;
	atmp_2_4 = atmp_4_2;
	atmp_2_5 = atmp_5_2;
	atmp_2_6 = atmp_6_2;
	atmp_2_7 = atmp_7_2;
	atmp_2_8 = atmp_8_2;
	atmp_2_9 = atmp_9_2;
	atmp_2_10 = atmp_10_2;
	atmp_2_11 = atmp_11_2;
	atmp_2_12 = atmp_12_2;
	atmp_2_13 = atmp_13_2;
	atmp_2_14 = atmp_14_2;
	atmp_2_15 = atmp_15_2;
	
	atmp_3_4 = atmp_4_3;
	atmp_3_5 = atmp_5_3;
	atmp_3_6 = atmp_6_3;
	atmp_3_7 = atmp_7_3;
	atmp_3_8 = atmp_8_3;
	atmp_3_9 = atmp_9_3;
	atmp_3_10 = atmp_10_3;
	atmp_3_11 = atmp_11_3;
	atmp_3_12 = atmp_12_3;
	atmp_3_13 = atmp_13_3;
	atmp_3_14 = atmp_14_3;
	atmp_3_15 = atmp_15_3;
	
	atmp_4_5 = atmp_5_4;
	atmp_4_6 = atmp_6_4;
	atmp_4_7 = atmp_7_4;
	atmp_4_8 = atmp_8_4;
	atmp_4_9 = atmp_9_4;
	atmp_4_10 = atmp_10_4;
	atmp_4_11 = atmp_11_4;
	atmp_4_12 = atmp_12_4;
	atmp_4_13 = atmp_13_4;
	atmp_4_14 = atmp_14_4;
	atmp_4_15 = atmp_15_4;
	
	atmp_5_6 = atmp_6_5;
	atmp_5_7 = atmp_7_5;
	atmp_5_8 = atmp_8_5;
	atmp_5_9 = atmp_9_5;
	atmp_5_10 = atmp_10_5;
	atmp_5_11 = atmp_11_5;
	atmp_5_12 = atmp_12_5;
	atmp_5_13 = atmp_13_5;
	atmp_5_14 = atmp_14_5;
	atmp_5_15 = atmp_15_5;
	
	atmp_6_7 = atmp_7_6;
	atmp_6_8 = atmp_8_6;
	atmp_6_9 = atmp_9_6;
	atmp_6_10 = atmp_10_6;
	atmp_6_11 = atmp_11_6;
	atmp_6_12 = atmp_12_6;
	atmp_6_13 = atmp_13_6;
	atmp_6_14 = atmp_14_6;
	atmp_6_15 = atmp_15_6;
	
	atmp_7_8 = atmp_8_7;
	atmp_7_9 = atmp_9_7;
	atmp_7_10 = atmp_10_7;
	atmp_7_11 = atmp_11_7;
	atmp_7_12 = atmp_12_7;
	atmp_7_13 = atmp_13_7;
	atmp_7_14 = atmp_14_7;
	atmp_7_15 = atmp_15_7;
	
	atmp_8_9 = atmp_9_8;
	atmp_8_10 = atmp_10_8;
	atmp_8_11 = atmp_11_8;
	atmp_8_12 = atmp_12_8;
	atmp_8_13 = atmp_13_8;
	atmp_8_14 = atmp_14_8;
	atmp_8_15 = atmp_15_8;
	
	atmp_9_10 = atmp_10_9;
	atmp_9_11 = atmp_11_9;
	atmp_9_12 = atmp_12_9;
	atmp_9_13 = atmp_13_9;
	atmp_9_14 = atmp_14_9;
	atmp_9_15 = atmp_15_9;
	
	atmp_10_11 = atmp_11_10;
	atmp_10_12 = atmp_12_10;
	atmp_10_13 = atmp_13_10;
	atmp_10_14 = atmp_14_10;
	atmp_10_15 = atmp_15_10;
	
	atmp_11_12 = atmp_12_11;
	atmp_11_13 = atmp_13_11;
	atmp_11_14 = atmp_14_11;
	atmp_11_15 = atmp_15_11;
	
	atmp_12_13 = atmp_13_12;
	atmp_12_14 = atmp_14_12;
	atmp_12_15 = atmp_15_12;
	
	atmp_13_14 = atmp_14_13;
	atmp_13_15 = atmp_15_13;
	
	atmp_14_15 = atmp_15_14;
    
    % Inertia matrix of the parallel robot
% Matrix relating the efforts in the chain 1 of the virtual robot to the real input efforts
	jfc112=(jc123.*j11-j21.*jc113)./dejc1;
	jfc113=(jc112.*j21)./dejc1;
	jfc122=(jc123.*j12-j22.*jc113)./dejc1;
	jfc123=(jc112.*j22)./dejc1;
	jfc132=(jc123.*j13-j23.*jc113)./dejc1;
	jfc133=(jc112.*j23)./dejc1;

% Matrix relating the efforts in the chain 2 of the virtual robot to the real input efforts
	jfc212=(jc233.*j21-j31.*jc223)./dejc2;
	jfc213=(jc222.*j31)./dejc2;
	jfc222=(jc233.*j22-j32.*jc223)./dejc2;
	jfc223=(jc222.*j32)./dejc2;
	jfc232=(jc233.*j23-j33.*jc223)./dejc2;
	jfc233=(jc222.*j33)./dejc2;

% Matrix relating the efforts in the chain 3 of the virtual robot to the real input efforts
	jfc312=(-j11.*jc333+jc313.*j31)./dejc3;
	jfc313=(jc332.*j11)./dejc3;
	jfc322=(-j12.*jc333+jc313.*j32)./dejc3;
	jfc323=(jc332.*j12)./dejc3;
	jfc332=(-j13.*jc333+jc313.*j33)./dejc3;
	jfc333=(jc332.*j13)./dejc3;	
    
    ttemp_1_1 = atmp_1_1+jfc112.*atmp_4_1+jfc113.*atmp_5_1-jfc113.*atmp_6_1-jfc112.*atmp_7_1+jfc212.*atmp_8_1+jfc213.*atmp_9_1-jfc213.*atmp_10_1-jfc212.*atmp_11_1+jfc312.*atmp_12_1+jfc313.*atmp_13_1-jfc313.*atmp_14_1-jfc312.*atmp_15_1;
    ttemp_1_2 = atmp_1_2+jfc112.*atmp_4_2+jfc113.*atmp_5_2-jfc113.*atmp_6_2-jfc112.*atmp_7_2+jfc212.*atmp_8_2+jfc213.*atmp_9_2-jfc213.*atmp_10_2-jfc212.*atmp_11_2+jfc312.*atmp_12_2+jfc313.*atmp_13_2-jfc313.*atmp_14_2-jfc312.*atmp_15_2;
    ttemp_1_3 = atmp_1_3+jfc112.*atmp_4_3+jfc113.*atmp_5_3-jfc113.*atmp_6_3-jfc112.*atmp_7_3+jfc212.*atmp_8_3+jfc213.*atmp_9_3-jfc213.*atmp_10_3-jfc212.*atmp_11_3+jfc312.*atmp_12_3+jfc313.*atmp_13_3-jfc313.*atmp_14_3-jfc312.*atmp_15_3;
    ttemp_1_4 = atmp_1_4+jfc112.*atmp_4_4+jfc113.*atmp_5_4-jfc113.*atmp_6_4-jfc112.*atmp_7_4+jfc212.*atmp_8_4+jfc213.*atmp_9_4-jfc213.*atmp_10_4-jfc212.*atmp_11_4+jfc312.*atmp_12_4+jfc313.*atmp_13_4-jfc313.*atmp_14_4-jfc312.*atmp_15_4;
    ttemp_1_5 = atmp_1_5+jfc112.*atmp_4_5+jfc113.*atmp_5_5-jfc113.*atmp_6_5-jfc112.*atmp_7_5+jfc212.*atmp_8_5+jfc213.*atmp_9_5-jfc213.*atmp_10_5-jfc212.*atmp_11_5+jfc312.*atmp_12_5+jfc313.*atmp_13_5-jfc313.*atmp_14_5-jfc312.*atmp_15_5;
    ttemp_1_6 = atmp_1_6+jfc112.*atmp_4_6+jfc113.*atmp_5_6-jfc113.*atmp_6_6-jfc112.*atmp_7_6+jfc212.*atmp_8_6+jfc213.*atmp_9_6-jfc213.*atmp_10_6-jfc212.*atmp_11_6+jfc312.*atmp_12_6+jfc313.*atmp_13_6-jfc313.*atmp_14_6-jfc312.*atmp_15_6;
    ttemp_1_7 = atmp_1_7+jfc112.*atmp_4_7+jfc113.*atmp_5_7-jfc113.*atmp_6_7-jfc112.*atmp_7_7+jfc212.*atmp_8_7+jfc213.*atmp_9_7-jfc213.*atmp_10_7-jfc212.*atmp_11_7+jfc312.*atmp_12_7+jfc313.*atmp_13_7-jfc313.*atmp_14_7-jfc312.*atmp_15_7;
    ttemp_1_8 = atmp_1_8+jfc112.*atmp_4_8+jfc113.*atmp_5_8-jfc113.*atmp_6_8-jfc112.*atmp_7_8+jfc212.*atmp_8_8+jfc213.*atmp_9_8-jfc213.*atmp_10_8-jfc212.*atmp_11_8+jfc312.*atmp_12_8+jfc313.*atmp_13_8-jfc313.*atmp_14_8-jfc312.*atmp_15_8;
    ttemp_1_9 = atmp_1_9+jfc112.*atmp_4_9+jfc113.*atmp_5_9-jfc113.*atmp_6_9-jfc112.*atmp_7_9+jfc212.*atmp_8_9+jfc213.*atmp_9_9-jfc213.*atmp_10_9-jfc212.*atmp_11_9+jfc312.*atmp_12_9+jfc313.*atmp_13_9-jfc313.*atmp_14_9-jfc312.*atmp_15_9;
    ttemp_1_10 = atmp_1_10+jfc112.*atmp_4_10+jfc113.*atmp_5_10-jfc113.*atmp_6_10-jfc112.*atmp_7_10+jfc212.*atmp_8_10+jfc213.*atmp_9_10-jfc213.*atmp_10_10-jfc212.*atmp_11_10+jfc312.*atmp_12_10+jfc313.*atmp_13_10-jfc313.*atmp_14_10-jfc312.*atmp_15_10;
    ttemp_1_11 = atmp_1_11+jfc112.*atmp_4_11+jfc113.*atmp_5_11-jfc113.*atmp_6_11-jfc112.*atmp_7_11+jfc212.*atmp_8_11+jfc213.*atmp_9_11-jfc213.*atmp_10_11-jfc212.*atmp_11_11+jfc312.*atmp_12_11+jfc313.*atmp_13_11-jfc313.*atmp_14_11-jfc312.*atmp_15_11;
    ttemp_1_12 = atmp_1_12+jfc112.*atmp_4_12+jfc113.*atmp_5_12-jfc113.*atmp_6_12-jfc112.*atmp_7_12+jfc212.*atmp_8_12+jfc213.*atmp_9_12-jfc213.*atmp_10_12-jfc212.*atmp_11_12+jfc312.*atmp_12_12+jfc313.*atmp_13_12-jfc313.*atmp_14_12-jfc312.*atmp_15_12;
    ttemp_1_13 = atmp_1_13+jfc112.*atmp_4_13+jfc113.*atmp_5_13-jfc113.*atmp_6_13-jfc112.*atmp_7_13+jfc212.*atmp_8_13+jfc213.*atmp_9_13-jfc213.*atmp_10_13-jfc212.*atmp_11_13+jfc312.*atmp_12_13+jfc313.*atmp_13_13-jfc313.*atmp_14_13-jfc312.*atmp_15_13;
    ttemp_1_14 = atmp_1_14+jfc112.*atmp_4_14+jfc113.*atmp_5_14-jfc113.*atmp_6_14-jfc112.*atmp_7_14+jfc212.*atmp_8_14+jfc213.*atmp_9_14-jfc213.*atmp_10_14-jfc212.*atmp_11_14+jfc312.*atmp_12_14+jfc313.*atmp_13_14-jfc313.*atmp_14_14-jfc312.*atmp_15_14;
    ttemp_1_15 = atmp_1_15+jfc112.*atmp_4_15+jfc113.*atmp_5_15-jfc113.*atmp_6_15-jfc112.*atmp_7_15+jfc212.*atmp_8_15+jfc213.*atmp_9_15-jfc213.*atmp_10_15-jfc212.*atmp_11_15+jfc312.*atmp_12_15+jfc313.*atmp_13_15-jfc313.*atmp_14_15-jfc312.*atmp_15_15;
    
    ttemp_2_1 = atmp_2_1+jfc122.*atmp_4_1+jfc123.*atmp_5_1-jfc123.*atmp_6_1-jfc122.*atmp_7_1+jfc222.*atmp_8_1+jfc223.*atmp_9_1-jfc223.*atmp_10_1-jfc222.*atmp_11_1+jfc322.*atmp_12_1+jfc323.*atmp_13_1-jfc323.*atmp_14_1-jfc322.*atmp_15_1;
    ttemp_2_2 = atmp_2_2+jfc122.*atmp_4_2+jfc123.*atmp_5_2-jfc123.*atmp_6_2-jfc122.*atmp_7_2+jfc222.*atmp_8_2+jfc223.*atmp_9_2-jfc223.*atmp_10_2-jfc222.*atmp_11_2+jfc322.*atmp_12_2+jfc323.*atmp_13_2-jfc323.*atmp_14_2-jfc322.*atmp_15_2;
    ttemp_2_3 = atmp_2_3+jfc122.*atmp_4_3+jfc123.*atmp_5_3-jfc123.*atmp_6_3-jfc122.*atmp_7_3+jfc222.*atmp_8_3+jfc223.*atmp_9_3-jfc223.*atmp_10_3-jfc222.*atmp_11_3+jfc322.*atmp_12_3+jfc323.*atmp_13_3-jfc323.*atmp_14_3-jfc322.*atmp_15_3;
    ttemp_2_4 = atmp_2_4+jfc122.*atmp_4_4+jfc123.*atmp_5_4-jfc123.*atmp_6_4-jfc122.*atmp_7_4+jfc222.*atmp_8_4+jfc223.*atmp_9_4-jfc223.*atmp_10_4-jfc222.*atmp_11_4+jfc322.*atmp_12_4+jfc323.*atmp_13_4-jfc323.*atmp_14_4-jfc322.*atmp_15_4;
    ttemp_2_5 = atmp_2_5+jfc122.*atmp_4_5+jfc123.*atmp_5_5-jfc123.*atmp_6_5-jfc122.*atmp_7_5+jfc222.*atmp_8_5+jfc223.*atmp_9_5-jfc223.*atmp_10_5-jfc222.*atmp_11_5+jfc322.*atmp_12_5+jfc323.*atmp_13_5-jfc323.*atmp_14_5-jfc322.*atmp_15_5;
    ttemp_2_6 = atmp_2_6+jfc122.*atmp_4_6+jfc123.*atmp_5_6-jfc123.*atmp_6_6-jfc122.*atmp_7_6+jfc222.*atmp_8_6+jfc223.*atmp_9_6-jfc223.*atmp_10_6-jfc222.*atmp_11_6+jfc322.*atmp_12_6+jfc323.*atmp_13_6-jfc323.*atmp_14_6-jfc322.*atmp_15_6;
    ttemp_2_7 = atmp_2_7+jfc122.*atmp_4_7+jfc123.*atmp_5_7-jfc123.*atmp_6_7-jfc122.*atmp_7_7+jfc222.*atmp_8_7+jfc223.*atmp_9_7-jfc223.*atmp_10_7-jfc222.*atmp_11_7+jfc322.*atmp_12_7+jfc323.*atmp_13_7-jfc323.*atmp_14_7-jfc322.*atmp_15_7;
    ttemp_2_8 = atmp_2_8+jfc122.*atmp_4_8+jfc123.*atmp_5_8-jfc123.*atmp_6_8-jfc122.*atmp_7_8+jfc222.*atmp_8_8+jfc223.*atmp_9_8-jfc223.*atmp_10_8-jfc222.*atmp_11_8+jfc322.*atmp_12_8+jfc323.*atmp_13_8-jfc323.*atmp_14_8-jfc322.*atmp_15_8;
    ttemp_2_9 = atmp_2_9+jfc122.*atmp_4_9+jfc123.*atmp_5_9-jfc123.*atmp_6_9-jfc122.*atmp_7_9+jfc222.*atmp_8_9+jfc223.*atmp_9_9-jfc223.*atmp_10_9-jfc222.*atmp_11_9+jfc322.*atmp_12_9+jfc323.*atmp_13_9-jfc323.*atmp_14_9-jfc322.*atmp_15_9;
    ttemp_2_10 = atmp_2_10+jfc122.*atmp_4_10+jfc123.*atmp_5_10-jfc123.*atmp_6_10-jfc122.*atmp_7_10+jfc222.*atmp_8_10+jfc223.*atmp_9_10-jfc223.*atmp_10_10-jfc222.*atmp_11_10+jfc322.*atmp_12_10+jfc323.*atmp_13_10-jfc323.*atmp_14_10-jfc322.*atmp_15_10;
    ttemp_2_11 = atmp_2_11+jfc122.*atmp_4_11+jfc123.*atmp_5_11-jfc123.*atmp_6_11-jfc122.*atmp_7_11+jfc222.*atmp_8_11+jfc223.*atmp_9_11-jfc223.*atmp_10_11-jfc222.*atmp_11_11+jfc322.*atmp_12_11+jfc323.*atmp_13_11-jfc323.*atmp_14_11-jfc322.*atmp_15_11;
    ttemp_2_12 = atmp_2_12+jfc122.*atmp_4_12+jfc123.*atmp_5_12-jfc123.*atmp_6_12-jfc122.*atmp_7_12+jfc222.*atmp_8_12+jfc223.*atmp_9_12-jfc223.*atmp_10_12-jfc222.*atmp_11_12+jfc322.*atmp_12_12+jfc323.*atmp_13_12-jfc323.*atmp_14_12-jfc322.*atmp_15_12;
    ttemp_2_13 = atmp_2_13+jfc122.*atmp_4_13+jfc123.*atmp_5_13-jfc123.*atmp_6_13-jfc122.*atmp_7_13+jfc222.*atmp_8_13+jfc223.*atmp_9_13-jfc223.*atmp_10_13-jfc222.*atmp_11_13+jfc322.*atmp_12_13+jfc323.*atmp_13_13-jfc323.*atmp_14_13-jfc322.*atmp_15_13;
    ttemp_2_14 = atmp_2_14+jfc122.*atmp_4_14+jfc123.*atmp_5_14-jfc123.*atmp_6_14-jfc122.*atmp_7_14+jfc222.*atmp_8_14+jfc223.*atmp_9_14-jfc223.*atmp_10_14-jfc222.*atmp_11_14+jfc322.*atmp_12_14+jfc323.*atmp_13_14-jfc323.*atmp_14_14-jfc322.*atmp_15_14;
    ttemp_2_15 = atmp_2_15+jfc122.*atmp_4_15+jfc123.*atmp_5_15-jfc123.*atmp_6_15-jfc122.*atmp_7_15+jfc222.*atmp_8_15+jfc223.*atmp_9_15-jfc223.*atmp_10_15-jfc222.*atmp_11_15+jfc322.*atmp_12_15+jfc323.*atmp_13_15-jfc323.*atmp_14_15-jfc322.*atmp_15_15;
	
    ttemp_3_1 = atmp_3_1+jfc132.*atmp_4_1+jfc133.*atmp_5_1-jfc123.*atmp_6_1-jfc132.*atmp_7_1+jfc232.*atmp_8_1+jfc233.*atmp_9_1-jfc233.*atmp_10_1-jfc232.*atmp_11_1+jfc332.*atmp_12_1+jfc333.*atmp_13_1-jfc333.*atmp_14_1-jfc332.*atmp_15_1;
    ttemp_3_2 = atmp_3_2+jfc132.*atmp_4_2+jfc133.*atmp_5_2-jfc123.*atmp_6_2-jfc132.*atmp_7_2+jfc232.*atmp_8_2+jfc233.*atmp_9_2-jfc233.*atmp_10_2-jfc232.*atmp_11_2+jfc332.*atmp_12_2+jfc333.*atmp_13_2-jfc333.*atmp_14_2-jfc332.*atmp_15_2;
    ttemp_3_3 = atmp_3_3+jfc132.*atmp_4_3+jfc133.*atmp_5_3-jfc123.*atmp_6_3-jfc132.*atmp_7_3+jfc232.*atmp_8_3+jfc233.*atmp_9_3-jfc233.*atmp_10_3-jfc232.*atmp_11_3+jfc332.*atmp_12_3+jfc333.*atmp_13_3-jfc333.*atmp_14_3-jfc332.*atmp_15_3;
    ttemp_3_4 = atmp_3_4+jfc132.*atmp_4_4+jfc133.*atmp_5_4-jfc123.*atmp_6_4-jfc132.*atmp_7_4+jfc232.*atmp_8_4+jfc233.*atmp_9_4-jfc233.*atmp_10_4-jfc232.*atmp_11_4+jfc332.*atmp_12_4+jfc333.*atmp_13_4-jfc333.*atmp_14_4-jfc332.*atmp_15_4;
    ttemp_3_5 = atmp_3_5+jfc132.*atmp_4_5+jfc133.*atmp_5_5-jfc123.*atmp_6_5-jfc132.*atmp_7_5+jfc232.*atmp_8_5+jfc233.*atmp_9_5-jfc233.*atmp_10_5-jfc232.*atmp_11_5+jfc332.*atmp_12_5+jfc333.*atmp_13_5-jfc333.*atmp_14_5-jfc332.*atmp_15_5;
    ttemp_3_6 = atmp_3_6+jfc132.*atmp_4_6+jfc133.*atmp_5_6-jfc123.*atmp_6_6-jfc132.*atmp_7_6+jfc232.*atmp_8_6+jfc233.*atmp_9_6-jfc233.*atmp_10_6-jfc232.*atmp_11_6+jfc332.*atmp_12_6+jfc333.*atmp_13_6-jfc333.*atmp_14_6-jfc332.*atmp_15_6;
    ttemp_3_7 = atmp_3_7+jfc132.*atmp_4_7+jfc133.*atmp_5_7-jfc123.*atmp_6_7-jfc132.*atmp_7_7+jfc232.*atmp_8_7+jfc233.*atmp_9_7-jfc233.*atmp_10_7-jfc232.*atmp_11_7+jfc332.*atmp_12_7+jfc333.*atmp_13_7-jfc333.*atmp_14_7-jfc332.*atmp_15_7;
    ttemp_3_8 = atmp_3_8+jfc132.*atmp_4_8+jfc133.*atmp_5_8-jfc123.*atmp_6_8-jfc132.*atmp_7_8+jfc232.*atmp_8_8+jfc233.*atmp_9_8-jfc233.*atmp_10_8-jfc232.*atmp_11_8+jfc332.*atmp_12_8+jfc333.*atmp_13_8-jfc333.*atmp_14_8-jfc332.*atmp_15_8;
    ttemp_3_9 = atmp_3_9+jfc132.*atmp_4_9+jfc133.*atmp_5_9-jfc123.*atmp_6_9-jfc132.*atmp_7_9+jfc232.*atmp_8_9+jfc233.*atmp_9_9-jfc233.*atmp_10_9-jfc232.*atmp_11_9+jfc332.*atmp_12_9+jfc333.*atmp_13_9-jfc333.*atmp_14_9-jfc332.*atmp_15_9;
    ttemp_3_10 = atmp_3_10+jfc132.*atmp_4_10+jfc133.*atmp_5_10-jfc123.*atmp_6_10-jfc132.*atmp_7_10+jfc232.*atmp_8_10+jfc233.*atmp_9_10-jfc233.*atmp_10_10-jfc232.*atmp_11_10+jfc332.*atmp_12_10+jfc333.*atmp_13_10-jfc333.*atmp_14_10-jfc332.*atmp_15_10;
    ttemp_3_11 = atmp_3_11+jfc132.*atmp_4_11+jfc133.*atmp_5_11-jfc123.*atmp_6_11-jfc132.*atmp_7_11+jfc232.*atmp_8_11+jfc233.*atmp_9_11-jfc233.*atmp_10_11-jfc232.*atmp_11_11+jfc332.*atmp_12_11+jfc333.*atmp_13_11-jfc333.*atmp_14_11-jfc332.*atmp_15_11;
    ttemp_3_12 = atmp_3_12+jfc132.*atmp_4_12+jfc133.*atmp_5_12-jfc123.*atmp_6_12-jfc132.*atmp_7_12+jfc232.*atmp_8_12+jfc233.*atmp_9_12-jfc233.*atmp_10_12-jfc232.*atmp_11_12+jfc332.*atmp_12_12+jfc333.*atmp_13_12-jfc333.*atmp_14_12-jfc332.*atmp_15_12;
    ttemp_3_13 = atmp_3_13+jfc132.*atmp_4_13+jfc133.*atmp_5_13-jfc123.*atmp_6_13-jfc132.*atmp_7_13+jfc232.*atmp_8_13+jfc233.*atmp_9_13-jfc233.*atmp_10_13-jfc232.*atmp_11_13+jfc332.*atmp_12_13+jfc333.*atmp_13_13-jfc333.*atmp_14_13-jfc332.*atmp_15_13;
    ttemp_3_14 = atmp_3_14+jfc132.*atmp_4_14+jfc133.*atmp_5_14-jfc123.*atmp_6_14-jfc132.*atmp_7_14+jfc232.*atmp_8_14+jfc233.*atmp_9_14-jfc233.*atmp_10_14-jfc232.*atmp_11_14+jfc332.*atmp_12_14+jfc333.*atmp_13_14-jfc333.*atmp_14_14-jfc332.*atmp_15_14;
    ttemp_3_15 = atmp_3_15+jfc132.*atmp_4_15+jfc133.*atmp_5_15-jfc123.*atmp_6_15-jfc132.*atmp_7_15+jfc232.*atmp_8_15+jfc233.*atmp_9_15-jfc233.*atmp_10_15-jfc232.*atmp_11_15+jfc332.*atmp_12_15+jfc333.*atmp_13_15-jfc333.*atmp_14_15-jfc332.*atmp_15_15;
     
    a_1_1 = ttemp_1_1+ttemp_1_4.*jfc112+ttemp_1_5.*jfc113-ttemp_1_6.*jfc113-ttemp_1_7.*jfc112+ttemp_1_8.*jfc212+ttemp_1_9.*jfc213-ttemp_1_10.*jfc213-ttemp_1_11.*jfc212+ttemp_1_12.*jfc312+ttemp_1_13.*jfc313-ttemp_1_14.*jfc313-ttemp_1_15.*jfc312;
    a_1_2 = ttemp_1_2+ttemp_1_4.*jfc122+ttemp_1_5.*jfc123-ttemp_1_6.*jfc123-ttemp_1_7.*jfc122+ttemp_1_8.*jfc222+ttemp_1_9.*jfc223-ttemp_1_10.*jfc223-ttemp_1_11.*jfc222+ttemp_1_12.*jfc322+ttemp_1_13.*jfc323-ttemp_1_14.*jfc323-ttemp_1_15.*jfc322;
    a_1_3 = ttemp_1_3+ttemp_1_4.*jfc132+ttemp_1_5.*jfc133-ttemp_1_6.*jfc123-ttemp_1_7.*jfc132+ttemp_1_8.*jfc232+ttemp_1_9.*jfc233-ttemp_1_10.*jfc233-ttemp_1_11.*jfc232+ttemp_1_12.*jfc332+ttemp_1_13.*jfc333-ttemp_1_14.*jfc333-ttemp_1_15.*jfc332;
    a_2_1 = a_1_2 ;
    a_2_2 = ttemp_2_2+ttemp_2_4.*jfc122+ttemp_2_5.*jfc123-ttemp_2_6.*jfc123-ttemp_2_7.*jfc122+ttemp_2_8.*jfc222+ttemp_2_9.*jfc223-ttemp_2_10.*jfc223-ttemp_2_11.*jfc222+ttemp_2_12.*jfc322+ttemp_2_13.*jfc323-ttemp_2_14.*jfc323-ttemp_2_15.*jfc322;
    a_2_3 = ttemp_2_3+ttemp_2_4.*jfc132+ttemp_2_5.*jfc133-ttemp_2_6.*jfc123-ttemp_2_7.*jfc132+ttemp_2_8.*jfc232+ttemp_2_9.*jfc233-ttemp_2_10.*jfc233-ttemp_2_11.*jfc232+ttemp_2_12.*jfc332+ttemp_2_13.*jfc333-ttemp_2_14.*jfc333-ttemp_2_15.*jfc332;
    a_3_1 = a_1_3 ;
    a_3_2 = a_2_3 ;
    a_3_3 = ttemp_3_3+ttemp_3_4.*jfc132+ttemp_3_5.*jfc133-ttemp_3_6.*jfc123-ttemp_3_7.*jfc132+ttemp_3_8.*jfc232+ttemp_3_9.*jfc233-ttemp_3_10.*jfc233-ttemp_3_11.*jfc232+ttemp_3_12.*jfc332+ttemp_3_13.*jfc333-ttemp_3_14.*jfc333-ttemp_3_15.*jfc332;
        
    % Adding the effect of the platform + the payload
    a_1_1 = a_1_1 + (m16+m17).*(j11.^2+j21.^2+j31.^2);
    a_1_2 = a_1_2 + (m16+m17).*(j11.*j12+j21.*j22+j31.*j32);
    a_1_3 = a_1_3 + (m16+m17).*(j11.*j13+j21.*j23+j31.*j33);
    a_2_1 = a_1_2 ;
    a_2_2 = a_2_2 + (m16+m17).*(j12.^2+j22.^2+j32.^2);
    a_2_3 = a_2_3 + (m16+m17).*(j12.*j13+j22.*j23+j32.*j33);
    a_3_1 = a_1_3 ;
    a_3_2 = a_2_3 ;
    a_3_3 = a_3_3 + (m16+m17).*(j13.^2+j23.^2+j33.^2);
    
% Vector of Coriolis Centrifugal gravity effects of the virtual tree structure
	f33=-(g2.*m3);
	dv334=-qd4.^2;
	f14=dv334.*mx4;
	f24=dv334.*my4;
	wi15=-(qd4.*s5);
	wi25=-(c5.*qd4);
	wp15=qd5.*wi25;
	wp25=-(qd5.*wi15);
	dv115=-wi15.^2;
	dv225=-wi25.^2;
	dv335=-qd5.^2;
	dv125=wi15.*wi25;
	dv135=qd5.*wi15;
	dv235=qd5.*wi25;
	u115=dv225 + dv335;
	u135=dv135 + wp25;
	u225=dv115 + dv335;
	u235=dv235 - wp15;
	u315=dv135 - wp25;
	u325=dv235 + wp15;
	u335=dv115 + dv225;
	vp15=g2.*s5;
	vp25=c5.*g2;
	f15=dv125.*my5 + mx5.*u115 + mz5.*u135 + m5.*vp15;
	f25=dv125.*mx5 + my5.*u225 + mz5.*u235 + m5.*vp25;
	f35=mx5.*u315 + my5.*u325 + mz5.*u335;
	pis15=-yy5 + zz5;
	pis25=xx5 - zz5;
	pis35=-xx5 + yy5;
	no15=dv235.*pis15 + wp15.*xx5 - u315.*xy5 + dv125.*xz5 + (-dv225 + dv335).*yz5;
	no25=dv135.*pis25 + u325.*xy5 + (dv115 - dv335).*xz5 + wp25.*yy5 - dv125.*yz5;
	no35=dv125.*pis35 + (-dv115 + dv225).*xy5 - u235.*xz5 + u135.*yz5;
	wi16=c6.*wi15 + s6.*wi25;
	wi26=-(s6.*wi15) + c6.*wi25;
	w36=qd5 + qd6;
	wp16=qd6.*wi26 + c6.*wp15 + s6.*wp25;
	wp26=-(qd6.*wi16) - s6.*wp15 + c6.*wp25;
	dv116=-wi16.^2;
	dv226=-wi26.^2;
	dv336=-w36.^2;
	dv126=wi16.*wi26;
	dv136=w36.*wi16;
	dv236=w36.*wi26;
	u116=dv226 + dv336;
	u136=dv136 + wp26;
	u226=dv116 + dv336;
	u236=dv236 - wp16;
	u316=dv136 - wp26;
	u326=dv236 + wp16;
	u336=dv116 + dv226;
	vsp16=d4.*u115 + vp15;
	vsp26=d4.*dv125 + vp25;
	vsp36=d4.*u315;
	vp16=c6.*vsp16 + s6.*vsp26;
	vp26=-(s6.*vsp16) + c6.*vsp26;
	f16=dv126.*my6 + mx6.*u116 + mz6.*u136 + m6.*vp16;
	f26=dv126.*mx6 + my6.*u226 + mz6.*u236 + m6.*vp26;
	f36=mx6.*u316 + my6.*u326 + mz6.*u336 + m6.*vsp36;
	pis16=-yy6 + zz6;
	pis26=xx6 - zz6;
	pis36=-xx6 + yy6;
	no16=dv236.*pis16 + wp16.*xx6 - u316.*xy6 + dv126.*xz6 + (-dv226 + dv336).*yz6;
	no26=dv136.*pis26 + u326.*xy6 + (dv116 - dv336).*xz6 + wp26.*yy6 - dv126.*yz6;
	no36=dv126.*pis36 + (-dv116 + dv226).*xy6 - u236.*xz6 + u136.*yz6;
	wi17=s7.*w36 + c7.*wi16;
	wi27=c7.*w36 - s7.*wi16;
	w37=qd7 - wi26;
	wp17=qd7.*wi27 + c7.*wp16;
	wp27=-(qd7.*wi17) - s7.*wp16;
	dv117=-wi17.^2;
	dv227=-wi27.^2;
	dv337=-w37.^2;
	dv127=wi17.*wi27;
	dv137=w37.*wi17;
	dv237=w37.*wi27;
	u117=dv227 + dv337;
	u127=dv127 + wp26;
	u137=dv137 + wp27;
	u217=dv127 - wp26;
	u227=dv117 + dv337;
	u237=dv237 - wp17;
	u317=dv137 - wp27;
	u327=dv237 + wp17;
	u337=dv117 + dv227;
	vp17=c7.*vp16 + s7.*vsp36;
	vp27=-(s7.*vp16) + c7.*vsp36;
	f17=mx7.*u117 + my7.*u127 + mz7.*u137 + m7.*vp17;
	f27=mx7.*u217 + my7.*u227 + mz7.*u237 + m7.*vp27;
	f37=mx7.*u317 + my7.*u327 + mz7.*u337 - m7.*vp26;
	pis17=-yy7 + zz7;
	pis27=xx7 - zz7;
	pis37=-xx7 + yy7;
	no17=dv237.*pis17 + wp17.*xx7 - u317.*xy7 + u217.*xz7 + (-dv227 + dv337).*yz7;
	no27=dv137.*pis27 + u327.*xy7 + (dv117 - dv337).*xz7 + wp27.*yy7 - u127.*yz7;
	no37=dv127.*pis37 + (-dv117 + dv227).*xy7 - u237.*xz7 + u137.*yz7 - wp26.*zz7;
	dv338=-qd8.^2;
	vp18=-(c8.*g2);
	vp28=g2.*s8;
	f18=dv338.*mx8 + m8.*vp18;
	f28=dv338.*my8 + m8.*vp28;
	wi19=-(qd8.*s9);
	wi29=-(c9.*qd8);
	wp19=qd9.*wi29;
	wp29=-(qd9.*wi19);
	dv119=-wi19.^2;
	dv229=-wi29.^2;
	dv339=-qd9.^2;
	dv129=wi19.*wi29;
	dv139=qd9.*wi19;
	dv239=qd9.*wi29;
	u119=dv229 + dv339;
	u139=dv139 + wp29;
	u229=dv119 + dv339;
	u239=dv239 - wp19;
	u319=dv139 - wp29;
	u329=dv239 + wp19;
	u339=dv119 + dv229;
	vp19=c9.*vp18;
	vp29=-(s9.*vp18);
	f19=dv129.*my9 + mx9.*u119 + mz9.*u139 + m9.*vp19;
	f29=dv129.*mx9 + my9.*u229 + mz9.*u239 + m9.*vp29;
	f39=mx9.*u319 + my9.*u329 + mz9.*u339 + m9.*vp28;
	pis19=-yy9 + zz9;
	pis29=xx9 - zz9;
	pis39=-xx9 + yy9;
	no19=dv239.*pis19 + wp19.*xx9 - u319.*xy9 + dv129.*xz9 + (-dv229 + dv339).*yz9;
	no29=dv139.*pis29 + u329.*xy9 + (dv119 - dv339).*xz9 + wp29.*yy9 - dv129.*yz9;
	no39=dv129.*pis39 + (-dv119 + dv229).*xy9 - u239.*xz9 + u139.*yz9;
	wi110=c10.*wi19 + s10.*wi29;
	wi210=-(s10.*wi19) + c10.*wi29;
	w310=qd10 + qd9;
	wp110=qd10.*wi210 + c10.*wp19 + s10.*wp29;
	wp210=-(qd10.*wi110) - s10.*wp19 + c10.*wp29;
	dv1110=-wi110.^2;
	dv2210=-wi210.^2;
	dv3310=-w310.^2;
	dv1210=wi110.*wi210;
	dv1310=w310.*wi110;
	dv2310=w310.*wi210;
	u1110=dv2210 + dv3310;
	u1310=dv1310 + wp210;
	u2210=dv1110 + dv3310;
	u2310=dv2310 - wp110;
	u3110=dv1310 - wp210;
	u3210=dv2310 + wp110;
	u3310=dv1110 + dv2210;
	vsp110=d4.*u119 + vp19;
	vsp210=d4.*dv129 + vp29;
	vsp310=d4.*u319 + vp28;
	vp110=c10.*vsp110 + s10.*vsp210;
	vp210=-(s10.*vsp110) + c10.*vsp210;
	f110=dv1210.*my10 + mx10.*u1110 + mz10.*u1310 + m10.*vp110;
	f210=dv1210.*mx10 + my10.*u2210 + mz10.*u2310 + m10.*vp210;
	f310=mx10.*u3110 + my10.*u3210 + mz10.*u3310 + m10.*vsp310;
	pis110=-yy10 + zz10;
	pis210=xx10 - zz10;
	pis310=-xx10 + yy10;
	no110=dv2310.*pis110 + wp110.*xx10 - u3110.*xy10 + dv1210.*xz10 + (-dv2210 + dv3310).*yz10;
	no210=dv1310.*pis210 + u3210.*xy10 + (dv1110 - dv3310).*xz10 + wp210.*yy10 - dv1210.*yz10;
	no310=dv1210.*pis310 + (-dv1110 + dv2210).*xy10 - u2310.*xz10 + u1310.*yz10;
	wi111=s11.*w310 + c11.*wi110;
	wi211=c11.*w310 - s11.*wi110;
	w311=qd11 - wi210;
	wp111=qd11.*wi211 + c11.*wp110;
	wp211=-(qd11.*wi111) - s11.*wp110;
	dv1111=-wi111.^2;
	dv2211=-wi211.^2;
	dv3311=-w311.^2;
	dv1211=wi111.*wi211;
	dv1311=w311.*wi111;
	dv2311=w311.*wi211;
	u1111=dv2211 + dv3311;
	u1211=dv1211 + wp210;
	u1311=dv1311 + wp211;
	u2111=dv1211 - wp210;
	u2211=dv1111 + dv3311;
	u2311=dv2311 - wp111;
	u3111=dv1311 - wp211;
	u3211=dv2311 + wp111;
	u3311=dv1111 + dv2211;
	vp111=c11.*vp110 + s11.*vsp310;
	vp211=-(s11.*vp110) + c11.*vsp310;
	f111=mx11.*u1111 + my11.*u1211 + mz11.*u1311 + m11.*vp111;
	f211=mx11.*u2111 + my11.*u2211 + mz11.*u2311 + m11.*vp211;
	f311=mx11.*u3111 + my11.*u3211 + mz11.*u3311 - m11.*vp210;
	pis111=-yy11 + zz11;
	pis211=xx11 - zz11;
	pis311=-xx11 + yy11;
	no111=dv2311.*pis111 + wp111.*xx11 - u3111.*xy11 + u2111.*xz11 + (-dv2211 + dv3311).*yz11;
	no211=dv1311.*pis211 + u3211.*xy11 + (dv1111 - dv3311).*xz11 + wp211.*yy11 - u1211.*yz11;
	no311=dv1211.*pis311 + (-dv1111 + dv2211).*xy11 - u2311.*xz11 + u1311.*yz11 - wp210.*zz11;
	dv3312=-qd12.^2;
	vp112=g2.*s12;
	vp212=c12.*g2;
	f112=dv3312.*mx12 + m12.*vp112;
	f212=dv3312.*my12 + m12.*vp212;
	wi113=-(qd12.*s13);
	wi213=-(c13.*qd12);
	wp113=qd13.*wi213;
	wp213=-(qd13.*wi113);
	dv1113=-wi113.^2;
	dv2213=-wi213.^2;
	dv3313=-qd13.^2;
	dv1213=wi113.*wi213;
	dv1313=qd13.*wi113;
	dv2313=qd13.*wi213;
	u1113=dv2213 + dv3313;
	u1313=dv1313 + wp213;
	u2213=dv1113 + dv3313;
	u2313=dv2313 - wp113;
	u3113=dv1313 - wp213;
	u3213=dv2313 + wp113;
	u3313=dv1113 + dv2213;
	vp113=c13.*vp112;
	vp213=-(s13.*vp112);
	f113=dv1213.*my13 + mx13.*u1113 + mz13.*u1313 + m13.*vp113;
	f213=dv1213.*mx13 + my13.*u2213 + mz13.*u2313 + m13.*vp213;
	f313=mx13.*u3113 + my13.*u3213 + mz13.*u3313 + m13.*vp212;
	pis113=-yy13 + zz13;
	pis213=xx13 - zz13;
	pis313=-xx13 + yy13;
	no113=dv2313.*pis113 + wp113.*xx13 - u3113.*xy13 + dv1213.*xz13 + (-dv2213 + dv3313).*yz13;
	no213=dv1313.*pis213 + u3213.*xy13 + (dv1113 - dv3313).*xz13 + wp213.*yy13 - dv1213.*yz13;
	no313=dv1213.*pis313 + (-dv1113 + dv2213).*xy13 - u2313.*xz13 + u1313.*yz13;
	wi114=c14.*wi113 + s14.*wi213;
	wi214=-(s14.*wi113) + c14.*wi213;
	w314=qd13 + qd14;
	wp114=qd14.*wi214 + c14.*wp113 + s14.*wp213;
	wp214=-(qd14.*wi114) - s14.*wp113 + c14.*wp213;
	dv1114=-wi114.^2;
	dv2214=-wi214.^2;
	dv3314=-w314.^2;
	dv1214=wi114.*wi214;
	dv1314=w314.*wi114;
	dv2314=w314.*wi214;
	u1114=dv2214 + dv3314;
	u1314=dv1314 + wp214;
	u2214=dv1114 + dv3314;
	u2314=dv2314 - wp114;
	u3114=dv1314 - wp214;
	u3214=dv2314 + wp114;
	u3314=dv1114 + dv2214;
	vsp114=d4.*u1113 + vp113;
	vsp214=d4.*dv1213 + vp213;
	vsp314=d4.*u3113 + vp212;
	vp114=c14.*vsp114 + s14.*vsp214;
	vp214=-(s14.*vsp114) + c14.*vsp214;
	f114=dv1214.*my14 + mx14.*u1114 + mz14.*u1314 + m14.*vp114;
	f214=dv1214.*mx14 + my14.*u2214 + mz14.*u2314 + m14.*vp214;
	f314=mx14.*u3114 + my14.*u3214 + mz14.*u3314 + m14.*vsp314;
	pis114=-yy14 + zz14;
	pis214=xx14 - zz14;
	pis314=-xx14 + yy14;
	no114=dv2314.*pis114 + wp114.*xx14 - u3114.*xy14 + dv1214.*xz14 + (-dv2214 + dv3314).*yz14;
	no214=dv1314.*pis214 + u3214.*xy14 + (dv1114 - dv3314).*xz14 + wp214.*yy14 - dv1214.*yz14;
	no314=dv1214.*pis314 + (-dv1114 + dv2214).*xy14 - u2314.*xz14 + u1314.*yz14;
	wi115=s15.*w314 + c15.*wi114;
	wi215=c15.*w314 - s15.*wi114;
	w315=qd15 - wi214;
	wp115=qd15.*wi215 + c15.*wp114;
	wp215=-(qd15.*wi115) - s15.*wp114;
	dv1115=-wi115.^2;
	dv2215=-wi215.^2;
	dv3315=-w315.^2;
	dv1215=wi115.*wi215;
	dv1315=w315.*wi115;
	dv2315=w315.*wi215;
	u1115=dv2215 + dv3315;
	u1215=dv1215 + wp214;
	u1315=dv1315 + wp215;
	u2115=dv1215 - wp214;
	u2215=dv1115 + dv3315;
	u2315=dv2315 - wp115;
	u3115=dv1315 - wp215;
	u3215=dv2315 + wp115;
	u3315=dv1115 + dv2215;
	vp115=c15.*vp114 + s15.*vsp314;
	vp215=-(s15.*vp114) + c15.*vsp314;
	f115=mx15.*u1115 + my15.*u1215 + mz15.*u1315 + m15.*vp115;
	f215=mx15.*u2115 + my15.*u2215 + mz15.*u2315 + m15.*vp215;
	f315=mx15.*u3115 + my15.*u3215 + mz15.*u3315 - m15.*vp214;
	pis115=-yy15 + zz15;
	pis215=xx15 - zz15;
	pis315=-xx15 + yy15;
	no115=dv2315.*pis115 + wp115.*xx15 - u3115.*xy15 + u2115.*xz15 + (-dv2215 + dv3315).*yz15;
	no215=dv1315.*pis215 + u3215.*xy15 + (dv1115 - dv3315).*xz15 + wp215.*yy15 - u1215.*yz15;
	no315=dv1215.*pis315 + (-dv1115 + dv2215).*xy15 - u2315.*xz15 + u1315.*yz15 - wp214.*zz15;
	n115=no115 - my15.*vp214 - mz15.*vp215;
	n215=no215 + mz15.*vp115 + mx15.*vp214;
	n315=no315 - my15.*vp115 + mx15.*vp215;
	fdi115=c15.*f115 - f215.*s15;
	fdi315=c15.*f215 + f115.*s15;
	e114=f114 + fdi115;
	e214=f214 - f315;
	e314=f314 + fdi315;
	n114=c15.*n115 + no114 - n215.*s15 - mz14.*vp214 + my14.*vsp314;
	n214=-n315 + no214 + mz14.*vp114 - mx14.*vsp314;
	n314=c15.*n215 + no314 + n115.*s15 - my14.*vp114 + mx14.*vp214;
	fdi114=c14.*e114 - e214.*s14;
	fdi214=c14.*e214 + e114.*s14;
	e113=f113 + fdi114;
	e213=f213 + fdi214;
	e313=e314 + f313;
	n113=c14.*n114 + no113 - n214.*s14 + my13.*vp212 - mz13.*vp213;
	n213=-(d4.*e314) + c14.*n214 + no213 + n114.*s14 + mz13.*vp113 - mx13.*vp212;
	n313=d4.*fdi214 + n314 + no313 - my13.*vp113 + mx13.*vp213;
	fdi113=c13.*e113 - e213.*s13;
	e112=f112 + fdi113;
	e212=e313 + f212;
	n312=-(c13.*n213) - n113.*s13 - my12.*vp112 + mx12.*vp212;
	fdi312=-(c12.*e212) - e112.*s12;
	n111=no111 - my11.*vp210 - mz11.*vp211;
	n211=no211 + mz11.*vp111 + mx11.*vp210;
	n311=no311 - my11.*vp111 + mx11.*vp211;
	fdi111=c11.*f111 - f211.*s11;
	fdi311=c11.*f211 + f111.*s11;
	e110=f110 + fdi111;
	e210=f210 - f311;
	e310=f310 + fdi311;
	n110=c11.*n111 + no110 - n211.*s11 - mz10.*vp210 + my10.*vsp310;
	n210=-n311 + no210 + mz10.*vp110 - mx10.*vsp310;
	n310=c11.*n211 + no310 + n111.*s11 - my10.*vp110 + mx10.*vp210;
	fdi110=c10.*e110 - e210.*s10;
	fdi210=c10.*e210 + e110.*s10;
	e19=f19 + fdi110;
	e29=f29 + fdi210;
	e39=e310 + f39;
	n19=c10.*n110 + no19 - n210.*s10 + my9.*vp28 - mz9.*vp29;
	n29=-(d4.*e310) + c10.*n210 + no29 + n110.*s10 + mz9.*vp19 - mx9.*vp28;
	n39=d4.*fdi210 + n310 + no39 - my9.*vp19 + mx9.*vp29;
	fdi19=c9.*e19 - e29.*s9;
	e18=f18 + fdi19;
	e28=e39 + f28;
	n38=-(c9.*n29) - n19.*s9 - my8.*vp18 + mx8.*vp28;
	fdi38=-(c8.*e28) - e18.*s8;
	n17=no17 - my7.*vp26 - mz7.*vp27;
	n27=no27 + mz7.*vp17 + mx7.*vp26;
	n37=no37 - my7.*vp17 + mx7.*vp27;
	fdi17=c7.*f17 - f27.*s7;
	fdi37=c7.*f27 + f17.*s7;
	e16=f16 + fdi17;
	e26=f26 - f37;
	e36=f36 + fdi37;
	n16=c7.*n17 + no16 - n27.*s7 - mz6.*vp26 + my6.*vsp36;
	n26=-n37 + no26 + mz6.*vp16 - mx6.*vsp36;
	n36=c7.*n27 + no36 + n17.*s7 - my6.*vp16 + mx6.*vp26;
	fdi16=c6.*e16 - e26.*s6;
	fdi26=c6.*e26 + e16.*s6;
	e15=f15 + fdi16;
	e25=f25 + fdi26;
	e35=e36 + f35;
	n15=c6.*n16 + no15 - n26.*s6 - mz5.*vp25;
	n25=-(d4.*e36) + c6.*n26 + no25 + n16.*s6 + mz5.*vp15;
	n35=d4.*fdi26 + n36 + no35 - my5.*vp15 + mx5.*vp25;
	fdi15=c5.*e15 - e25.*s5;
	e14=f14 + fdi15;
	e24=e35 + f24;
	n34=-(c5.*n25) - n15.*s5;
	fdi34=-(c4.*e24) - e14.*s4;
	e33=f33 + fdi312;
	htemp1=fdi34 + fv1.*qd1 + fs1.*sign(qd1);
	htemp2=fdi38 + fv2.*qd2 + fs2.*sign(qd2);
	htemp3=e33 + fv3.*qd3 + fs3.*sign(qd3);
	htemp4=n34 + fv4.*qd4 + fs4.*sign(qd4);
	htemp5=n35 + fv5.*qd5 + fs5.*sign(qd5);
	htemp6=n36 + fv6.*qd6 + fs6.*sign(qd6);
	htemp7=n37 + fv7.*qd7 + fs7.*sign(qd7);
	htemp8=n38 + fv8.*qd8 + fs8.*sign(qd8);
	htemp9=n39 + fv9.*qd9 + fs9.*sign(qd9);
	htemp10=n310 + fv10.*qd10 + fs10.*sign(qd10);
	htemp11=n311 + fv11.*qd11 + fs11.*sign(qd11);
	htemp12=n312 + fv12.*qd12 + fs12.*sign(qd12);
	htemp13=n313 + fv13.*qd13 + fs13.*sign(qd13);
	htemp14=n314 + fv14.*qd14 + fs14.*sign(qd14);
	htemp15=n315 + fv15.*qd15 + fs15.*sign(qd15);

    
% Vector of Coriolis, centrifugal and graivity effects of the real robot
    h1 = htemp1+jfc112.*htemp4+jfc113.*htemp5-jfc113.*htemp6-jfc112.*htemp7+jfc212.*htemp8+jfc213.*htemp9-jfc213.*htemp10-jfc212.*htemp11+jfc312.*htemp12+jfc313.*htemp13-jfc313.*htemp14-jfc312.*htemp15;
    h2 = htemp2+jfc122.*htemp4+jfc123.*htemp5-jfc123.*htemp6-jfc122.*htemp7+jfc222.*htemp8+jfc223.*htemp9-jfc223.*htemp10-jfc222.*htemp11+jfc322.*htemp12+jfc323.*htemp13-jfc323.*htemp14-jfc322.*htemp15;
    h3 = htemp3+jfc132.*htemp4+jfc133.*htemp5-jfc123.*htemp6-jfc132.*htemp7+jfc232.*htemp8+jfc233.*htemp9-jfc233.*htemp10-jfc232.*htemp11+jfc332.*htemp12+jfc333.*htemp13-jfc333.*htemp14-jfc332.*htemp15;
    
    % Derivative of the matrix linking the effort of the virtual chain 1 to the real input efforts
    dejc12 = dejc1.^2;
	jfc112d=((jc123d.*j11-j21d.*jc113 + jc123.*j11d-j21.*jc113d).*dejc1 - (jc123.*j11-j21.*jc113).*dejc1d)./dejc12;
	jfc113d=((jc112d.*j21 + jc112.*j21d).*dejc1 - (jc112.*j21).*dejc1d)./dejc12;
	jfc122d=((jc123d.*j12-j22d.*jc113 + jc123.*j12d-j22.*jc113d).*dejc1 - (jc123.*j12-j22.*jc113).*dejc1d)./dejc12;
	jfc123d=((jc112d.*j22 + jc112.*j22d).*dejc1 - (jc112.*j22).*dejc1d)./dejc12;
	jfc132d=((jc123d.*j13-j23d.*jc113 + jc123.*j13d-j23.*jc113d).*dejc1 - (jc123.*j13-j23.*jc113).*dejc1d)./dejc12;
	jfc133d=((jc112d.*j23 + jc112.*j23d).*dejc1 - (jc112.*j23).*dejc1d)./dejc12;

    % Derivative of the matrix linking the effort of the virtual chain 2 to the real input efforts
	dejc22 = dejc2.^2;
    jfc212d=((jc233d.*j21-j31d.*jc223 + jc233.*j21d-j31.*jc223d).*dejc2 - (jc233.*j21-j31.*jc223).*dejc2d)./dejc22;
	jfc213d=((jc222d.*j31 + jc222.*j31d).*dejc2 - (jc222.*j31).*dejc2d)./dejc22;
	jfc222d=((jc233d.*j22-j32d.*jc223 + jc233.*j22d-j32.*jc223d).*dejc2 - (jc233.*j22-j32.*jc223).*dejc2d)./dejc22;
	jfc223d=((jc222d.*j32 + jc222.*j32d).*dejc2 - (jc222.*j32).*dejc2d)./dejc22;
	jfc232d=((jc233d.*j23-j33d.*jc223 + jc233.*j23d-j33.*jc223d).*dejc2 - (jc233.*j23-j33.*jc223).*dejc2d)./dejc22;
	jfc233d=((jc222d.*j33 + jc222.*j33d).*dejc2 - (jc222.*j33).*dejc2d)./dejc22;

    % Derivative of the matrix linking the effort of the virtual chain 3 to the real input efforts
	dejc32 = dejc3.^2;
    jfc312d=((-j11d.*jc333+jc313d.*j31 - j11.*jc333d+jc313.*j31d).*dejc3 - (-j11.*jc333+jc313.*j31).*dejc3d)./dejc32;
	jfc313d=((jc332d.*j11 + jc332.*j11d).*dejc3 - (jc332.*j11).*dejc3d)./dejc32;
	jfc322d=((-j12d.*jc333+jc313d.*j32 - j12.*jc333d+jc313.*j32d).*dejc3 - (-j12.*jc333+jc313.*j32).*dejc3d)./dejc32;
	jfc323d=((jc332d.*j12 + jc332.*j12d).*dejc3 - (jc332.*j12).*dejc3d)./dejc32;
	jfc332d=((-j13d.*jc333+jc313d.*j33 - j13.*jc333d+jc313.*j33d).*dejc3 - (-j13.*jc333+jc313.*j33).*dejc3d)./dejc32;
	jfc333d=((jc332d.*j13 + jc332.*j13d).*dejc3 - (jc332.*j13).*dejc3d)./dejc32;	
    
    h1 = h1+(ttemp_1_4.*jfc112d+ttemp_1_5.*jfc113d-ttemp_1_6.*jfc113d-ttemp_1_7.*jfc112d+ttemp_1_8.*jfc212d+ttemp_1_9.*jfc213d-ttemp_1_10.*jfc213d-ttemp_1_11.*jfc212d+ttemp_1_12.*jfc312d+ttemp_1_13.*jfc313d-ttemp_1_14.*jfc313d-ttemp_1_15.*jfc312d).*qd1+(ttemp_1_4.*jfc122d+ttemp_1_5.*jfc123d-ttemp_1_6.*jfc123d-ttemp_1_7.*jfc122d+ttemp_1_8.*jfc222d+ttemp_1_9.*jfc223d-ttemp_1_10.*jfc223d-ttemp_1_11.*jfc222d+ttemp_1_12.*jfc322d+ttemp_1_13.*jfc323d-ttemp_1_14.*jfc323d-ttemp_1_15.*jfc322d).*qd2+(ttemp_1_4.*jfc132d+ttemp_1_5.*jfc133d-ttemp_1_6.*jfc123d-ttemp_1_7.*jfc132d+ttemp_1_8.*jfc232d+ttemp_1_9.*jfc233d-ttemp_1_10.*jfc233d-ttemp_1_11.*jfc232d+ttemp_1_12.*jfc332d+ttemp_1_13.*jfc333d-ttemp_1_14.*jfc333d-ttemp_1_15.*jfc332d).*qd3;
    h2 = h2+(ttemp_2_4.*jfc112d+ttemp_2_5.*jfc113d-ttemp_2_6.*jfc113d-ttemp_2_7.*jfc112d+ttemp_2_8.*jfc212d+ttemp_2_9.*jfc213d-ttemp_2_10.*jfc213d-ttemp_2_11.*jfc212d+ttemp_2_12.*jfc312d+ttemp_2_13.*jfc313d-ttemp_2_14.*jfc313d-ttemp_2_15.*jfc312d).*qd1+(ttemp_2_4.*jfc122d+ttemp_2_5.*jfc123d-ttemp_2_6.*jfc123d-ttemp_2_7.*jfc122d+ttemp_2_8.*jfc222d+ttemp_2_9.*jfc223d-ttemp_2_10.*jfc223d-ttemp_2_11.*jfc222d+ttemp_2_12.*jfc322d+ttemp_2_13.*jfc323d-ttemp_2_14.*jfc323d-ttemp_2_15.*jfc322d).*qd2+(ttemp_2_4.*jfc132d+ttemp_2_5.*jfc133d-ttemp_2_6.*jfc123d-ttemp_2_7.*jfc132d+ttemp_2_8.*jfc232d+ttemp_2_9.*jfc233d-ttemp_2_10.*jfc233d-ttemp_2_11.*jfc232d+ttemp_2_12.*jfc332d+ttemp_2_13.*jfc333d-ttemp_2_14.*jfc333d-ttemp_2_15.*jfc332d).*qd3;
    h3 = h3+(ttemp_3_4.*jfc112d+ttemp_3_5.*jfc113d-ttemp_3_6.*jfc113d-ttemp_3_7.*jfc112d+ttemp_3_8.*jfc212d+ttemp_3_9.*jfc213d-ttemp_3_10.*jfc213d-ttemp_3_11.*jfc212d+ttemp_3_12.*jfc312d+ttemp_3_13.*jfc313d-ttemp_3_14.*jfc313d-ttemp_3_15.*jfc312d).*qd1+(ttemp_3_4.*jfc122d+ttemp_3_5.*jfc123d-ttemp_3_6.*jfc123d-ttemp_3_7.*jfc122d+ttemp_3_8.*jfc222d+ttemp_3_9.*jfc223d-ttemp_3_10.*jfc223d-ttemp_3_11.*jfc222d+ttemp_3_12.*jfc322d+ttemp_3_13.*jfc323d-ttemp_3_14.*jfc323d-ttemp_3_15.*jfc322d).*qd2+(ttemp_3_4.*jfc132d+ttemp_3_5.*jfc133d-ttemp_3_6.*jfc123d-ttemp_3_7.*jfc132d+ttemp_3_8.*jfc232d+ttemp_3_9.*jfc233d-ttemp_3_10.*jfc233d-ttemp_3_11.*jfc232d+ttemp_3_12.*jfc332d+ttemp_3_13.*jfc333d-ttemp_3_14.*jfc333d-ttemp_3_15.*jfc332d).*qd3;
    
    % Adding the effect of the platform + payload
    h1 = h1+((j11.*j11d+j21.*j21d+j31.*j31d).*qd1+(j11.*j12d+j21.*j22d+j31.*j32d).*qd2+(j11.*j13d+j21.*j23d+j31.*j33d).*qd3 - g2.*j21).*m17;
    h2 = h2+((j12.*j11d+j22.*j21d+j32.*j31d).*qd1+(j12.*j12d+j22.*j22d+j32.*j32d).*qd2+(j12.*j13d+j22.*j23d+j32.*j33d).*qd3 - g2.*j22).*m17;
    h3 = h3+((j13.*j11d+j23.*j21d+j33.*j31d).*qd1+(j13.*j12d+j23.*j22d+j33.*j32d).*qd2+(j13.*j13d+j23.*j23d+j33.*j33d).*qd3 - g2.*j23).*m17; 
    
    % Solving the direct dynamic model
    detmata = a_1_1.*a_2_2.*a_3_3-a_1_1.*a_2_3.*a_3_2-a_2_1.*a_1_2.*a_3_3+a_2_1.*a_1_3.*a_3_2+a_3_1.*a_1_2.*a_2_3-a_3_1.*a_1_3.*a_2_2 ;
    ia_1_1 = (a_2_2.*a_3_3-a_2_3.*a_3_2)./detmata ;
    ia_1_2 = (-a_1_2.*a_3_3+a_1_3.*a_3_2)./detmata ;
    ia_1_3 = (a_1_2.*a_2_3-a_1_3.*a_2_2)./detmata ;
    ia_2_1 = ia_1_2 ;
    ia_2_2 = (a_1_1.*a_3_3-a_1_3.*a_3_1)./detmata ;
    ia_2_3 = (-a_1_1.*a_2_3+a_1_3.*a_2_1)./detmata ;
    ia_3_1 = ia_1_3 ;
    ia_3_2 = ia_2_3 ;
    ia_3_3 = (a_1_1.*a_2_2-a_1_2.*a_2_1)./detmata ;
    
    qdd1tmp=ia_1_1.*(gam1-h1)+ia_1_2.*(gam2-h2)+ia_1_3.*(gam3-h3);
    qdd2tmp=ia_2_1.*(gam1-h1)+ia_2_2.*(gam2-h2)+ia_2_3.*(gam3-h3);
    qdd3tmp=ia_3_1.*(gam1-h1)+ia_3_2.*(gam2-h2)+ia_3_3.*(gam3-h3);

