#!/usr/bin/env python
# coding: utf-8

# ## 目的地への到達時間が最小になるように制御する
# 
# [JModelica Users Guide - Version 2.10](https://jmodelica.org/downloads/UsersGuide-2.10.pdf) 6.5.2.2 Minimum time problem を実習します。このモデルは、__Mac版ではエラー__になってしまいました。__Windows版のみ成功__しました。
# 
# ### 概要
# #### 制御対象の微分代数方程式モデル (DAEs)
# 状態変数を $x_1, x_2$, 制御変数を $u$ とする以下の微分代数方程式系の初期値問題を考えます。Van der Pos oscillator モデルです。
# $$
# \begin{align}
# \frac{d x_1}{d t} =& (1-x_2^2) x_1 -x_2 +u\\
# \frac{d x_2}{d t} =& x_1
# \end{align}
# $$
# 初期条件を、
# $$
# x_1(0)=0, x_2(0)=1
# $$
# 制約条件を、
# $$
# x_1(t_f) = 0, x_2(t_f)= 0
# $$
# と
# $$
# -1  \leq u \leq 1 
# $$
# とします。
# 
# #### 到達時間を最小化する問題 (Minimum time problem)
# 状態変数 $(x_1, x_2)$ は、時刻 $t=0$ に初期値 $(0,1)$ から出発して、時刻 $t_f$ に目的値 $(0,0)$ に到達します。到達時間 $t_f$ が最小になるように制御変数 $u$ を制御します。
# 
# ### モデル化と解法
# 最適化問題モデルのソースコードを作成します。
# 目標関数として finalTime を設定し、finalTime のパラメータ free を true にし、パラメータ initialGuess に予測値を設定します。制御変数 $u$ の制約条件となる下限と上限はパラメータ min と max で設定します。 

# In[1]:


get_ipython().run_cell_magic('writefile', 'VDP_Opt_Min_Time.mop', 'optimization VDP_Opt_Min_Time (\n  objective = finalTime,\n  startTime = 0,\n  finalTime(free=true,min=0.2,initialGuess=1))\n \n  // The states\n  Real x1(start = 0,fixed=true);\n  Real x2(start = 1,fixed=true);\n  \n  // The control signal\n  input Real u(free=true,min=-1,max=1);\n\nequation\n  // Dynamic equations\n  der(x1) = (1 - x2^2) * x1 - x2 + u;\n  der(x2) = x1;\n    \nconstraint\n  // terminal constraints\n  x1(finalTime)=0;\n  x2(finalTime)=0;\nend VDP_Opt_Min_Time;\n')


# 必要なモジュールをインポートします。

# In[2]:


get_ipython().run_line_magic('matplotlib', 'notebook')
# Import numerical libraries
import numpy as N
import matplotlib.pyplot as plt
# Import the JModelica.org Python packages
from pymodelica import compile_fmu
from pyfmi import load_fmu
from pyjmi import transfer_optimization_problem


# モデルをロードして最適化問題を解きます。

# In[3]:


vdp = transfer_optimization_problem("VDP_Opt_Min_Time", "VDP_Opt_Min_Time.mop")
res = vdp.optimize()


# 結果をプロットします。

# In[4]:


# Extract variable profiles
x1=res['x1']
x2=res['x2']
u=res['u']
t=res['time']

# Plot
plt.figure(1)

plt.subplot(311)
plt.plot(t,x1)
plt.grid()
plt.ylabel('x1')

plt.subplot(312)
plt.plot(t,x2)
plt.grid()
plt.ylabel('x2')

plt.subplot(313)
plt.plot(t,u)
plt.grid()
plt.ylabel('u')
plt.xlabel('time')
plt.show()


# 軌跡をプロットします。

# In[5]:


plt.figure(2)
plt.plot(x1,x2)
plt.xlim(-2,2)
plt.ylim(-2,2)
plt.axes().set_aspect('equal')


# In[ ]: