# Control of Robot Manipulator Using Passivity-Based Nonlinear MPC

This example shows how to design a passivity-based controller for a robot manipulator using nonlinear model predictive control (MPC).

### Overview

The dynamics for a two-link robot manipulator can be written 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$.

• $\mathit{H}\left(\mathit{q}\right)$ is the manipulator inertia matrix.

• $\mathit{C}\left(\mathit{q},\stackrel{˙}{\mathit{q}}\right)$ is the Coriolis matrix.

• $\mathit{G}\left(\mathit{q}\right)$ is the gravity vector.

These robot dynamics are implemented in `manipulatorStateFcn.m`.

The control objective is to select torque $\tau$ such that the joint angles $\mathit{q}$ track a desired reference ${\mathit{q}}_{\mathit{d}}$. To enforce closed-loop stability, the controller includes a passivity constraint .

### Passivity Constraint

To define the passivity constraint, first define the tracking error vector as the difference between the joint angles and the desired reference angles.

`${\mathit{e}}_{\mathit{q}}=\mathit{q}-{\mathit{q}}_{\mathit{d}}$`

To achieve good tracking performance, consider the storage function $\mathit{V}=\frac{1}{2}\left({\stackrel{˙}{\mathit{q}}}^{\mathit{T}}\mathit{H}\left(\mathit{q}\right)\stackrel{˙}{\mathit{q}}+{\mathit{e}}_{\mathit{q}}^{\mathit{T}}{\mathrm{Ke}}_{\mathit{q}}\right)$, where $\mathit{K}>0$. Take the derivative of $\mathit{V}$ to obtain the relationship $\stackrel{˙}{\mathit{V}}={\mathit{u}}^{\mathit{T}}\stackrel{˙}{\mathit{q}}$, where

$\mathit{u}=\tau -\mathit{G}\left(\mathit{q}\right)+{\mathrm{Ke}}_{\mathit{q}}$.

Therefore, the system is passive from $\mathit{u}$ to $\stackrel{˙}{\mathit{q}}$. The relationship between the passivity input $\mathit{u}$ and torque $\tau$ is described in the helper function `getPassivityInput.m`.

To enforce closed-loop stability, define the passivity constraint as follows .

${\mathit{u}}^{\mathit{T}}\stackrel{˙}{\mathit{q}}\le -\rho {\stackrel{˙}{\mathit{q}}}^{\mathit{T}}\stackrel{˙}{\mathit{q}}$ with $\rho >0$.

For a nonlinear MPC controller, you define the passivity constraint by setting the `Passivity` property of the nonlinear MPC object.

### Design Nonlinear MPC Controller

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

`nlobj = nlmpc(4,4,2);`
```In standard cost function, zero weights are applied by default to one or more OVs because there are fewer MVs than OVs. ```

Specify the prediction model state function using the robot dynamics function.

`nlobj.Model.StateFcn = "manipulatorStateFcn";`

Specify a sample time of `0.1` seconds and use default prediction and control horizons.

`nlobj.Ts = 0.1;`

The default cost function in nonlinear MPC is a standard quadratic cost function, which is suitable for reference tracking. In this example, the goal is to have the first two states follow a given reference trajectory. Therefore, specify nonzero tuning weights for the first two output variables.

```nlobj.Weights.OutputVariables = [2 1 0 0]; nlobj.Weights.ManipulatedVariablesRate = [0 0];```

Specify the fields of the passivity property of the nonlinear MPC object.

```nlobj.Passivity.EnforceConstraint = true; nlobj.Passivity.InputFcn = "getPassivityInput"; nlobj.Passivity.OutputFcn = "getPassivityOutput";```

### Closed-Loop Simulation

Specify the initial conditions of the states.

`x0 = [-2;-1;1;1];`

```mdl = "manipulatorNLMPC"; open_system(mdl)``` Run the model.

`sim(mdl);`

View the manipulator states. Both joint angles reach and stay at the target value 1.

`open_system(mdl + "/Manipulator/states")` To view the performance of the nonlinear MPC controller without the passivity constraint, remove the passivity constraint from the controller.

`nlobj.Passivity.EnforceConstraint = false;`

Run the simulation.

`sim(mdl);`

Without the passivity constraint, the closed-loop system becomes unstable with the same controller design parameters. ### 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.

 Raff, Tobias, Christian Ebenbauer, and Frank Allgöwer. "Nonlinear model predictive control: A passivity-based approach." In Assessment and Future Directions of Nonlinear Model Predictive Control, edited by Findeisen, Rolf, Frank Allgöwer, and Lorenz T. Biegler, 151-162; New York: Springer, 2007.
{"ft_lang_label":"__label__en","ft_lang_prob":0.7580262,"math_prob":0.99183595,"size":3736,"snap":"2022-40-2023-06","text_gpt3_token_len":852,"char_repetition_ratio":0.14683816,"word_repetition_ratio":0.007751938,"special_character_ratio":0.20851178,"punctuation_ratio":0.18303572,"nsfw_num_words":0,"has_unicode_error":false,"math_prob_llama3":0.9993549,"pos_list":[0,1,2,3,4,5,6],"im_url_duplicate_count":[null,1,null,1,null,1,null],"WARC_HEADER":"{\"WARC-Type\":\"response\",\"WARC-Date\":\"2023-01-29T11:32:13Z\",\"WARC-Record-ID\":\"<urn:uuid:61cadffb-68b0-410b-bdac-e3ea36c4251d>\",\"Content-Length\":\"92713\",\"Content-Type\":\"application/http; msgtype=response\",\"WARC-Warcinfo-ID\":\"<urn:uuid:08d10f2c-204d-452c-a392-a0dcd8e51d24>\",\"WARC-Concurrent-To\":\"<urn:uuid:1501792e-09c7-48dc-b578-f07991588942>\",\"WARC-IP-Address\":\"104.86.80.92\",\"WARC-Target-URI\":\"https://au.mathworks.com/help/mpc/ug/control-of-robot-manipulator-using-passivity-based-nonlinear-mpc.html\",\"WARC-Payload-Digest\":\"sha1:K7G26BJU6ITG6VU4OGM6WATWKSLD6XJP\",\"WARC-Block-Digest\":\"sha1:3P46BESUBUEBQN2DF62TZUBUDJ6GL6OO\",\"WARC-Identified-Payload-Type\":\"text/html\",\"warc_filename\":\"/cc_download/warc_2023/CC-MAIN-2023-06/CC-MAIN-2023-06_segments_1674764499713.50_warc_CC-MAIN-20230129112153-20230129142153-00320.warc.gz\"}"}