8.10. Real-time SQP Solver: Robotic Arm Manipulator

In this example we illustrate the use of the real-time Sequential Quadratic Programming (SQP) solver. In particular, we use a robotic arm manipulator described by a set of ordinary differential equations (ODEs):

\[\begin{split}\ddot{\theta}_1 =~& \gamma \\ \ddot{\theta}_2 =~& \frac{1}{\beta_2}(\tau_2 - \beta_1\gamma - \beta_3\dot{\theta}_1^2 - \beta_4) \\ \dot{\tau}_1 =~& u_1 \\ \dot{\tau}_2 =~& u_2\end{split}\]

where \(\theta_1, \theta_2\) are joint angles modelling the manipulator configuration, \(u_1, u_2\) are the rates (inputs) of the torques \(\tau_1, \tau_2\) applied to the joints and

\[\gamma~\hat{=}~\frac{1}{\alpha_1 - \alpha_2 \frac{\beta_1}{\beta_2}}(\frac{\alpha_2}{\beta_2}(\beta_4 + \beta_3\dot{\theta}_1^2 - \tau_2) - \alpha_3\dot{\theta}_1\dot{\theta}_2 - \alpha_4\dot{\theta}_2 - \alpha_5 + \tau_1).\]

The coefficients \(\alpha_1, \alpha_2, \alpha_3, \alpha_4, \alpha_5\) and \(\beta_1, \beta_2, \beta_3, \beta_4\) depend on the inertia and mass of the robot arm components. Their expressions can be found in [SicSci09]. The optimal control problem is formalized from the state \(x\) defined by

\[x~\hat{=}~(\theta_1, \dot{\theta}_1, \theta_2, \dot{\theta}_2, \tau_1, \tau_2)^{\top}\]

and the input \(u\) defined as

\[u~\hat{=}~(\dot{\tau}_1, \dot{\tau}_2)^{\top}.\]

The control objective is to make the first joint angle \(\theta_1\) follow a reference of \(1.2\,\mathrm{rad}\) from \(0\) to \(10\,\mathrm{s}\) and \(-1.2\,\mathrm{rad}\) from \(10\) to \(20\,\mathrm{s}\). Similarly, the second joint angle \(\theta_2\) should follow a reference of \(-1.2\,\mathrm{rad}\) from \(0\) to \(10\,\mathrm{s}\) and \(1.2\,\mathrm{rad}\) from \(10\) to \(20\,\mathrm{s}\). The stage variable \(z\) is defined by stacking the input and differential state variables:

\[z = (\dot{\tau}_1, \dot{\tau}_2, \theta_1, \dot{\theta}_1, \theta_2, \dot{\theta}_2, \tau_1, \tau_2)^\top\]

You can download the Matlab code of this example to try it out for yourself by clicking here along with the dynamics here.

8.10.1. Defining the MPC problem

8.10.1.1. Tracking objective

Our goal is to minimize the distance of the joint angles to the reference, which can be translated in the following stage cost function:

\[f(z,p) = 1000(z_3 - 1.2p)^2 + 0.1z_4^2 + 1000(z_5 + 1.2p)^2 + 0.1z_6^2 + 0.01z_7^2 + 0.01z_8^2 + 0.01z_1^2 + 0.01z_2^2,\]

where \(p\) is a run-time parameter taking value \(1\) from \(0\) to \(10\,\mathrm{s}\) and \(-1\) from \(10\) to \(20\,\mathrm{s}\).

The stage cost function is coded in MATLAB as the least-squares vector:

model.LSobjective = @(z,p)[sqrt(1000) * (z(3)-p(1)*1.2);...
                           sqrt(0.1) * z(4);...
                           sqrt(1000) * (z(5)+p(1)*1.2);...
                           sqrt(0.1) * z(6);...
                           sqrt(0.01) * z(7);...
                           sqrt(0.01) * z(8);...
                           sqrt(0.01) * z(1);...
                           sqrt(0.01) * z(2)];

This is needed to compute a Gauss-Newton approximation from the jacobian of the least-squares vector.

8.10.1.2. State and input constraints

The following constraints are imposed on the torques and torque rates:

\begin{align*} -100\,\mathrm{Nm}\leq & \tau_1 \leq 70\,\mathrm{Nm} \\ -100\,\mathrm{Nm}\leq & \tau_2 \leq 70\,\mathrm{Nm} \\ -200\,\mathrm{Nm/s}\leq & \dot{\tau}_1 \leq 200\,\mathrm{Nm/s} \\ -200\,\mathrm{Nm/s}\leq & \dot{\tau}_2 \leq 200\,\mathrm{Nm/s} \\ \end{align*}

This corresponds to the MATLAB code below.

% upper/lower variable bounds lb <= x <= ub
model.lb = [ -200,  -200,     -pi,  -100,  -pi,  -100,   -100,   -100  ];
model.ub = [  200,   200,      pi,   100,   pi,   100,    70,    70  ];

8.10.1.3. Initial condition and horizon length

The prediction horizon is set to \(21\) and the following initial condition is set

model.xinit = [-0.4 0 0.4 0 0 0 ]';
model.xinitidx = 3:8;

8.10.2. Generating a real-time SQP solver

We have now populated model with the necessary fields to generate an SQP solver, which requires settings a few options, as follows:

%% Define solver options
codeoptions = getOptions('RobotArmSolver');
codeoptions.maxit = 200;                                    % Maximum number of iterations of inner QP solver
codeoptions.printlevel = 0;                                 % Use printlevel = 2 to print progress (but not for timing)
codeoptions.optlevel = 3;
% Explicit Runge-Kutta 4 integrator
codeoptions.nlp.integrator.Ts = integrator_stepsize;
codeoptions.nlp.integrator.nodes = 5;
codeoptions.nlp.integrator.type = 'ERK4';
% Options for SQP solver
codeoptions.solvemethod = 'SQP_NLP';                        % SQP algorithm
codeoptions.nlp.hessian_approximation = 'gauss-newton';     % Gauss-Newton hessian approximation of nonlinear least-squares objective
codeoptions.sqp_nlp.use_line_search = 0;                    % Disable line-search for efficiency (only doable with Gauss-Newton approximation)

%% Generate real-time SQP solver
FORCES_NLP(model, codeoptions);

The number of solved QPs in every iteration is set via sqp_nlp.maxSQPit. It is important to note that disabling the line search in the SQP algorithm does not guarantee global convergence and hence may result in less robust performance, but produces much faster solve times. Turning off the line search via sqp_nlp.use_line_search is only allowed when the Gauss-Newton approximation is on.

8.10.3. Calling the generated SQP solver

Once all parameters have been populated, the MEX interface of the solver can be used to invoke it:

% Set primal initial guess
x0i = model.lb+(model.ub-model.lb)/2;
x0 = repmat(x0i',model.N,1);
problem.x0 = x0;

% Set reference as run-time parameter
problem.all_parameters = ones(model.N,1);

% Set initial condition
problem.xinit = X(:,i);

% Call SQP solver
[output, exitflag, info] = RobotArmSolver(problem);

The RobotArmSolver is expected to return an exitflag equal to \(1\), which corresponds to a successful solver. However, note that the QP could become infeasible in some cases. In this case, one should expect an exitflag of \(-8\).

8.10.4. Results

The control objective is to track the joint references of \(-1.2\,\mathrm{rad}\) and \(1.2\,\mathrm{rad}\) respectively, while keeping the input torque rates below \(200\,\mathrm{Nm/s}\) in magnitude and the torque states between \(-100\,\mathrm{N}\) and \(70\,\mathrm{Nm}\).

The joint angle and torques trajectories are shown in Figure Figure 8.46 and Figure Figure 8.47 respectively, while the input torque rates are plotted in Figure Figure 8.48. The closed-loop objective, which is a measure of the control performance is shown in Figure Figure 8.49.

../../_images/jointAngle.png

Figure 8.46 Manipulator’s joint angle.

../../_images/torques.png

Figure 8.47 Manipulator’s torques at joints.

../../_images/torqueRates.png

Figure 8.48 Manipulator’s torque rates.

../../_images/closedLoopObj.png

Figure 8.49 Manipulator’s closed loop objective.

SicSci09

Siciliano, B.; Sciavicco, L.; Villani, L.; Oriolo, G.. Robotics: Modelling, planning and control. Berlin: Springer, 2009.