%% Forover- og inverskinematikk til RR-manipulator
% Author: Christian Fredrik Sætre (christian.f.satre@ntnu.no)
% AIST1001 - Automatisering, introduksjon
%
% Dette skriptet skal vi bruke Robotics System Toolbox (RST) til å bygge og illustrere RR-manipulatoren vist i figuren under. Toolboxen kan innstalleres via "Get More Apps" i Apps-fanen.
%
%
%% Konstruere roboten

% Første bestemmer vi Lenkelengdene %[m]:
l1 = 0.5; 
l2 = 0.5; 
% Vi "bygger" så robot vha. RST:
robot = rigidBodyTree('DataFormat','column','MaxNumBodies',3);
% Ledd 1 og lenke 1:
body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint,trvec2tform([0 0 0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'base');
% Ledd 2 og lenke 2:
body = rigidBody('link2');
joint = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint, trvec2tform([l1,0,0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'link1');
% Ende-effektor/vektøyet:
body = rigidBody('ende-effektor');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([l2, 0, 0]));
body.Joint = joint;
addBody(robot, body, 'link2');

%% Foroverkinematikk
% Fra forelesningsnotatene vet vi at posisjonen (spesifisert i meter) til ende-effektoren, (x_G,y_G),  kan finnes fra leddvinklene q_1 og q_2 [rad] via x_G=l_1\cos(q_1)+l_2\cos(q_1+q_2) og y_G=l_1\sin(q_1)+l_2\sin(q_1+q_2).  La oss se at dette stemmer:

% Velg leddvinklene
q1 = 1;    
q2 = pi/2; 
% Regn ut at dette stemmer:
x_G = l1*cos(q1)+l2*cos(q1+q2);
disp(strcat("x_G blir ",num2str(x_G)))
y_G = l1*sin(q1)+l2*sin(q1+q2);
disp(strcat("y_G blir ",num2str(y_G)))
% Bruk RTS til å visualisere roboten for å se at dette stemmer:
figure(1);clf(1);
hold on;
grid on;
plottManipulator(robot,[q1;q2]); % Denne funksjonen er definert til slutt

%% Inverskinematikk
% I forelesningsnotatene fant vi at
% q_2=\text{Atan} (\pm\sqrt{1-\Omega^2},  \Omega) og q_1 = \text{Atan}(y_G, x_G) - \text{Atan}(k_2, k_1)
% hvor \Omega=\frac{x_G^2+y_G^2-l_1^2-l_2^}{2l_1l_2} , k_1=l_1+l_2\cos(q_2) og k_2=l_2\sin(q_2).
%  La oss se at dette stemmer:

% Velg den ønskede posisjonen til ende-effektoren:
x_G = 0.5;
y_G = 0.75;
% Finn q_1 og q_2:
Omega = (x_G^2+y_G^2-l1^2-l2^2)/(2*l1*l2);
q2 = atan2(sqrt(1-Omega^2),Omega) % eller 
% q2 atan2(-sqrt(1-Omega^2),Omega)
k1 =l1+l2*cos(q2);
k2 =l2*sin(q2);
q1 = atan2(y_G,x_G)-atan2(k2,k1)
% Bruk RTS til å visualisere roboten for å se at dette stemmer:
figure(2);clf(2);
hold on;
grid on
% plottManipulator(robot,[q1;q2])

%% Man kan også la RTS løse dette for oss:

% Velg den ønskede posisjonen til ende-effektoren:
x_G = 0.5;
y_G = 0.75;
% Definere løser og vekter
ik = inverseKinematics('RigidBodyTree', robot);
vekter =  [0, 0, 0, 1, 1, 0]; % Bare bry oss om x- og y-pos, ikke orientering
endeEffektor = 'ende-effektor'; 
q0 = [0;0]; % Use home configuration as the initial guess

qSol = ik(endeEffektor,trvec2tform([x_G,y_G,0]),vekter,q0)
% Bruk RTS til å visualisere roboten for å se at dette stemmer:
figure(3);clf(3);
hold on;
grid on;
plottManipulator(robot,qSol);

%
%% Plottefunksjon til slutt:

function plottManipulator(rob,q)
    f1=show(rob,q,"Visuals","on");
    view(2)
    ax = gca;
    ax.Projection = 'orthographic';
    % Lab litt tykkere linjer i figuren
    h=findall(f1);
    hlines=h.findobj('Type','Line'); 
    hax=h.findobj('Type','Axes');
    n=size(hlines);
    for i=1:n
        hlines(i).LineWidth=1.8; % Endre linjebredden
    end
end