% SCAMODE.M generisanje dinamickog modela EE2 robota sa 2 s.s.

l1=sym('l1');
l2=sym('l2');
g=sym('0,-g,0,0');

A1=sym('cos(t1),-sin(t1),0,l*cos(t1);sin(t1),cos(t1),0,l*sin(t1);0,0,1,0;0,0,0,1');
A2=sym('cos(t2),-sin(t2),0,l*cos(t2);sin(t2),cos(t2),0,l*sin(t2);0,0,1,0;0,0,0,1');

poma=symmul(l1,'cos(t1)');
A1=sym(A1,1,4,poma);
poma=symmul(l1,'sin(t1)');
A1=sym(A1,2,4,poma);

poma=symmul(l2,'cos(t2)');
A2=sym(A2,1,4,poma);
poma=symmul(l2,'sin(t2)');
A2=sym(A2,2,4,poma);

A12=simple(symmul(A1,A2));

Q=[0 -1 0 0;1 0 0 0;0 0 0 0;0 0 0 0];

U11=simple(symmul(Q,A1));
U21=simple(symmul(Q,A12));
U22=simple(symmul(symmul(A1,Q),A2));

J1=sym('m1*l^2/3,0,0,-m1*l/2;0,0,0,0;0,0,0,0;-m1*l/2,0,0,m1');
poma=symmul(l1,l1);
pomb=symmul('m1/3',poma);
J1=sym(J1,1,1,pomb);
poma=symmul('-m1/2',l1);
J1=sym(J1,1,4,poma);
J1=sym(J1,4,1,poma);

J2=sym('m2*l^2/3,0,0,-m2*l/2;0,0,0,0;0,0,0,0;-m2*l/2,0,0,m2');
poma=symmul(l2,l2);
pomb=symmul('m2/3',poma);
J2=sym(J2,1,1,pomb);
poma=symmul('-m2/2',l2);
J2=sym(J2,1,4,poma);
J2=sym(J2,4,1,poma);

poma=simple(symmul(symmul(U11,J1),transpose(U11)));
pomb=simple(symmul(symmul(U21,J2),transpose(U21)));

D11=0;
for ii=1:4,
  D11=symadd(D11,sym(poma,ii,ii));
end
for ii=1:4,
  D11=symadd(D11,sym(pomb,ii,ii));
end

D12=0;
poma=symmul(symmul(U22,J2),transpose(U21));
for ii=1:4,
  D12=symadd(D12,sym(poma,ii,ii));
end
D21=D12;

D22=0;
poma=symmul(symmul(U22,J2),transpose(U22));
for ii=1:4,
  D22=symadd(D22,sym(poma,ii,ii));
end

D11=simple(D11);
D12=simple(D12);
D21=D12;
D22=simple(D22);

U111=symdiff(symdiff(A1,'t1'),'t1');
U211=symdiff(symdiff(A12,'t1'),'t1');
poma=symmul(symmul(U111,J1),transpose(U11));
pomb=symmul(symmul(U211,J2),transpose(U21));
h111=0;
for ii=1:4,
  h111=symadd(h111,sym(poma,ii,ii));
end
for ii=1:4,
  h111=symadd(h111,sym(pomb,ii,ii));
end

U212=symdiff(symdiff(A12,'t1'),'t2');
poma=symmul(symmul(U212,J2),transpose(U21));
h112=0;
for ii=1:4,
  h112=symadd(h112,sym(poma,ii,ii));
end

U221=symdiff(symdiff(A12,'t2'),'t1');
poma=symmul(symmul(U221,J2),transpose(U21));
h121=0;
for ii=1:4,
  h121=symadd(h121,sym(poma,ii,ii));
end

U222=symdiff(symdiff(A12,'t2'),'t2');
poma=symmul(symmul(U222,J2),transpose(U21));
h122=0;
for ii=1:4,
  h122=symadd(h122,sym(poma,ii,ii));
end

h1a=symadd( symmul(h111,'t1d^2'), symmul(h112,'t1d*t2d') );
h1b=symadd( symmul(h121,'t1d*t2d'), symmul(h122,'t2d^2') );
h1=symadd(h1a,h1b);
h1=simple(h1);

U211=symdiff(symdiff(A12,'t1'),'t1');
poma=symmul(symmul(U211,J2),transpose(U22));
h211=0;
for ii=1:4,
  h211=symadd(h211,sym(poma,ii,ii));
end

U212=symdiff(symdiff(A12,'t1'),'t2');
poma=symmul(symmul(U212,J2),transpose(U22));
h212=0;
for ii=1:4,
  h212=symadd(h212,sym(poma,ii,ii));
end

U221=symdiff(symdiff(A12,'t2'),'t1');
poma=symmul(symmul(U221,J2),transpose(U22));
h221=0;
for ii=1:4,
  h221=symadd(h221,sym(poma,ii,ii));
end

U222=symdiff(symdiff(A12,'t2'),'t2');
poma=symmul(symmul(U222,J2),transpose(U22));
h222=0;
for ii=1:4,
  h222=symadd(h222,sym(poma,ii,ii));
end

h2a=symadd( symmul(h211,'t1d^2'), symmul(h212,'t1d*t2d') );
h2b=symadd( symmul(h221,'t1d*t2d'), symmul(h222,'t2d^2') );
h2=symadd(h2a,h2b);
h2=simple(h2);

poma=symmul('-1/2',l1);
r1=sym('0;0;0;1');
r1=sym(r1,1,1,poma);

r2=r1;
poma=symmul('-1/2',l2);
r2=sym(r2,1,1,poma);

poma=symmul('-m1',g);
pomb=symmul(U11,r1);
C1a=symmul(poma,pomb);

poma=symmul('-m2',g);
pomb=symmul(U21,r2);
C1b=symmul(poma,pomb);

C1=simple(symadd(C1a,C1b));

poma=symmul('-m2',g);
pomb=symmul(U22,r2);
C2=simple(symmul(poma,pomb));

D=sym('0,0;0,0');
D=sym(D,1,1,D11);
D=sym(D,1,2,D12);
D=sym(D,2,1,D21);
D=sym(D,2,2,D22);

C=sym('0;0');
C=sym(C,1,1,C1);
C=sym(C,2,1,C2);

h=sym('0;0');
h=sym(h,1,1,h1);
h=sym(h,2,1,h2);

disp('H=');
disp(D)
disp('C=');
disp(h);
disp('G=');
disp(C);

