%% Matriz homogénea

clear all
clc

syms m1 m2 q1 q2 q3 d1 l1 a1 th1 l2 l3 l0 lc1 lc2

A = [cos(th1) -sin(th1)*cos(a1) sin(th1)*sin(a1) l1*cos(th1)

sin(th1) cos(a1)*cos(th1) -sin(a1)*cos(th1) l1*sin(th1)
0 sin(a1) cos(a1) d1
0 0 0 1];

A01c = subs(A, [l1, d1, a1, th1], [lc1, 0, 0, q1]);

A01 = subs(A, [l1, d1, a1, th1], [l1, 0, 0, q1]);
A12c = subs(A, [l1, d1, a1, th1], [lc2, 0, 0, q2]);
A12 = subs(A, [l1, d1, a1, th1], [l2, 0, 0, q2]);

A02c = A01 * A12;

P1 = [A01c(1:2,4)];
J1 = jacobian(P1,[q1,q2]);

A02c = A01 * A12c;

P2 = [A02c(1:2,4)];
J2 = jacobian(P2,[q1,q2]);

syms q1p q2p g

v1 = J1*[q1p;q2p];
v1c = simplify(transpose(v1)*v1);

v2 = J2*[q1p;q2p];
v2c = simplify(transpose(v2)*v2);

K = 0.2*l1*q1p*q1p + 0.5*m1*v1c + 0.5*l2*(q1p + q2p) + 0.5*m2*v2c;

U = m2*g*P1(2) + m2*g*P2(2);
L = simplify(K-U);

dldq1 = diff(L,q1);
dldq1p = diff(L,q1p);
syms q1pt(t) q1pp
dldq1p = subs(dldq1p,q1p,q1pt);
dldq1pdt = diff(dldq1p, t);
dldq1pdt = subs(dldq1pdt, diff(q1pt(t), t), q1pp);

dldq2 = diff(L,q2);
dldq2p = diff(L,q2p);
syms q2pt(t) q2pp
dldq2p = subs(dldq2p,q2p,q2pt);
dldq2pdt = diff(dldq2p, t);

1
dldq2pdt = subs(dldq2pdt, diff(q2pt(t), t), q2pp);

t1 = dldq1pdt - dldq1;
t2 = dldq2pdt - dldq2;

Tor = simplify([t1;t2;]);

G = jacobian(Tor,g)*g;
C = simplify(q1pp);
M = jacobian(Tor,[q1pp,q2pp]);

C = simplify(Tor-M*[q1pp;q2pp]-G);

syms u1 u2 u3

qpp = inv(M) * ([u1;u2]-C-G);

jacobian(qpp,[q1;q2]);

F=[q1p; qpp(1); q2p; qpp(2)];

As = jacobian(F,[q1,q1p,q2,q2p]);
Bs = jacobian(F,[u1,u2]);

syms q1d q2d u1d u2d

q1 = q1d;
q2 = q2d;
q2p = 0;
q1p = 0;

% Define la función solver correctamente o utiliza otra función de solución de

ecuaciones.

A = eval(As);
B = eval(Bs);

q1d = pi/6;
q2d = pi/6;
lc1 = 0.5;
lc2 = 0.5;
l1 = 1;
l2 = 1;
m1 = 0.3;
m2 = 0.3;
g = 9.8;
i1 = 0.3;
i2 = 0.3;

% Aplicar eval

2
A = eval(A);
B = eval(B);

contr = [B,A*B,A^2*B,A^3*B];
rank(contr);

% C = [1 0 0 0: 0 0 1 0]
%
% Ae = [A, zeros(4,2);-C zeros(2,2)]
% Be = [B; zeros(2,2)]
%
% z = 0.7; ts = 5; wn = 4.6/(ts*z)
%
% pd = conv([1 2*z*wn wn^2], [1 2*z*wn wn^2])
% pd = conv(pd, [1 2*z*wn wn^2])
%
% phia = Ae^6 + pd(2)*Ae^5 + pd(3)*Ae^4 + pde(4)*A^3 + pd(5)*Ae^2 + pd(6) * Ae +
pd(7)*eye(6)
% Ke = [0 0 0 0 0 1]*inv([Be,Ae*Be,Ae^2*Be,Ae^3*Be, Ae^4*Be, Ae^5*Be]) * phia