2_robot_models(日本語).ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction to `robotoc` \n",
    "# 2: Robot  Models"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 本章の目的\n",
    "URDFからロボット (`robotoc::Robot`) モデルを作成することが目的です.  \n",
    "ここで， `robotoc::Robot`は [`Pinocchio`](https://github.com/stack-of-tasks/pinocchio) のAPIをラップしています.  \n",
    "より深く知りたい方は， [`Pinocchio` の documentation](https://stack-of-tasks.github.io/pinocchio/) を参照してください."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## What is URDF?\n",
    "URDF (universal robot description format) はロボットのモデルを記述するフォーマットです.  \n",
    "既成のURDFを`robotoc`で利用するだけであれば，URDFについての知識はほとんど必要ありません．   \n",
    "本リポジトリの `urdf`フォルダに既成のURDFパッケージがあります．   \n",
    "その中身を見れば分かりますが，URDFパッケージはurdf ファイル(`~.urdf`) とメッシュファイル (e.g., `~.dae` or `~.obj`)から成ります．   \n",
    "urdf ファイルは他関節ロボットのツリー構造を記述し，メッシュファイルは可視化や衝突検証のために用いられます．   \n",
    "より詳しく知りたい方はROSのチュートリアル http://wiki.ros.org/urdf/Tutorials などを参照してください.  "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Simplest example: ロボットマニピュレータ `iiwa14`\n",
    "\n",
    "まずは最もシンプルな `robotoc.Robot` (`robotoc::Robot` in C++) の例としてロボットマニピュレータ（ロボットアーム）を扱います.  \n",
    "ここで，C++での `robotoc::Robot` はPythonより非常に多くの機能があります.（Pythonでは最低限の機能しか提供していません．）"
   ]
  },
  {
   "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": [
    "ロボットの様々な情報は以下のようにプリントできます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(iiwa14)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "プリントされた情報は以下の通りです．\n",
    "- `dimq`: configuration（一般化座標）の次元\n",
    "- `dimv`: 一般加速度の次元 (`dimq`と異なる場合があります)\n",
    "- `dimu`: 入力トルクの次元\n",
    "- `dim_passive`: passiveなジョイント（力をだせないジョイント）の次元． floating base を含みます．\n",
    "- `frames`: ロボットのフレーム．評価関数や制約でエンドエフェクタを指定する際に用います．\n",
    "- `joints`: ジョイントの情報．\n",
    "- `effort limit`: ジョイントのトルクの上限．\n",
    "- `velocity limit`: ジョイントの速度の上限．\n",
    "- `lower position limit`, `upper position limit`:　ジョイントの位置の上下限．\n",
    "\n",
    "これらのうち一部は以下のように個々に得られます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.dimq()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.dimv()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.dimu()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.dim_passive()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.joint_effort_limit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.joint_velocity_limit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.lower_joint_position_limit()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "iiwa14.upper_joint_position_limit()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "`robotoc`でロボットの可視化もできます．試しにランダムな関節角について可視化してみましょう．　　　   \n",
    "（実際には，ランダムな関節角速度を生成し，それを積分して関節角軌道を生成しています．）"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "steps = 100\n",
    "dt = 0.1\n",
    "q = []\n",
    "q.append(iiwa14.generate_feasible_configuration())\n",
    "for i in range(steps):\n",
    "    v = np.random.randn(iiwa14.dimv())\n",
    "    q.append(iiwa14.integrate_configuration(q[-1], v, dt))\n",
    "\n",
    "time_discretization = robotoc.TimeDiscretization(T=dt*steps, N=steps)\n",
    "    \n",
    "viewer = robotoc.utils.TrajectoryViewer(model_info=model_info, viewer_type='meshcat')\n",
    "viewer.display(time_discretization, q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pythonでもシンプルな forward kinematics （指定した関節角に対する end-effector の位置計算） を試すことができます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "end_effector_frame = 'iiwa_link_ee_kuka'\n",
    "for e in q:\n",
    "    iiwa14.forward_kinematics(e)\n",
    "    print('q: ', e)\n",
    "    print('frame position: ', iiwa14.frame_position(end_effector_frame))\n",
    "    print('frame rotation: ', iiwa14.frame_rotation(end_effector_frame))\n",
    "    print('frame placement (SE3): ', iiwa14.frame_placement(end_effector_frame))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4脚ロボット `ANYmal`\n",
    "\n",
    "次に４脚ロボットを試してみます．    \n",
    "4脚ロボットは**4つの点接触 (Point Contacts)**を伴う点がマニピュレータとの大きな違いです．   \n",
    "接触がある場合には， Baumgarte の安定化法のパラメータを設定する必要があります．これにより加速度レベルでの rigid contact を表現します.    \n",
    "詳細は [こちらの論文](https://www.researchgate.net/publication/234610391_A_Parametric_Study_on_the_Baumgarte_Stabilization_Method_for_Forward_Dynamics_of_Constrained_Multibody_Systems) などを参照して下さい.  \n",
    "適切なパラメータはMPCの離散化の時間ステップなどに依存します.  手元で少し試して決めてください．   \n",
    "私のこれまでの経験では，MPCの時間ステップの 2~10倍程度に Baumgarte の安定化法の時間ステップを設定すればうまくいきます．   \n",
    "ここでは 0.04とします．\n",
    "これら接触についての情報は，接触モデル`robotoc.ContactModelInfo`（C++では`robotoc::ContactModelInfo`）にまとめます．  例えば，"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "baumgarte_time_step = 0.04\n",
    "robotoc.ContactModelInfo('FL_foot', baumgarte_time_step)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "またこのロボットは **Floating Base** を伴う点がロボットアームと異なります．　　\n",
    "**URDF のパス**， **接触モデル**, **ベースの種類  (floating or fixed base)**といった情報は，ロボットモデルの設定を表す `robotoc.RobotModelInfo` (C++では `robotoc::RobotModelInfo`) で指定し，ロボットモデルを作成します．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "model_info = robotoc.RobotModelInfo()\n",
    "model_info.urdf_path = 'urdf/anymal_b_simple_description/urdf/anymal.urdf'\n",
    "model_info.base_joint_type = robotoc.BaseJointType.FloatingBase\n",
    "baumgarte_time_step = 0.04\n",
    "model_info.point_contacts = [robotoc.ContactModelInfo('LF_FOOT', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('LH_FOOT', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('RF_FOOT', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('RH_FOOT', baumgarte_time_step)]\n",
    "anymal = robotoc.Robot(model_info)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "ロボットモデルの詳細は以下で見れます．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(anymal)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "再び，以下で可視化してみましょう．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "steps = 100\n",
    "dt = 0.1\n",
    "q = []\n",
    "q.append(anymal.generate_feasible_configuration())\n",
    "q[0][0:3] = np.zeros(3)\n",
    "for i in range(steps):\n",
    "    v = np.concatenate([0.1*np.random.randn(6), np.random.randn(anymal.dimu())])\n",
    "    q.append(anymal.integrate_configuration(q[-1], v, dt))\n",
    "\n",
    "time_discretization = robotoc.TimeDiscretization(T=dt*steps, N=steps)\n",
    "\n",
    "viewer = robotoc.utils.TrajectoryViewer(model_info=model_info, viewer_type='meshcat')\n",
    "viewer.display(time_discretization, q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## ヒューマノイドロボット `iCub`\n",
    "\n",
    "次に，ヒューマノイドロボットを扱います．   \n",
    "ヒューマノイドロボットは**面接触 (Surface Contacts)** を2つ伴う点が4脚ロボットとの違いです．以下のようにモデルを設定します．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "model_info = robotoc.RobotModelInfo()\n",
    "model_info.urdf_path = 'urdf/icub_description/urdf/icub.urdf'\n",
    "model_info.base_joint_type = robotoc.BaseJointType.FloatingBase\n",
    "baumgarte_time_step = 0.05\n",
    "model_info.surface_contacts = [robotoc.ContactModelInfo('l_sole', baumgarte_time_step),\n",
    "                               robotoc.ContactModelInfo('r_sole', baumgarte_time_step)]\n",
    "icub = robotoc.Robot(model_info)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "ロボットモデルの詳細を見てみましょう．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(icub)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "また，以下で可視化してみましょう．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "steps = 100\n",
    "dt = 0.1\n",
    "q = []\n",
    "q.append(icub.generate_feasible_configuration())\n",
    "q[0][0:3] = np.zeros(3)\n",
    "for i in range(steps):\n",
    "    v = np.concatenate([0.1*np.random.randn(6), np.random.randn(icub.dimu())])\n",
    "    q.append(icub.integrate_configuration(q[-1], v, dt))\n",
    "\n",
    "time_discretization = robotoc.TimeDiscretization(T=dt*steps, N=steps)\n",
    "    \n",
    "viewer = robotoc.utils.TrajectoryViewer(model_info=model_info, viewer_type='meshcat')\n",
    "viewer.display(time_discretization, q)"
   ]
  }
 ],
 "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
}