MCP-Enabled Robotics Control Systems with MATLAB

% Two-Joint Robot MPC Simulation

clear; close all; clc;


%% Robot / Model Parameters (Simplified Per-Joint Dynamics)

I1 = 0.05;           % Inertia joint 1 [kg*m^2]

I2 = 0.03;           % Inertia joint 2 [kg*m^2]

b1 = 0.01;           % Viscous damping joint 1

b2 = 0.01;           % Viscous damping joint 2


% State-space matrices for each joint

A1_c = [0 1; 0 -b1/I1];

B1_c = [0; 1/I1];

A2_c = [0 1; 0 -b2/I2];

B2_c = [0; 1/I2];


% Combined system matrices

A_c = blkdiag(A1_c, A2_c);

B_c = blkdiag(B1_c, B2_c);


%% Discretize Model

Ts = 0.05;           % Sampling time [s]

sysc = ss(A_c, B_c, eye(4), zeros(4,2)); % Continuous-time state-space model

sysd = c2d(sysc, Ts);                    % Discrete-time state-space model

Ad = sysd.A;

Bd = sysd.B;


%% MPC Parameters

Np = 20;             % Prediction horizon

nx = size(Ad,1);     % Number of states (4: q1, qd1, q2, qd2)

nu = size(Bd,2);     % Number of inputs (2: torques for joint 1, joint 2)

Qx = diag([100, 1, 100, 1]); % State weighting matrix

Ru = 0.01 * eye(nu);         % Input weighting matrix

tau_max = 5;                 % Max torque [Nm]

tau_min = -tau_max;          % Min torque [Nm]

u_max = tau_max * ones(nu,1);

u_min = tau_min * ones(nu,1);

q1_lim = [-pi/2, pi/2];      % Joint 1 angle limits [rad]

q2_lim = [-pi/2, pi/2];      % Joint 2 angle limits [rad]


%% Build Prediction Matrices Sx, Su

Sx = zeros(nx*Np, nx);

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

for i = 1:Np

    Sx((i-1)*nx+1:i*nx, :) = Ad^i;

    for j = 1:i

        Su((i-1)*nx+1:i*nx, (j-1)*nu+1:j*nu) = Ad^(i-j) * Bd;

    end

end




% Weighting matrices for prediction horizon

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

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


%% Reference Trajectory

Tsim = 10;           % Simulation time [s]

Nsim = round(Tsim / Ts); % Number of simulation steps

t = (0:Nsim-1)' * Ts;    % Time vector

q1_ref_traj = 0.6 * sin(0.5 * t); % Reference trajectory for joint 1

q2_ref_traj = 0.5 * sin(0.7 * t + 0.5); % Reference trajectory for joint 2

qd1_ref_traj = [0; diff(q1_ref_traj)/Ts]; % Velocity for joint 1

qd2_ref_traj = [0; diff(q2_ref_traj)/Ts]; % Velocity for joint 2

x_ref_traj = [q1_ref_traj, qd1_ref_traj, q2_ref_traj, qd2_ref_traj]; % State reference


%% Initial Conditions

x = [0; 0; 0; 0];   % Initial state: [q1; qd1; q2; qd2]

Xlog = zeros(Nsim, nx); % Log states

Ulog = zeros(Nsim, nu); % Log inputs


%% Check for quadprog Availability

have_quadprog = license('test','optimization_toolbox') && exist('quadprog','file');

if ~have_quadprog

    fprintf('quadprog not available. MPC QP will fallback to LQR-based tracking. ');

    [K_lqr, ~, ~] = dlqr(Ad, Bd, Qx, Ru);

    K_lqr = -K_lqr; % LQR gain

end


%% Precompute QP Hessian

H = Su' * Qbar * Su + Rbar;

H = (H + H') / 2; % Ensure symmetry



%% Simulation Loop

fprintf('Starting simulation (%d steps)... ', Nsim);

for k = 1:Nsim

    % Prepare reference trajectory for prediction horizon

    idx_end = min(k + Np - 1, Nsim);

    horizon_len = idx_end - k + 1;

    xref_stack = zeros(nx * Np, 1);


    % Stack reference states

    for j = 1:horizon_len

        xref = x_ref_traj(k + j - 1, :)';

        xref_stack((j-1)*nx+1:j*nx) = xref;

    end

    if horizon_len < Np

        last_ref = xref_stack((horizon_len-1)*nx+1:horizon_len*nx);

        for j = horizon_len+1:Np

            xref_stack((j-1)*nx+1:j*nx) = last_ref;

        end

    end

    

    % Compute control input

    if have_quadprog

        % QP cost: f = -Su'*Qbar*(xref_stack - Sx*x)

        f = -Su' * Qbar * (xref_stack - Sx * x);

        

        % Constraints: u_min