ff

SCARA MPC Pick-and-Place Project

%% SCARA MPC Pick-and-Place Project

% Design, Modeling, and Control of a SCARA Robot for Automated

% Pick-and-Place Operations Using MATLAB

%

% Student: Ayyadah Alshammari

% Academic No.: 202417059

% Course: Robotics Mechanics and Control

% Instructor: Dr. Muath Bani Salim

% Software: MATLAB only

%

% The program transfers a small part from point A to point B. It includes:

% 1) SCARA geometry and workspace

% 2) Forward and inverse kinematics

% 3) Jacobian calculation

% 4) Discrete state-space model

% 5) Constrained finite-horizon MPC

% 6) PID comparison

% 7) Noise and disturbance robustness test

% 8) Figures and numerical error results

clc; clear; close all;

rng(7);

%% 1. Robot and task parameters

L1 = 0.32; % Link 1 length (m)

L2 = 0.28; % Link 2 length (m)

Apoint = [0.34; 0.10]; % Pick point A (m)

Bpoint = [0.50; 0.32]; % Place point B (m)

Ts = 0.02; % Sampling time (s)

Tf = 5.0; % Transfer time (s)

t = 0:Ts:Tf;

Nsim = numel(t);

%% 2. Smooth Cartesian reference trajectory A -> B

sigma = 3*(t/Tf).^2 - 2*(t/Tf).^3;

sigmaDot = 6*(t/Tf).*(1 - t/Tf)/Tf;

xd = Apoint(1) + (Bpoint(1)-Apoint(1))*sigma;

yd = Apoint(2) + (Bpoint(2)-Apoint(2))*sigma;

vxd = (Bpoint(1)-Apoint(1))*sigmaDot;

vyd = (Bpoint(2)-Apoint(2))*sigmaDot;

reference = [xd(:), yd(:), vxd(:), vyd(:)];

%% 3. Forward and inverse kinematics of the desired path

[q1d,q2d] = scaraIK(xd,yd,L1,L2,+1); % elbow-up branch

[xCheck,yCheck] = scaraFK(q1d,q2d,L1,L2);

kinematicCheckError = max(hypot(xCheck-xd,yCheck-yd));

%% 4. Discrete state-space model in Cartesian task space

% State: xs = [x; y; vx; vy]

% Input: u = [ax; ay]

Ad = [1 0 Ts 0;

0 1 0 Ts;

0 0 1 0;

0 0 0 1];

Bd = [0.5*Ts^2 0;

0 0.5*Ts^2;

Ts 0;

0 Ts];

Cd = [1 0 0 0;

0 1 0 0];

Dd = zeros(2);

%% 5. MPC tuning and constraints

Np = 15; % Prediction horizon

Q = diag([5000 5000 40 40]); % State/reference error weight

R = diag([0.08 0.08]); % Input effort weight

Rdu = diag([0.50 0.50]); % Input movement weight

uMax = 1.20; % Acceleration limit (m/s^2)

vMax = [0.22; 0.22]; % Velocity limits (m/s)

pMin = [0.05; -0.15]; % Workspace lower bound (m)

pMax = [0.59; 0.59]; % Workspace upper bound (m)

[Phi,Gamma] = predictionMatrices(Ad,Bd,Np);

Qbar = kron(eye(Np),Q);

Rbar = kron(eye(Np),R);

RduBar = kron(eye(Np),Rdu);

Dmove = moveDifferenceMatrix(2,Np);

Sprev = zeros(2*Np,2); Sprev(1:2,:) = eye(2);

H = 2*(Gamma'*Qbar*Gamma + Rbar + Dmove'*RduBar*Dmove);

H = (H+H')/2 + 1e-9*eye(size(H));

% Linear state constraints over the prediction horizon

Cp = [1 0 0 0; 0 1 0 0];

Cv = [0 0 1 0; 0 0 0 1];

Sp = kron(eye(Np),Cp);

Sv = kron(eye(Np),Cv);

Gp = Sp*Gamma; Pp = Sp*Phi;

Gv = Sv*Gamma; Pv = Sv*Phi;

lb = -uMax*ones(2*Np,1);

ub = uMax*ones(2*Np,1);

quadOptions = optimoptions('quadprog','Display','off');

%% 6. Nominal MPC and PID simulations

x0 = [Apoint(1)-0.012; Apoint(2)-0.008; 0; 0];

[xMPC,uMPC] = simulateController('MPC',x0,reference,Ad,Bd,Ts,Np, ...

Phi,Gamma,Qbar,RduBar,Dmove,Sprev,H,Gp,Pp,Gv,Pv, ...

pMin,pMax,vMax,lb,ub,quadOptions,false,false);

[xPID,uPID] = simulateController('PID',x0,reference,Ad,Bd,Ts,Np, ...

Phi,Gamma,Qbar,RduBar,Dmove,Sprev,H,Gp,Pp,Gv,Pv, ...

pMin,pMax,vMax,lb,ub,quadOptions,false,false);

%% 7. Robustness simulations: sensor noise + external disturbance

[xMPCRobust,~] = simulateController('MPC',x0,reference,Ad,Bd,Ts,Np, ...

Phi,Gamma,Qbar,RduBar,Dmove,Sprev,H,Gp,Pp,Gv,Pv, ...

pMin,pMax,vMax,lb,ub,quadOptions,true,true);

[xPIDRobust,~] = simulateController('PID',x0,reference,Ad,Bd,Ts,Np, ...

Phi,Gamma,Qbar,RduBar,Dmove,Sprev,H,Gp,Pp,Gv,Pv, ...

pMin,pMax,vMax,lb,ub,quadOptions,true,true);

%% 8. Joint trajectories and Jacobian

[q1MPC,q2MPC] = scaraIK(xMPC(:,1)',xMPC(:,2)',L1,L2,+1);

q1Vel = gradient(q1MPC,Ts);

q2Vel = gradient(q2MPC,Ts);

Jmid = scaraJacobian(q1MPC(round(Nsim/2)),q2MPC(round(Nsim/2)),L1,L2);

%% 9. Numerical performance metrics

errMPC = hypot(reference(:,1)-xMPC(:,1),reference(:,2)-xMPC(:,2));

errPID = hypot(reference(:,1)-xPID(:,1),reference(:,2)-xPID(:,2));

errMPCRobust = hypot(reference(:,1)-xMPCRobust(:,1),reference(:,2)-xMPCRobust(:,2));

errPIDRobust = hypot(reference(:,1)-xPIDRobust(:,1),reference(:,2)-xPIDRobust(:,2));

RMSE_MPC = sqrt(mean(errMPC.^2))*1000;

MEAN_MPC = mean(errMPC)*1000;

MAX_MPC = max(errMPC)*1000;

FINAL_MPC = errMPC(end)*1000;

RMSE_PID = sqrt(mean(errPID.^2))*1000;

MEAN_PID = mean(errPID)*1000;

MAX_PID = max(errPID)*1000;

FINAL_PID = errPID(end)*1000;

ROBUST_RMSE_MPC = sqrt(mean(errMPCRobust.^2))*1000;

ROBUST_RMSE_PID = sqrt(mean(errPIDRobust.^2))*1000;

fprintf('\nSCARA MPC PICK-AND-PLACE RESULTS\n');

fprintf('--------------------------------\n');

fprintf('Forward/inverse kinematic verification = %.6f mm\n',kinematicCheckError*1000);

fprintf('RMSE MPC = %.3f mm\n',RMSE_MPC);

fprintf('Mean error MPC = %.3f mm\n',MEAN_MPC);

fprintf('Maximum error MPC = %.3f mm\n',MAX_MPC);

fprintf('Final error MPC = %.3f mm\n',FINAL_MPC);

fprintf('RMSE PID = %.3f mm\n',RMSE_PID);

fprintf('Mean error PID = %.3f mm\n',MEAN_PID);

fprintf('Maximum error PID = %.3f mm\n',MAX_PID);

fprintf('Final error PID = %.3f mm\n',FINAL_PID);

fprintf('Robust RMSE MPC = %.3f mm\n',ROBUST_RMSE_MPC);

fprintf('Robust RMSE PID = %.3f mm\n',ROBUST_RMSE_PID);

fprintf('Jacobian at mid-path:\n'); disp(Jmid);

Results = table(RMSE_MPC,MEAN_MPC,MAX_MPC,FINAL_MPC, ...

RMSE_PID,MEAN_PID,MAX_PID,FINAL_PID, ...

ROBUST_RMSE_MPC,ROBUST_RMSE_PID);

writetable(Results,'SCARA_MPC_Numerical_Results.csv');

%% 10. Figures

% Figure 1: SCARA layout

mid = round(Nsim/2);

[x1,y1,x2,y2] = scaraLinks(q1MPC(mid),q2MPC(mid),L1,L2);

figure('Name','SCARA Layout','Color','w'); hold on; grid on; axis equal;

rectangle('Position',[-0.08 -0.08 0.75 0.50],'LineWidth',1.8);

plot([0 x1],[0 y1],'LineWidth',6);

plot([x1 x2],[y1 y2],'LineWidth',6);

plot(xd,yd,'--','LineWidth',1.8);

plot(Apoint(1),Apoint(2),'ks','MarkerFaceColor',[0.7 0.7 0.7]);

plot(Bpoint(1),Bpoint(2),'ks','MarkerFaceColor',[0.2 0.2 0.2]);

text(Apoint(1)+0.01,Apoint(2),'Pick point A');

text(Bpoint(1)-0.08,Bpoint(2)+0.02,'Place point B');

xlabel('x (m)'); ylabel('y (m)'); title('SCARA Pick-and-Place Layout');

exportgraphics(gcf,'Figure_1_SCARA_Layout.png','Resolution',220);

% Figure 2: Tracking

figure('Name','Trajectory Tracking','Color','w'); hold on; grid on; axis equal;

plot(xd,yd,'--','LineWidth',2);

plot(xMPC(:,1),xMPC(:,2),'LineWidth',2);

plot(xPID(:,1),xPID(:,2),'LineWidth',1.5);

legend('Desired trajectory','MPC path','PID path','Location','best');

xlabel('x (m)'); ylabel('y (m)'); title('End-Effector Trajectory Tracking');

exportgraphics(gcf,'Figure_2_Trajectory_Tracking.png','Resolution',220);

% Figure 3: Error

figure('Name','Tracking Error','Color','w'); hold on; grid on;

plot(t,errMPC*1000,'LineWidth',2);

plot(t,errPID*1000,'LineWidth',1.5);

xlabel('Time (s)'); ylabel('Position error (mm)');

legend('MPC error','PID error'); title('Nominal Tracking Error');

exportgraphics(gcf,'Figure_3_Tracking_Error.png','Resolution',220);

% Figure 4: Joint angles

figure('Name','Joint Angles','Color','w'); hold on; grid on;

plot(t,rad2deg(q1MPC)); plot(t,rad2deg(q2MPC));

xlabel('Time (s)'); ylabel('Angle (deg)');

legend('q1','q2'); title('Joint Angles from Inverse Kinematics');

exportgraphics(gcf,'Figure_4_Joint_Angles.png','Resolution',220);

% Figure 5: Joint velocities

figure('Name','Joint Velocities','Color','w'); hold on; grid on;

plot(t,rad2deg(q1Vel)); plot(t,rad2deg(q2Vel));

xlabel('Time (s)'); ylabel('Angular velocity (deg/s)');

legend('dq1/dt','dq2/dt'); title('Joint Velocity Response');

exportgraphics(gcf,'Figure_5_Joint_Velocities.png','Resolution',220);

% Figure 6: MPC control inputs

figure('Name','MPC Inputs','Color','w'); hold on; grid on;

plot(t,uMPC(:,1)); plot(t,uMPC(:,2));

yline(uMax,'--'); yline(-uMax,'--');

xlabel('Time (s)'); ylabel('Acceleration command (m/s^2)');

legend('u_x','u_y','Upper limit','Lower limit'); title('MPC Control Inputs and Limits');

exportgraphics(gcf,'Figure_6_MPC_Control_Inputs.png','Resolution',220);

% Figure 7: Robustness

figure('Name','Robustness','Color','w'); hold on; grid on;

plot(t,errMPCRobust*1000,'LineWidth',2);

plot(t,errPIDRobust*1000,'LineWidth',1.5);

xline(2.5,'--'); xlabel('Time (s)'); ylabel('Position error (mm)');

legend('MPC: noise + disturbance','PID: noise + disturbance','Disturbance');

title('Robustness Test');

exportgraphics(gcf,'Figure_7_Robustness.png','Resolution',220);

%% Local functions

function [x,y] = scaraFK(q1,q2,L1,L2)

x = L1*cos(q1) + L2*cos(q1+q2);

y = L1*sin(q1) + L2*sin(q1+q2);

end

function [q1,q2] = scaraIK(x,y,L1,L2,elbowSign)

c2 = (x.^2 + y.^2 - L1^2 - L2^2)/(2*L1*L2);

c2 = max(min(c2,1),-1);

s2 = elbowSign*sqrt(max(0,1-c2.^2));

q2 = atan2(s2,c2);

q1 = atan2(y,x) - atan2(L2*s2,L1+L2*c2);

end

function J = scaraJacobian(q1,q2,L1,L2)

J = [-L1*sin(q1)-L2*sin(q1+q2), -L2*sin(q1+q2);

L1*cos(q1)+L2*cos(q1+q2), L2*cos(q1+q2)];

end

function [x1,y1,x2,y2] = scaraLinks(q1,q2,L1,L2)

x1 = L1*cos(q1); y1 = L1*sin(q1);

x2 = x1 + L2*cos(q1+q2);

y2 = y1 + L2*sin(q1+q2);

end

function [Phi,Gamma] = predictionMatrices(A,B,Np)

nx = size(A,1); nu = size(B,2);

Phi = zeros(nx*Np,nx);

Gamma = zeros(nx*Np,nu*Np);

for i = 1:Np

Phi((i-1)*nx+1:i*nx,:) = A^i;

for j = 1:i

Gamma((i-1)*nx+1:i*nx,(j-1)*nu+1:j*nu) = A^(i-j)*B;

end

end

end

function D = moveDifferenceMatrix(nu,Np)

D = zeros(nu*Np,nu*Np);

for i = 1:Np

D((i-1)*nu+1:i*nu,(i-1)*nu+1:i*nu) = eye(nu);

if i > 1

D((i-1)*nu+1:i*nu,(i-2)*nu+1:(i-1)*nu) = -eye(nu);

end

end

end

function [x,u] = simulateController(type,x0,reference,A,B,Ts,Np, ...

Phi,Gamma,Qbar,RduBar,Dmove,Sprev,H,Gp,Pp,Gv,Pv, ...

pMin,pMax,vMax,lb,ub,qpOptions,addNoise,addDisturbance)

Nsim = size(reference,1);

x = zeros(Nsim,4); u = zeros(Nsim,2); x(1,:) = x0';

integralError = zeros(2,1);

rng(7);

for k = 1:Nsim-1

measured = x(k,:)';

if addNoise

measured(1:2) = measured(1:2) + 0.0005*randn(2,1);

end

if strcmpi(type,'MPC')

idx = min((k+1:k+Np),Nsim);

rStack = reshape(reference(idx,:).',[],1);

uPrevious = zeros(2,1);

if k > 1, uPrevious = u(k-1,:)'; end

f = 2*(Gamma'*Qbar*(Phi*measured-rStack) ...

- Dmove'*RduBar*Sprev*uPrevious);

pMinStack = repmat(pMin,Np,1); pMaxStack = repmat(pMax,Np,1);

vMaxStack = repmat(vMax,Np,1);

Aineq = [ Gv; -Gv; Gp; -Gp ];

bineq = [ vMaxStack-Pv*measured;

vMaxStack+Pv*measured;

pMaxStack-Pp*measured;

-pMinStack+Pp*measured ];

if exist('quadprog','file') == 2

U = quadprog(H,f,Aineq,bineq,[],[],lb,ub,[],qpOptions);

else

warning('quadprog not found. Using unconstrained MPC fallback.');

U = -(H\f);

U = min(max(U,lb),ub);

end

if isempty(U)

U = -(H\f); U = min(max(U,lb),ub);

end

u(k,:) = U(1:2)';

else

positionError = reference(k,1:2)' - measured(1:2);

velocityError = reference(k,3:4)' - measured(3:4);

integralError = integralError + positionError*Ts;

command = 7.5*positionError + 2.0*velocityError + 0.4*integralError;

command = min(max(command,-1.2),1.2);

u(k,:) = command';

end

x(k+1,:) = (A*x(k,:)' + B*u(k,:)')';

if addDisturbance && abs(k*Ts-2.5) < Ts/2

x(k+1,3:4) = x(k+1,3:4) + [0.055 -0.045];

end

end

u(end,:) = u(end-1,:);

end