05_ExperimentGroundtruth.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "from IPython.core.debugger import set_trace"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import pickle\n",
    "import argparse\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "from pathlib import Path\n",
    "import matplotlib.pyplot as plt\n",
    "plt.ion()\n",
    "import time\n",
    "import datetime\n",
    "import itertools\n",
    "\n",
    "from reacher_def import RotReacherEnv\n",
    "import utils\n",
    "import ce_planner\n",
    "\n",
    "params = {'axes.labelsize': 12,   \n",
    "          'font.size': 12,   \n",
    "          'legend.fontsize': 10,   \n",
    "          'xtick.labelsize': 12,\n",
    "          'ytick.labelsize': 12,   \n",
    "          'text.usetex': False,   \n",
    "          'figure.figsize': [4, 4]}\n",
    "plt.rcParams.update(params)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def make_groundtruth_experiments(suffix, dist_type, num_batches, horizon, gen, unit='m',\n",
    "                                 save_dir=None, render_on=False, plot_on=False, **kwargs):\n",
    "    ''' Here we perform the test experiments in the 60 degree rotation environment in GROUND TRUTH!\n",
    "    We do this mainly as to determine the effects of the planner parameters on the final performance.\n",
    "    Every experiment has a certain amount of episodes of which each has 28 steps, i.e. 2s, max.\n",
    "    We store the outcome in a DataFrame so that we can easily access the results.'''\n",
    "    assert unit in ['m', 'cm']\n",
    "    \n",
    "    if save_dir is None:\n",
    "        save_dir = os.path.join('analysis', 'ex_groundtruth_{}'.format(datetime.datetime.now().strftime(\"%d_%H_%M_%S\")))\n",
    "    else:\n",
    "        save_dir = os.path.join('analysis', save_dir)\n",
    "    utils.make_dir(save_dir)\n",
    "    save_dir = os.path.join(save_dir, str(suffix)+'.pkl')\n",
    "    \n",
    "    env_path = os.path.join(Path().resolve(), 'rot_reacher_humanlike.xml')\n",
    "    \n",
    "    unit_scaling = 100. if unit == 'm' else 1.\n",
    "\n",
    "    data_to_store = pd.DataFrame()\n",
    "    \n",
    "    is_rendering = False\n",
    "    \n",
    "    ### loop through all parameters and perform the experiments\n",
    "    for exp in range(kwargs['n_experiments']):\n",
    "        ex_time = time.time()\n",
    "        print('Running experiment {}...'.format(exp+1))\n",
    "        ### initialize the environment\n",
    "        env = RotReacherEnv(mode='original', \n",
    "                            model_path=env_path, \n",
    "                            max_action=kwargs[\"max_action\"],\n",
    "                            test_mode=True)\n",
    "\n",
    "        observations = np.ones([kwargs['n_trials']*(kwargs['n_env_step_max']+1),2])*np.nan\n",
    "        velocities = np.ones([kwargs['n_trials']*(kwargs['n_env_step_max']+1),2])*np.nan\n",
    "        actions = np.ones([kwargs['n_trials']*(kwargs['n_env_step_max']+1),2])*np.nan\n",
    "        reset_bits = np.ones([kwargs['n_trials']*(kwargs['n_env_step_max']+1),1])*np.nan\n",
    "\n",
    "        global_step_counter = 0\n",
    "\n",
    "        ### now loop through the trials in one experiment\n",
    "        for trial in range(kwargs['n_trials']):\n",
    "            obs = env.reset_model()[:2]\n",
    "            vels = env.sim.get_state().qvel[:2]\n",
    "            observations[global_step_counter] = obs\n",
    "            velocities[global_step_counter] = vels\n",
    "            \n",
    "            new_plan = None\n",
    "            local_step_counter = 0\n",
    "            at_goal_counter = 0\n",
    "            goal = env.goal\n",
    "                            \n",
    "            ### these are the variables we want to store (we rotate our observations such that \n",
    "            ### they are all normalized)\n",
    "            angular_error = np.nan\n",
    "            error = utils.error_helper(obs, goal)\n",
    "            obs_normalized = utils.normalize_obs(obs, goal)\n",
    "            vels_normalized = utils.normalize_obs(vels, goal)\n",
    "            ##### here we store the variables\n",
    "            df = pd.DataFrame({'timepoint': [0], \n",
    "                               'x (cm)': [obs_normalized[0]*100], \n",
    "                               'y (cm)': [obs_normalized[1]*100], \n",
    "                               'vel_x (cm/s)': [vels_normalized[0]*unit_scaling],\n",
    "                               'vel_y (cm/s)': [vels_normalized[1]*unit_scaling],\n",
    "                               'error (cm)': [error*unit_scaling], \n",
    "                               'angular error (rad)': [angular_error], \n",
    "                               'experiment': [exp+1], \n",
    "                               'trial': [trial+1], \n",
    "                               'type': dist_type, \n",
    "                               'n_batch': [num_batches], \n",
    "                               'horizon': [horizon], \n",
    "                               'n_generations': [gen]})\n",
    "            data_to_store = data_to_store.append(df)\n",
    "\n",
    "            ### do steps in the environment until we reach the maximum step size (e.g. 28 or 2s)\n",
    "            while True:\n",
    "                if render_on:\n",
    "                    is_rendering = True\n",
    "                    env.render()\n",
    "                    time.sleep(0.05)\n",
    "\n",
    "                ### choose action according to our ce planner\n",
    "                next_action, new_plan = ce_planner.agent_step(model=None, \n",
    "                                                         ground_truth_env=env,\n",
    "                                                         prev_obs=observations[:global_step_counter+1], \n",
    "                                                         prev_vels=velocities[:global_step_counter+1],\n",
    "                                                         prev_actions=actions[:global_step_counter], \n",
    "                                                         prev_resets=reset_bits[:global_step_counter],\n",
    "                                                         distribution=dist_type,\n",
    "                                                         goal=goal, \n",
    "                                                         horizon=horizon, \n",
    "                                                         n_traj=num_batches, \n",
    "                                                         generations=gen, \n",
    "                                                         plan_params=None, \n",
    "                                                         plot=plot_on,\n",
    "                                                         unit_scaling=unit_scaling,\n",
    "                                                         **kwargs)\n",
    "                ### the next action is subject to motor noise\n",
    "                actions[global_step_counter] = next_action\n",
    "\n",
    "                ### decide if we terminate (either early termination or step limit is reached)\n",
    "                if np.linalg.norm(obs-goal) <= 1.6/unit_scaling:\n",
    "                    at_goal_counter += 1\n",
    "                else:\n",
    "                    at_goal_counter = 0\n",
    "                if at_goal_counter == 7 or local_step_counter == kwargs['n_env_step_max']:\n",
    "                    reset_bits[global_step_counter] = 1\n",
    "                    global_step_counter += 1\n",
    "                    break\n",
    "                else:\n",
    "                    reset_bits[global_step_counter] = 0\n",
    "                    local_step_counter += 1\n",
    "                    global_step_counter += 1\n",
    "\n",
    "                ### we did not terminate early so perform the chosen action in the actual domain\n",
    "                obs, _, _, _ = env.step(next_action)\n",
    "                obs = obs[:2]\n",
    "                vels = env.sim.get_state().qvel[:2]\n",
    "                observations[global_step_counter] = obs\n",
    "                velocities[global_step_counter] = vels\n",
    "\n",
    "                ### determine variables to be measured and stored\n",
    "                angular_error = utils.ang_err_helper(obs, goal)\n",
    "                error = utils.error_helper(obs, goal)\n",
    "                obs_normalized = utils.normalize_obs(obs, goal)\n",
    "                vels_normalized = utils.normalize_obs(vels, goal)\n",
    "                ### here we store the variables\n",
    "                df = pd.DataFrame({'timepoint': [local_step_counter], \n",
    "                               'x (cm)': [obs_normalized[0]*100], \n",
    "                               'y (cm)': [obs_normalized[1]*100], \n",
    "                               'vel_x (cm/s)': [vels_normalized[0]*unit_scaling],\n",
    "                               'vel_y (cm/s)': [vels_normalized[1]*unit_scaling],\n",
    "                               'error (cm)': [error*unit_scaling], \n",
    "                               'angular error (rad)': [angular_error], \n",
    "                               'experiment': [exp+1], \n",
    "                               'trial': [trial+1], \n",
    "                               'type': dist_type, \n",
    "                               'n_batch': [num_batches], \n",
    "                               'horizon': [horizon], \n",
    "                               'n_generations': [gen]})\n",
    "                data_to_store = data_to_store.append(df)\n",
    "                \n",
    "        if is_rendering:\n",
    "            env.close()\n",
    "            is_rendering = False    \n",
    "            \n",
    "        print(('Finished experiment {} ({:.2f} min): | distribution: {} | n: {:d} | ' + \\\n",
    "              'horizon: {:d} | generations: {:d}').format(exp+1, (time.time()-ex_time)/60, dist_type, \n",
    "                                                          num_batches, horizon, gen))\n",
    "        data_to_store.to_pickle(save_dir)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "### This is for the notebook execution!\n",
    "%matplotlib notebook\n",
    "settings = {'n_experiments': 100,\n",
    "            'n_trials': 1,\n",
    "            'n_env_step_max': 28,\n",
    "            'max_action': 0.2,\n",
    "           \n",
    "            ### planner parameters\n",
    "            'elite_fraction': 0.1,\n",
    "            'init_std': 0.3,\n",
    "            'min_std': 0.05}\n",
    "\n",
    "### these are the parameters we want to iterate over\n",
    "planner_distribution = ['independent', 'multivariate']\n",
    "planner_num_batches = [50, 75, 100]\n",
    "planner_horizon = [2, 5, 10]\n",
    "planner_generations = [2, 5, 7]\n",
    "\n",
    "job_list = list(itertools.product(planner_distribution, planner_num_batches, planner_horizon, \n",
    "                                  planner_generations))\n",
    "\n",
    "job_id = 1\n",
    "\n",
    "job = job_list[job_id-1]\n",
    "make_groundtruth_experiments(suffix=job_id, \n",
    "                             dist_type=job[0], \n",
    "                             num_batches=job[1], \n",
    "                             horizon=job[2], \n",
    "                             gen=job[3],\n",
    "                             render_on=False,\n",
    "                             plot_on=False,\n",
    "                             save_dir='groundtruth_test',\n",
    "                             **settings)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "##### this section is for .py file #####\n",
    "if __name__ == '__main__':\n",
    "    parser = argparse.ArgumentParser()\n",
    "    parser.add_argument('job_id', type=int)\n",
    "    parser.add_argument('save_dir', type=str)\n",
    "    parser.add_argument('--render_on', type=bool, default=False)\n",
    "    args = vars(parser.parse_args())\n",
    "    job_id = args['job_id']\n",
    "    render_on = args['render_on']\n",
    "    save_dir = args['save_dir']\n",
    "\n",
    "    settings = {'n_experiments': 100,\n",
    "            'n_trials': 1,\n",
    "            'n_env_step_max': 28,\n",
    "            'max_action': 0.2,\n",
    "           \n",
    "            ### planner parameters\n",
    "            'elite_fraction': 0.1,\n",
    "            'init_std': 0.3,\n",
    "            'min_std': 0.05}\n",
    "\n",
    "    ### these are the parameters we want to iterate over\n",
    "    planner_distribution = ['independent', 'multivariate']\n",
    "    planner_num_batches = [50, 75, 100]\n",
    "    planner_horizon = [2, 5, 10]\n",
    "    planner_generations = [2, 5, 7]\n",
    "\n",
    "    job_list = list(itertools.product(planner_distribution, planner_num_batches, planner_horizon, planner_generations))\n",
    "\n",
    "    job = job_list[job_id-1]\n",
    "    make_groundtruth_experiments(suffix=job_id, \n",
    "                                 dist_type=job[0], \n",
    "                                 num_batches=job[1], \n",
    "                                 horizon=job[2], \n",
    "                                 gen=job[3],\n",
    "                                 render_on=render_on,\n",
    "                                 save_dir=save_dir,\n",
    "                                 **settings)"
   ]
  }
 ],
 "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.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}