# Control Robot Manipulator Using C/GMRES Solver

This example shows how to design a multistage nonlinear model predictive controller that uses the C/GMRES solver to control a robot manipulator.

### Background

Nonlinear receding horizon control (RHC), also known as nonlinear model predictive control (MPC), is a strategy to control dynamic systems. It consists in calculating the opimal sequence of control actions over a finite time horizon while implementing only the first action. The control sequence is recalculated as time progresses, considering updated measurements and a shifted time horizon.

To guide the optimization process, RHC incorporates the concepts of stage cost and terminal cost, where:

• Stage cost: The stage cost represents the cost associated with each stage (the time step) of the control problem. It quantifies the immediate cost (also known as penalty) incurred at each step. In this example, the stage cost is defined using a standard quadratic cost function that penalizes the deviation of the state vector $\mathit{x}$ from the desired state ${\mathit{x}}_{\mathit{f}}$ at each time point within the horizon, $\mathit{k}$, and the control effort, $\mathit{u}$, required to achieve the desired state.

$\mathit{L}\left(\mathit{x}\left(\tau \right),\mathit{u}\left(\tau \right),\mathit{pv}\right)={\left(\mathit{x}-{\mathit{x}}_{\mathit{f}}\right)}^{\mathit{T}}\text{\hspace{0.17em}}\mathbit{Q}\text{\hspace{0.17em}}\left(\mathit{x}-{\mathit{x}}_{\mathit{f}}\right)+{\mathit{u}}^{\mathit{T}}\mathbit{R}\text{\hspace{0.17em}}\mathit{u}$

• Terminal cost: This term represents a cost (also sometimes called penalty) associated with the final state of the system. The terminal penalty penalizes the deviation of the state at the end of the horizon ${\mathit{x}}_{\mathit{p}+1}$, from the desired state ${\mathit{x}}_{\mathit{f}}$, where $\mathit{p}+1$ is the final stage, which corresponds to the end of the prediction horizon, $\mathit{t}+\mathit{T}$.

$\phi \left(\mathit{x}\left(\mathit{t}+\mathit{T}\right),\mathit{pv}\right)=\frac{1}{2}{\left(\mathit{x}\left(\mathit{t}+\mathit{T}\right)-{\mathit{x}}_{\mathit{f}}\right)}^{\mathit{T}}\text{\hspace{0.17em}}{\mathbit{S}}_{\mathbit{f}}\text{\hspace{0.17em}}\left(\mathit{x}\left(\mathit{t}+\mathit{T}\right)-{\mathit{x}}_{\mathit{f}}\right)$

Here, the matrices $\mathbit{Q}$ and $\mathbit{R}$ determine the weight of the state deviation from the desired state, and the control effort, respectively. The matrix ${\mathbit{S}}_{\mathbit{f}}$ determines the weight for the terminal penalty, and $\mathit{pv}$ is the stage parameter vector.

The overall performance index for the receding horizon control problem is given by:

$\mathit{J}=\phi \left(\mathit{x}\left(\mathit{t}+\mathit{T}\right),\mathit{pv}\right)+\frac{1}{2}{\int }_{\mathit{t}}^{\mathit{t}+\mathit{T}}\mathit{L}\left(\mathit{x}\left(\tau \right),\mathit{u}\left(\tau \right),\mathit{pv}\right)\text{\hspace{0.17em}}\mathrm{d}\tau$

The formulation of RHC allows for the independent specification of stage costs for each time step, making it suitable for multistage nonlinear MPC controllers.

### Nonlinear Model

This figure gives the schematics of the two-link planar robot. The configuration of the robot is determined by the joint angles ${\theta }_{1}$ and ${\theta }_{2}$, along with the torques ${\tau }_{1}$ and ${\tau }_{2}$ applied at the joints to control the position of the robot's tip.

The state vector is defined as:

$\mathit{x}\left(\mathit{t}\right)=\left[\begin{array}{c}\mathit{q}\\ \stackrel{˙}{\text{\hspace{0.17em}}\mathit{q}}\end{array}\right]$, where $\mathit{q}=\left[\begin{array}{c}{\theta }_{1}\\ {\theta }_{2}\end{array}\right]$

The dynamics for a two-link robot manipulator can be expressed as follows :

$\mathit{H}\left(\mathit{q}\right)\stackrel{¨}{\mathit{q}}+\mathit{C}\left(\mathit{q},\stackrel{˙}{\mathit{q}}\right)\stackrel{˙}{\mathit{q}}+\mathit{G}\left(\mathit{q}\right)=\tau$

Here, $\mathit{q}$, $\stackrel{˙}{\mathit{q}}$, and $\stackrel{¨}{\mathit{q}}$ are 2-by-1 vectors that represent the joint angles, velocities, and accelerations. The control input vector is the torque $\tau$. The remaining terms correspond to manipulator inertia matrix, $\mathit{H}\left(\mathit{q}\right)$, Coriolis matrix, $\mathit{C}\left(\mathit{q},\stackrel{˙}{\mathit{q}}\right)$, and gravity vector, $\mathit{G}\left(\mathit{q}\right)$.

This robot dynamics is implemented in twolinkStateFcn.m. This file is a MATLAB® function that takes as inputs the state vector, $\mathit{x}$, the control input vector, $\mathit{u}$, and a vector of model parameters, $\mathit{pvstate}$. The function's output is the vector of the state derivatives with respect to time.

The physical parameters of the robot are defined in the following table:

% Two-Link Robot Physical Parameter Values. Value = [0.3; 0.15; 0.2; 6.0e-3; 0.3; 0.257; 0.7; 5.1e-2; 9.81]; Units = ["m"; "m"; "kg"; "kg*m^2"; "m"; "m"; "kg"; "kg*m^2"; "m/s^2"]; Param = [ ... "Length, L1", "Center of mass, s1", "Mass, m1", ... "Moment of Inertia, J1", "Length, L2", ... "Center of mass, s2", "Mass, m2", ... "Moment of Inertia, J2", "Gravity, g"]; % Convert arrays to table ParamTable = array2table(Value, "RowNames", Param); UnitsTable = table(categorical(Units), ... VariableNames = "Units"); % Display parameter values. disp([ParamTable UnitsTable])
 Value Units _____ ______ Length, L1 0.3 m Center of mass, s1 0.15 m Mass, m1 0.2 kg Moment of Inertia, J1 0.006 kg*m^2 Length, L2 0.3 m Center of mass, s2 0.257 m Mass, m2 0.7 kg Moment of Inertia, J2 0.051 kg*m^2 Gravity, g 9.81 m/s^2 

Define model parameter vector:

pvstate = ParamTable.Value;

### Define Objective Cost Function

The control strategy involves optimizing the performance index, $\mathit{J}$, over a finite time horizon and is given by:

$\mathit{J}=\frac{1}{2}{\left(\mathit{x}\left(\mathit{t}+\mathit{T}\right)-{\mathit{x}}_{\mathit{f}}\right)}^{\mathit{T}}\text{\hspace{0.17em}}{\mathbit{S}}_{\mathbit{f}}\text{\hspace{0.17em}}\left(\mathit{x}\left(\mathit{t}+\mathit{T}\right)-{\mathit{x}}_{\mathit{f}}\right)+\frac{1}{2}{\int }_{\tau }^{\tau +\mathit{T}}\left({\left(\mathit{x}-{\mathit{x}}_{\mathit{f}}\right)}^{\mathit{T}}\text{\hspace{0.17em}}\mathbit{Q}\text{\hspace{0.17em}}\left(\mathit{x}-{\mathit{x}}_{\mathit{f}}\right)+{\mathit{u}}^{\mathit{T}}\mathbit{R}\text{\hspace{0.17em}}\mathit{u}\right)\mathit{d}\tau$

where:

• ${\mathit{x}}_{\mathit{f}}$ represents the desired state.

• ${\mathbit{S}}_{\mathbit{f}}$, $\mathbit{Q}$, and $\mathbit{R}$ are weight matrices that determine the importance of different terms in the performance index.

The ${\mathbit{S}}_{\mathbit{f}}$, $\mathbit{Q}$, and $\mathbit{R}$ values impact the control strategy and the trade-off between tracking the objective state and minimizing control effort.

For this example, define the weight matrices as a column vector and combine state parameter into the pvcost variable.

% Stage Parameters Sf = [20; 10; 1e-3; 1e-3]; % Terminal Weight Matrix. Q = [30; 30; 1e-3; 1e-3]; % State Weight Matrix. R = [0.1; 0.1]; % Control Weighting Matrix.

Append the prediction horizon of the controller and desired state to the stage parameter. For this example, the prediction horizon uses 50 steps.

p = 50; % Prediction Horizon. xf = [1; 1; 0; 0]; % Terminal State. % Combine stage parameters into a column array. pvcost = [xf; Sf; Q; R; p];

The objective cost function is implemented in the MATLAB function twolinkCostFcn.m.

type("twolinkCostFcn.m")
function L = twolinkCostFcn(stage, x, u, pvcost) % twolinkCostFcn % L = twolinkCostFcn(stage, x, u, pvcost) % % This function computes the objective cost for the two-link robot % manipulator example. % % Inputs: % - stage Current stage of the Multistage MPC problem % - x Current state vector % - u Control action vector at the previous interval % - pvcost Vector containing various stage parameters % % Output: % - L: Cost value calculated by the cost function. % Copyright 2023 The MathWorks, Inc. % Extract Desired State xf = pvcost(1:4); % Extract and Initialize Wiegthing Matrices Sf = diag(pvcost(5:8)); % Terminal Weight Matrix Q = diag(pvcost(9:12)); % State Weight Matrix R = diag(pvcost(12+(1:length(u)))); % Control Weight Matrix p = pvcost(15); % Prediction Horizon if stage == p + 1 % Calculate cost for the terminal stage L = 0.5*(x - xf).'*Sf*(x - xf); else % Calculate cost for intermediate stages L = 0.5*(x - xf).'*Q*(x - xf) + 0.5*u.'*R*u; end end 

### Robot Environment

The robot environment contains a two-link robot. For this example, the objective is to have the robot follow a given reference trajectory, starting at zero angular position and velocity.

% Define Initial State. x0 = [0; 0; 0; 0]; % Initialize Two Link Robot plot. figure("Color", "w") hPlot = helperRobotEnvironment( ... Length1 = pvstate(1), ... Length2 = pvstate(5)); % Set Initial angular position and desired state. hPlot.Theta1 = x0(1); hPlot.Theta2 = x0(2); hPlot.xf = xf; The animation plot is initialized to display both the initial position of the manipulator robot and the target position.

### Design Nonlinear MPC Controller

Create a nonlinear multistage MPC object with four states and two inputs.

nx = 4; nmv = 2; msobj = nlmpcMultistage(p,nx,nmv);

In this example, the target prediction time is 0.1 seconds. Therefore, for a prediction horizon of 50 steps specify a sample time of 0.002 seconds.

Ts = 2e-3; msobj.Ts = Ts;

To compute the optimal control action of the manipulator robot using the c/gmres solver, the jacobian functions for the state and objective cost must be provided, as the C/GMRES solver does not support Jacobians that are calculated at every step using numerical perturbations.

Note that the constraint-related properties EqConFcn, EqConJacFcn, IneqConFcn, and IneqConJacFcn are not supported with the C/GMRES solver. If you need to use these constraint-related properties, consider using the fmincon solver. For more details on the solver limitations, see nlmpcMultistage and Configure C/GMRES Solver Options.

Specify the prediction model state and jacobian functions using the robot dynamics functions.

msobj.Model.StateFcn = "twolinkStateFcn"; msobj.Model.StateJacFcn = "twolinkStateJacFcn"; msobj.Model.ParameterLength = length(pvstate);

Specify this cost function using a named function. For more information on specifying cost functions, see Specify Cost Function for Nonlinear MPC.

% Setting cost function for each control interval. for k = 1:p+1 msobj.Stages(k).CostFcn = "twolinkCostFcn"; msobj.Stages(k).CostJacFcn = "twolinkCostJacFcn"; msobj.Stages(k).ParameterLength = length(pvcost); end

Specify hard bounds on the two joint motors.

msobj.ManipulatedVariables(1).Min = -10; msobj.ManipulatedVariables(1).Max = 10; msobj.ManipulatedVariables(2).Min = -10; msobj.ManipulatedVariables(2).Max = 10;

The state and stage functions require state and stage parameters. Use getSimulationData to initialize data structure. For more information on simulation data structure for multistage MPC, see getSimulationData.

simdata = getSimulationData(msobj); simdata.StateFcnParameter = pvstate; simdata.StageParameter = repmat(pvcost, p+1, 1);

### Setting the Optimization Solver

The optimization solver finds the optimal control inputs that minimize a given cost function while satisfying constraints. Configuring the solver options correctly can improve the efficiency and accuracy of the optimization process.

Specify the optimization solver using the Solver property of the multistage MPC object. Choose either fmincon or cgmres based on your requirements.

For this example, set the multistage MPC object to use the cgmres solver .

msobj.Optimization.Solver = "cgmres";

It is recommended to tune the stabilization parameter according to the prediction model sample time to further optimize solver performance. Adjust the StabilizationParameter property of the multistage MPC object by assigning an appropriate value between $\frac{1}{{\mathit{T}}_{\mathit{s}}}$ and $\frac{2}{{\mathit{T}}_{\mathit{s}}}$. Smaller values results in lower step sizes for the solver.

% Adjust the Stabilization Parameter % based on the prediction model sample time. msobj.Optimization.SolverOptions.StabilizationParameter = ... 1/msobj.Ts; % Set the solver parameters. msobj.Optimization.SolverOptions.MaxIterations = 10; msobj.Optimization.SolverOptions.Restart = 3; msobj.Optimization.SolverOptions.BarrierParameter = 1e-3; msobj.Optimization.SolverOptions.TerminationTolerance = 1e-6;

For more information on configuring the cgmres solver, see Configure C/GMRES Solver Options and the Optimization property of nlmpcMultistage.

### Closed-Loop Simulation in MATLAB

Simulate the system for 2 seconds with a target trajectory to follow.

% Simulation duration in seconds. Duration = 2; x0 = zeros(nx,1); u0 = zeros(nmv,1); % Display waitbar to show simulation progress. hbar = waitbar(0,"","CreateCancelBtn", ... "setappdata(gcbf,""canceling"",1)"); hbar.Name = "Closed-Loop Simulation (MATLAB)"; % Store states and control for plotting purposes. xHistory1 = x0.'; uHistory1 = u0.'; % Initialize control. uk = uHistory1(1,:); % Initialize the accumulated elapsed time % for computing the optimal control action calculation. timerVal1 = 0; % Simulation loop for k = 1:(Duration/Ts) % Compute optimal control action using nlmpcmove. xk = xHistory1(k,:).'; % Call the nlmpcmove function. tic [uk, simdata] = nlmpcmove(msobj, xk, uk, simdata); % Accumulate the elapsed time. timerVal1 = timerVal1 + toc; % Simulate Two-Link Robot for the next control interval. ODEFUN = @(t,xk) twolinkStateFcn(xk,uk,pvstate); [TOUT, XOUT] = ode45(ODEFUN, [0 Ts], xHistory1(k,:)); % Log states and control. xHistory1(k+1,:) = XOUT(end,:); uHistory1(k+1,:) = uk; % Update two-link robot plot. hPlot.Theta1 = xHistory1(k+1,1); hPlot.Theta2 = xHistory1(k+1,2); drawnow limitrate % Check for clicked Cancel button. if getappdata(hbar,"canceling") delete(hbar) break end % Update waitbar figure. waitMsg = sprintf("Simulation Progress: %3.3d/%d", ... k, (Duration/Ts)); waitbar(k/(Duration/Ts), hbar, waitMsg); end The animation shows that the manipulator robot successfully moves from the start position to desired target position.

% Delete waitbar. delete(hbar) % Plot the closed loop results. figure; helperPlotResults(xHistory1, uHistory1, Ts, xf) The plot shows the joint angles, velocities, and torques of the manipulator. Additionally, it displays the desired states, indicating the successful attainment of the objective using the MATLAB function.

### Code Generation in MATLAB

To speed up computation time during simulation, use getCodeGenerationData and buildMEX to generate code for the Multistage MPC object with the specified properties.

Generate data structures using getCodeGenerationData.

[coredata, onlinedata] = getCodeGenerationData( ... msobj, x0, u0, pvstate);

Generate a MEX function called nlmpcControllerMEX.

buildMEX(msobj, "nlmpcControllerMEX", coredata, onlinedata);
Generating MEX function "nlmpcControllerMEX" from your "nlmpcMultistage" object to speed up simulation. Code generation successful. MEX function "nlmpcControllerMEX" successfully generated. 

It is best practice to compare the optimization results from both the standard nlmpcmove and nlmpcmoveCodeGeneration function. Since optimization results might vary for distinct initial guess, use the same run-time data structure for comparison of the optimal solution for Multistage MPC.

Update the onlinedata structure with the state and stage parameters, and use the InitialGuess generated using nlmpcmove.

% Update onlinedata structure with state and stage parameters. onlinedata.StateFcnParameter = simdata.StateFcnParameter; onlinedata.StageParameter = simdata.StageParameter; simdata.InitialGuess = onlinedata.InitialGuess;

Run and compare the optimization results using the same initial guess.

[~, ~, info1] = nlmpcmove(msobj, x0, u0, onlinedata); [~, ~, info2] = nlmpcmoveCodeGeneration(coredata, ... x0, u0, onlinedata); [~, ~, info3] = nlmpcControllerMEX(x0, u0, onlinedata); 

Plot optimization results:

figure; for ix = 1:nx subplot(3,2,ix); hold on; box on; plot(info1.Topt, info1.Xopt(:,ix), "-."); plot(info2.Topt, info2.Xopt(:,ix), "-x"); plot(info3.Topt, info3.Xopt(:,ix), "-+"); title(['State, x_' num2str(ix)]); xlabel("Time, s"); end for imv = 1:nmv subplot(3,2,ix+imv); hold on; box on; plot(info1.Topt, info1.MVopt(:,imv), "-."); plot(info2.Topt, info2.MVopt(:,imv), "-x"); plot(info3.Topt, info3.MVopt(:,imv), "-+"); title(['Control, u_' num2str(imv)]); xlabel("Time, s"); end legend("nlmpcmove", ... "nlmpcmoveCodeGeneration", ... "nlmpcControllerMEX") The plot shows a comparison between three distinct computations of the control action, demonstrating their equivalence as their results closely align with each other.

### Closed-Loop Simulation using MEX Function

Simulate the system for 2 seconds with a target trajectory to follow using the MEX Function.

% Simulation duration in seconds. Duration = 2; % Store states and control for plotting purposes. xHistory2 = x0.'; uHistory2 = u0.'; % Initialize current state and control. xk = xHistory2(1,:).'; uk = uHistory2(1,:).'; % Initialize the accumulated elapsed time % for computing the optimal control action calculation. timerVal2 = 0; % Simulation loop for k = 1:(Duration/Ts) % Compute optimal control action using nlmpcmove. xk = xHistory2(k,:).'; % Call the nlmpcControllerMEX function. tic [uk, onlinedata] = nlmpcControllerMEX(xk, uk, onlinedata); % Accumulate the elapsed time. timerVal2 = timerVal2 + toc; % Simulate Two-Link Robot for the next control interval. ODEFUN = @(t,xk) twolinkStateFcn(xk,uk,pvstate); [TOUT, XOUT] = ode45(ODEFUN, [0 Ts], xHistory2(k,:)); % Log states and control. xHistory2(k+1,:) = XOUT(end,:); uHistory2(k+1,:) = uk; % Update two-link robot plot. hPlot.Theta1 = xHistory2(k+1,1); hPlot.Theta2 = xHistory2(k+1,2); drawnow limitrate end The animation shows that the manipulator robot successfully moves from the start position to desired target position using the MEX function.

% Delete waitbar. delete(hbar) % Plot the closed loop results. figure; helperPlotResults(xHistory2, uHistory2, Ts, xf, 'show') The plot shows the joint angles, velocities, and torques of the manipulator robot. Additionally, it displays the desired states, indicating the successful attainment of the objective using the MEX function.

mdl = 'twolinkMultistageNLMPC'; open_system(mdl) Run the simulation.

% Turn off the usage of MEX Function. set_param([mdl '/Multistage Nonlinear MPC'], 'UseMex', 'off'); % Run and time simulation. tic simHistory1 = sim(mdl, Duration); timerVal3 = toc;

Run the simulation with the MEX Function to speed up simulation.

% Turn on the usage of MEX Function. set_param([mdl '/Multistage Nonlinear MPC'], 'UseMex', 'on'); set_param([mdl '/Multistage Nonlinear MPC'], 'mexname', ... 'nlmpcControllerMEX'); % Run and time simulation. tic simHistory2 = sim(mdl, Duration); The animation shows that the manipulator robot successfully moves from the start position to desired target position using Simulink.

timerVal4 = toc;

Plot all the simulation results.

% Use helperPlotResults function % to plot results stored in xHistory1, % xHistory2, simHistory1, and simHistory2. figure("Color", "w"); hold on; helperPlotResults(simHistory1) helperPlotResults(simHistory2) helperPlotResults(xHistory1, uHistory1, Ts) helperPlotResults(xHistory2, uHistory2, Ts, [], "show") Note that all employed simulations produce consistent state responses and control actions. Additionally, the elapsed time for each simulation is displayed in the following table:

timerVals = [timerVal1; timerVal2; timerVal3; timerVal4]; simNames = {'MATLAB Function'; 'MEX Function'; 'Simulink'; 'Simulink/MEX'}; SummaryTable = array2table(timerVals, ... "VariableNames", ... "Elapsed Time, sec"); SummaryTable.Properties.RowNames = simNames; disp(SummaryTable)
 Elapsed Time, sec _________________ MATLAB Function 256.37 MEX Function 2.8464 Simulink 369.76 Simulink/MEX 27.073 

### References

 Hatanaka, Takeshi, Nikhil Chopra, Masayuki Fujita, and Mark W. Spong. Passivity-Based Control and Estimation in Networked Robotics. Communications and Control Engineering. Cham: Springer International Publishing, 2015. https://doi.org/10.1007/978-3-319-15171-7.

 Ohtsuka, T. “Continuation/GMRES Method for Fast Algorithm of Nonlinear Receding Horizon Control.” Proceedings of the 39th IEEE Conference on Decision and Control (Cat. No.00CH37187), vol. 1, 2000, pp. 766–71 vol.1. IEEE Xplore, https://doi.org/10.1109/CDC.2000.912861.