TwoLinkPlanarArmCLIK.m

% --- PARAMETERS --- 
L1 = 0.5; L2 = 0.5;
q = [0.1; 0.2];         % Initial joint configuration 
xd = [0.4; 0.6];        % Desired end-effector position
dt = 0.02;              % Sampling time step
K = 5;                  % Proportional gain (responsiveness)
raggio = 0.2;           % Circle radius
centro = [0.4; 0.6];    % Circle center (x0, y0)
omega = 2;              % Angular velocity

% --- SIMULATION ---
figure('Color', 'w');
for t = 1:100
    
    tempo = t * dt;
    % UPDATING TARGET (xd is moving!)
    % xd = [centro(1) + raggio * cos(omega * tempo);
    %      centro(2) + raggio * sin(omega * tempo)];

    % 1. Forward Kinematics (Current end-effector position)
    xe = [L1*cos(q(1)) + L2*cos(q(1)+q(2));
          L1*sin(q(1)) + L2*sin(q(1)+q(2))];
    
    % 2. Operational space error calculation
    error = xd - xe;
    
    % 3. Geometric Jacobian calculation
    J = [-L1*sin(q(1))-L2*sin(q(1)+q(2)), -L2*sin(q(1)+q(2));
          L1*cos(q(1))+L2*cos(q(1)+q(2)),  L2*cos(q(1)+q(2))];
    
    % 4. Kinematic Control Law (CLIK)
    % Compute the required joint velocities
    dq = pinv(J) * (K * error);
    
    % 5. Numerical Integration (State update)
    q = q + dq * dt;
    
    % --- VISUALIZATION ---
    cla;
    % Plot desired position
    plot(xd(1), xd(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2); hold on;
    % Plot robot arm links and joints
    line([0, L1*cos(q(1)), xe(1)], [0, L1*sin(q(1)), xe(2)], 'LineWidth', 3 , 'Marker', 'o');
    axis([-1.1 1.1 -1.1 1.1]); grid on;
    title(['Tracking Error: ' num2str(norm(error), '%.4f')]);
    drawnow;
end