1_getting_started(日本語).ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction to `robotoc` \n",
    "# 1: Getting Started"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## インストール (Ubuntu)\n",
    "1. OS が Ubuntu 22.04, 20.04, or 18.04であるか確認してください．（未確認ですが，Macでもインストール可能かもしれません．）\n",
    "2. ビルドに必要なツールを以下でインストールしてください． \n",
    "```\n",
    "sudo apt install build-essential\n",
    "```\n",
    "3. `Eigen3` を以下でインストールして下さい．\n",
    "```\n",
    "sudo apt install libeigen3-dev\n",
    "```\n",
    "4. [`Pinocchio`](https://github.com/stack-of-tasks/pinocchio) とそのビジュアライゼーションツール ([`gepeto-viewer-corba`](https://github.com/Gepetto/gepetto-viewer-corba))をインストールします． (詳細は [公式ドキュメンテーション](https://stack-of-tasks.github.io/pinocchio/download.html) を参照)\n",
    "  - Ubuntu 22.04:\n",
    "```\n",
    "sudo apt install -qqy lsb-release gnupg2 curl -y\n",
    "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg\" | sudo tee /etc/apt/sources.list.d/robotpkg.list\n",
    "curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -\n",
    "sudo apt update -y\n",
    "sudo apt install -qqy robotpkg-py310-pinocchio -y\n",
    "echo export PATH=/opt/openrobots/bin:$PATH >> ~/.bashrc\n",
    "echo export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH >> ~/.bashrc\n",
    "echo export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH >> ~/.bashrc\n",
    "echo export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH >> ~/.bashrc\n",
    "echo export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH >> ~/.bashrc\n",
    "sudo apt update && sudo apt install robotpkg-py310-qt5-gepetto-viewer-corba -y\n",
    "```\n",
    "  - Ubuntu 20.04:\n",
    "```\n",
    "sudo apt install -qqy lsb-release gnupg2 curl -y\n",
    "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg\" | sudo tee /etc/apt/sources.list.d/robotpkg.list\n",
    "curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add -\n",
    "sudo apt update -y\n",
    "sudo apt install -qqy robotpkg-py38-pinocchio -y\n",
    "echo export PATH=/opt/openrobots/bin:$PATH >> ~/.bashrc\n",
    "echo export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH >> ~/.bashrc\n",
    "echo export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH >> ~/.bashrc\n",
    "echo export PYTHONPATH=/opt/openrobots/lib/python3.8/site-packages:$PYTHONPATH >> ~/.bashrc\n",
    "echo export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH >> ~/.bashrc\n",
    "sudo apt update && sudo apt install robotpkg-py38-qt5-gepetto-viewer-corba -y\n",
    "```\n",
    "5. `robotoc` を以下でインストールします．\n",
    "```\n",
    "git clone https://github.com/mayataka/robotoc\n",
    "cd robotoc\n",
    "mkdir build && cd build\n",
    "cmake .. -DCMAKE_BUILD_TYPE=Release -DOPTIMIZE_FOR_NATIVE=ON\n",
    "sudo make install -j$(nproc)\n",
    "```\n",
    "  インストールできたら， Python の PATH を設定します．\n",
    "  - Ubuntu 22.04:\n",
    "```\n",
    "echo export PYTHONPATH=/usr/local/lib/python3.10/site-packages:$PYTHONPATH >> ~/.bashrc\n",
    "```\n",
    "  - Ubuntu 20.04:\n",
    "```\n",
    "echo export PYTHONPATH=/usr/local/lib/python3.8/site-packages:$PYTHONPATH >> ~/.bashrc\n",
    "```\n",
    "6. `pybullet` と `meshcat` をインストールします．\n",
    "```\n",
    "pip3 install pybullet meshcat\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## インストール (Mac)\n",
    "以下は　Mac でのインストール方法ですがおそらくコンパイルエラーが起きます．   \n",
    "Ubuntu の使用を強くお勧めします.   \n",
    "1. `Pinocchio` をインストール\n",
    "```\n",
    "brew tap gepetto/homebrew-gepetto\n",
    "brew install pinocchio\n",
    "```\n",
    "2. `g++` のフルパスを確認. (Mac のデフォルトCC++コンパイラは clang のため)\n",
    "```\n",
    "ls -l /usr/local/bin | grep g++\n",
    "```\n",
    "3. `robotoc` をインストール\n",
    "```\n",
    "git clone https://github.com/mayataka/robotoc\n",
    "cd robotoc\n",
    "mkdir build && cd build\n",
    "cmake .. -DCMAKE_BUILD_TYPE=Release -DOPTIMIZE_FOR_NATIVE=ON -DCMAKE_CXX_COMPILER=FULL_PATH_TO_GPLUSPLUS\n",
    "sudo make install -j$(nproc)\n",
    "```\n",
    "  Python の PATH をセット\n",
    "```\n",
    "echo export PYTHONPATH=/usr/local/lib/python3.9/site-packages:$PYTHONPATH >> ~/.zshrc\n",
    "```\n",
    "4. `pybullet` と `meshcat` をインストール\n",
    "```\n",
    "pip3 install pybullet meshcat\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Whole-body MPC\n",
    "インストールがうまくいったかを確認するために， whole-body MPC をテストしてみます．   \n",
    "細かい説明はこれ以降の notebook を参照してください．"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### モジュールのインポート"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import robotoc\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### ロボットの作成"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "model_info = robotoc.RobotModelInfo()\n",
    "model_info.urdf_path = 'urdf/a1_description/urdf/a1.urdf'\n",
    "model_info.base_joint_type = robotoc.BaseJointType.FloatingBase\n",
    "baumgarte_time_step = 0.05\n",
    "model_info.point_contacts = [robotoc.ContactModelInfo('FL_foot', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('RL_foot', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('FR_foot', baumgarte_time_step),\n",
    "                             robotoc.ContactModelInfo('RR_foot', baumgarte_time_step)]\n",
    "robot = robotoc.Robot(model_info)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Whole-body MPCの作成"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "T = 0.5\n",
    "N = 20\n",
    "mpc = robotoc.MPCFlyingTrot(robot, T, N)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Whole-body MPC に送るコマンド (`vcom_cmd`, `yaw_rate_cmd`) を設定"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# cmd_type = 'forward'\n",
    "# cmd_type = 'backward'\n",
    "# cmd_type = 'side'\n",
    "# cmd_type = 'curve'\n",
    "cmd_type = 'rotation'\n",
    "\n",
    "if cmd_type == 'forward':\n",
    "    step_length = np.array([0.2, 0.0, 0.0]) \n",
    "    step_yaw = 0.\n",
    "elif cmd_type == 'backward':\n",
    "    step_length = np.array([-0.1, 0.0, 0.0]) \n",
    "    step_yaw = 0.\n",
    "elif cmd_type == 'side':\n",
    "    step_length = np.array([0.0, 0.15, 0.0]) \n",
    "    step_yaw = 0.\n",
    "elif cmd_type == 'curve':\n",
    "    step_length = np.array([0.1, 0.0, 0.0]) \n",
    "    step_yaw = np.pi / 30.\n",
    "elif cmd_type == 'rotation':\n",
    "    step_length = np.array([0.0, 0.0, 0.0]) \n",
    "    step_yaw = np.pi / 20.\n",
    "    \n",
    "step_height = 0.1\n",
    "stance_time = 0.15\n",
    "flying_time = 0.1\n",
    "swing_start_time = 0.5\n",
    "\n",
    "vcom_cmd = 0.5 * step_length / (flying_time+stance_time)\n",
    "yaw_rate_cmd = step_yaw / flying_time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Foot step planner を作成"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "planner = robotoc.FlyingTrotFootStepPlanner(robot)\n",
    "raibert_gain = 0.9\n",
    "planner.set_raibert_gait_pattern(vcom_cmd, yaw_rate_cmd, flying_time, stance_time, raibert_gain)\n",
    "mpc.set_gait_pattern(planner, step_height, flying_time, stance_time, swing_start_time)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### シミュレーションの初期状態 (`q`, `v`) と初期時刻 (`t`) を設定"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.array([0, 0, 0.3181, 0, 0, 0, 1, \n",
    "              0.0,  0.67, -1.3, \n",
    "              0.0,  0.67, -1.3, \n",
    "              0.0,  0.67, -1.3, \n",
    "              0.0,  0.67, -1.3])\n",
    "v = np.zeros(robot.dimv())\n",
    "t = 0.0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### MPCを初期化 (Newton反復を少し行います．)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "option_init = robotoc.SolverOptions()\n",
    "option_init.max_iter = 10\n",
    "option_init.nthreads = 4\n",
    "mpc.init(t, q, v, option_init)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### MPC にオプション（サンプリング周期ごとの反復回数，並列計算で使用するスレッド数）を設定 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "option_mpc = robotoc.SolverOptions()\n",
    "option_mpc.max_iter = 2 # MPC iterations\n",
    "option_mpc.nthreads = 4\n",
    "mpc.set_solver_options(option_mpc)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### シミュレータを作成し，シミュレーションを行います．"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from a1_simulator import A1Simulator\n",
    "from robotoc_sim import CameraSettings, MPCSimulation\n",
    "\n",
    "time_step = 0.0025 # 400 Hz MPC\n",
    "t0 = 0.0\n",
    "q0 = q.copy()\n",
    "simulation_time = 5.0\n",
    "\n",
    "a1_simulator = A1Simulator(urdf_path=model_info.urdf_path, time_step=time_step)\n",
    "camera_settings = CameraSettings(camera_distance=2.0, camera_yaw=45, camera_pitch=-10.0, \n",
    "                                 camera_target_pos=q0[0:3]+np.array([0.1, 0.5, 0.0]))\n",
    "a1_simulator.set_camera_settings(camera_settings=camera_settings)\n",
    "\n",
    "simulation = MPCSimulation(simulator=a1_simulator)\n",
    "simulation.run(mpc=mpc, t0=t0, q0=q0, simulation_time=simulation_time, \n",
    "               feedback_delay=True, verbose=False, log=True, name='a1_flying_trot')\n",
    "a1_simulator.disconnect()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### シミュレーション結果をプロット\n",
    "(`matplotlib`, `seaborn` が必要です．例えば，`pip install matplotlib seaborn`でインストールしてください．)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q_log = np.genfromtxt(simulation.q_log)\n",
    "v_log = np.genfromtxt(simulation.v_log)\n",
    "t_log = np.genfromtxt(simulation.t_log)\n",
    "sim_steps = t_log.shape[0]\n",
    "\n",
    "vcom_log = []\n",
    "wcom_log = []\n",
    "vcom_cmd_log = []\n",
    "yaw_rate_cmd_log = []\n",
    "for i in range(sim_steps):\n",
    "    R = robotoc.utils.rotation_matrix_from_quaternion(q_log[i][3:7])\n",
    "    robot.forward_kinematics(q_log[i], v_log[i])\n",
    "    vcom_log.append(R.T@robot.com_velocity()) # robot.com_velocity() is expressed in the world coordinate\n",
    "    wcom_log.append(v_log[i][3:6])\n",
    "    vcom_cmd_log.append(vcom_cmd)\n",
    "    yaw_rate_cmd_log.append(yaw_rate_cmd)\n",
    "\n",
    "%matplotlib inline\n",
    "plot_mpc = robotoc.utils.PlotCoMVelocity()\n",
    "plot_mpc.plot(t_log, vcom_log, wcom_log, vcom_cmd_log, yaw_rate_cmd_log, simulation.name)"
   ]
  }
 ],
 "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
}