Course_OpenAIBaselines_Unit8.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Using OpenAI with ROS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/robotignite_logo_text.png\" width=\"700\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Unit 8: Training a Fetch Robot. Part 2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Estimated time to completion: <b>2 hours</b><br><br>\n",
    "In this unit, you are going to have a step-by-step look at how to build the Task Environment for training a Fetch robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:green;color:white;\">END OF SUMMARY</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "With the Robot Environment covered, let's now have a look at how to build a Task Environment for training our Fetch robot. In this case, we are going to train the robot to reach a cube, which will be on top of a flat surface. For that, let's first spawn our whole scene."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#FF5E00;color:white;\">**Exercise 7.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Create a new file named <i><b>cube.urdf</b></i> in your **catkin_ws/src** folder."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<table style=\"float:left;background: #407EAF\">\n",
    "<tr>\n",
    "<th>\n",
    "<p class=\"transparent\">Execute in WebShell #1</p>\n",
    "</th>\n",
    "</tr>\n",
    "</table>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "roscd; cd src"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "touch cube.urdf"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Paste the following code into that file."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "<robot name=\"simple_box\">\n",
    "  <link name=\"my_box\">\n",
    "    <inertial>\n",
    "      <origin xyz=\"0 0 0.0145\"/>\n",
    "      <mass value=\"0.1\" />\n",
    "      <inertia  ixx=\"0.0001\" ixy=\"0.0\"  ixz=\"0.0\"  iyy=\"0.0001\"  iyz=\"0.0\"  izz=\"0.0001\" />\n",
    "    </inertial>\n",
    "    <visual>\n",
    "      <origin xyz=\"-0.23 0 0.215\"/>\n",
    "      <geometry>\n",
    "        <box size=\"0.47 0.46 0.3\"/>\n",
    "      </geometry>\n",
    "    </visual>\n",
    "    <collision>\n",
    "      <origin xyz=\"-0.23 0 0.215\"/>\n",
    "      <geometry>\n",
    "        <box size=\"0.47 0.46 0.3\"/>\n",
    "      </geometry>\n",
    "    </collision>\n",
    "  </link>\n",
    "  <gazebo reference=\"my_box\">\n",
    "    <material>Gazebo/Wood</material>\n",
    "  </gazebo>\n",
    "  <gazebo>\n",
    "    <static>true</static>\n",
    "  </gazebo>\n",
    "</robot>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Execute the following command in order to spawn the cube right in front of the Fetch robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun gazebo_ros spawn_model -file /home/user/catkin_ws/src/cube.urdf -urdf -x 1 -model my_object"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "c) Now, execute the next command in order to spawn the grasping block."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "rosrun gazebo_ros spawn_model -database demo_cube -gazebo -model grasp_cube -x 0.70 -y 0 -z 0.35"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the end, you should have something like this:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"img/fetch_reach_scene.png\" width=\"600\" />"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#FF5E00;color:white;\">**End of Exercise 7.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! Now we are done with the scene, let's keep working on our environments."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Task Environment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will start by creating our Task Environment. This Task Environment, as you may already know, will be in charge of providing all the necessary functions and methods related to this specific training. This means that it will contain all the basic functions to be able to learn how to reach the cube, in this case."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will start, as we did in the previous chapter, with the basic task environment template."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#407EAF;color:white;\">**template_my_task_env.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from gym import spaces\n",
    "import my_robot_env\n",
    "from gym.envs.registration import register\n",
    "import rospy\n",
    "\n",
    "# The path is __init__.py of openai_ros, where we import the MovingCubeOneDiskWalkEnv directly\n",
    "timestep_limit_per_episode = 1000 # Can be any Value\n",
    "\n",
    "register(\n",
    "        id='MyTrainingEnv-v0',\n",
    "        entry_point='template_my_training_env:MovingCubeOneDiskWalkEnv',\n",
    "        timestep_limit=timestep_limit_per_episode,\n",
    "    )\n",
    "\n",
    "class MyTrainingEnv(cube_single_disk_env.MyRobotEnv):\n",
    "    def __init__(self):\n",
    "        \n",
    "        # Only variable needed to be set here\n",
    "        number_actions = rospy.get_param('/my_robot_namespace/n_actions')\n",
    "        self.action_space = spaces.Discrete(number_actions)\n",
    "        \n",
    "        # This is the most common case of Box observation type\n",
    "        high = numpy.array([\n",
    "            obs1_max_value,\n",
    "            obs12_max_value,\n",
    "            ...\n",
    "            obsN_max_value\n",
    "            ])\n",
    "            \n",
    "        self.observation_space = spaces.Box(-high, high)\n",
    "        \n",
    "        # Variables that we retrieve through the param server, loaded when launch training launch.\n",
    "        \n",
    "\n",
    "\n",
    "        # Here we will add any init functions prior to starting the MyRobotEnv\n",
    "        super(MyTrainingEnv, self).__init__()\n",
    "\n",
    "\n",
    "    def _set_init_pose(self):\n",
    "        \"\"\"Sets the Robot in its init pose\n",
    "        \"\"\"\n",
    "        # TODO\n",
    "\n",
    "    def _init_env_variables(self):\n",
    "        \"\"\"\n",
    "        Inits variables needed to be initialised each time we reset at the start\n",
    "        of an episode.\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        # TODO\n",
    "\n",
    "\n",
    "    def _set_action(self, action):\n",
    "        \"\"\"\n",
    "        Move the robot based on the action variable given\n",
    "        \"\"\"\n",
    "        # TODO: Move robot\n",
    "\n",
    "    def _get_obs(self):\n",
    "        \"\"\"\n",
    "        Here we define what sensor data of our robots observations\n",
    "        To know which Variables we have access to, we need to read the\n",
    "        MyRobotEnv API DOCS\n",
    "        :return: observations\n",
    "        \"\"\"\n",
    "        # TODO\n",
    "        return observations\n",
    "\n",
    "    def _is_done(self, observations):\n",
    "        \"\"\"\n",
    "        Decide if episode is done based on the observations\n",
    "        \"\"\"\n",
    "        # TODO\n",
    "        return done\n",
    "\n",
    "    def _compute_reward(self, observations, done):\n",
    "        \"\"\"\n",
    "        Return the reward based on the observations given\n",
    "        \"\"\"\n",
    "        # TODO\n",
    "        return reward\n",
    "        \n",
    "    # Internal TaskEnv Methods\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#407EAF;color:white;\">**template_my_task_env.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#FF5E00;color:white;\">**Exercise 7.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "b) Inside the scripts folder of the package you created in the previous unit, create a new file named **fetch_reach.py**, and paste the above template inside. You can also rename the environment to FetchReachEnv. You will then make all the necessary changes to adapt it to the specific task to be trained."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#FF5E00;color:white;\">**End of Exercise 7.3**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Initialization of the class"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's go by parts. First of all, we'll need to fill the initialization of the class. Here you can see an example of this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def __init__(self):\n",
    "        \n",
    "        self.get_params()\n",
    "        \n",
    "        fetch_env.FetchEnv.__init__(self)\n",
    "        utils.EzPickle.__init__(self)\n",
    "        \n",
    "        self._env_setup(initial_qpos=self.init_pos)\n",
    "        obs = self._get_obs()\n",
    "        \n",
    "        self.action_space = spaces.Box(-1., 1., shape=(self.n_actions,), dtype='float32')\n",
    "        self.observation_space = spaces.Dict(dict(\n",
    "            desired_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'),\n",
    "            achieved_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'),\n",
    "            observation=spaces.Box(-np.inf, np.inf, shape=obs['observation'].shape, dtype='float32'),\n",
    "        ))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first thing we are doing, as you can see, is calling the **get_params()** function, which will get all the required parameters that the Fetch robot needs in order to perform the training."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.get_params()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we are calling the **_env_setup()** function, which basically will set up everything required to start the training, and will send the arm of the Fetch robot to its initial position:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self._env_setup(initial_qpos=self.init_pos)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Also, we get a first observation, since we will need it to generate the **observation_space**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "obs = self._get_obs()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we define both the **action_space** and the **observation_space**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.action_space = spaces.Box(-1., 1., shape=(self.n_actions,), dtype='float32')\n",
    "self.observation_space = spaces.Dict(dict(\n",
    "    desired_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'),\n",
    "    achieved_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'),\n",
    "    observation=spaces.Box(-np.inf, np.inf, shape=obs['observation'].shape, dtype='float32'),\n",
    "))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see, the observation space is a dict, which is specific to the Goal-based Environment, as we explained in the previous unit."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Methods needed by the Task Environment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, let's continue with the creation of our script by creating the methods needed by the Task Environment. These are all the methods that will be particular to this training. Let's have a look at all the functions, and then we'll comment on them, one by one."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_params(self):\n",
    "    #get configuration parameters\n",
    "    \"\"\"\n",
    "    self.n_actions = rospy.get_param('/fetch/n_actions')\n",
    "    self.has_object = rospy.get_param('/fetch/has_object')\n",
    "    self.block_gripper = rospy.get_param('/fetch/block_gripper')\n",
    "    self.n_substeps = rospy.get_param('/fetch/n_substeps')\n",
    "    self.gripper_extra_height = rospy.get_param('/fetch/gripper_extra_height')\n",
    "    self.target_in_the_air = rospy.get_param('/fetch/target_in_the_air')\n",
    "    self.target_offset = rospy.get_param('/fetch/target_offset')\n",
    "    self.obj_range = rospy.get_param('/fetch/obj_range')\n",
    "    self.target_range = rospy.get_param('/fetch/target_range')\n",
    "    self.distance_threshold = rospy.get_param('/fetch/distance_threshold')\n",
    "    self.init_pos = rospy.get_param('/fetch/init_pos')\n",
    "    self.reward_type = rospy.get_param('/fetch/reward_type')\n",
    "    \"\"\"\n",
    "    self.n_actions = 4\n",
    "    self.has_object = False\n",
    "    self.block_gripper = True\n",
    "    self.n_substeps = 20\n",
    "    self.gripper_extra_height = 0.2\n",
    "    self.target_in_the_air = True\n",
    "    self.target_offset = 0.0\n",
    "    self.obj_range = 0.15\n",
    "    self.target_range = 0.15\n",
    "    self.distance_threshold = 0.05\n",
    "    self.reward_type = \"sparse\"\n",
    "    self.init_pos = {\n",
    "        'joint0': 0.0,\n",
    "        'joint1': 0.0,\n",
    "        'joint2': 0.0,\n",
    "        'joint3': -1.5,\n",
    "        'joint4': 0.0,\n",
    "        'joint5': 1.5,\n",
    "        'joint6': 0.0,\n",
    "    }\n",
    "\n",
    "def _set_action(self, action):\n",
    "\n",
    "    # Take action\n",
    "    assert action.shape == (4,)\n",
    "    action = action.copy()  # ensure that we don't change the action outside of this scope\n",
    "    pos_ctrl, gripper_ctrl = action[:3], action[3]\n",
    "\n",
    "    #pos_ctrl *= 0.05  # limit maximum change in position\n",
    "    rot_ctrl = [1., 0., 1., 0.]  # fixed rotation of the end effector, expressed as a quaternion\n",
    "    gripper_ctrl = np.array([gripper_ctrl, gripper_ctrl])\n",
    "    assert gripper_ctrl.shape == (2,)\n",
    "    if self.block_gripper:\n",
    "        gripper_ctrl = np.zeros_like(gripper_ctrl)\n",
    "    action = np.concatenate([pos_ctrl, rot_ctrl, gripper_ctrl])\n",
    "\n",
    "\n",
    "    # Apply action to simulation.\n",
    "    self.set_trajectory_ee(action)\n",
    "\n",
    "def _get_obs(self):\n",
    "\n",
    "    grip_pos = self.get_ee_pose()\n",
    "    grip_pos_array = np.array([grip_pos.pose.position.x, grip_pos.pose.position.y, grip_pos.pose.position.z])\n",
    "    #dt = self.sim.nsubsteps * self.sim.model.opt.timestep #What is this??\n",
    "    #grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt\n",
    "    grip_rpy = self.get_ee_rpy()\n",
    "    #print grip_rpy\n",
    "    grip_velp = np.array([grip_rpy.y, grip_rpy.y])\n",
    "    robot_qpos, robot_qvel = fetch_utils.robot_get_obs(self.joints)\n",
    "    if self.has_object:\n",
    "        object_pos = self.sim.data.get_site_xpos('object0')\n",
    "        # rotations\n",
    "        object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0'))\n",
    "        # velocities\n",
    "        object_velp = self.sim.data.get_site_xvelp('object0') * dt\n",
    "        object_velr = self.sim.data.get_site_xvelr('object0') * dt\n",
    "        # gripper state\n",
    "        object_rel_pos = object_pos - grip_pos\n",
    "        object_velp -= grip_velp\n",
    "    else:\n",
    "        object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0)\n",
    "\n",
    "    gripper_state = robot_qpos[-2:]\n",
    "    gripper_vel = robot_qvel[-2:] #* dt  # change to a scalar if the gripper is made symmetric\n",
    "    \"\"\"\n",
    "    if not self.has_object:\n",
    "        achieved_goal = grip_pos_array.copy()\n",
    "    else:\n",
    "        achieved_goal = np.squeeze(object_pos.copy())\n",
    "    \"\"\"    \n",
    "    achieved_goal = self._sample_achieved_goal(grip_pos_array, object_pos)\n",
    "\n",
    "    obs = np.concatenate([\n",
    "        grip_pos_array, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(),\n",
    "        object_velp.ravel(), object_velr.ravel(), gripper_vel,\n",
    "    ])\n",
    "\n",
    "    return {\n",
    "        'observation': obs.copy(),\n",
    "        'achieved_goal': achieved_goal.copy(),\n",
    "        'desired_goal': self.goal.copy(),\n",
    "    }\n",
    "\n",
    "def _is_done(self, observations):\n",
    "\n",
    "    d = self.goal_distance(observations['achieved_goal'], self.goal)\n",
    "\n",
    "    return (d < self.distance_threshold).astype(np.float32)\n",
    "\n",
    "def _compute_reward(self, observations, done):\n",
    "\n",
    "    d = self.goal_distance(observations['achieved_goal'], self.goal)\n",
    "    if self.reward_type == 'sparse':\n",
    "        return -(d > self.distance_threshold).astype(np.float32)\n",
    "    else:\n",
    "        return -d\n",
    "\n",
    "def _set_init_pose(self):\n",
    "    \"\"\"Sets the Robot in its init pose\n",
    "    \"\"\"\n",
    "    self.gazebo.unpauseSim()\n",
    "    self.set_trajectory_joints(self.init_pos)\n",
    "\n",
    "    return True\n",
    "\n",
    "def goal_distance(self, goal_a, goal_b):\n",
    "    assert goal_a.shape == goal_b.shape\n",
    "    return np.linalg.norm(goal_a - goal_b, axis=-1)\n",
    "\n",
    "\n",
    "def _sample_goal(self):\n",
    "    if self.has_object:\n",
    "        goal = self.initial_gripper_xpos[:3] + self.np_random.uniform(-self.target_range, self.target_range, size=3)\n",
    "        goal += self.target_offset\n",
    "        goal[2] = self.height_offset\n",
    "        if self.target_in_the_air and self.np_random.uniform() < 0.5:\n",
    "            goal[2] += self.np_random.uniform(0, 0.45)\n",
    "    else:\n",
    "        goal = self.initial_gripper_xpos[:3] + self.np_random.uniform(-0.15, 0.15, size=3)\n",
    "\n",
    "    #return goal.copy()\n",
    "    return goal\n",
    "\n",
    "def _sample_achieved_goal(self, grip_pos_array, object_pos):\n",
    "    if not self.has_object:\n",
    "        achieved_goal = grip_pos_array.copy()\n",
    "    else:\n",
    "        achieved_goal = np.squeeze(object_pos.copy())\n",
    "\n",
    "    #return achieved_goal.copy()\n",
    "    return achieved_goal\n",
    "\n",
    "def _env_setup(self, initial_qpos):\n",
    "    print (\"Init Pos:\")\n",
    "    print (initial_qpos)\n",
    "    #for name, value in initial_qpos.items():\n",
    "    self.gazebo.unpauseSim()\n",
    "    self.set_trajectory_joints(initial_qpos)\n",
    "        #self.execute_trajectory()\n",
    "    #utils.reset_mocap_welds(self.sim)\n",
    "    #self.sim.forward()\n",
    "\n",
    "    # Move end effector into position.\n",
    "    gripper_target = np.array([0.498, 0.005, 0.431 + self.gripper_extra_height])# + self.sim.data.get_site_xpos('robot0:grip')\n",
    "    gripper_rotation = np.array([1., 0., 1., 0.])\n",
    "    #self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)\n",
    "    #self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)\n",
    "    action = np.concatenate([gripper_target, gripper_rotation])\n",
    "    self.set_trajectory_ee(action)\n",
    "    #self.execute_trajectory()\n",
    "    #for _ in range(10):\n",
    "        #self.sim.step()\n",
    "        #self.step()\n",
    "\n",
    "    # Extract information for sampling goals.\n",
    "    #self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()\n",
    "    gripper_pos = self.get_ee_pose()\n",
    "    gripper_pose_array = np.array([gripper_pos.pose.position.x, gripper_pos.pose.position.y, gripper_pos.pose.position.z])\n",
    "    self.initial_gripper_xpos = gripper_pose_array.copy()\n",
    "    if self.has_object:\n",
    "        self.height_offset = self.sim.data.get_site_xpos('object0')[2]\n",
    "\n",
    "    self.goal = self._sample_goal()\n",
    "    self._get_obs()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You already know the **get_params()** function from the previous chapters, so I will skip it. It's exactly the same, just using different parameters (in this case, the ones needed for the Fetch robot)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, first of all, we have the **_set_action (self, action)** function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _set_action(self, action):\n",
    "\n",
    "    # Take action\n",
    "    assert action.shape == (4,)\n",
    "    action = action.copy()  # ensure that we don't change the action outside of this scope\n",
    "    pos_ctrl, gripper_ctrl = action[:3], action[3]\n",
    "\n",
    "    #pos_ctrl *= 0.05  # limit maximum change in position\n",
    "    rot_ctrl = [1., 0., 1., 0.]  # fixed rotation of the end effector, expressed as a quaternion\n",
    "    gripper_ctrl = np.array([gripper_ctrl, gripper_ctrl])\n",
    "    assert gripper_ctrl.shape == (2,)\n",
    "    if self.block_gripper:\n",
    "        gripper_ctrl = np.zeros_like(gripper_ctrl)\n",
    "    action = np.concatenate([pos_ctrl, rot_ctrl, gripper_ctrl])\n",
    "\n",
    "\n",
    "    # Apply action to simulation.\n",
    "    self.set_trajectory_ee(action)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function will receive an **action** variable as input. This action variable will contain a desired goal position for the end effector of the manipulator. Then, we basically reformat this action variable and send it through the **set_trajectory_ee(action)** function, which we explained in the previous chapter. This function will call the required Service of the support script (also from the previous Chapter) in order to execute the motion."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we have the **_get_obs(self)** function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _get_obs(self):\n",
    "\n",
    "    grip_pos = self.get_ee_pose()\n",
    "    grip_pos_array = np.array([grip_pos.pose.position.x, grip_pos.pose.position.y, grip_pos.pose.position.z])\n",
    "    #dt = self.sim.nsubsteps * self.sim.model.opt.timestep #What is this??\n",
    "    #grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt\n",
    "    grip_rpy = self.get_ee_rpy()\n",
    "    #print grip_rpy\n",
    "    grip_velp = np.array([grip_rpy.y, grip_rpy.y])\n",
    "    robot_qpos, robot_qvel = self.robot_get_obs(self.joints)\n",
    "    if self.has_object:\n",
    "        object_pos = self.sim.data.get_site_xpos('object0')\n",
    "        # rotations\n",
    "        object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0'))\n",
    "        # velocities\n",
    "        object_velp = self.sim.data.get_site_xvelp('object0') * dt\n",
    "        object_velr = self.sim.data.get_site_xvelr('object0') * dt\n",
    "        # gripper state\n",
    "        object_rel_pos = object_pos - grip_pos\n",
    "        object_velp -= grip_velp\n",
    "    else:\n",
    "        object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0)\n",
    "\n",
    "    gripper_state = robot_qpos[-2:]\n",
    "    gripper_vel = robot_qvel[-2:] #* dt  # change to a scalar if the gripper is made symmetric\n",
    "    \"\"\"\n",
    "    if not self.has_object:\n",
    "        achieved_goal = grip_pos_array.copy()\n",
    "    else:\n",
    "        achieved_goal = np.squeeze(object_pos.copy())\n",
    "    \"\"\"    \n",
    "    achieved_goal = self._sample_achieved_goal(grip_pos_array, object_pos)\n",
    "\n",
    "    obs = np.concatenate([\n",
    "        grip_pos_array, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(),\n",
    "        object_velp.ravel(), object_velr.ravel(), gripper_vel,\n",
    "    ])\n",
    "\n",
    "    return {\n",
    "        'observation': obs.copy(),\n",
    "        'achieved_goal': achieved_goal.copy(),\n",
    "        'desired_goal': self.goal.copy(),\n",
    "    }\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The **joints_callback()** function is the callback for the Subscriber that we declared in the initialization of the class (**self.joint_states_sub**). It just stores the data from the **/joint_states topic** in the variable of the class, **self.joints**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The next one is the **is_done (self, observations)** function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _is_done(self, observations):\n",
    "\n",
    "    d = self.goal_distance(observations['achieved_goal'], self.goal)\n",
    "\n",
    "    return (d < self.distance_threshold).astype(np.float32)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function basically checks whether the goal has been achieved or not. In this case, the goal would be to reach the cube with the end effector of the Fetch robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we have the **_compute_reward (self, observations, done)** function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _compute_reward(self, observations, done):\n",
    "\n",
    "    d = self.goal_distance(observations['achieved_goal'], self.goal)\n",
    "    if self.reward_type == 'sparse':\n",
    "        return -(d > self.distance_threshold).astype(np.float32)\n",
    "    else:\n",
    "        return -d\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function, as the name itself implies, will calculate the reward for each action taken, depending on the distance between the end effector and the cube."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We also have the **_set_init_pose (self)** function, which is in charge of sending the arm of the Fetch robot to an initial position after each step of the training has finished."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _set_init_pose(self):\n",
    "    \"\"\"Sets the Robot in its init pose\n",
    "    \"\"\"\n",
    "    self.gazebo.unpauseSim()\n",
    "    self.set_trajectory_joints(self.init_pos)\n",
    "\n",
    "    return True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also see the **_sample_goal (self)** and the **_sample_achieved_goal (self, grip_pos_array, object_pos)** functions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _sample_goal(self):\n",
    "    if self.has_object:\n",
    "        goal = self.initial_gripper_xpos[:3] + self.np_random.uniform(-self.target_range, self.target_range, size=3)\n",
    "        goal += self.target_offset\n",
    "        goal[2] = self.height_offset\n",
    "        if self.target_in_the_air and self.np_random.uniform() < 0.5:\n",
    "            goal[2] += self.np_random.uniform(0, 0.45)\n",
    "    else:\n",
    "        goal = self.initial_gripper_xpos[:3] + self.np_random.uniform(-0.15, 0.15, size=3)\n",
    "\n",
    "    #return goal.copy()\n",
    "    return goal\n",
    "\n",
    "def _sample_achieved_goal(self, grip_pos_array, object_pos):\n",
    "    if not self.has_object:\n",
    "        achieved_goal = grip_pos_array.copy()\n",
    "    else:\n",
    "        achieved_goal = np.squeeze(object_pos.copy())\n",
    "\n",
    "    #return achieved_goal.copy()\n",
    "    return achieved_goal"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function formats and returns both the current goal to be reached and the actual goal that has been reached, respectively."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we have the **_env_setup (self, initial_qpos)** function, which we mentioned before."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def _env_setup(self, initial_qpos):\n",
    "    print (\"Init Pos:\")\n",
    "    print (initial_qpos)\n",
    "    #for name, value in initial_qpos.items():\n",
    "    self.gazebo.unpauseSim()\n",
    "    self.set_trajectory_joints(initial_qpos)\n",
    "        #self.execute_trajectory()\n",
    "    #utils.reset_mocap_welds(self.sim)\n",
    "    #self.sim.forward()\n",
    "\n",
    "    # Move end effector into position.\n",
    "    gripper_target = np.array([0.498, 0.005, 0.431 + self.gripper_extra_height])# + self.sim.data.get_site_xpos('robot0:grip')\n",
    "    gripper_rotation = np.array([1., 0., 1., 0.])\n",
    "    #self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)\n",
    "    #self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)\n",
    "    action = np.concatenate([gripper_target, gripper_rotation])\n",
    "    self.set_trajectory_ee(action)\n",
    "    #self.execute_trajectory()\n",
    "    #for _ in range(10):\n",
    "        #self.sim.step()\n",
    "        #self.step()\n",
    "\n",
    "    # Extract information for sampling goals.\n",
    "    #self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()\n",
    "    gripper_pos = self.get_ee_pose()\n",
    "    gripper_pose_array = np.array([gripper_pos.pose.position.x, gripper_pos.pose.position.y, gripper_pos.pose.position.z])\n",
    "    self.initial_gripper_xpos = gripper_pose_array.copy()\n",
    "    if self.has_object:\n",
    "        self.height_offset = self.sim.data.get_site_xpos('object0')[2]\n",
    "\n",
    "    self.goal = self._sample_goal()\n",
    "    self._get_obs()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function sets up the environment so that everything is ready for the training to start."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we have the **robot_get_obs(data)** function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def robot_get_obs(data):\n",
    "        \n",
    "        \"\"\"\n",
    "        Returns all joint positions and velocities associated with a robot.\n",
    "        \"\"\"\n",
    "    \n",
    "        if data.position is not None and data.name:\n",
    "            #names = [n for n in data.name if n.startswith('robot')]\n",
    "            names = [n for n in data.name]\n",
    "            i = 0\n",
    "            r = 0\n",
    "            for name in names:\n",
    "                r += 1\n",
    "                \n",
    "            return (\n",
    "                np.array([data.position[i] for i in range(r)]),\n",
    "                np.array([data.velocity[i] for i in range(r)]),\n",
    "            )\n",
    "        return np.zeros(0), np.zeros(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This function gets a **data** variable as input, which contains data about all the joints, and returns all joint positions and velocities associated with the Fetch robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And that's it! With the above methods, we will be able to train our Fetch robot in the task of reaching a cube. At the end, you should have a file like this one:"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#407EAF;color:white;\">**fetch_reach.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from gym import utils\n",
    "\n",
    "import rospy\n",
    "from gym import spaces\n",
    "from openai_ros.robot_envs import fetch_env\n",
    "from gym.envs.registration import register\n",
    "import numpy as np\n",
    "from sensor_msgs.msg import JointState\n",
    "from fetch_train.srv import EePose, EePoseRequest, EeRpy, EeRpyRequest, EeTraj, EeTrajRequest, JointTraj, JointTrajRequest\n",
    "\n",
    "\n",
    "register(\n",
    "        id='FetchReach-v0',\n",
    "        entry_point='openai_ros:FetchReachEnv',\n",
    "        timestep_limit=1000,\n",
    "    )\n",
    "\n",
    "\n",
    "class FetchReachEnv(fetch_env.FetchEnv, utils.EzPickle):\n",
    "    def __init__(self):\n",
    "        \n",
    "        print (\"Entered Reach Env\")\n",
    "        \n",
    "        self.get_params()\n",
    "        \n",
    "        fetch_env.FetchEnv.__init__(self)\n",
    "        utils.EzPickle.__init__(self)\n",
    "        \n",
    "        print (\"Call env setup\")\n",
    "        self._env_setup(initial_qpos=self.init_pos)\n",
    "        \n",
    "        print (\"Call get_obs\")\n",
    "        obs = self._get_obs()\n",
    "        \n",
    "        self.action_space = spaces.Box(-1., 1., shape=(self.n_actions,), dtype='float32')\n",
    "        self.observation_space = spaces.Dict(dict(\n",
    "            desired_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'),\n",
    "            achieved_goal=spaces.Box(-np.inf, np.inf, shape=obs['achieved_goal'].shape, dtype='float32'),\n",
    "            observation=spaces.Box(-np.inf, np.inf, shape=obs['observation'].shape, dtype='float32'),\n",
    "        ))\n",
    "\n",
    "        \n",
    "    def get_params(self):\n",
    "        #get configuration parameters\n",
    "        \"\"\"\n",
    "        self.n_actions = rospy.get_param('/fetch/n_actions')\n",
    "        self.has_object = rospy.get_param('/fetch/has_object')\n",
    "        self.block_gripper = rospy.get_param('/fetch/block_gripper')\n",
    "        self.n_substeps = rospy.get_param('/fetch/n_substeps')\n",
    "        self.gripper_extra_height = rospy.get_param('/fetch/gripper_extra_height')\n",
    "        self.target_in_the_air = rospy.get_param('/fetch/target_in_the_air')\n",
    "        self.target_offset = rospy.get_param('/fetch/target_offset')\n",
    "        self.obj_range = rospy.get_param('/fetch/obj_range')\n",
    "        self.target_range = rospy.get_param('/fetch/target_range')\n",
    "        self.distance_threshold = rospy.get_param('/fetch/distance_threshold')\n",
    "        self.init_pos = rospy.get_param('/fetch/init_pos')\n",
    "        self.reward_type = rospy.get_param('/fetch/reward_type')\n",
    "        \"\"\"\n",
    "        self.n_actions = 4\n",
    "        self.has_object = False\n",
    "        self.block_gripper = True\n",
    "        self.n_substeps = 20\n",
    "        self.gripper_extra_height = 0.2\n",
    "        self.target_in_the_air = True\n",
    "        self.target_offset = 0.0\n",
    "        self.obj_range = 0.15\n",
    "        self.target_range = 0.15\n",
    "        self.distance_threshold = 0.05\n",
    "        self.reward_type = \"sparse\"\n",
    "        self.init_pos = {\n",
    "            'joint0': 0.0,\n",
    "            'joint1': 0.0,\n",
    "            'joint2': 0.0,\n",
    "            'joint3': -1.5,\n",
    "            'joint4': 0.0,\n",
    "            'joint5': 1.5,\n",
    "            'joint6': 0.0,\n",
    "        }\n",
    "        \n",
    "    def _set_action(self, action):\n",
    "        \n",
    "        # Take action\n",
    "        assert action.shape == (4,)\n",
    "        action = action.copy()  # ensure that we don't change the action outside of this scope\n",
    "        pos_ctrl, gripper_ctrl = action[:3], action[3]\n",
    "\n",
    "        #pos_ctrl *= 0.05  # limit maximum change in position\n",
    "        rot_ctrl = [1., 0., 1., 0.]  # fixed rotation of the end effector, expressed as a quaternion\n",
    "        gripper_ctrl = np.array([gripper_ctrl, gripper_ctrl])\n",
    "        assert gripper_ctrl.shape == (2,)\n",
    "        if self.block_gripper:\n",
    "            gripper_ctrl = np.zeros_like(gripper_ctrl)\n",
    "        action = np.concatenate([pos_ctrl, rot_ctrl, gripper_ctrl])\n",
    "            \n",
    "            \n",
    "        # Apply action to simulation.\n",
    "        self.set_trajectory_ee(action)\n",
    "\n",
    "    def _get_obs(self):\n",
    "        \n",
    "        grip_pos = self.get_ee_pose()\n",
    "        grip_pos_array = np.array([grip_pos.pose.position.x, grip_pos.pose.position.y, grip_pos.pose.position.z])\n",
    "        #dt = self.sim.nsubsteps * self.sim.model.opt.timestep #What is this??\n",
    "        #grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt\n",
    "        grip_rpy = self.get_ee_rpy()\n",
    "        #print grip_rpy\n",
    "        grip_velp = np.array([grip_rpy.y, grip_rpy.y])\n",
    "        robot_qpos, robot_qvel = self.robot_get_obs(self.joints)\n",
    "        if self.has_object:\n",
    "            object_pos = self.sim.data.get_site_xpos('object0')\n",
    "            # rotations\n",
    "            object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0'))\n",
    "            # velocities\n",
    "            object_velp = self.sim.data.get_site_xvelp('object0') * dt\n",
    "            object_velr = self.sim.data.get_site_xvelr('object0') * dt\n",
    "            # gripper state\n",
    "            object_rel_pos = object_pos - grip_pos\n",
    "            object_velp -= grip_velp\n",
    "        else:\n",
    "            object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0)\n",
    "            \n",
    "        gripper_state = robot_qpos[-2:]\n",
    "        gripper_vel = robot_qvel[-2:] #* dt  # change to a scalar if the gripper is made symmetric\n",
    "        \"\"\"\n",
    "        if not self.has_object:\n",
    "            achieved_goal = grip_pos_array.copy()\n",
    "        else:\n",
    "            achieved_goal = np.squeeze(object_pos.copy())\n",
    "        \"\"\"    \n",
    "        achieved_goal = self._sample_achieved_goal(grip_pos_array, object_pos)\n",
    "            \n",
    "        obs = np.concatenate([\n",
    "            grip_pos_array, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(),\n",
    "            object_velp.ravel(), object_velr.ravel(), gripper_vel,\n",
    "        ])\n",
    "\n",
    "        return {\n",
    "            'observation': obs.copy(),\n",
    "            'achieved_goal': achieved_goal.copy(),\n",
    "            'desired_goal': self.goal.copy(),\n",
    "        }\n",
    "        \n",
    "    def _is_done(self, observations):\n",
    "        \n",
    "        d = self.goal_distance(observations['achieved_goal'], self.goal)\n",
    "        \n",
    "        return (d < self.distance_threshold).astype(np.float32)\n",
    "        \n",
    "    def _compute_reward(self, observations, done):\n",
    "\n",
    "        d = self.goal_distance(observations['achieved_goal'], self.goal)\n",
    "        if self.reward_type == 'sparse':\n",
    "            return -(d > self.distance_threshold).astype(np.float32)\n",
    "        else:\n",
    "            return -d\n",
    "        \n",
    "    def _init_env_variables(self):\n",
    "        \"\"\"\n",
    "        Inits variables needed to be initialized each time we reset at the start\n",
    "        of an episode.\n",
    "        :return:\n",
    "        \"\"\"\n",
    "        pass\n",
    "    \n",
    "    def _set_init_pose(self):\n",
    "        \"\"\"Sets the Robot in its init pose\n",
    "        \"\"\"\n",
    "        self.gazebo.unpauseSim()\n",
    "        self.set_trajectory_joints(self.init_pos)\n",
    "\n",
    "        return True\n",
    "        \n",
    "    def goal_distance(self, goal_a, goal_b):\n",
    "        assert goal_a.shape == goal_b.shape\n",
    "        return np.linalg.norm(goal_a - goal_b, axis=-1)\n",
    "        \n",
    "\n",
    "    def _sample_goal(self):\n",
    "        if self.has_object:\n",
    "            goal = self.initial_gripper_xpos[:3] + self.np_random.uniform(-self.target_range, self.target_range, size=3)\n",
    "            goal += self.target_offset\n",
    "            goal[2] = self.height_offset\n",
    "            if self.target_in_the_air and self.np_random.uniform() < 0.5:\n",
    "                goal[2] += self.np_random.uniform(0, 0.45)\n",
    "        else:\n",
    "            goal = self.initial_gripper_xpos[:3] + self.np_random.uniform(-0.15, 0.15, size=3)\n",
    "        \n",
    "        #return goal.copy()\n",
    "        return goal\n",
    "        \n",
    "    def _sample_achieved_goal(self, grip_pos_array, object_pos):\n",
    "        if not self.has_object:\n",
    "            achieved_goal = grip_pos_array.copy()\n",
    "        else:\n",
    "            achieved_goal = np.squeeze(object_pos.copy())\n",
    "        \n",
    "        #return achieved_goal.copy()\n",
    "        return achieved_goal\n",
    "        \n",
    "    def _env_setup(self, initial_qpos):\n",
    "        print (\"Init Pos:\")\n",
    "        print (initial_qpos)\n",
    "        #for name, value in initial_qpos.items():\n",
    "        self.gazebo.unpauseSim()\n",
    "        self.set_trajectory_joints(initial_qpos)\n",
    "            #self.execute_trajectory()\n",
    "        #utils.reset_mocap_welds(self.sim)\n",
    "        #self.sim.forward()\n",
    "\n",
    "        # Move end effector into position.\n",
    "        gripper_target = np.array([0.498, 0.005, 0.431 + self.gripper_extra_height])# + self.sim.data.get_site_xpos('robot0:grip')\n",
    "        gripper_rotation = np.array([1., 0., 1., 0.])\n",
    "        #self.sim.data.set_mocap_pos('robot0:mocap', gripper_target)\n",
    "        #self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation)\n",
    "        action = np.concatenate([gripper_target, gripper_rotation])\n",
    "        self.set_trajectory_ee(action)\n",
    "        #self.execute_trajectory()\n",
    "        #for _ in range(10):\n",
    "            #self.sim.step()\n",
    "            #self.step()\n",
    "\n",
    "        # Extract information for sampling goals.\n",
    "        #self.initial_gripper_xpos = self.sim.data.get_site_xpos('robot0:grip').copy()\n",
    "        gripper_pos = self.get_ee_pose()\n",
    "        gripper_pose_array = np.array([gripper_pos.pose.position.x, gripper_pos.pose.position.y, gripper_pos.pose.position.z])\n",
    "        self.initial_gripper_xpos = gripper_pose_array.copy()\n",
    "        if self.has_object:\n",
    "            self.height_offset = self.sim.data.get_site_xpos('object0')[2]\n",
    "            \n",
    "        self.goal = self._sample_goal()\n",
    "        self._get_obs()\n",
    "        \n",
    "    def robot_get_obs(data):\n",
    "        \n",
    "        \"\"\"\n",
    "        Returns all joint positions and velocities associated with a robot.\n",
    "        \"\"\"\n",
    "    \n",
    "        if data.position is not None and data.name:\n",
    "            #names = [n for n in data.name if n.startswith('robot')]\n",
    "            names = [n for n in data.name]\n",
    "            i = 0\n",
    "            r = 0\n",
    "            for name in names:\n",
    "                r += 1\n",
    "                \n",
    "            return (\n",
    "                np.array([data.position[i] for i in range(r)]),\n",
    "                np.array([data.velocity[i] for i in range(r)]),\n",
    "            )\n",
    "        return np.zeros(0), np.zeros(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#407EAF;color:white;\">**fetch_reach.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Training Script (HER)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Everything is ready to start with the training! We already have all our environment structures ready, so it's time to execute the learning algorithm! For this case, we'll be using HER, which stands for **H**indsight **E**xperience **R**eplay."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "To understand what HER does, let’s look at it in the context of our current task, where we need to learn to reach a cube on top of a table. Our first attempt will very likely not be a successful one. Unless we get very lucky, the next few attempts will likely also not succeed. Typical reinforcement learning algorithms would not learn anything from this experience since they just obtain a constant reward (in this case: -1) that does not contain any learning signal.\n",
    "\n",
    "The key insight that HER formalizes is what humans do intuitively: Even though we have not succeeded at a specific goal, we have at least achieved a different one. So, why not just pretend that we wanted to achieve this goal to begin with, instead of the one that we set out to achieve originally? By doing this substitution, the reinforcement learning algorithm can obtain a learning signal since it has achieved a goal, even if it wasn’t the one that we meant to achieve originally. If we repeat this process, we will eventually learn how to achieve arbitrary goals, including the goals that we really want to achieve.\n",
    "\n",
    "This approach let's us learn how to reach a cube on top of the table, even though our reward is sparse, and even though we may have never actually hit the desired goal early on. This technique is called Hindsight Experience Replay because it replays experiences (a technique often used in off-policy RL algorithms, like DQN and DDPG) with goals that are chosen in hindsight, after the episode has finished."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Everything that you saw in the previous unit about Goal-based environments probably makes more sense to you now, right?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Great! So with the HER algorithm properly introduced, let's try it! But, as we had to do with the deepq algorithm, we'll need to modify some things in the scripts in order to make it work properly.\n",
    "\n",
    "Also, keep in mind that we are not going to explain the contents of the HER algorithm scripts because it is quite a complex algorithm and it would take a complete course to do so. So, for now, we are just going to focus on having it running, in order to be able to use it to train our robot. Let's go then!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#FF5E00;color:white;\">**Exercise 7.4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "a) Modify the necessary scripts to have the HER algorithm working. You will find the HER algorithm scripts in **baselines/baselines/her**."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**STEP 1**: The script that will launch the training session is **experiments/train.py**. Here, you will need to import your Task Environment, and also initialize it. Below you can see an example of this file already modified."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#407EAF;color:white;\">**train.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import os\n",
    "import sys\n",
    "\n",
    "import click\n",
    "import numpy as np\n",
    "import json\n",
    "from mpi4py import MPI\n",
    "\n",
    "from baselines import logger\n",
    "from baselines.common import set_global_seeds\n",
    "from baselines.common.mpi_moments import mpi_moments\n",
    "import baselines.her.experiment.config as config\n",
    "from baselines.her.rollout import RolloutWorker\n",
    "from baselines.her.util import mpi_fork\n",
    "\n",
    "from subprocess import CalledProcessError\n",
    "\n",
    "#from gym.envs.robotics.fetch import reach\n",
    "from openai_ros.task_envs.fetch_reach import fetch_reach\n",
    "import rospy\n",
    "\n",
    "def mpi_average(value):\n",
    "    if value == []:\n",
    "        value = [0.]\n",
    "    if not isinstance(value, list):\n",
    "        value = [value]\n",
    "    return mpi_moments(np.array(value))[0]\n",
    "\n",
    "\n",
    "def train(policy, rollout_worker, evaluator,\n",
    "          n_epochs, n_test_rollouts, n_cycles, n_batches, policy_save_interval,\n",
    "          save_policies, **kwargs):\n",
    "    rank = MPI.COMM_WORLD.Get_rank()\n",
    "\n",
    "    latest_policy_path = os.path.join(logger.get_dir(), 'policy_latest.pkl')\n",
    "    best_policy_path = os.path.join(logger.get_dir(), 'policy_best.pkl')\n",
    "    periodic_policy_path = os.path.join(logger.get_dir(), 'policy_{}.pkl')\n",
    "\n",
    "    logger.info(\"Training...\")\n",
    "    best_success_rate = -1\n",
    "    for epoch in range(n_epochs):\n",
    "        # train\n",
    "        rollout_worker.clear_history()\n",
    "        for _ in range(n_cycles):\n",
    "            episode = rollout_worker.generate_rollouts()\n",
    "            policy.store_episode(episode)\n",
    "            for _ in range(n_batches):\n",
    "                policy.train()\n",
    "            policy.update_target_net()\n",
    "\n",
    "        # test\n",
    "        evaluator.clear_history()\n",
    "        for _ in range(n_test_rollouts):\n",
    "            evaluator.generate_rollouts()\n",
    "\n",
    "        # record logs\n",
    "        logger.record_tabular('epoch', epoch)\n",
    "        for key, val in evaluator.logs('test'):\n",
    "            logger.record_tabular(key, mpi_average(val))\n",
    "        for key, val in rollout_worker.logs('train'):\n",
    "            logger.record_tabular(key, mpi_average(val))\n",
    "        for key, val in policy.logs():\n",
    "            logger.record_tabular(key, mpi_average(val))\n",
    "\n",
    "        if rank == 0:\n",
    "            logger.dump_tabular()\n",
    "\n",
    "        # save the policy if it's better than the previous ones\n",
    "        success_rate = mpi_average(evaluator.current_success_rate())\n",
    "        if rank == 0 and success_rate >= best_success_rate and save_policies:\n",
    "            best_success_rate = success_rate\n",
    "            logger.info('New best success rate: {}. Saving policy to {} ...'.format(best_success_rate, best_policy_path))\n",
    "            evaluator.save_policy(best_policy_path)\n",
    "            evaluator.save_policy(latest_policy_path)\n",
    "        if rank == 0 and policy_save_interval > 0 and epoch % policy_save_interval == 0 and save_policies:\n",
    "            policy_path = periodic_policy_path.format(epoch)\n",
    "            logger.info('Saving periodic policy to {} ...'.format(policy_path))\n",
    "            evaluator.save_policy(policy_path)\n",
    "\n",
    "        # make sure that different threads have different seeds\n",
    "        local_uniform = np.random.uniform(size=(1,))\n",
    "        root_uniform = local_uniform.copy()\n",
    "        MPI.COMM_WORLD.Bcast(root_uniform, root=0)\n",
    "        if rank != 0:\n",
    "            assert local_uniform[0] != root_uniform[0]\n",
    "\n",
    "\n",
    "def launch(\n",
    "    env, logdir, n_epochs, num_cpu, seed, replay_strategy, policy_save_interval, clip_return,\n",
    "    override_params={}, save_policies=True\n",
    "):\n",
    "    # Fork for multi-CPU MPI implementation.\n",
    "    if num_cpu > 1:\n",
    "        try:\n",
    "            whoami = mpi_fork(num_cpu, ['--bind-to', 'core'])\n",
    "        except CalledProcessError:\n",
    "            # fancy version of mpi call failed, try simple version\n",
    "            whoami = mpi_fork(num_cpu)\n",
    "\n",
    "        if whoami == 'parent':\n",
    "            sys.exit(0)\n",
    "        import baselines.common.tf_util as U\n",
    "        U.single_threaded_session().__enter__()\n",
    "    rank = MPI.COMM_WORLD.Get_rank()\n",
    "\n",
    "    # Configure logging\n",
    "    if rank == 0:\n",
    "        if logdir or logger.get_dir() is None:\n",
    "            logger.configure(dir=logdir)\n",
    "    else:\n",
    "        logger.configure()\n",
    "    logdir = logger.get_dir()\n",
    "    assert logdir is not None\n",
    "    os.makedirs(logdir, exist_ok=True)\n",
    "\n",
    "    # Seed everything.\n",
    "    rank_seed = seed + 1000000 * rank\n",
    "    set_global_seeds(rank_seed)\n",
    "\n",
    "    # Prepare params.\n",
    "    params = config.DEFAULT_PARAMS\n",
    "    params['env_name'] = env\n",
    "    params['replay_strategy'] = replay_strategy\n",
    "    if env in config.DEFAULT_ENV_PARAMS:\n",
    "        params.update(config.DEFAULT_ENV_PARAMS[env])  # merge env-specific parameters in\n",
    "    params.update(**override_params)  # makes it possible to override any parameter\n",
    "    with open(os.path.join(logger.get_dir(), 'params.json'), 'w') as f:\n",
    "        json.dump(params, f)\n",
    "    params = config.prepare_params(params)\n",
    "    config.log_params(params, logger=logger)\n",
    "\n",
    "    if num_cpu == 1:\n",
    "        logger.warn()\n",
    "        logger.warn('*** Warning ***')\n",
    "        logger.warn(\n",
    "            'You are running HER with just a single MPI worker. This will work, but the ' +\n",
    "            'experiments that we report in Plappert et al. (2018, https://arxiv.org/abs/1802.09464) ' +\n",
    "            'were obtained with --num_cpu 19. This makes a significant difference and if you ' +\n",
    "            'are looking to reproduce those results, be aware of this. Please also refer to ' +\n",
    "            'https://github.com/openai/baselines/issues/314 for further details.')\n",
    "        logger.warn('****************')\n",
    "        logger.warn()\n",
    "\n",
    "    dims = config.configure_dims(params)\n",
    "    policy = config.configure_ddpg(dims=dims, params=params, clip_return=clip_return)\n",
    "\n",
    "    rollout_params = {\n",
    "        'exploit': False,\n",
    "        'use_target_net': False,\n",
    "        'use_demo_states': True,\n",
    "        'compute_Q': False,\n",
    "        'T': params['T'],\n",
    "    }\n",
    "\n",
    "    eval_params = {\n",
    "        'exploit': True,\n",
    "        'use_target_net': params['test_with_polyak'],\n",
    "        'use_demo_states': False,\n",
    "        'compute_Q': True,\n",
    "        'T': params['T'],\n",
    "    }\n",
    "\n",
    "    for name in ['T', 'rollout_batch_size', 'gamma', 'noise_eps', 'random_eps']:\n",
    "        rollout_params[name] = params[name]\n",
    "        eval_params[name] = params[name]\n",
    "\n",
    "    rollout_worker = RolloutWorker(params['make_env'], policy, dims, logger, **rollout_params)\n",
    "    rollout_worker.seed(rank_seed)\n",
    "\n",
    "    evaluator = RolloutWorker(params['make_env'], policy, dims, logger, **eval_params)\n",
    "    evaluator.seed(rank_seed)\n",
    "\n",
    "    train(\n",
    "        logdir=logdir, policy=policy, rollout_worker=rollout_worker,\n",
    "        evaluator=evaluator, n_epochs=n_epochs, n_test_rollouts=params['n_test_rollouts'],\n",
    "        n_cycles=params['n_cycles'], n_batches=params['n_batches'],\n",
    "        policy_save_interval=policy_save_interval, save_policies=save_policies)\n",
    "\n",
    "\n",
    "@click.command()\n",
    "@click.option('--env', type=str, default='FetchReach-v0', help='the name of the OpenAI Gym environment that you want to train on')\n",
    "@click.option('--logdir', type=str, default=None, help='the path to where logs and policy pickles should go. If not specified, creates a folder in /tmp/')\n",
    "@click.option('--n_epochs', type=int, default=50, help='the number of training epochs to run')\n",
    "@click.option('--num_cpu', type=int, default=1, help='the number of CPU cores to use (using MPI)')\n",
    "@click.option('--seed', type=int, default=0, help='the random seed used to seed both the environment and the training code')\n",
    "@click.option('--policy_save_interval', type=int, default=5, help='the interval with which policy pickles are saved. If set to 0, only the best and latest policy will be pickled.')\n",
    "@click.option('--replay_strategy', type=click.Choice(['future', 'none']), default='future', help='the HER replay strategy to be used. \"future\" uses HER, \"none\" disables HER.')\n",
    "@click.option('--clip_return', type=int, default=1, help='whether or not returns should be clipped')\n",
    "def main(**kwargs):\n",
    "    rospy.init_node(\"train_fetch_her\")\n",
    "    launch(**kwargs)\n",
    "\n",
    "\n",
    "if __name__ == '__main__':\n",
    "    main()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#407EAF;color:white;\">**train.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**STEP 2**: You will also need to modify the **rollout.py** file. Below you can see an example of the file already modified."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#407EAF;color:white;\">**rollout.py**</p>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from collections import deque\n",
    "\n",
    "import numpy as np\n",
    "import pickle\n",
    "#from mujoco_py import MujocoException\n",
    "\n",
    "from baselines.her.util import convert_episode_to_batch_major, store_args\n",
    "\n",
    "\n",
    "class RolloutWorker:\n",
    "\n",
    "    @store_args\n",
    "    def __init__(self, make_env, policy, dims, logger, T, rollout_batch_size=1,\n",
    "                 exploit=False, use_target_net=False, compute_Q=False, noise_eps=0,\n",
    "                 random_eps=0, history_len=100, render=False, **kwargs):\n",
    "        \"\"\"Rollout worker generates experience by interacting with one or many environments.\n",
    "\n",
    "        Args:\n",
    "            make_env (function): a factory function that creates a new instance of the environment\n",
    "                when called\n",
    "            policy (object): the policy that is used to act\n",
    "            dims (dict of ints): the dimensions for observations (o), goals (g), and actions (u)\n",
    "            logger (object): the logger that is used by the rollout worker\n",
    "            rollout_batch_size (int): the number of parallel rollouts that should be used\n",
    "            exploit (boolean): whether or not to exploit, i.e. to act optimally according to the\n",
    "                current policy without any exploration\n",
    "            use_target_net (boolean): whether or not to use the target net for rollouts\n",
    "            compute_Q (boolean): whether or not to compute the Q values alongside the actions\n",
    "            noise_eps (float): scale of the additive Gaussian noise\n",
    "            random_eps (float): probability of selecting a completely random action\n",
    "            history_len (int): length of history for statistics smoothing\n",
    "            render (boolean): whether or not to render the rollouts\n",
    "        \"\"\"\n",
    "        self.envs = [make_env() for _ in range(rollout_batch_size)]\n",
    "        assert self.T > 0\n",
    "\n",
    "        self.info_keys = [key.replace('info_', '') for key in dims.keys() if key.startswith('info_')]\n",
    "\n",
    "        self.success_history = deque(maxlen=history_len)\n",
    "        self.Q_history = deque(maxlen=history_len)\n",
    "\n",
    "        self.n_episodes = 0\n",
    "        self.g = np.empty((self.rollout_batch_size, self.dims['g']), np.float32)  # goals\n",
    "        self.initial_o = np.empty((self.rollout_batch_size, self.dims['o']), np.float32)  # observations\n",
    "        self.initial_ag = np.empty((self.rollout_batch_size, self.dims['g']), np.float32)  # achieved goals\n",
    "        self.reset_all_rollouts()\n",
    "        self.clear_history()\n",
    "\n",
    "    def reset_rollout(self, i):\n",
    "        \"\"\"Resets the `i`-th rollout environment, re-samples a new goal, and updates the `initial_o`\n",
    "        and `g` arrays accordingly.\n",
    "        \"\"\"\n",
    "        obs = self.envs[i].reset()\n",
    "        self.initial_o[i] = obs['observation']\n",
    "        self.initial_ag[i] = obs['achieved_goal']\n",
    "        self.g[i] = obs['desired_goal']\n",
    "\n",
    "    def reset_all_rollouts(self):\n",
    "        \"\"\"Resets all `rollout_batch_size` rollout workers.\n",
    "        \"\"\"\n",
    "        for i in range(self.rollout_batch_size):\n",
    "            self.reset_rollout(i)\n",
    "\n",
    "    def generate_rollouts(self):\n",
    "        \"\"\"Performs `rollout_batch_size` rollouts in parallel for time horizon `T` with the current\n",
    "        policy acting on it accordingly.\n",
    "        \"\"\"\n",
    "        self.reset_all_rollouts()\n",
    "\n",
    "        # compute observations\n",
    "        o = np.empty((self.rollout_batch_size, self.dims['o']), np.float32)  # observations\n",
    "        ag = np.empty((self.rollout_batch_size, self.dims['g']), np.float32)  # achieved goals\n",
    "        o[:] = self.initial_o\n",
    "        ag[:] = self.initial_ag\n",
    "\n",
    "        # generate episodes\n",
    "        obs, achieved_goals, acts, goals, successes = [], [], [], [], []\n",
    "        info_values = [np.empty((self.T, self.rollout_batch_size, self.dims['info_' + key]), np.float32) for key in self.info_keys]\n",
    "        Qs = []\n",
    "        for t in range(self.T):\n",
    "            policy_output = self.policy.get_actions(\n",
    "                o, ag, self.g,\n",
    "                compute_Q=self.compute_Q,\n",
    "                noise_eps=self.noise_eps if not self.exploit else 0.,\n",
    "                random_eps=self.random_eps if not self.exploit else 0.,\n",
    "                use_target_net=self.use_target_net)\n",
    "\n",
    "            if self.compute_Q:\n",
    "                u, Q = policy_output\n",
    "                Qs.append(Q)\n",
    "            else:\n",
    "                u = policy_output\n",
    "\n",
    "            if u.ndim == 1:\n",
    "                # The non-batched case should still have a reasonable shape.\n",
    "                u = u.reshape(1, -1)\n",
    "\n",
    "            o_new = np.empty((self.rollout_batch_size, self.dims['o']))\n",
    "            ag_new = np.empty((self.rollout_batch_size, self.dims['g']))\n",
    "            success = np.zeros(self.rollout_batch_size)\n",
    "            # compute new states and observations\n",
    "            for i in range(self.rollout_batch_size):\n",
    "                try:\n",
    "                    # We fully ignore the reward here because it will have to be re-computed\n",
    "                    # for HER.\n",
    "                    curr_o_new, _, _, info = self.envs[i].step(u[i])\n",
    "                    if 'is_success' in info:\n",
    "                        success[i] = info['is_success']\n",
    "                    o_new[i] = curr_o_new['observation']\n",
    "                    ag_new[i] = curr_o_new['achieved_goal']\n",
    "                    for idx, key in enumerate(self.info_keys):\n",
    "                        info_values[idx][t, i] = info[key]\n",
    "                    if self.render:\n",
    "                        self.envs[i].render()\n",
    "                #except MujocoException as e:\n",
    "                    #return self.generate_rollouts()\n",
    "                except Exception: \n",
    "                    #pass\n",
    "                    return self.generate_rollouts()\n",
    "\n",
    "            if np.isnan(o_new).any():\n",
    "                self.logger.warning('NaN caught during rollout generation. Trying again...')\n",
    "                self.reset_all_rollouts()\n",
    "                return self.generate_rollouts()\n",
    "\n",
    "            obs.append(o.copy())\n",
    "            achieved_goals.append(ag.copy())\n",
    "            successes.append(success.copy())\n",
    "            acts.append(u.copy())\n",
    "            goals.append(self.g.copy())\n",
    "            o[...] = o_new\n",
    "            ag[...] = ag_new\n",
    "        obs.append(o.copy())\n",
    "        achieved_goals.append(ag.copy())\n",
    "        self.initial_o[:] = o\n",
    "\n",
    "        episode = dict(o=obs,\n",
    "                       u=acts,\n",
    "                       g=goals,\n",
    "                       ag=achieved_goals)\n",
    "        for key, value in zip(self.info_keys, info_values):\n",
    "            episode['info_{}'.format(key)] = value\n",
    "\n",
    "        # stats\n",
    "        successful = np.array(successes)[-1, :]\n",
    "        assert successful.shape == (self.rollout_batch_size,)\n",
    "        success_rate = np.mean(successful)\n",
    "        self.success_history.append(success_rate)\n",
    "        if self.compute_Q:\n",
    "            self.Q_history.append(np.mean(Qs))\n",
    "        self.n_episodes += self.rollout_batch_size\n",
    "\n",
    "        return convert_episode_to_batch_major(episode)\n",
    "\n",
    "    def clear_history(self):\n",
    "        \"\"\"Clears all histories that are used for statistics\n",
    "        \"\"\"\n",
    "        self.success_history.clear()\n",
    "        self.Q_history.clear()\n",
    "\n",
    "    def current_success_rate(self):\n",
    "        return np.mean(self.success_history)\n",
    "\n",
    "    def current_mean_Q(self):\n",
    "        return np.mean(self.Q_history)\n",
    "\n",
    "    def save_policy(self, path):\n",
    "        \"\"\"Pickles the current policy for later inspection.\n",
    "        \"\"\"\n",
    "        with open(path, 'wb') as f:\n",
    "            pickle.dump(self.policy, f)\n",
    "\n",
    "    def logs(self, prefix='worker'):\n",
    "        \"\"\"Generates a dictionary that contains all collected statistics.\n",
    "        \"\"\"\n",
    "        logs = []\n",
    "        logs += [('success_rate', np.mean(self.success_history))]\n",
    "        if self.compute_Q:\n",
    "            logs += [('mean_Q', np.mean(self.Q_history))]\n",
    "        logs += [('episode', self.n_episodes)]\n",
    "\n",
    "        if prefix is not '' and not prefix.endswith('/'):\n",
    "            return [(prefix + '/' + key, val) for key, val in logs]\n",
    "        else:\n",
    "            return logs\n",
    "\n",
    "    def seed(self, seed):\n",
    "        \"\"\"Seeds each environment with a distinct seed derived from the passed in global seed.\n",
    "        \"\"\"\n",
    "        for idx, env in enumerate(self.envs):\n",
    "            env.seed(seed + 1000 * idx)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#407EAF;color:white;\">**rollout.py**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Execute the **train.py** script and see how Fetch starts training."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:red;\">**IMPORTANT:** Remember that, in order to execute the HER algorithm, you will need to do it from a **Python 3 virtual env**, just as you did with the deepq algorithm.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#FF5E00;color:white;\">**End of Exercise 7.4**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you may have noticed, the Fetch arm stays long periods of time without performing any motion. This is because the space where the arm can perform motions is reduced. There are many values that will cause the motion plan to fail, due to different reasons."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you have a look at the Task Environment we have created, the action_space takes values between -1 and 1."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "self.action_space = spaces.Box(-1., 1., shape=(self.n_actions,), dtype='float32')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, for instance, a possible goal position for the end effector could be [0.5, 0, -1] in [x,y,z]."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#FF5E00;color:white;\">**Exercise 7.5**</p>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try to improve the ratio in which motion plans are successfully calculated. For that, you will need to do some modifications in the Task Environment. For instance, you could try to process the values received from the action_space, so that we remove some values that we know will cause the motion plan to fail."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<p style=\"background:#FF5E00;color:white;\">**End of Exercise 7.5**</p>"
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [Root]",
   "language": "python",
   "name": "Python [Root]"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}