% (********************************************)
% (** 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.dyn




%      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



%  external forces,friction parameters, joint velocities and accelerations

% j       fx      fy      fz      cx      cy      cz      fs      fv      qp      qdp

% 1       0       0       0       0       0       0       fs1     fv1     qd1     qdd1

% 2       0       0       0       0       0       0       fs2     fv2     qd2     qdd2

% 3       0       0       0       0       0       0       fs3     fv3     qd3     qdd3

% 4       0       0       0       0       0       0       0       0       qd4     qdd4

% 5       0       0       0       0       0       0       fs5     0       qd5     qdd5

% 6       0       0       0       0       0       0       0       0       qd6     qdd6

% 7       0       0       0       0       0       0       0       0       qd7     qdd7

% 8       0       0       0       0       0       0       0       0       qd8     qdd8

% 9       0       0       0       0       0       0       fs9     0       qd9     qdd9

% 10      0       0       0       0       0       0       0       0       qd10    qdd10

% 11      0       0       0       0       0       0       0       0       qd11    qdd11

% 12      0       0       0       0       0       0       0       0       qd12    qdd12

% 13      0       0       0       0       0       0       0       0       qd13    qdd13

% 14      0       0       0       0       0       0       0       0       qd14    qdd14

% 15      0       0       0       0       0       0       0       0       qd15    qdd15

% base velocity, base accelerations, and gravity

% j     w0    wp0   v0    vp0   g

% 1     0     0     0     0     0

% 2     0     0     0     0     g2

% 3     0     0     0     0     0

%  dynamic model: newton euler method
% equations:

% declaration of the function
function orthoglide_dyn()

% declaration of global input variables
global a g2 d4 d6


% actuator position, velocity and acceleration
global q1 q2 q3
global qd1 qd2 qd3
global qdd1 qdd2 qdd3



global q4 q5 q6 q7 q8 q9 q10 q11 q12 q13
global q14 q15 qdd1 qdd2 qdd3 qd4


% Function description:
    un = ones(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 of the first order
	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;	
	
% Forward kinematic model of the second order
	rhodd1=qdd1;
	rhodd2=qdd2;
	rhodd3=qdd3;	
	ad11=posdx;
	ad12=posdy;
	ad13=posdz - rhod1;
	ad21=posdx - rhod2;
	ad22=posdy;
	ad23=posdz;
	ad31=posdx;
	ad32=posdy - rhod3;
	ad33=posdz;
	bd11=ad13;
	bd22=ad21;
	bd33=ad32;
	m1mcd2o11=(-a22.*ad11.*a33 + a22.*ad31.*a13 + ad21.*a12.*a33 + ad11.*a23.*a32 - ad31.*a12.*a23 - ad21.*a13.*a32)./deta;
	m1mcd2o12=(-a22.*ad12.*a33 + a22.*ad32.*a13 + ad22.*a12.*a33 - ad32.*a12.*a23 + ad12.*a23.*a32 - ad22.*a13.*a32)./deta;
	m1mcd2o13=(-a22.*ad13.*a33 + a22.*ad33.*a13 + ad23.*a12.*a33 + ad13.*a23.*a32 - ad33.*a12.*a23 - ad23.*a13.*a32)./deta;
	m1mcd2o21=((-ad21.*a33 + ad31.*a23).*a11 + ad11.*a21.*a33 - ad31.*a13.*a21 - ad11.*a23.*a31 + ad21.*a13.*a31)./deta;
	m1mcd2o22=((-ad22.*a33 + ad32.*a23).*a11 + ad12.*a21.*a33 - ad12.*a23.*a31 - ad32.*a13.*a21 + ad22.*a13.*a31)./deta;
	m1mcd2o23=((-ad23.*a33 + ad33.*a23).*a11 + ad13.*a21.*a33 - ad13.*a23.*a31 + ad23.*a13.*a31 - ad33.*a13.*a21)./deta;
	m1mcd2o31=((-ad31.*a22 + ad21.*a32).*a11 + ad11.*a31.*a22 + ad31.*a12.*a21 - ad21.*a12.*a31 - ad11.*a21.*a32)./deta;
	m1mcd2o32=((-ad32.*a22 + ad22.*a32).*a11 + ad12.*a31.*a22 - ad12.*a21.*a32 - ad22.*a12.*a31 + ad32.*a12.*a21)./deta;
	m1mcd2o33=((-ad33.*a22 + ad23.*a32).*a11 + ad13.*a31.*a22 - ad23.*a12.*a31 + ad33.*a12.*a21 - ad13.*a21.*a32)./deta;
	m2mcd2o11=((a22.*a33 - a23.*a32).*ad13)./deta;
	m2mcd2o12=(-(a12.*a33 - a13.*a32).*ad21)./deta;
	m2mcd2o13=((a12.*a23 - a13.*a22).*ad32)./deta;
	m2mcd2o21=(-(a21.*a33 - a23.*a31).*ad13)./deta;
	m2mcd2o22=((a11.*a33 - a13.*a31).*ad21)./deta;
	m2mcd2o23=(-(a11.*a23 - a13.*a21).*ad32)./deta;
	m2mcd2o31=((a21.*a32 - a22.*a31).*ad13)./deta;
	m2mcd2o32=(-(a11.*a32 - a12.*a31).*ad21)./deta;
	m2mcd2o33=((a11.*a22 - a12.*a21).*ad32)./deta;
	m3mcd2o11=((a22.*a33 - a23.*a32).*a13)./deta;
	m3mcd2o12=(-(a12.*a33 - a13.*a32).*a21)./deta;
	m3mcd2o13=((a12.*a23 - a13.*a22).*a32)./deta;
	m3mcd2o21=(-(a21.*a33 - a23.*a31).*a13)./deta;
	m3mcd2o22=((a11.*a33 - a13.*a31).*a21)./deta;
	m3mcd2o23=(-(a11.*a23 - a13.*a21).*a32)./deta;
	m3mcd2o31=((a21.*a32 - a22.*a31).*a13)./deta;
	m3mcd2o32=(-(a11.*a32 - a12.*a31).*a21)./deta;
	m3mcd2o33=((a11.*a22 - a12.*a21).*a32)./deta;	
	posddx=(m1mcd2o11.*posdx + m1mcd2o12.*posdy + m1mcd2o13.*posdz) + (m2mcd2o11.*rhod1 + m2mcd2o12.*rhod2 + m2mcd2o13.*rhod3) + (m3mcd2o11.*rhodd1 + m3mcd2o12.*rhodd2 + m3mcd2o13.*rhodd3);
	posddy=(m1mcd2o21.*posdx + m1mcd2o22.*posdy + m1mcd2o23.*posdz) + (m2mcd2o21.*rhod1 + m2mcd2o22.*rhod2 + m2mcd2o23.*rhod3) + (m3mcd2o21.*rhodd1 + m3mcd2o22.*rhodd2 + m3mcd2o23.*rhodd3);
	posddz=(m1mcd2o31.*posdx + m1mcd2o32.*posdy + m1mcd2o33.*posdz) + (m2mcd2o31.*rhod1 + m2mcd2o32.*rhod2 + m2mcd2o33.*rhod3) + (m3mcd2o31.*rhodd1 + m3mcd2o32.*rhodd2 + m3mcd2o33.*rhodd3);
	
	jc1d12=-d4.*(qd4.*c4.*c5 - qd5.*s4.*s5);
	jc1d13=-d4.*(-qd4.*s4.*s5 + qd5.*c4.*c5);
	jc1d23=+d4.*qd5.*s5;
	jc1d32=-d4.*(-qd4.*s4.*c5 - qd5.*c4.*s5);
	jc1d33=d4.*(qd4.*c4.*s5 + qd5.*s4.*c5);
	mc1mcd2o12=(jc123.*(jc1d12.*jc132 - jc112.*jc1d32))./dejc1;
	mc1mcd2o13=(jc123.*jc132.*jc1d13 + jc133.*jc112.*jc1d23 - jc1d23.*jc113.*jc132 - jc112.*jc123.*jc1d33)./dejc1;
	mc1mcd2o22=(-jc1d12.*jc123)./dejc1;
	mc1mcd2o23=(-jc1d13.*jc123 + jc113.*jc1d23)./dejc1;
	mc1mcd2o33=(-jc112.*jc1d23)./dejc1;
	qdd4=(ijc121.*posddx + ijc122.*posddy) + (mc1mcd2o22.*qd4 + mc1mcd2o23.*qd5);
	qdd5=(ijc132.*posddy) + (mc1mcd2o33.*qd5);
	qdd6=-qdd5;
	qdd7=-qdd4;
	
	jc2d12=-d4.*(-qd8.*s8.*c9 - qd9.*c8.*s9);
	jc2d13=d4.*(qd8.*c8.*s9 + qd9.*s8.*c9);
	jc2d22=-d4.*(qd8.*c8.*c9 - qd9.*s8.*s9);
	jc2d23=-d4.*(-qd8.*s8.*s9 + qd9.*c8.*c9);
	jc2d33=+d4.*qd9.*s9;
	mc2mcd2o12=(-jc222.*jc233.*jc2d12 + jc233.*jc212.*jc2d22)./dejc2;
	mc2mcd2o13=((jc233.*jc2d23 - jc223.*jc2d33).*jc212 - jc222.*jc233.*jc2d13 + jc222.*jc2d33.*jc213)./dejc2;
	mc2mcd2o22=(-jc233.*jc2d22)./dejc2;
	mc2mcd2o23=(-jc233.*jc2d23 + jc223.*jc2d33)./dejc2;
	mc2mcd2o33=(-jc222.*jc2d33)./dejc2;
	qdd8=(ijc222.*posddy + ijc223.*posddz) + (mc2mcd2o22.*qd8 + mc2mcd2o23.*qd9);
	qdd9=(ijc233.*posddz) + (mc2mcd2o33.*qd9);
	qdd10=-qdd9;
	qdd11=-qdd8;
	
	jc3d13=+d4.*qd13.*s13;
	jc3d22=-d4.*(-qd12.*s12.*c13 - qd13.*c12.*s13);
	jc3d23=d4.*(qd12.*c12.*s13 + qd13.*s12.*c13);
	jc3d32=-d4.*(qd12.*c12.*c13 - qd13.*s12.*s13);
	jc3d33=-d4.*(-qd12.*s12.*s13 + qd13.*c12.*c13);
	mc3mcd2o12=(jc313.*(-jc332.*jc3d22 + jc322.*jc3d32))./dejc3;
	mc3mcd2o13=((-jc332.*jc3d23 + jc322.*jc3d33).*jc313 - jc3d13.*jc322.*jc333 + jc332.*jc3d13.*jc323)./dejc3;
	mc3mcd2o22=(-jc313.*jc3d32)./dejc3;
	mc3mcd2o23=(jc333.*jc3d13 - jc313.*jc3d33)./dejc3;
	mc3mcd2o33=(-jc332.*jc3d13)./dejc3;
	qdd12=(ijc321.*posddx + ijc323.*posddz) + (mc3mcd2o22.*qd12 + mc3mcd2o23.*qd13);
	qdd13=(ijc331.*posddx) + (mc3mcd2o33.*qd13);
	qdd14=-qdd13;
	qdd15=-qdd12;