3_optimal_control_robot_manipulation(日本語).ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction to `robotoc` \n",
    "# 3: Optimal Control of a Robot Manipulator"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 本章の目的\n",
    "ここでは，ロボットマニピュレータ KUKA iiwa14 のシンプルな最適制御問題（OCP）を定式化し，解くことが目的です．"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "KUKA iiwa14 の `robotoc.Robot` モデルを作成します．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotoc\n",
    "model_info = robotoc.RobotModelInfo()\n",
    "model_info.urdf_path = \"urdf/iiwa_description/urdf/iiwa14.urdf\"\n",
    "iiwa14 = robotoc.Robot(model_info)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "iiwa14の関節角速度およびトルクの上限を設定します．    \n",
    "(この値は制御などの目的により変わると思います．`robotoc`のAPIから\n",
    "好きに設定できます．)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "iiwa14.set_joint_effort_limit(np.full(iiwa14.dimu(), 50))\n",
    "iiwa14.set_joint_velocity_limit(np.full(iiwa14.dimv(), 0.5*np.pi))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "それらの設定した値が反映されているか以下で確認できます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(iiwa14)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Cost Function（評価関数）\n",
    "次に，評価関数 (cost function) を作成します．   \n",
    "`robotoc`では，様々な評価関数のコンポーネント（**cost components**）を組み合わせて評価関数, `robotoc.CostFunction` (`std::shared_ptr<robotoc::CostFunction>` in C++), を作ります．  \n",
    "各 **cost component** はある特定の評価関数の項を表しており，`robotoc.CostFunctionComponentBase` (`robotoc::CostFunctionComponentBase` in C++) を継承しています.   \n",
    "**cost component** を評価関数 `robotoc.CostFunction` に集め， 評価関数 `robotoc.CostFunction` をソルバーに渡します．   \n",
    "このようにすることで，複数の**cost components** を簡単に組み合わせて評価関数が設計できるようになっています."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "基本的な **cost components** は `robotoc` のパッケージに既に多数実装されています.  \n",
    "この例では， configuration つまり関節角，角速度，トルクなどについての**cost component**を考えます．   \n",
    "以下がその **cost component** です．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "config_cost = robotoc.ConfigurationSpaceCost(iiwa14)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "レファレンスと重みパラメータを設定していきます．   \n",
    "以下のような命名規則があります．   \n",
    "\n",
    "- `q_ref`: configuration (関節角) のレファレンス (目標値）\n",
    "- `q_weight`: configuration の重みパラメータ\n",
    "- `v_weight`: 一般化速度 (関節角速度) の重みパラメータ\n",
    "- `a_weight`: 一般化加速度 (関節角加速度) の重みパラメータ\n",
    "- `u_weight`: 関節トルクの重みパラメータ\n",
    "- `q_weight_terminal`, `v_weight_terminal`: ホライゾン終端での configuration と一般化速度の重みパラメータ"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q_ref = np.array([0, 0.5*np.pi, 0, 0.5*np.pi, 0, 0.5*np.pi, 0]) \n",
    "config_cost.set_q_ref(q_ref)\n",
    "config_cost.set_q_weight(np.full(iiwa14.dimv(), 10))\n",
    "config_cost.set_v_weight(np.full(iiwa14.dimv(), 0.01))\n",
    "config_cost.set_a_weight(np.full(iiwa14.dimv(), 0.01))\n",
    "config_cost.set_q_weight_terminal(np.full(iiwa14.dimv(), 10))\n",
    "config_cost.set_v_weight_terminal(np.full(iiwa14.dimv(), 0.01))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "評価関数 `robotoc.CostFunction` を作成して，上で作成した **cost component** を加えましょう．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cost = robotoc.CostFunction()\n",
    "cost.add(\"config_cost\", config_cost)\n",
    "print(cost)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Constraints（制約）\n",
    "制約についても評価関数と同様に作成します．   \n",
    "すなわち， `robotoc`では様々な**constraint components**を集めて制約, `robotoc.Constraints` (`std::shared_ptr<robotoc::Constraints>` in C++), を作ります．   \n",
    "各 **constraints component** はある特定の制約を表しており， `robotoc.ConstraintComponentBase` (`robotoc::ConstraintComponentBase` in C++) を継承しています．     \n",
    "**constraint component** を制約 `robotoc.Constraints` に集め， 制約 `robotoc.Constraints` をソルバーに渡します．   \n",
    "これにより，様々な **constraint components** を簡単に組み合わせて制約を設計できます."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "基本的な **constraint components** のは既に `robotoc` に多数実装されています.  \n",
    "この例では，関節角度，角速度，トルクの制約を考えます．     \n",
    "(ここで，これらの制約には `iiwa14.set_joint_effort_limit()` `iiwa14.set_joint_velocity_limit()` でセットした値が使われます)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "joint_position_lower  = robotoc.JointPositionLowerLimit(iiwa14)\n",
    "joint_position_upper  = robotoc.JointPositionUpperLimit(iiwa14)\n",
    "joint_velocity_lower  = robotoc.JointVelocityLowerLimit(iiwa14)\n",
    "joint_velocity_upper  = robotoc.JointVelocityUpperLimit(iiwa14)\n",
    "joint_torques_lower   = robotoc.JointTorquesLowerLimit(iiwa14)\n",
    "joint_torques_upper   = robotoc.JointTorquesUpperLimit(iiwa14)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "制約`robotoc.Constraints`を作り，上で作成した **constraint components** を加えます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "constraints = robotoc.Constraints()\n",
    "constraints.add(\"joint_position_lower\", joint_position_lower)\n",
    "constraints.add(\"joint_position_upper\", joint_position_upper)\n",
    "constraints.add(\"joint_velocity_lower\", joint_velocity_lower)\n",
    "constraints.add(\"joint_velocity_upper\", joint_velocity_upper)\n",
    "constraints.add(\"joint_torques_lower\", joint_torques_lower)\n",
    "constraints.add(\"joint_torques_upper\", joint_torques_upper)\n",
    "print(constraints)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Optimal Control Problem (OCP) and Solver\n",
    "\n",
    "ここでは最適制御問題 (OCP) とそのソルバーを作ります．  \n",
    "ホライゾン長さ `T` と離散化のグリッドの数 `N` を設定し，OCPを記述する `robotoc.OCP` (`robotoc::OCP` in C++)　を作ります．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T = 3.0\n",
    "N = 100\n",
    "ocp = robotoc.OCP(robot=iiwa14, cost=cost, constraints=constraints, T=T, N=N)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "ではこのOCPを解くためのソルバーを作ります． \n",
    "まず， `robotoc.UnconstrOCPSolver` (`robotoc::UnconstrOCPSolver` in C++)　を試します.  \n",
    "このソルバーは以下の特徴があります: \n",
    "- Direct multiple-shooting 法　と primal-dual interior point 法 （主双対内点法）\n",
    "- Inverse dynamics を用いた **unconstrained** OCP　 (i.e., no contacts or floating bases) の効率的なアルゴリズム.\n",
    "- Riccati recursion によるニュートンステップの計算\n",
    "\n",
    "種々のソルバーのオプションを `robotoc.SolverOptions` により設定できます．(ここではデフォルトのパラメータを用います).  \n",
    "例えば，ソルバー作成時に並列計算で用いるスレッドの数 (`nthreads`) を指定します."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "solver_options = robotoc.SolverOptions()\n",
    "solver_options.nthreads = 4\n",
    "ocp_solver = robotoc.UnconstrOCPSolver(ocp=ocp, solver_options=solver_options)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Solve the OCP"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "初期状態 (`q`, `v`) と初期時刻 (`t`) が以下で与えられるOCP（最適制御問題）を考えます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = 0.0\n",
    "q = np.array([0.5*np.pi, 0, 0.5*np.pi, 0, 0.5*np.pi, 0, 0.5*np.pi]) \n",
    "v = np.zeros(iiwa14.dimv())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "まずはソルバーを初期化します．   \n",
    "以下は初期推定解を上の`q`, `v`に設定します．   \n",
    "これは非常にシンプルですが高速な収束に割と重要です．   \n",
    "初期推定解をセットする前に`discretize(t)`関数によりOCPを離散化します．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ocp_solver.discretize(t)\n",
    "ocp_solver.set_solution(\"q\", q)\n",
    "ocp_solver.set_solution(\"v\", v)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "また，不等式制約に関してソルバーを初期化します．    \n",
    "(具体的には，主双対内点法のスラック変数とラグランジュ乗数を初期化しますが，特に内部について意識する必要はありません)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ocp_solver.init_constraints()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "ソルバーの現在の推定解がどの程度最適解に近いか，ということを判断するために，本ソルバーでは **KKT error** という指標を用います．   \n",
    "これは， Karush–Kuhn–Tucker (KKT) 条件（1次の最適性の必要条件）のl2ノルムです. (最適解でこれは0になります.)   \n",
    "まず，初期化後の KKT error を見てみます． "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ocp_solver.KKT_error(t, q, v)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "次にOCPを解きます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ocp_solver.solve(t, q, v)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "KKT error がとても小さくなっていることが分かります．    \n",
    "(具体的には，`robotoc.SolverOption`で設定した閾値より小さくなっています)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ocp_solver.KKT_error(t, q, v)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "また，各反復での KKT error やステップサイズといった情報も以下で見れます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "stats = ocp_solver.get_solver_statistics()\n",
    "print(stats)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "最適解は以下のようにして集めることができます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q_opt = ocp_solver.get_solution('q')\n",
    "v_opt = ocp_solver.get_solution('v')\n",
    "u_opt = ocp_solver.get_solution('u')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "解をプロットしてみます．   \n",
    "グレーの破線が制約を表しており，最適解は制約を厳密に守っていることがわかります．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn\n",
    "\n",
    "plt.rcParams['figure.figsize'] = 10, 9\n",
    "fig = plt.figure()\n",
    "ax_q = fig.add_subplot(3, 1, 1)\n",
    "ax_v = fig.add_subplot(3, 1, 2)\n",
    "ax_u = fig.add_subplot(3, 1, 3)\n",
    "\n",
    "# Plot results\n",
    "ax_q.plot(np.linspace(t, t+T, N+1), q_opt)\n",
    "ax_v.plot(np.linspace(t, t+T, N+1), v_opt)\n",
    "ax_u.plot(np.linspace(t, t+T-(T/N), N), u_opt)\n",
    "\n",
    "# Plot constraints boundaries\n",
    "ax_v.plot(np.linspace(t, t+T, N+1), np.full(N+1, 0.5*np.pi), linestyle='--', color='gray')\n",
    "ax_v.plot(np.linspace(t, t+T, N+1), np.full(N+1, -0.5*np.pi), linestyle='--', color='gray')\n",
    "ax_u.plot(np.linspace(t, t+T-(T/N), N), np.full(N, 50.0), linestyle='--', color='gray')\n",
    "ax_u.plot(np.linspace(t, t+T-(T/N), N), np.full(N, -50.0), linestyle='--', color='gray')\n",
    "\n",
    "ax_q.set_xlabel('')\n",
    "ax_v.set_xlabel('')\n",
    "ax_u.set_xlabel('t [s]')\n",
    "\n",
    "ax_q.set_ylabel('q [rad]')\n",
    "ax_v.set_ylabel('v [rad/s]')\n",
    "ax_u.set_ylabel('u [Nm]')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "また，以下で最適化された軌道を可視化できます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viewer = robotoc.utils.TrajectoryViewer(model_info=model_info,\n",
    "                                        viewer_type='meshcat')\n",
    "viewer.display(ocp_solver.get_time_discretization(), \n",
    "               ocp_solver.get_solution('q'))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Try Another Solver\n",
    "\n",
    "次に，別のソルバー `robotoc.UnconstrParNMPCSolver` (`robotoc::UnconstrParNMPCSolver` in C++) を試してみましょう.   \n",
    "このソルバーは以下の特徴があります: \n",
    "- Direct multiple-shooting 法　と primal-dual interior point 法 （主双対内点法）\n",
    "- Inverse dynamics を用いた **unconstrained** OCP　 (i.e., no contacts or floating bases) の効率的なアルゴリズム.\n",
    "- ParNMPC アルゴリズム（並列化ニュートン法） によるニュートンステップの計算\n",
    "- 各ニュートン反復はCPUのコア数が多ければ高速になります．  \n",
    "- 一方で，収束性能（収束までに必要な反復回数）は先ほどの `robotoc.UnconstrOCPSolver` と比べると悪化します． これはまた，ソルバーのロバスト性も悪くなることを意味します．\n",
    "\n",
    "種々のソルバーのオプションを `robotoc.SolverOptions` により設定できます．(ここではデフォルトのパラメータを用います).  \n",
    "例えば，ソルバー作成時に並列計算で用いるスレッドの数 (`nthreads`) を指定します."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "solver_options = robotoc.SolverOptions()\n",
    "solver_options.nthreads = 8\n",
    "parnmpc_solver = robotoc.UnconstrParNMPCSolver(ocp=ocp, solver_options=solver_options)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "parnmpc_solver.set_solution(\"q\", q)\n",
    "parnmpc_solver.set_solution(\"v\", v)\n",
    "parnmpc_solver.init_constraints()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "parnmpc_solver.init_constraints()\n",
    "parnmpc_solver.init_backward_correction()\n",
    "parnmpc_solver.solve(t, q, v)\n",
    "stats = parnmpc_solver.get_solver_statistics()\n",
    "print(stats)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}