SCARA robot kinematic inversion

Good evening everyone. I have to implement the kinematic inversion scheme with inverse and transpose of the jacobian for a SCARA manipulator. I wrote this code but I'm not sure how it works. I'm mostly not sure how I feedback the error. The Jacobian is a 4x4 matrix while the error (variable 'error') is a 6x1000 matrix. I deleted 2 lines from the error variable but I'm not sure if it's possible to do that. How can I improve the code I wrote?

close all
clc

% numero di giunti

n=4;   

% numeri punti simulazione

N=5; 
q=zeros(n,N);
  

% vettore variabili di giunto (th1 , th2 , d3 , th4)

q(:,1) = [90/180*pi -90/180*pi 0.3 0/180*pi]';  
q(:,2) = [60/180*pi -120/180*pi 0 0/180*pi]';  
q(:,3) = [15/180*pi -30/180*pi 0 0/180*pi]'; 
q(:,4)= [45/180*pi 90/180*pi 0 0/180*pi]';
q(:,5) = [45/180*pi  -60/180*pi 0 0/180*pi]';


a=[0.5  0.5  0  0]';
alpha=[0 pi 0 0]';
d=[0 0 q(3,1) 0]';
theta=[q(1,1) q(2,1) 0 q(4,1)]';

DH=[a alpha d theta];

DrawRobot(DH);

Unrecognized function or variable 'DrawRobot'.

% definizione della traiettoria

l = -10: 1 : 10;
raggio = 10;
y = sqrt (raggio^2 -l .^ 2);
k = zeros(length(l));
z = k(1,:);

C=[  1.0500    0.3000    0.0001    0.6500    0.0001    0.45    0.45    0.001 0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.015	0.001	0.15;

     0.0001    0.3000    0.3000    0.200    1.15    0.45    0.0001 0.9 0.0653835	0.0246165	0.0171214	0.0128786	0.00990381	0.00757346	0.00561361	0.0038785	0.00227873	0.000751884	0.000751884	0.00227873	0.0038785	0.00561361	0.00757346	0.00990381	0.0128786	0.0171214	0.0246165	0.0653835	0.45	0.9;
   
     0.0001    0.4500    0.0001    0.4000    0.1    0.6    0.0001  0.15 0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001	0.0001 0.0001];


punti_di_percorso = [0 70 90 90 50 50 20 -10 l 10 0; 
                    90 90 70 50 40 -30 -60 -60 y 30 90; 
                    0 0 30  30 50 50 10 10 z 0 0]/100;

[xd,dxd,ddxd,tvec,pp] = trapveltraj(punti_di_percorso, 1000, PeakVelocity=C);


% l'algoritmo calcola la cinematica inversa adottando l'inverso dello
% jacobiano o la sua trasposta. Per visualizzare bisogna modificare 'algorithm'
%con 'transpose' o 'inverse'

Jacobian_scara_rectangular = Jacobian_Scara(DH);
Jacobian_scara_square = [Jacobian_scara_rectangular(1,:); Jacobian_scara_rectangular(2,:); Jacobian_scara_rectangular(3,:); Jacobian_scara_rectangular(6,:)];


algorithm='inverse';


if strcmp(algorithm,'inverse')
    K = 1*diag([10 10 10 10]);
    fprintf('\n algoritmo: inversa dello jacobiano \n')
else
    K = diag([9 9 12 12]);
    fprintf('\n algoritmo: trasposta dello jacobiano \n')
end


tf = 1;
Ts = 1e-3;
N = length(tvec);
r=3;

x   = zeros(r,N);
q   = zeros(n,N);
dq  = zeros(n,N);
quat   = zeros(4,N);
quat_d = zeros(4,N);
error_pos  = zeros(r,N);
error_quat = zeros(3,N);
error      = zeros(6,N);

for i=1:N
    
    xd(:,i);
    quat_d(:,i) = [0 0 0 1]';

    DH(:,4) = q(:,i);  
    T = DirectKinematics(DH);
    x(1:2,i) = T(1:2,4,n);
    quat(:,i) = Rot2Quat(T(1:3,1:3,n));

    J = Jacobian_scara_square;

    error_pos(:,i)  = xd(:,i) - x(:,i);
    error_quat(:,i) = QuatError(quat_d(:,i),quat(:,i));
    error(:,i)      = [error_pos(:,i);error_quat(:,i)];
    error1=[error(1,:);error(2,:);error(4,:);error(5,:)];

    if strcmp(algorithm,'transpose')
        dq(:,i) = J'*K*error1(:,i);
    else
        dq(:,i) = inv(J)*K*error1(:,i);
    end

    if i<N
        q(:,i+1) = q(:,i) + Ts*dq(:,i);
    end


end

figure
subplot(211)
plot(tvec,q, 'LineWidth',2)
set(gca, 'FontSize',12)
grid
ylabel('[rad]')
title('posizione dei giunti')
legend('posizione q1','posizione q2','posizione q3','posizione q4')

subplot(212)
plot(tvec,dq, 'LineWidth',2)
set(gca, 'FontSize',12)
grid
ylabel('[rad/s]')
xlabel('time [s]')
title('velocità dei giunti')
legend('velocità q1','velocità q2','velocità q3','velocità q4')

figure
subplot(211)
plot(tvec,error(1:2,:), 'LineWidth',2)
ylabel('[m]')
set(gca, 'FontSize',12)
grid
title('errore di posizione')

subplot(212)
plot(tvec,error(3:5,:), 'LineWidth',2)
xlabel('time [s]')
set(gca, 'FontSize',12)
grid
title('errore di orientamento')

Answer 1

[Umang Pandey]

Hi Lorenzo,

You have used a "Jacobian_Scara" function in your code which I cannot find in the attached files.

However, you can refer to the following File Exchange submission for understanding and implementing inverse kinematics in SCARA -

https://www.mathworks.com/matlabcentral/fileexchange/128529-inverse-kinematics-of-scara-robot?s_tid=FX_rc1_behav

https://www.mathworks.com/matlabcentral/fileexchange/156433-forward-and-inverse-kinematics-of-scara-robot

Hope this helps!

Best,

Umang