result_visualization.ipynb

{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Results Visualization"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Swing with Different Foot Step Displacement"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Unoptimized swing path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib import pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "_FOOT_STEP_X_DISP_LIST = np.array([0.1, 0.2, 0.3])\n",
    "_FOOT_STEP_Y_DISP_LIST = np.array([-0.1, 0, 0.1])\n",
    "_WITH_OPTIMIZATION = False\n",
    "\n",
    "foot_path_list = []\n",
    "foot_opt_path_list = []\n",
    "foot_real_path_list = []\n",
    "for foot_step_x_disp in _FOOT_STEP_X_DISP_LIST:\n",
    "    for foot_step_y_disp in _FOOT_STEP_Y_DISP_LIST:\n",
    "        parameters = \"_disp_%.2f_%.2f\"%(foot_step_x_disp, foot_step_y_disp) + \\\n",
    "                     (\"_optimized\" if _WITH_OPTIMIZATION else \"\")\n",
    "        path = \"results/my_swing_example\" + parameters + \"/\"\n",
    "\n",
    "        plt.figure()\n",
    "        ax = plt.axes(projection='3d')\n",
    "\n",
    "        foot_path = np.loadtxt('%sfoot_path%s.txt'%(path, parameters), delimiter=',')\n",
    "        ax.plot3D(foot_path[:, 0], foot_path[:, 1], foot_path[:, 2], 'b')\n",
    "        if _WITH_OPTIMIZATION:\n",
    "            foot_opt_path = np.loadtxt('%sfoot_opt_path%s.txt'%(path, parameters), delimiter=',')\n",
    "            ax.plot3D(foot_opt_path[1:, 0], foot_opt_path[1:, 1], foot_opt_path[1:, 2], 'r')\n",
    "        # foot_real_path = np.loadtxt('%sfoot_real_path%s.txt'%(path, parameters), delimiter=',')\n",
    "        # ax.plot3D(foot_real_path[:, 0], foot_real_path[:, 1], foot_real_path[:, 2], 'g')\n",
    "        ax.set_xlabel('x (mm)')\n",
    "        ax.set_ylabel('y (mm)')\n",
    "        ax.set_zlabel('z (mm)')\n",
    "        ax.set_xlim([0, 0.3])\n",
    "        ax.set_ylim([-0.3, 0])\n",
    "        ax.set_zlim([-0.33, -0.2])\n",
    "        ax.set_xticks([0, 0.1, 0.2, 0.3])\n",
    "        ax.set_yticks([-0.3, -0.2, -0.1, 0])\n",
    "        ax.set_zticks([-0.3, -0.25, -0.2])\n",
    "        plt.savefig('%spath%s.png'%(path, parameters), dpi=300)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Optimized Swing Path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib import pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "_FOOT_STEP_X_DISP_LIST = np.array([0.1, 0.2, 0.3])\n",
    "_FOOT_STEP_Y_DISP_LIST = np.array([-0.1, 0, 0.1])\n",
    "_WITH_OPTIMIZATION = True\n",
    "\n",
    "foot_path_list = []\n",
    "foot_opt_path_list = []\n",
    "foot_real_path_list = []\n",
    "for foot_step_x_disp in _FOOT_STEP_X_DISP_LIST:\n",
    "    for foot_step_y_disp in _FOOT_STEP_Y_DISP_LIST:\n",
    "        parameters = \"_disp_%.2f_%.2f\"%(foot_step_x_disp, foot_step_y_disp) + \\\n",
    "                     (\"_optimized\" if _WITH_OPTIMIZATION else \"\")\n",
    "        path = \"results/my_swing_example\" + parameters + \"/\"\n",
    "\n",
    "        plt.figure()\n",
    "        ax = plt.axes(projection='3d')\n",
    "\n",
    "        foot_path = np.loadtxt('%sfoot_path%s.txt'%(path, parameters), delimiter=',')\n",
    "        ax.plot3D(foot_path[:, 0], foot_path[:, 1], foot_path[:, 2], 'b')\n",
    "        if _WITH_OPTIMIZATION:\n",
    "            foot_opt_path = np.loadtxt('%sfoot_opt_path%s.txt'%(path, parameters), delimiter=',')\n",
    "            ax.plot3D(foot_opt_path[1:, 0], foot_opt_path[1:, 1], foot_opt_path[1:, 2], 'r')\n",
    "        # foot_real_path = np.loadtxt('%sfoot_real_path%s.txt'%(path, parameters), delimiter=',')\n",
    "        # ax.plot3D(foot_real_path[:, 0], foot_real_path[:, 1], foot_real_path[:, 2], 'g')\n",
    "        ax.set_xlabel('x (mm)')\n",
    "        ax.set_ylabel('y (mm)')\n",
    "        ax.set_zlabel('z (mm)')\n",
    "        ax.set_xlim([0, 0.3])\n",
    "        ax.set_ylim([-0.3, 0])\n",
    "        ax.set_zlim([-0.33, -0.2])\n",
    "        ax.set_xticks([0, 0.1, 0.2, 0.3])\n",
    "        ax.set_yticks([-0.3, -0.2, -0.1, 0])\n",
    "        ax.set_zticks([-0.3, -0.25, -0.2])\n",
    "        plt.savefig('%spath%s.png'%(path, parameters), dpi=300)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Swing with Different Obstacles"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Unoptimized Swing Path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib import pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "_OBSTACLE_HALF_THICKNESS = 0.015\n",
    "_OBSTACLE_HALF_WIDTH = 0.15\n",
    "_OBSTACLE_HALF_HEIGHT_LIST = np.array([0.01, 0.02, 0.03, 0.04])\n",
    "_OBSTACLE_POS = [0.175, -0.15, -0.33]\n",
    "_WITH_OPTIMIZATION = False\n",
    "\n",
    "foot_path_list = []\n",
    "foot_opt_path_list = []\n",
    "foot_real_path_list = []\n",
    "for obstacle_half_height in _OBSTACLE_HALF_HEIGHT_LIST:\n",
    "    parameters = \"_disp_0.30_0.00_obstacle_%.2f\"%(obstacle_half_height*2) + \\\n",
    "                 (\"_optimized\" if _WITH_OPTIMIZATION else \"\")\n",
    "    path = \"results/my_swing_example\" + parameters + \"/\"\n",
    "\n",
    "    plt.figure()\n",
    "    ax = plt.axes(projection='3d')\n",
    "\n",
    "    foot_path = np.loadtxt('%sfoot_path%s.txt'%(path, parameters), delimiter=',')\n",
    "    ax.plot3D(foot_path[:, 0], foot_path[:, 1], foot_path[:, 2], 'b')\n",
    "    if _WITH_OPTIMIZATION:\n",
    "        foot_opt_path = np.loadtxt('%sfoot_opt_path%s.txt'%(path, parameters), delimiter=',')\n",
    "        ax.plot3D(foot_opt_path[1:, 0], foot_opt_path[1:, 1], foot_opt_path[1:, 2], 'r')\n",
    "    # foot_real_path = np.loadtxt('%sfoot_real_path%s.txt'%(path, parameters), delimiter=',')\n",
    "    # ax.plot3D(foot_real_path[:, 0], foot_real_path[:, 1], foot_real_path[:, 2], 'g')\n",
    "\n",
    "    obstacle_point_1 = np.array([_OBSTACLE_POS[0] - _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] -     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2] +   obstacle_half_height*2])\n",
    "    obstacle_point_2 = np.array([_OBSTACLE_POS[0] + _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] -     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2] +   obstacle_half_height*2])\n",
    "    obstacle_point_3 = np.array([_OBSTACLE_POS[0] + _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] +     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2] +   obstacle_half_height*2])\n",
    "    obstacle_point_4 = np.array([_OBSTACLE_POS[0] - _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] +     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2] +   obstacle_half_height*2])\n",
    "    obstacle_point_5 = np.array([_OBSTACLE_POS[0] - _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] -     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2]])\n",
    "    obstacle_point_6 = np.array([_OBSTACLE_POS[0] + _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] -     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2]])\n",
    "    obstacle_point_7 = np.array([_OBSTACLE_POS[0] + _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] +     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2]])\n",
    "    obstacle_point_8 = np.array([_OBSTACLE_POS[0] - _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] +     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2]])\n",
    "    \n",
    "    ax.plot3D([obstacle_point_1[0], obstacle_point_2[0], obstacle_point_3[0], obstacle_point_4[0], obstacle_point_1[0]],\n",
    "              [obstacle_point_1[1], obstacle_point_2[1], obstacle_point_3[1], obstacle_point_4[1], obstacle_point_1[1]],\n",
    "              [obstacle_point_1[2], obstacle_point_2[2], obstacle_point_3[2], obstacle_point_4[2], obstacle_point_1[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_5[0], obstacle_point_6[0], obstacle_point_7[0], obstacle_point_8[0], obstacle_point_5[0]],\n",
    "              [obstacle_point_5[1], obstacle_point_6[1], obstacle_point_7[1], obstacle_point_8[1], obstacle_point_5[1]],\n",
    "              [obstacle_point_5[2], obstacle_point_6[2], obstacle_point_7[2], obstacle_point_8[2], obstacle_point_5[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_1[0], obstacle_point_5[0]],\n",
    "              [obstacle_point_1[1], obstacle_point_5[1]],\n",
    "              [obstacle_point_1[2], obstacle_point_5[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_2[0], obstacle_point_6[0]],\n",
    "              [obstacle_point_2[1], obstacle_point_6[1]],\n",
    "              [obstacle_point_2[2], obstacle_point_6[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_3[0], obstacle_point_7[0]],\n",
    "              [obstacle_point_3[1], obstacle_point_7[1]],\n",
    "              [obstacle_point_3[2], obstacle_point_7[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_4[0], obstacle_point_8[0]],\n",
    "              [obstacle_point_4[1], obstacle_point_8[1]],\n",
    "              [obstacle_point_4[2], obstacle_point_8[2]], 'k')\n",
    "    \n",
    "    ax.set_xlabel('x (mm)')\n",
    "    ax.set_ylabel('y (mm)')\n",
    "    ax.set_zlabel('z (mm)')\n",
    "    ax.set_xlim([0, 0.3])\n",
    "    ax.set_ylim([-0.3, 0])\n",
    "    ax.set_zlim([-0.33, -0.2])\n",
    "    ax.set_xticks([0, 0.1, 0.2, 0.3])\n",
    "    ax.set_yticks([-0.3, -0.2, -0.1, 0])\n",
    "    ax.set_zticks([-0.3, -0.25, -0.2])\n",
    "    plt.savefig('%spath%s.png'%(path, parameters), dpi=300)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Optimized Swing Path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib import pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "_OBSTACLE_HALF_THICKNESS = 0.015\n",
    "_OBSTACLE_HALF_WIDTH = 0.15\n",
    "_OBSTACLE_HALF_HEIGHT_LIST = np.array([0.01, 0.02, 0.03, 0.04])\n",
    "_OBSTACLE_POS = [0.175, -0.15, -0.33]\n",
    "_WITH_OPTIMIZATION = True\n",
    "\n",
    "foot_path_list = []\n",
    "foot_opt_path_list = []\n",
    "foot_real_path_list = []\n",
    "for obstacle_half_height in _OBSTACLE_HALF_HEIGHT_LIST:\n",
    "    parameters = \"_disp_0.30_0.00_obstacle_%.2f\"%(obstacle_half_height*2) + \\\n",
    "                 (\"_optimized\" if _WITH_OPTIMIZATION else \"\")\n",
    "    path = \"results/my_swing_example\" + parameters + \"/\"\n",
    "\n",
    "    plt.figure()\n",
    "    ax = plt.axes(projection='3d')\n",
    "\n",
    "    foot_path = np.loadtxt('%sfoot_path%s.txt'%(path, parameters), delimiter=',')\n",
    "    ax.plot3D(foot_path[:, 0], foot_path[:, 1], foot_path[:, 2], 'b')\n",
    "    if _WITH_OPTIMIZATION:\n",
    "        foot_opt_path = np.loadtxt('%sfoot_opt_path%s.txt'%(path, parameters), delimiter=',')\n",
    "        ax.plot3D(foot_opt_path[1:, 0], foot_opt_path[1:, 1], foot_opt_path[1:, 2], 'r')\n",
    "    # foot_real_path = np.loadtxt('%sfoot_real_path%s.txt'%(path, parameters), delimiter=',')\n",
    "    # ax.plot3D(foot_real_path[:, 0], foot_real_path[:, 1], foot_real_path[:, 2], 'g')\n",
    "\n",
    "    obstacle_point_1 = np.array([_OBSTACLE_POS[0] - _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] -     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2] +   obstacle_half_height*2])\n",
    "    obstacle_point_2 = np.array([_OBSTACLE_POS[0] + _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] -     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2] +   obstacle_half_height*2])\n",
    "    obstacle_point_3 = np.array([_OBSTACLE_POS[0] + _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] +     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2] +   obstacle_half_height*2])\n",
    "    obstacle_point_4 = np.array([_OBSTACLE_POS[0] - _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] +     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2] +   obstacle_half_height*2])\n",
    "    obstacle_point_5 = np.array([_OBSTACLE_POS[0] - _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] -     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2]])\n",
    "    obstacle_point_6 = np.array([_OBSTACLE_POS[0] + _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] -     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2]])\n",
    "    obstacle_point_7 = np.array([_OBSTACLE_POS[0] + _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] +     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2]])\n",
    "    obstacle_point_8 = np.array([_OBSTACLE_POS[0] - _OBSTACLE_HALF_THICKNESS,\n",
    "                                 _OBSTACLE_POS[1] +     _OBSTACLE_HALF_WIDTH,\n",
    "                                 _OBSTACLE_POS[2]])\n",
    "    \n",
    "    ax.plot3D([obstacle_point_1[0], obstacle_point_2[0], obstacle_point_3[0], obstacle_point_4[0], obstacle_point_1[0]],\n",
    "              [obstacle_point_1[1], obstacle_point_2[1], obstacle_point_3[1], obstacle_point_4[1], obstacle_point_1[1]],\n",
    "              [obstacle_point_1[2], obstacle_point_2[2], obstacle_point_3[2], obstacle_point_4[2], obstacle_point_1[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_5[0], obstacle_point_6[0], obstacle_point_7[0], obstacle_point_8[0], obstacle_point_5[0]],\n",
    "              [obstacle_point_5[1], obstacle_point_6[1], obstacle_point_7[1], obstacle_point_8[1], obstacle_point_5[1]],\n",
    "              [obstacle_point_5[2], obstacle_point_6[2], obstacle_point_7[2], obstacle_point_8[2], obstacle_point_5[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_1[0], obstacle_point_5[0]],\n",
    "              [obstacle_point_1[1], obstacle_point_5[1]],\n",
    "              [obstacle_point_1[2], obstacle_point_5[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_2[0], obstacle_point_6[0]],\n",
    "              [obstacle_point_2[1], obstacle_point_6[1]],\n",
    "              [obstacle_point_2[2], obstacle_point_6[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_3[0], obstacle_point_7[0]],\n",
    "              [obstacle_point_3[1], obstacle_point_7[1]],\n",
    "              [obstacle_point_3[2], obstacle_point_7[2]], 'k')\n",
    "    ax.plot3D([obstacle_point_4[0], obstacle_point_8[0]],\n",
    "              [obstacle_point_4[1], obstacle_point_8[1]],\n",
    "              [obstacle_point_4[2], obstacle_point_8[2]], 'k')\n",
    "    \n",
    "    ax.set_xlabel('x (mm)')\n",
    "    ax.set_ylabel('y (mm)')\n",
    "    ax.set_zlabel('z (mm)')\n",
    "    ax.set_xlim([0, 0.3])\n",
    "    ax.set_ylim([-0.3, 0])\n",
    "    ax.set_zlim([-0.33, -0.2])\n",
    "    ax.set_xticks([0, 0.1, 0.2, 0.3])\n",
    "    ax.set_yticks([-0.3, -0.2, -0.1, 0])\n",
    "    ax.set_zticks([-0.3, -0.25, -0.2])\n",
    "    plt.savefig('%spath%s.png'%(path, parameters), dpi=300)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Video Processing"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import cv2\n",
    "\n",
    "video_path_list = []\n",
    "for root, dirs, files in os.walk('./results'):\n",
    "    for file in files:\n",
    "        if file.endswith('.mp4'):\n",
    "            video_path_list.append(os.path.join(root, file))\n",
    "\n",
    "for video_path in video_path_list:\n",
    "    video = cv2.VideoCapture(video_path)\n",
    "    total_frame = int(video.get(cv2.CAP_PROP_FRAME_COUNT))\n",
    "    video.set(cv2.CAP_PROP_POS_FRAMES, total_frame-1)\n",
    "    ret, frame = video.read()\n",
    "    cv2.imwrite('%s.png'%video_path[:-4], frame)\n",
    "    video.release()\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "motionImitation",
   "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.7.0"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}