kuka_rl.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "# Kuka Reinforcement Learning (DQN)\n",
    "\n",
    "\n",
    "This notebook shows how to train a Deep Q Learning (DQN) agent\n",
    "on the pyBullet Kuka environment based on:\n",
    "https://pytorch.org/tutorials/intermediate/reinforcement_q_learning.html\n",
    "\n",
    "**Task**\n",
    "The agent has to decide between seven (2+2+2+1) actions - moving the manipulator \n",
    "in $x$ (2), or $y$ (2) directions by vertical angle offset (2) or not moving at all (1) - so that the manipulator can grasp an object. We assume that the velocity for each directions equal and there is a \"height hack\" where the gripper automatically moves down for each action.\n",
    "\n",
    "As the agent observes the current state of the environment and chooses\n",
    "an action, the environment *transitions* to a new state, and also\n",
    "returns a reward that indicates the consequences of the action. In this\n",
    "task, the reward is 1 if one of the objects is above height .2 at the end \n",
    "of the episode.\n",
    "\n",
    "The pyBullet Kuka is designed so that the inputs to the agent are (48, 48, 3) images\n",
    "representing the environment state.\n",
    "Neural networks can solve the task purely by looking at the\n",
    "scene, so we'll use stack of consecutive screens as an input. In this way, we are hoping to capture the dynamics of the environment.\n",
    "\n",
    "**Packages**\n",
    "\n",
    "First, let's import needed packages. Firstly, we need gym\n",
    "(Install using `pip install gym`) and pyBullet (Install using `pip install pybullet`.\n",
    "We'll also use the following from PyTorch:\n",
    "\n",
    "-  neural networks (``torch.nn``)\n",
    "-  optimization (``torch.optim``)\n",
    "-  automatic differentiation (``torch.autograd``)\n",
    "-  utilities for vision tasks (``torchvision`` - `a separate\n",
    "   package <https://github.com/pytorch/vision>`__).\n",
    "\n",
    "\n",
    "Also for display the average reward and agent progress we are going to use tensorboarX (`pip install tensorboardX`.) To run tensorboard web server, you need to install it using `pip install tensorboard`.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Requirement already satisfied: pybullet in /home/monster/anaconda3/envs/python36/lib/python3.6/site-packages (2.6.1)\n",
      "Requirement already satisfied: tensorboardX in /home/monster/anaconda3/envs/python36/lib/python3.6/site-packages (2.0)\n",
      "Requirement already satisfied: six in /home/monster/anaconda3/envs/python36/lib/python3.6/site-packages (from tensorboardX) (1.13.0)\n",
      "Requirement already satisfied: numpy in /home/monster/anaconda3/envs/python36/lib/python3.6/site-packages (from tensorboardX) (1.17.4)\n",
      "Requirement already satisfied: protobuf>=3.8.0 in /home/monster/anaconda3/envs/python36/lib/python3.6/site-packages (from tensorboardX) (3.11.2)\n",
      "Requirement already satisfied: setuptools in /home/monster/anaconda3/envs/python36/lib/python3.6/site-packages (from protobuf>=3.8.0->tensorboardX) (42.0.2.post20191203)\n",
      "current_dir=/home/monster/anaconda3/envs/python36/lib/python3.6/site-packages/pybullet_envs/bullet\n"
     ]
    }
   ],
   "source": [
    "!pip install pybullet\n",
    "!pip install tensorboardX\n",
    "import gym\n",
    "import math\n",
    "import random\n",
    "import numpy as np\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "from collections import namedtuple\n",
    "import collections\n",
    "from itertools import count\n",
    "import timeit\n",
    "from datetime import timedelta\n",
    "from PIL import Image\n",
    "from tensorboardX import SummaryWriter\n",
    "\n",
    "import torch\n",
    "import torch.nn as nn\n",
    "import torch.optim as optim\n",
    "import torch.nn.functional as F\n",
    "import torchvision.transforms as T\n",
    "\n",
    "\n",
    "from pybullet_envs.bullet.kuka_diverse_object_gym_env import KukaDiverseObjectEnv\n",
    "from gym import spaces\n",
    "import pybullet as p\n",
    "\n",
    "env = KukaDiverseObjectEnv(renders=False, isDiscrete=True, removeHeightHack=False, maxSteps=20)\n",
    "env.cid = p.connect(p.DIRECT)\n",
    "\n",
    "# set up matplotlib\n",
    "is_ipython = 'inline' in matplotlib.get_backend()\n",
    "if is_ipython:\n",
    "    from IPython import display\n",
    "\n",
    "plt.ion()\n",
    "\n",
    "# if gpu is to be used\n",
    "device = torch.device(\"cuda\" if torch.cuda.is_available() else \"cpu\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Replay Memory\n",
    "\n",
    "\n",
    "We'll be using experience replay memory for training our DQN. It stores\n",
    "the transitions that the agent observes, allowing us to reuse this data\n",
    "later. By sampling from it randomly, the transitions that build up a\n",
    "batch are decorrelated. It has been shown that this greatly stabilizes\n",
    "and improves the DQN training procedure.\n",
    "\n",
    "For this, we're going to need two classses:\n",
    "\n",
    "-  ``Transition`` - a named tuple representing a single transition in\n",
    "   our environment. It essentially maps (state, action) pairs\n",
    "   to their (next_state, reward) result, with the state being the\n",
    "   consecutive stacked screens image as described later on.\n",
    "-  ``ReplayMemory`` - a cyclic buffer of bounded size that holds the\n",
    "   transitions observed recently. It also implements a ``.sample()``\n",
    "   method for selecting a random batch of transitions for training.\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "Transition = namedtuple('Transition',\n",
    "                        ('state', 'action', 'next_state', 'reward'))\n",
    "\n",
    "\n",
    "class ReplayMemory(object):\n",
    "\n",
    "    def __init__(self, capacity):\n",
    "        self.capacity = capacity\n",
    "        self.memory = []\n",
    "        self.position = 0\n",
    "\n",
    "    def push(self, *args):\n",
    "        \"\"\"Saves a transition.\"\"\"\n",
    "        if len(self.memory) < self.capacity:\n",
    "            self.memory.append(None)\n",
    "        self.memory[self.position] = Transition(*args)\n",
    "        self.position = (self.position + 1) % self.capacity\n",
    "\n",
    "    def sample(self, batch_size):\n",
    "        return random.sample(self.memory, batch_size)\n",
    "\n",
    "    def __len__(self):\n",
    "        return len(self.memory)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, let's define our model. But first, let's quickly recap what a DQN is.\n",
    "\n",
    "# DQN algorithm\n",
    "\n",
    "\n",
    "Our aim will be to train a policy that tries to maximize the discounted,\n",
    "cumulative reward\n",
    "$R_{t_0} = \\sum_{t=t_0}^{\\infty} \\gamma^{t - t_0} r_t$, where\n",
    "$R_{t_0}$ is also known as the *return*. The discount,\n",
    "$\\gamma$, should be a constant between $0$ and $1$\n",
    "that ensures the sum converges. It makes rewards from the uncertain far\n",
    "future less important for our agent than the ones in the near future\n",
    "that it can be fairly confident about.\n",
    "\n",
    "The main idea behind Q-learning is that if we had a function\n",
    "$Q^*: State \\times Action \\rightarrow \\mathbb{R}$, that could tell\n",
    "us what our return would be, if we were to take an action in a given\n",
    "state, then we could easily construct a policy that maximizes our\n",
    "rewards:\n",
    "\n",
    "\\begin{align}\\pi^*(s) = \\arg\\!\\max_a \\ Q^*(s, a)\\end{align}\n",
    "\n",
    "However, we don't know everything about the world, so we don't have\n",
    "access to $Q^*$. But, since neural networks are universal function\n",
    "approximators, we can simply create one and train it to resemble\n",
    "$Q^*$.\n",
    "\n",
    "For our training update rule, we'll use a fact that every $Q$\n",
    "function for some policy obeys the Bellman equation:\n",
    "\n",
    "\\begin{align}Q^{\\pi}(s, a) = r + \\gamma Q^{\\pi}(s', \\pi(s'))\\end{align}\n",
    "\n",
    "The difference between the two sides of the equality is known as the\n",
    "temporal difference error, $\\delta$:\n",
    "\n",
    "\\begin{align}\\delta = Q(s, a) - (r + \\gamma \\max_a Q(s', a))\\end{align}\n",
    "\n",
    "To minimise this error, we will use the `Huber\n",
    "loss <https://en.wikipedia.org/wiki/Huber_loss>`__. The Huber loss acts\n",
    "like the mean squared error when the error is small, but like the mean\n",
    "absolute error when the error is large - this makes it more robust to\n",
    "outliers when the estimates of $Q$ are very noisy. We calculate\n",
    "this over a batch of transitions, $B$, sampled from the replay\n",
    "memory:\n",
    "\n",
    "\\begin{align}\\mathcal{L} = \\frac{1}{|B|}\\sum_{(s, a, s', r) \\ \\in \\ B} \\mathcal{L}(\\delta)\\end{align}\n",
    "\n",
    "\\begin{align}\\text{where} \\quad \\mathcal{L}(\\delta) = \\begin{cases}\n",
    "     \\frac{1}{2}{\\delta^2}  & \\text{for } |\\delta| \\le 1, \\\\\n",
    "     |\\delta| - \\frac{1}{2} & \\text{otherwise.}\n",
    "   \\end{cases}\\end{align}\n",
    "\n",
    "## Q-network\n",
    "\n",
    "\n",
    "Our model will be a convolutional neural network that takes in the\n",
    "stacked screen patches. It has seven\n",
    "outputs, representing $Q(s, d_x = d_v)$, $Q(s, d_x = -d_v)$, $Q(s, d_y = d_v)$, $Q(s, d_y = -d_v)$, $Q(s, d_a=0.25)$, \n",
    "and $Q(s, d_a=-0.25)$ (where $d_v=0.06$ is the velocity in PyBullet, $d_a$ is vertical angle offset for the gripper, and $s$ is the input to the\n",
    "network). This discretization of action is based on the continuous action which is a 5-vector parameterizing XYZ offset, vertical angle offset (radians), and grasp angle (radians). The continuous action (5-vector) is discretized as follows with \"height hack\" where the gripper automatically moves down for each action, and grasp angle in PyBullet is set to 0.3:\n",
    "```\n",
    "        dx = [0, -dv, dv, 0, 0, 0, 0][action]\n",
    "        dy = [0, 0, 0, -dv, dv, 0, 0][action]\n",
    "        dz = -dv\n",
    "        da = [0, 0, 0, 0, 0, -0.25, 0.25][action]\n",
    "        \n",
    "        action_continuous = [dx, dy, dz, da, 0.3]\n",
    "```\n",
    "In effect, the network is trying to predict the *expected return* of\n",
    "taking each action given the current input. In order to capture the dynamics of the environment, we stack 5 consecutive screens of the environment after each step together and then feed them to the network."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "STACK_SIZE = 5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "class DQN(nn.Module):\n",
    "    def __init__(self, h, w, outputs):\n",
    "        super(DQN, self).__init__()  \n",
    "        self.conv1 = nn.Conv2d(STACK_SIZE, 32, kernel_size=8, stride=4)\n",
    "        self.bn1 = nn.BatchNorm2d(32)\n",
    "        self.conv2 = nn.Conv2d(32, 64, kernel_size=4, stride=2)\n",
    "        self.bn2 = nn.BatchNorm2d(64)\n",
    "        self.conv3 = nn.Conv2d(64, 64, kernel_size=3, stride=1)\n",
    "\n",
    "        # Number of Linear input connections depends on output of conv2d layers\n",
    "        # and therefore the input image size, so compute it.\n",
    "        def conv2d_size_out(size, kernel_size = 5, stride = 2):\n",
    "            return (size - (kernel_size - 1) - 1) // stride  + 1\n",
    "        convw = conv2d_size_out(conv2d_size_out(conv2d_size_out(w,8,4),4,2),3,1)\n",
    "        convh = conv2d_size_out(conv2d_size_out(conv2d_size_out(h,8,4),4,2),3,1)\n",
    "        linear_input_size = convw * convh * 64\n",
    "        self.linear = nn.Linear(linear_input_size, 512)\n",
    "        self.head = nn.Linear(512, outputs)\n",
    "\n",
    "    # Called with either one element to determine next action, or a batch\n",
    "    # during optimization. Returns tensor([[left0exp,right0exp]...]).\n",
    "    def forward(self, x):\n",
    "        x = F.relu(self.bn1(self.conv1(x)))\n",
    "        x = F.relu(self.bn2(self.conv2(x)))\n",
    "        x = F.relu(self.conv3(x))\n",
    "        x = F.relu(self.linear(x.view(x.size(0), -1)))\n",
    "        return self.head(x)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Input extraction\n",
    "\n",
    "\n",
    "The code below are utilities for extracting and processing rendered\n",
    "images from the environment. It uses the ``torchvision`` package, which\n",
    "makes it easy to compose image transforms. Once you run the cell it will\n",
    "display an example patch that it extracted.\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "preprocess = T.Compose([T.ToPILImage(),\n",
    "                    T.Grayscale(num_output_channels=1),\n",
    "                    T.Resize(40, interpolation=Image.CUBIC),\n",
    "                    T.ToTensor()])\n",
    "\n",
    "def get_screen():\n",
    "    global stacked_screens\n",
    "    # Returned screen requested by gym is 400x600x3, but is sometimes larger\n",
    "    # such as 800x1200x3. Transpose it into torch order (CHW).\n",
    "    screen = env._get_observation().transpose((2, 0, 1))\n",
    "    # Convert to float, rescale, convert to torch tensor\n",
    "    # (this doesn't require a copy)\n",
    "    \n",
    "    screen = np.ascontiguousarray(screen, dtype=np.float32) / 255\n",
    "    screen = torch.from_numpy(screen)\n",
    "    # Resize, and add a batch dimension (BCHW)\n",
    "    return preprocess(screen).unsqueeze(0).to(device)\n",
    "\n",
    "\n",
    "env.reset()\n",
    "plt.figure()\n",
    "plt.imshow(get_screen().cpu().squeeze(0)[-1].numpy(),cmap='Greys',\n",
    "           interpolation='none')\n",
    "plt.title('Example extracted screen')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Training\n",
    "\n",
    "\n",
    "## Hyperparameters and utility function\n",
    "\n",
    "This cell instantiates our model and its optimizer, and defines the\n",
    "utility function:\n",
    "\n",
    "-  ``select_action`` - will select an action accordingly to an epsilon\n",
    "   greedy policy. Simply put, we'll sometimes use our model for choosing\n",
    "   the action, and sometimes we'll just sample one uniformly. The\n",
    "   probability of choosing a random action will start at ``EPS_START``\n",
    "   and will decay exponentially towards ``EPS_END``. ``EPS_DECAY``\n",
    "   controls the rate of the decay.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "BATCH_SIZE = 32\n",
    "GAMMA = 0.99\n",
    "EPS_START = 0.9\n",
    "EPS_END = 0.1\n",
    "EPS_DECAY = 200\n",
    "EPS_DECAY_LAST_FRAME = 10**4\n",
    "TARGET_UPDATE = 1000\n",
    "LEARNING_RATE = 1e-4\n",
    "\n",
    "# Get screen size so that we can initialize layers correctly based on shape\n",
    "# returned from pybullet (48, 48, 3).  \n",
    "init_screen = get_screen()\n",
    "_, _, screen_height, screen_width = init_screen.shape\n",
    "\n",
    "# Get number of actions from gym action space\n",
    "n_actions = env.action_space.n\n",
    "\n",
    "policy_net = DQN(screen_height, screen_width, n_actions).to(device)\n",
    "target_net = DQN(screen_height, screen_width, n_actions).to(device)\n",
    "target_net.load_state_dict(policy_net.state_dict())\n",
    "target_net.eval()\n",
    "\n",
    "optimizer = optim.Adam(policy_net.parameters(), lr=LEARNING_RATE)\n",
    "memory = ReplayMemory(10000)\n",
    "\n",
    "\n",
    "eps_threshold = 0\n",
    "\n",
    "def select_action(state, i_episode):\n",
    "    global steps_done\n",
    "    global eps_threshold\n",
    "    sample = random.random()\n",
    "    eps_threshold = max(EPS_END, EPS_START - i_episode / EPS_DECAY_LAST_FRAME)\n",
    "    if sample > eps_threshold:\n",
    "        with torch.no_grad():\n",
    "            # t.max(1) will return largest column value of each row.\n",
    "            # second column on max result is index of where max element was\n",
    "            # found, so we pick action with the larger expected reward.\n",
    "            return policy_net(state).max(1)[1].view(1, 1)\n",
    "    else:\n",
    "        return torch.tensor([[random.randrange(n_actions)]], device=device, dtype=torch.long)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Training loop\n",
    "\n",
    "Finally, the code for training our model.\n",
    "\n",
    "Here, you can find an ``optimize_model`` function that performs a\n",
    "single step of the optimization. It first samples a batch, concatenates\n",
    "all the tensors into a single one, computes $Q(s_t, a_t)$ and\n",
    "$V(s_{t+1}) = \\max_a Q(s_{t+1}, a)$, and combines them into our\n",
    "loss. By defition we set $V(s) = 0$ if $s$ is a terminal\n",
    "state. We also use a target network to compute $V(s_{t+1})$ for\n",
    "added stability. The target network has its weights kept frozen most of\n",
    "the time, but is updated with the policy network's weights every so often.\n",
    "This is usually a set number of steps but we shall use episodes for\n",
    "simplicity.\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "def optimize_model():\n",
    "    if len(memory) < BATCH_SIZE:\n",
    "        return\n",
    "    transitions = memory.sample(BATCH_SIZE)\n",
    "    # Transpose the batch (see https://stackoverflow.com/a/19343/3343043 for\n",
    "    # detailed explanation). This converts batch-array of Transitions\n",
    "    # to Transition of batch-arrays.\n",
    "    batch = Transition(*zip(*transitions))\n",
    "\n",
    "    # Compute a mask of non-final states and concatenate the batch elements\n",
    "    # (a final state would've been the one after which simulation ended)\n",
    "    non_final_mask = torch.tensor(tuple(map(lambda s: s is not None,\n",
    "                                          batch.next_state)), device=device, dtype=torch.bool)\n",
    "    non_final_next_states = torch.cat([s for s in batch.next_state\n",
    "                                                if s is not None])\n",
    "    state_batch = torch.cat(batch.state)\n",
    "    action_batch = torch.cat(batch.action)\n",
    "    reward_batch = torch.cat(batch.reward)\n",
    "\n",
    "    # Compute Q(s_t, a) - the model computes Q(s_t), then we select the\n",
    "    # columns of actions taken. These are the actions which would've been taken\n",
    "    # for each batch state according to policy_net\n",
    "    state_action_values = policy_net(state_batch).gather(1, action_batch)\n",
    "\n",
    "    # Compute V(s_{t+1}) for all next states.\n",
    "    # Expected values of actions for non_final_next_states are computed based\n",
    "    # on the \"older\" target_net; selecting their best reward with max(1)[0].\n",
    "    # This is merged based on the mask, such that we'll have either the expected\n",
    "    # state value or 0 in case the state was final.\n",
    "    next_state_values = torch.zeros(BATCH_SIZE, device=device)\n",
    "    next_state_values[non_final_mask] = target_net(non_final_next_states).max(1)[0].detach()\n",
    "    # Compute the expected Q values\n",
    "    expected_state_action_values = (next_state_values * GAMMA) + reward_batch\n",
    "\n",
    "    # Compute Huber loss\n",
    "    loss = F.smooth_l1_loss(state_action_values, expected_state_action_values.unsqueeze(1))\n",
    "\n",
    "    # Optimize the model\n",
    "    optimizer.zero_grad()\n",
    "    loss.backward()\n",
    "    for param in policy_net.parameters():\n",
    "        param.grad.data.clamp_(-1, 1)\n",
    "    optimizer.step()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Below, you can find the main training loop. At the beginning we reset\n",
    "the environment and initialize the ``state`` Tensor. Then, we sample\n",
    "an action, execute it, observe the next screen and the reward (always\n",
    "1), and optimize our model once. When the episode ends (our model\n",
    "fails), we restart the loop."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "PATH = 'policy_dqn.pt'"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Best mean reward updated 36.0 -> 37.0, model saved\n",
      "Best mean reward updated 37.0 -> 38.0, model saved\n",
      "Best mean reward updated 38.0 -> 39.0, model saved\n",
      "Best mean reward updated 39.0 -> 40.0, model saved\n",
      "Best mean reward updated 40.0 -> 41.0, model saved\n",
      "Best mean reward updated 41.0 -> 42.0, model saved\n",
      "Best mean reward updated 42.0 -> 43.0, model saved\n",
      "Best mean reward updated 43.0 -> 44.0, model saved\n",
      "Best mean reward updated 44.0 -> 45.0, model saved\n",
      "Best mean reward updated 45.0 -> 46.0, model saved\n",
      "Best mean reward updated 46.0 -> 47.0, model saved\n",
      "Best mean reward updated 47.0 -> 48.0, model saved\n",
      "Best mean reward updated 48.0 -> 49.0, model saved\n",
      "Best mean reward updated 49.0 -> 50.0, model saved\n",
      "Best mean reward updated 50.0 -> 51.0, model saved\n",
      "Environment solved in 7412 episodes!\tAverage Score: 51.00\n",
      "Average Score: 51.00\n",
      "Elapsed time: 3:11:17.312433\n"
     ]
    }
   ],
   "source": [
    "num_episodes = 10000000\n",
    "writer = SummaryWriter()\n",
    "total_rewards = []\n",
    "ten_rewards = 0\n",
    "best_mean_reward = None\n",
    "start_time = timeit.default_timer()\n",
    "for i_episode in range(num_episodes):\n",
    "    # Initialize the environment and state\n",
    "    env.reset()\n",
    "    state = get_screen()\n",
    "    stacked_states = collections.deque(STACK_SIZE*[state],maxlen=STACK_SIZE)\n",
    "    for t in count():\n",
    "        stacked_states_t =  torch.cat(tuple(stacked_states),dim=1)\n",
    "        # Select and perform an action\n",
    "        action = select_action(stacked_states_t, i_episode)\n",
    "        _, reward, done, _ = env.step(action.item())\n",
    "        reward = torch.tensor([reward], device=device)\n",
    "\n",
    "        # Observe new state\n",
    "        next_state = get_screen()\n",
    "        if not done:\n",
    "            next_stacked_states = stacked_states\n",
    "            next_stacked_states.append(next_state)\n",
    "            next_stacked_states_t =  torch.cat(tuple(next_stacked_states),dim=1)\n",
    "        else:\n",
    "            next_stacked_states_t = None\n",
    "            \n",
    "        # Store the transition in memory\n",
    "        memory.push(stacked_states_t, action, next_stacked_states_t, reward)\n",
    "\n",
    "        # Move to the next state\n",
    "        stacked_states = next_stacked_states\n",
    "        \n",
    "        # Perform one step of the optimization (on the target network)\n",
    "        optimize_model()\n",
    "        if done:\n",
    "            reward = reward.cpu().numpy().item()\n",
    "            ten_rewards += reward\n",
    "            total_rewards.append(reward)\n",
    "            mean_reward = np.mean(total_rewards[-100:])*100\n",
    "            writer.add_scalar(\"epsilon\", eps_threshold, i_episode)\n",
    "            if (best_mean_reward is None or best_mean_reward < mean_reward) and i_episode > 100:\n",
    "                # For saving the model and possibly resuming training\n",
    "                torch.save({\n",
    "                        'policy_net_state_dict': policy_net.state_dict(),\n",
    "                        'target_net_state_dict': target_net.state_dict(),\n",
    "                        'optimizer_policy_net_state_dict': optimizer.state_dict()\n",
    "                        }, PATH)\n",
    "                if best_mean_reward is not None:\n",
    "                    print(\"Best mean reward updated %.1f -> %.1f, model saved\" % (best_mean_reward, mean_reward))\n",
    "                best_mean_reward = mean_reward\n",
    "            break\n",
    "            \n",
    "    if i_episode%10 == 0:\n",
    "            writer.add_scalar('ten episodes average rewards', ten_rewards/10.0, i_episode)\n",
    "            ten_rewards = 0\n",
    "    \n",
    "    # Update the target network, copying all weights and biases in DQN\n",
    "    if i_episode % TARGET_UPDATE == 0:\n",
    "        target_net.load_state_dict(policy_net.state_dict())\n",
    "    if i_episode>=200 and mean_reward>50:\n",
    "        print('Environment solved in {:d} episodes!\\tAverage Score: {:.2f}'.format(i_episode+1, mean_reward))\n",
    "        break\n",
    "\n",
    "\n",
    "print('Average Score: {:.2f}'.format(mean_reward))\n",
    "elapsed = timeit.default_timer() - start_time\n",
    "print(\"Elapsed time: {}\".format(timedelta(seconds=elapsed)))\n",
    "writer.close()\n",
    "env.close()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Actions are chosen either randomly or based on a policy, getting the next\n",
    "step sample from the gym environment. We record the results in the\n",
    "replay memory and also run optimization step on every iteration.\n",
    "Optimization picks a random batch from the replay memory to do training of the\n",
    "new policy. \"Older\" target_net is also used in optimization to compute the\n",
    "expected Q values; it is updated occasionally to keep it current."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Evaluation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Episode: 1, reward: 1\n",
      "Episode: 2, reward: 0\n",
      "Episode: 3, reward: 0\n",
      "Episode: 4, reward: 1\n",
      "Episode: 5, reward: 0\n",
      "Episode: 6, reward: 1\n",
      "Episode: 7, reward: 0\n",
      "Episode: 8, reward: 1\n",
      "Episode: 9, reward: 0\n",
      "Episode: 10, reward: 1\n"
     ]
    }
   ],
   "source": [
    "episode = 10\n",
    "scores_window = collections.deque(maxlen=100)  # last 100 scores\n",
    "env = KukaDiverseObjectEnv(renders=False, isDiscrete=True, removeHeightHack=False, maxSteps=20, isTest=True)\n",
    "env.cid = p.connect(p.DIRECT)\n",
    "# load the model\n",
    "checkpoint = torch.load(PATH)\n",
    "policy_net.load_state_dict(checkpoint['policy_net_state_dict'])\n",
    "\n",
    "# evaluate the model\n",
    "for i_episode in range(episode):\n",
    "    env.reset()\n",
    "    state = get_screen()\n",
    "    stacked_states = collections.deque(STACK_SIZE*[state],maxlen=STACK_SIZE)\n",
    "    for t in count():\n",
    "        stacked_states_t =  torch.cat(tuple(stacked_states),dim=1)\n",
    "        # Select and perform an action\n",
    "        action = policy_net(stacked_states_t).max(1)[1].view(1, 1)\n",
    "        _, reward, done, _ = env.step(action.item())\n",
    "        # Observe new state\n",
    "        next_state = get_screen()\n",
    "        stacked_states.append(next_state)\n",
    "        if done:\n",
    "            break\n",
    "    print(\"Episode: {0:d}, reward: {1}\".format(i_episode+1, reward), end=\"\\n\")"
   ]
  }
 ],
 "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}