function [x, y] = MyFK(L1, L2, Q1, Q2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function [q1 q2 q1_ q2_] = MyFK(L1, L2, Q1, Q2)
%
% Task: returns the x y coordinates in the cartesian space
%       from a given point in the q1 q2 joint space
%       using geometry rules since we know that we have two links only of length L1 and L2
%       computation way faster than the method with matrices
%
% Inputs:
%-L1: length of the first link of the 2d planar 2 link robot arm
%-L2: length of the second link of the 2d planar 2 link robot arm
%-q1: q1 coordinate of the given point
%-q2: q2 coordinate of the given point
%
% Outputs:
%-x: x coordinate of the given point
%-y: x coordinate of the given point
%		
%
% Thomas OLIVE (thomas.olive@ecam.fr)
% 18/12/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  
  x = L1*cosd(Q1) + L2*cosd(Q1+Q2);
  y = L1*sind(Q1) + L2*sind(Q1+Q2);
endfunction