CasADiでFATROPを使用してモデル予測制御を行う

# FATROPとは
FATROPは、高速で幅広いクラスの最適制御問題をサポートし、高い数値的堅牢性を実現することを目的とした非線形最適制御問題ソルバーです。最適制御問題の構造を活用しロボットの動作計画や制御を高速に計算することができます。

https://arxiv.org/abs/2303.16746

https://github.com/meco-group/fatrop

本記事ではCasADiでFATROPを使用する方法を説明し、実際にモデル予測制御に使用して計算時間を計測してみた結果についてまとめます。

# CasADiで使用する手順
FATROPはCasADiのソルバーとしてサポートされているのでビルドオプションで指定するだけで使用することができます。

## ビルド方法

以下の手順でビルド、インストールを行います。

```sh
# 依存関係のインストール
sudo apt install -y git cmake gcc g++ gfortran pkg-config liblapack-dev pkg-config coinor-libipopt-dev --install-recommends

# CasADiのクローン
git clone https://github.com/casadi/casadi.git casadi
echo "export CMAKE_PREFIX_PATH="$CMAKE_PREFIX_PATH:$HOME/local/"" >> ~/.bashrc
source ~/.bashrc
cd casadi
mkdir build
cd build

# FATROPを使用できるようにオプション指定してビルド
cmake .. -DWITH_IPOPT=ON -DWITH_FATROP=ON -DWITH_BUILD_REQUIRED=ON
make -j$(nproc)
sudo make install

# 共有ライブラリのパスを通す
sudo ldconfig
```

## プログラム内での使用方法
プログラムから使用する際には、IPOPTや他のnlpソルバと同様にソルバー名に```fatrop```を指定すればよいです。

```c++
casadi::Dict config;
casadi::MXDict casadi_prob;
casadi::Function solver = casadi::nlpsol("solver", "fatrop", casadi_prob, config);
```

またFATROPは一般の非線形最適化ソルバとしても機能しますが、これとは別に最適制御問題の問題構造に特化した動作モードがあり、使用するには最適制御問題の問題構造に合うように問題の定式化を行い、辞書型のオプションで設定を行う必要があります。

```c++
casadi::Dict config;
config["structure_detection"] = "auto";

std::vector<bool> equality; // 制約が等式制約か不等式制約か
config["equality"] = equality;
```
以上の設定をすることで最適制御問題の構造を自動で検出し、効率的に解いてくれます。

# ベンチマーク用プログラム
今回は自作のCasADi用MPCライブラリを用いて、exampleのシミュレーションの実行時間を計測し、ソルバーごとにどれほど違いが生まれるのかを調べます。

https://github.com/Kotakku/simple_casadi_mpc


## 実験に使用するソルバー
### IPOPTの線形ソルバー
IPOPTは内部で使用する線形ソルバーを選択することができます。デフォルトではMUMPSが使用されますが、アカデミックライセンスや法人ライセンスでHSLを購入することで高速なソルバーを選択することもできます。詳しくは以下の記事を参照してください。

https://qiita.com/Kotakku/items/c695e1a5cb22c6e6dcc4

### 実験するソルバーの一覧
| 条件名       | ソルバー | 条件                               | 
| :----------: | :------: | ---------------------------------------- | 
| IPOPT(MUMPS) | IPOPT    | 線形ソルバーにMUMPSを使用する            | 
| IPOPT(MA27)  | IPOPT    | 線形ソルバーにMA27を使用する            | 
| IPOPT(MA97)  | IPOPT    | 線形ソルバーにMA97を使用する            | 
| FATROP(NONE) | FATROP   | 汎用非線形ソルバーとして使用する         | 
| FATROP(AUTO) | FATROP   | ブロック構造を利用して効率的に問題を解く | 

## 実験条件
長くなるので「"(Dictのkey)"=(Dictのvalue)」形式で書いてます。

| 説明                   | 設定値 | IPOPTでの設定方法                     | FATROPでの設定方法                    | 
| :--------------------: | :----: | :-----------------------------------: | :-----------------------------------: | 
| 最大イテレーション回数 | 500    | "ipopt.max_iter" = 500                | "fatrop.max_iter" = 500               | 
| ウォームスタート       | あり   | "ipopt.warm_start_init_point" = "yes" | "fatrop.warm_start_init_point" = true | 
| 収束条件               | 1e-6   | "ipopt.tol" = 1e-6                    | "fatrop.tol" = 1e-6                   | 
| 許容可能収束条件       | 1e-4   | "ipopt.acceptable_tol" = 1e-4         | "fatrop.acceptable_tol" = 1e-4        | 


## ベンチマーク1 (2重積分器)
exampleから```double_integrator_mpc_example```の実行時間を5回計測し、その平均時間で比較します。
制御周期は10msで10秒間のシミュレーションを行います。

動作例

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/207366/c00fa846-a2b5-4ec9-95fd-1725ce4c755a.png)

### ベンチマーク結果

![mpc_time3.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/207366/8fc2c019-8828-4ba6-8211-6373ea1e664e.jpeg)

FATROP(AUTO)が最速でIPOPT(MUMPS)と比較して78%程高速になりました。

## ベンチマーク2 (倒立振子)

exampleから```cartpole_mpc_example```の実行時間を5回計測し、その平均時間で比較します。
制御周期は10msで12秒間のシミュレーションを行います。

動作例

![video.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/207366/538c9f01-0bff-439a-8127-2da6c48c13f0.gif)

### ベンチマーク結果

![mpc_time.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/207366/ac18cd29-73db-4fb9-896e-45c6346479da.jpeg)

![mpc_time2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/207366/c6090501-a72a-4932-b6d8-af0262fc3ef8.jpeg)

IPOPT(MA27)とFATROP(AUTO)が並んで最速となりました。IPOPT(MUMPS)と比較すると43%程高速になってます。

# まとめ
本記事ではCasADiでFATROPを用いてモデル予測制御を行い、FATROPの性能の高さを実感することができました。みなさんもぜひFATROPを使ってみてください！