% Chapter 9 Demos

% function sym2Rplanar
% % 2R-Planar Manipulator Dynamics
% % Put in form of:
% %    Q = M(q)qddot + C(q,qdot)qdot + G(q) + F(qdot) + J(q)'*g
% syms t q1 q2 q1dot q2dot q1ddot q2ddot
% syms L1 L2 m1 m2 grav
% 
% % Poses centers of gravity
% cg1 = simple([L1/2 * cos(q1);
%               L1/2 * sin(q1)]);
% cg2 = simple([L1 * cos(q1) + L2/2 * cos(q1+q2); 
%               L1 * sin(q1) + L2/2 * sin(q1+q2)]);
% phi = q1 + q2;
% 
% % Velocities of the centers of gravity
% cg1dot = simple(diff(cg1,q1)*q1dot);
% cg2dot = simple(diff(cg2,q1)*q1dot + diff(cg2,q2)*q2dot);
% % phi1dot = q1dot;
% % phi2dot = q1dot + q2dot;
% 
% % Potential energy
% U = grav * (m1 * cg1(2) + m2 * cg2(2));
% Uhndl = matlabFunction(U)
% 
% % Kinetic energy 
% I1 = m1 * L1^2 / 12;
% I2 = m2 * L2^2 / 12;
% T = simple((m1 * transpose(cg1dot) * cg1dot) / 2 ...
%          + (m2 * transpose(cg2dot) * cg2dot) / 2 ...
%          + (I1 * transpose(q1dot) * q1dot) / 2 ...
%          + (I2 * transpose(q2dot) * q2dot) / 2);
% 
% % Lagrangian = T - U
% L = simple(T - U);
% 
% % Newton-Euler Equation
% %      d/dt (\partial L wrt qdot) - \partial L wrt q = 0
% %   => d/dt (\partial T wrt qdot) - \partial T wrt q + \partial U wrt q = 0
% %
% %    Further analysis gives:
% %          Mij = second partial of T wrt qidot and qjdot
% %          Ci  = sum_j (second partial of T wrt qidot and qj) * qjdot ...
% %                      - partial of T wrt qi
% %          Gi  = partial of U wrt qi
% %
% % % temp1 = simple(diff(T,q1dot));
% % % M11 = subs(temp1,{q1dot,q2dot},[1, 0]);  % Coefficient of q1dot and M11
% % % M12 = subs(temp1,{q1dot,q2dot},[0, 1]);  % Coefficient of q2dot and M12
% % % C1 = diff(M11, q2) * q2dot * q1dot + diff(M12, q2) * q2dot^2;
% % % G1 = diff(U,q1);
% % % 
% % % temp1 = simple(diff(T,q2dot));
% % % M21 = subs(temp1,{q1dot,q2dot},[1, 0]);  % Coefficient of q1dot and M21
% % % M22 = subs(temp1,{q1dot,q2dot},[0, 1]);  % Coefficient of q2dot and M22
% % % C2 = diff(M21, q2) * q2dot * q1dot +  diff(M22, q2) * q2dot^2;
% % % C2 = C2 - diff(T,q2);  % Forget this term the first time through
% % % G2 = diff(U,q2);
% 
% M11 = simple(diff(L,q1dot,2));
% M12 = simple(diff(diff(L,q1dot),q2dot));
% M22 = simple(diff(L,q2dot,2));
% C1 = simple(diff(diff(T,q1dot),q1)*q1dot ...
%            +diff(diff(T,q1dot),q2)*q2dot ...
%            -diff(T,q1));
% C2 = simple(diff(diff(T,q2dot),q1)*q1dot ...
%            +diff(diff(T,q2dot),q2)*q2dot ...
%            -diff(T,q2));
% G1 = diff(U,q1);
% G2 = diff(U,q2);
% 
% % Construct the mass matrix and gravity and velocity product vectors
% Mhndl = matlabFunction([M11  M12;  M12  M22])
% Ghndl = matlabFunction([G1;  G2])
% Chndl = matlabFunction([C1;  C2])
% end


% Finally we want to add input torques Q and friction F(qdot), but move to
% numerical expressions first.
% Initialization
function simArm
clear all;

% Set simulation time period
t0 = 0;
tfinal = 6;    % Integration time
tspan = [t0  tfinal];

% Set nitial conditions
q0 = [0;  0.3];
qdot0 = zeros(2,1);
y0 = [q0; qdot0];

options = odeset('RelTol',1e-8);
% tic;  [t,y] = ode45(@armDyn,tspan,y0,options);  toc
tic;  [t,y] = ode15s(@armDyn,tspan,y0,options);  toc
q = y(:,1:2);
qdot = y(:,3:4);
figure;
plot(t,q);
grid; legend('q1', 'q2');
figure;
plot(t,qdot);
grid; legend('q1dot', 'q2dot');

len = length(t);
pe_all = zeros(len,1);
ke_all = zeros(len,1);
te_all = zeros(len,1);
for i = 1:len
    ke_all(i) = kineticE(q(i,:), qdot(i,:));
    pe_all(i) = potentialE(q(i,:));
    te_all(i) = pe_all(i) + ke_all(i);
end
figure;
plot(t,te_all,'r');
grid; legend('Total Energy');
figure;
plot(t,ke_all,'r', t,pe_all,'g');
grid; legend('KE', 'PE');
end


function dy = armDyn(t,y)
q = y(1:2);  qdot = y(3:4);
L1 = 1.5;
L2 = 1.0;
m1 = 1.5;
m2 = 1.0;
grav = -9.81;
M = Mass(q);
C = zeros(2,1);
C(1) = -L1*L2*m2*sin(q(2))*(1/2*qdot(2)^2 + qdot(1)*qdot(2));
C(2) =  L1*L2*m2*sin(q(2))*qdot(1)^2/2;
%  - (L1*L2*m2*q2dot^2*sin(q2))/2 - L1*L2*m2*q1dot*q2dot*sin(q2)
%                                   L1*L2*m2*q1dot^2*sin(q2)/2
G = zeros(2,1);
G(1) = -grav*(m2*((L2*cos(q(1) + q(2)))/2 + L1*cos(q(1))) + (L1*m1*cos(q(1)))/2);
G(2) = -grav*(L2*m2*cos(q(1) + q(2)))/2;
%  grav*(m2*((L2*cos(q1 + q2))/2 + L1*cos(q1)) + (L1*m1*cos(q1))/2)
%                                       (L2*grav*m2*cos(q1 + q2))/2
F = zeros(2,1);
F = 0*diag([1  1]) * qdot;
% Add coulomb friction approximation
% For demonstrating dependence of simulation speed on non-smooth friction
% funtion, use qdot_lim = 1e-1 or 1e-2 or 1e-3. 
% ode45 => cpu seconds will be about 2.5 and 23 seconds, and never. 
% ode15s => cpu seconds will be about 0.5 and 0.5 and 0.5 and ... 0.5 
qdot_lim = 1e-10;  % Friction clipping speed 
mu1 = 6.0;  % Maximum friction torque on joint 1
mu2 = 2.0;  % Maximum friction torque on joint 2
% mu1 = 0.0;  % Maximum friction torque on joint 1
% mu2 = 0.0;  % Maximum friction torque on joint 2
if abs(qdot(1)) > qdot_lim
    F(1) = F(1) + mu1 * sign(qdot(1));
else
    F(1) = F(1) + mu1 * qdot(1) / qdot_lim; 
end
if abs(qdot(2)) > qdot_lim
    F(2) = F(2) + mu2 * sign(qdot(2));
else
    F(2) = F(2) + mu2 * qdot(2) / qdot_lim;
end

% Let's not add any torque yet.
Q = zeros(2,1);

dq = qdot;
dqdot = M \ (Q - G - C - F);

dy = [dq;  dqdot];
end

function M = Mass(q)
L1 = 1.5;
L2 = 1.0;
m1 = 1.5;
m2 = 1.0;
M = zeros(2,2);
M(1,1) = (L1^2*m1)/3 + L1^2*m2 + (L2^2*m2)/4 + L1*L2*m2*cos(q(2));
M(1,2) = (L2*m2*(L2 + 2*L1*cos(q(2))))/4;
M(2,1) = M(1,2);
M(2,2) = (L2^2*m2)/3;
% [ (L1^2*m1)/3 + L1^2*m2 + (L2^2*m2)/4 + L1*L2*m2*cos(q2), (L2*m2*(L2 + 2*L1*cos(q2)))/4]
% [                          (L2*m2*(L2 + 2*L1*cos(q2)))/4,                   (L2^2*m2)/3]
end

function KE = kineticE(q,qdot)
% L1 = 1.5;
% L2 = 1.0;
% m1 = 1.5;
% m2 = 1.0;
% cg1dot = zeros(2,1);
% cg1dot(1) = -L1/2*qdot(1)*sin(q(1));
% cg1dot(2) =  L1/2*qdot(1)*cos(q(1));
% cg2dot = zeros(2,1);
% cg2dot(1) = -(L1*sin(q(1)) + L2/2*sin(q(1)+q(2))) * qdot(1) ...
%             +(L2/2*sin(q(1)+q(2))) * qdot(2);
% cg2dot(2) =  (L1*cos(q(1)) + L2/2*cos(q(1)+q(2))) * qdot(1) ...
%             +(L2/2*cos(q(1)+q(2))) * qdot(2);
% KE = m1 * cg1dot' * cg1dot / 2;
% KE = KE + qdot(1) * m1*L1*L1/12 * qdot(1) / 2;
% KE = KE + m2 * cg2dot' * cg2dot / 2;
% KE = KE + qdot(2) * m2*L2*L2/12 * qdot(2) / 2;
KE = 1/2 * qdot * Mass(q) * qdot';
end

function PE = potentialE(q)
L1 = 1.5;
L2 = 1.0;
m1 = 1.5;
m2 = 1.0;
grav = -9.81;
PE = -grav*m1*L1/2*sin(q(1));
PE = PE - grav*m2*(L1*sin(q(1)) + L2/2*sin(q(1)+q(2)));
end


% % Puma 560 Dynamics
% mdl_puma560;
% p560.plot(qn);
% Q = p560.rne(qn, zeros(1,6), zeros(1,6))
% accGrav = [0, 0, 9.81];  % Oddly, this is the default direction of gravity.
% Q = p560.rne(qn, zeros(1,6), zeros(1,6), -accGrav)
% Q = p560.rne(qn, zeros(1,6), zeros(1,6), [1, 0, 0]);
% p560.links(1).dyn
% p560.links(2).dyn
% p560.plot(qr);
% p560.gravload(qr)