tutorial10/gps_notebook.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Guided Policy Search\n",
    "\n",
    "<h2 id=\"tocheading\">Index</h2>\n",
    "<div id=\"toc\"></div>\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/javascript": [
       "$.getScript('https://kmahelona.github.io/ipython_notebook_goodies/ipython_notebook_toc.js')"
      ],
      "text/plain": [
       "<IPython.core.display.Javascript object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "%%javascript\n",
    "$.getScript('https://kmahelona.github.io/ipython_notebook_goodies/ipython_notebook_toc.js')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction\n",
    "Guided Policy Search (GPS) is a technique that transforms the Reinforcement Learning (RL) task of policy search into a Supervised Learning problem, where the training set is generated by a simple trajectory-centric RL algorithm. \n",
    "\n",
    "This algorithm optimizes linear-Gaussian controllers $p_i (u_t | x_t)$. Each $p_i (u_t | x_t)$ succeeds in the task from different initial states which helps the algorithm to generalize to other states from the same distribution. The final policy $\\pi_\\theta(u_t | o_t )$ learned with GPS is only provided with observations $o_t$ of the full state $x_t$, and assumed dynamics are assumed to be unknown. \n",
    "\n",
    "![](gps_illustration.png)\n",
    "\n",
    "We draw sample trajectories $\\tau_i^j$ for each initial state on the physical system by running the corresponding controller $p_i(u_t | x_t)$. The samples are used to fit the dynamics $p_i (x_{t+1} | x_t, u_t)$ that are used to improve the controllers $p_i(u_t | x_t)$, and serve as training data for the policy $\\pi_\\theta(u_t | o_t )$. Within the graph we can observe how there's a loop that alternates between optimizing each trajectory distribution $p_i (\\tau)$ and optimizing the policy $\\pi_\\theta(u_t | o_t )$ to match these trajectory distributions.\n",
    "\n",
    "\n",
    "This work is based on https://arxiv.org/abs/1504.00702. Refer to http://rll.berkeley.edu/gps/ for the original implementation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Definitions and notations\n",
    "\n",
    "| corresponding symbol | definition |\n",
    "|--------|------------|\n",
    "| $p_i(u_t | x_t)$ | linear-Gaussian controllers, they induce the trajectory distributions $p_i (\\tau)$ |\n",
    "| $\\hat{p_i}(u_t | x_t)$ | previous controllers, previous time step t-1 |\n",
    "| $\\pi_\\theta(u_t | o_t )$| final policy learned |\n",
    "| $p_i (\\tau)$| trajectory distribution induced from the linear-Gaussian controllers, guiding distribution |\n",
    "| $\\tau_i^j$ | sample trajectories, sampled from the distribution |\n",
    "| $o_t$ | observations |\n",
    "| $x_t$ | full state |\n",
    "| $p_i (x_{t+1} | x_t, u_t)$ | system dynamics |"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test environment\n",
    "The following test environment will be used for the purpose of implementing GPS.\n",
    "\n",
    "![](gps_testenv.png)"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "import gym\n",
    "\n",
    "env = gym.make('Pendulum-v0')\n",
    "env.reset()\n",
    "for _ in range(1000):\n",
    "    env.render()\n",
    "    env.step(env.action_space.sample()) # take a random action"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## GPS implementation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Utils\n",
    "A set of utility functions used along the code."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "def gauss_fit_joint_prior(pts, mu0, Phi, m, n0, dwts, dX, dU, sig_reg):\n",
    "    \"\"\" Perform Gaussian fit to data with a prior. \"\"\"\n",
    "    # Build weights matrix.\n",
    "    D = np.diag(dwts)\n",
    "    # Compute empirical mean and covariance.\n",
    "    mun = np.sum((pts.T * dwts).T, axis=0)\n",
    "    diff = pts - mun\n",
    "    empsig = diff.T.dot(D).dot(diff)\n",
    "    empsig = 0.5 * (empsig + empsig.T)\n",
    "    # MAP estimate of joint distribution.\n",
    "    N = dwts.shape[0]\n",
    "    mu = mun\n",
    "    sigma = (N * empsig + Phi + (N * m) / (N + m) *\n",
    "             np.outer(mun - mu0, mun - mu0)) / (N + n0)\n",
    "    sigma = 0.5 * (sigma + sigma.T)\n",
    "    # Add sigma regularization.\n",
    "    sigma += sig_reg\n",
    "    # Conditioning to get dynamics.\n",
    "    fd = np.linalg.solve(sigma[:dX, :dX], sigma[:dX, dX:dX+dU]).T\n",
    "    fc = mu[dX:dX+dU] - fd.dot(mu[:dX])\n",
    "    dynsig = sigma[dX:dX+dU, dX:dX+dU] - fd.dot(sigma[:dX, :dX]).dot(fd.T)\n",
    "    dynsig = 0.5 * (dynsig + dynsig.T)\n",
    "    return fd, fc, dynsig\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `Sample`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from gps_pb2 import ACTION\n",
    "\n",
    "\n",
    "class Sample(object):\n",
    "    \"\"\"\n",
    "    Class that handles the representation of a trajectory and stores a\n",
    "    single trajectory.\n",
    "    Note: must be serializable for easy saving, no C++ references!\n",
    "    \"\"\"\n",
    "    def __init__(self, agent):\n",
    "        self.agent = agent\n",
    "\n",
    "        self.T = agent.T\n",
    "        self.dX = agent.dX\n",
    "        self.dU = agent.dU\n",
    "        self.dO = agent.dO\n",
    "        self.dM = agent.dM\n",
    "\n",
    "        # Dictionary containing the sample data from various sensors.\n",
    "        self._data = {}\n",
    "\n",
    "        self._X = np.empty((self.T, self.dX))\n",
    "        self._X.fill(np.nan)\n",
    "        self._obs = np.empty((self.T, self.dO))\n",
    "        self._obs.fill(np.nan)\n",
    "        self._meta = np.empty(self.dM)\n",
    "        self._meta.fill(np.nan)\n",
    "\n",
    "    def set(self, sensor_name, sensor_data, t=None):\n",
    "        \"\"\" Set trajectory data for a particular sensor. \"\"\"\n",
    "        if t is None:\n",
    "            self._data[sensor_name] = sensor_data\n",
    "            self._X.fill(np.nan)  # Invalidate existing X.\n",
    "            self._obs.fill(np.nan)  # Invalidate existing obs.\n",
    "            self._meta.fill(np.nan)  # Invalidate existing meta data.\n",
    "        else:\n",
    "            if sensor_name not in self._data:\n",
    "                self._data[sensor_name] = \\\n",
    "                        np.empty((self.T,) + sensor_data.shape)\n",
    "                self._data[sensor_name].fill(np.nan)\n",
    "            self._data[sensor_name][t, :] = sensor_data\n",
    "            self._X[t, :].fill(np.nan)\n",
    "            self._obs[t, :].fill(np.nan)\n",
    "\n",
    "    def get(self, sensor_name, t=None):\n",
    "        \"\"\" Get trajectory data for a particular sensor. \"\"\"\n",
    "        return (self._data[sensor_name] if t is None\n",
    "                else self._data[sensor_name][t, :])\n",
    "\n",
    "    def get_X(self, t=None):\n",
    "        \"\"\" Get the state. Put it together if not precomputed. \"\"\"\n",
    "        X = self._X if t is None else self._X[t, :]\n",
    "        if np.any(np.isnan(X)):\n",
    "            for data_type in self._data:\n",
    "                if data_type not in self.agent.x_data_types:\n",
    "                    continue\n",
    "                data = (self._data[data_type] if t is None\n",
    "                        else self._data[data_type][t, :])\n",
    "                self.agent.pack_data_x(X, data, data_types=[data_type])\n",
    "        return X\n",
    "\n",
    "    def get_U(self, t=None):\n",
    "        \"\"\" Get the action. \"\"\"\n",
    "        return self._data[ACTION] if t is None else self._data[ACTION][t, :]\n",
    "\n",
    "    def get_obs(self, t=None):\n",
    "        \"\"\" Get the observation. Put it together if not precomputed. \"\"\"\n",
    "        obs = self._obs if t is None else self._obs[t, :]\n",
    "        if np.any(np.isnan(obs)):\n",
    "            for data_type in self._data:\n",
    "                if data_type not in self.agent.obs_data_types:\n",
    "                    continue\n",
    "                if data_type in self.agent.meta_data_types:\n",
    "                    continue\n",
    "                data = (self._data[data_type] if t is None\n",
    "                        else self._data[data_type][t, :])\n",
    "                self.agent.pack_data_obs(obs, data, data_types=[data_type])\n",
    "        return obs\n",
    "\n",
    "    def get_meta(self):\n",
    "        \"\"\" Get the meta data. Put it together if not precomputed. \"\"\"\n",
    "        meta = self._meta\n",
    "        if np.any(np.isnan(meta)):\n",
    "            for data_type in self._data:\n",
    "                if data_type not in self.agent.meta_data_types:\n",
    "                    continue\n",
    "                data = self._data[data_type]\n",
    "                self.agent.pack_data_meta(meta, data, data_types=[data_type])\n",
    "        return meta\n",
    "\n",
    "    # For pickling.\n",
    "    def __getstate__(self):\n",
    "        state = self.__dict__.copy()\n",
    "        state.pop('agent')\n",
    "        return state\n",
    "\n",
    "    # For unpickling.\n",
    "    def __setstate__(self, state):\n",
    "        self.__dict__ = state\n",
    "        self.__dict__['agent'] = None\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `SampleList`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#NOISE = 19  # initially generated by the protocol buffer compiler. \n",
    "from gps_pb2 import NOISE\n",
    "\n",
    "class SampleList(object):\n",
    "    \"\"\" Class that handles writes and reads to sample data. \"\"\"\n",
    "    def __init__(self, samples):\n",
    "        self._samples = samples\n",
    "\n",
    "    def get_X(self, idx=None):\n",
    "        \"\"\" Returns N x T x dX numpy array of states. \"\"\"\n",
    "        if idx is None:\n",
    "            idx = range(len(self._samples))\n",
    "        return np.asarray([self._samples[i].get_X() for i in idx])\n",
    "\n",
    "    def get_U(self, idx=None):\n",
    "        \"\"\" Returns N x T x dU numpy array of actions. \"\"\"\n",
    "        if idx is None:\n",
    "            idx = range(len(self._samples))\n",
    "        return np.asarray([self._samples[i].get_U() for i in idx])\n",
    "\n",
    "    def get_noise(self, idx=None):\n",
    "        \"\"\" Returns N x T x dU numpy array of noise generated during rollouts. \"\"\"\n",
    "        if idx is None:\n",
    "            idx = range(len(self._samples))\n",
    "        return np.asarray([self._samples[i].get(NOISE) for i in idx])\n",
    "\n",
    "    def get_obs(self, idx=None):\n",
    "        \"\"\" Returns N x T x dO numpy array of features. \"\"\"\n",
    "        if idx is None:\n",
    "            idx = range(len(self._samples))\n",
    "        return np.asarray([self._samples[i].get_obs() for i in idx])\n",
    "\n",
    "    def get_samples(self, idx=None):\n",
    "        \"\"\" Returns N sample objects. \"\"\"\n",
    "        if idx is None:\n",
    "            idx = range(len(self._samples))\n",
    "        return [self._samples[i] for i in idx]\n",
    "\n",
    "    def num_samples(self):\n",
    "        \"\"\" Returns number of samples. \"\"\"\n",
    "        return len(self._samples)\n",
    "\n",
    "    # Convenience methods.\n",
    "    def __len__(self):\n",
    "        return self.num_samples()\n",
    "\n",
    "    def __getitem__(self, idx):\n",
    "        return self.get_samples([idx])[0]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Dynamics\n",
    "#### Dynamics superclass: `Dynamics`\n",
    "The dynamical model superclass which assumes dynamics are always linear with $x_t$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import abc\n",
    "\n",
    "class Dynamics(object):\n",
    "    \"\"\" Dynamics superclass. \"\"\"\n",
    "    __metaclass__ = abc.ABCMeta\n",
    "\n",
    "    def __init__(self, hyperparams):\n",
    "    #def __init__(self):\n",
    "        self._hyperparams = hyperparams\n",
    "\n",
    "        # TODO - Currently assuming that dynamics will always be linear\n",
    "        #        with X.\n",
    "        # TODO - Allocate arrays using hyperparams dU, dX, T.\n",
    "\n",
    "        # Fitted dynamics: x_t+1 = Fm * [x_t;u_t] + fv.\n",
    "        self.Fm = np.array(np.nan)\n",
    "        self.fv = np.array(np.nan)\n",
    "        self.dyn_covar = np.array(np.nan)  # Covariance.\n",
    "\n",
    "    @abc.abstractmethod\n",
    "    def update_prior(self, sample):\n",
    "        \"\"\" Update dynamics prior. \"\"\"\n",
    "        raise NotImplementedError(\"Must be implemented in subclass.\")\n",
    "\n",
    "    @abc.abstractmethod\n",
    "    def get_prior(self):\n",
    "        \"\"\" Returns prior object. \"\"\"\n",
    "        raise NotImplementedError(\"Must be implemented in subclass.\")\n",
    "\n",
    "    @abc.abstractmethod\n",
    "    def fit(self, sample_list):\n",
    "        \"\"\" Fit dynamics. \"\"\"\n",
    "        raise NotImplementedError(\"Must be implemented in subclass.\")\n",
    "\n",
    "    def copy(self):\n",
    "        \"\"\" Return a copy of the dynamics estimate. \"\"\"\n",
    "        dyn = type(self)(self._hyperparams)\n",
    "        #dyn = type(self)()\n",
    "        dyn.Fm = np.copy(self.Fm)\n",
    "        dyn.fv = np.copy(self.fv)\n",
    "        dyn.dyn_covar = np.copy(self.dyn_covar)\n",
    "        return dyn"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Gaussian mixture model (GMM) class: `GMM`"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We define the Gaussian mixture model (GMM) class"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import logging\n",
    "import scipy.linalg\n",
    "\n",
    "def logsum(vec, axis=0, keepdims=True):\n",
    "    #TODO: Add a docstring.\n",
    "    maxv = np.max(vec, axis=axis, keepdims=keepdims)\n",
    "    maxv[maxv == -float('inf')] = 0\n",
    "    return np.log(np.sum(np.exp(vec-maxv), axis=axis, keepdims=keepdims)) + maxv\n",
    "\n",
    "class GMM(object):\n",
    "    \"\"\" Gaussian Mixture Model. \"\"\"\n",
    "    def __init__(self, init_sequential=False, eigreg=False, warmstart=True):\n",
    "        self.init_sequential = init_sequential\n",
    "        self.eigreg = eigreg\n",
    "        self.warmstart = warmstart\n",
    "        self.sigma = None\n",
    "\n",
    "    def inference(self, pts):\n",
    "        \"\"\"\n",
    "        Evaluate dynamics prior.\n",
    "        Args:\n",
    "            pts: A N x D array of points.\n",
    "        \"\"\"\n",
    "        # Compute posterior cluster weights.\n",
    "        logwts = self.clusterwts(pts)\n",
    "\n",
    "        # Compute posterior mean and covariance.\n",
    "        mu0, Phi = self.moments(logwts)\n",
    "\n",
    "        # Set hyperparameters.\n",
    "        m = self.N\n",
    "        n0 = m - 2 - mu0.shape[0]\n",
    "\n",
    "        # Normalize.\n",
    "        m = float(m) / self.N\n",
    "        n0 = float(n0) / self.N\n",
    "        return mu0, Phi, m, n0\n",
    "\n",
    "    def estep(self, data):\n",
    "        \"\"\"\n",
    "        Compute log observation probabilities under GMM.\n",
    "        Args:\n",
    "            data: A N x D array of points.\n",
    "        Returns:\n",
    "            logobs: A N x K array of log probabilities (for each point\n",
    "                on each cluster).\n",
    "        \"\"\"\n",
    "        # Constants.\n",
    "        N, D = data.shape\n",
    "        K = self.sigma.shape[0]\n",
    "\n",
    "        logobs = -0.5*np.ones((N, K))*D*np.log(2*np.pi)\n",
    "        for i in range(K):\n",
    "            mu, sigma = self.mu[i], self.sigma[i]\n",
    "            L = scipy.linalg.cholesky(sigma, lower=True)\n",
    "            logobs[:, i] -= np.sum(np.log(np.diag(L)))\n",
    "\n",
    "            diff = (data - mu).T\n",
    "            soln = scipy.linalg.solve_triangular(L, diff, lower=True)\n",
    "            logobs[:, i] -= 0.5*np.sum(soln**2, axis=0)\n",
    "\n",
    "        logobs += self.logmass.T\n",
    "        return logobs\n",
    "\n",
    "    def moments(self, logwts):\n",
    "        \"\"\"\n",
    "        Compute the moments of the cluster mixture with logwts.\n",
    "        Args:\n",
    "            logwts: A K x 1 array of log cluster probabilities.\n",
    "        Returns:\n",
    "            mu: A (D,) mean vector.\n",
    "            sigma: A D x D covariance matrix.\n",
    "        \"\"\"\n",
    "        # Exponentiate.\n",
    "        wts = np.exp(logwts)\n",
    "\n",
    "        # Compute overall mean.\n",
    "        mu = np.sum(self.mu * wts, axis=0)\n",
    "\n",
    "        # Compute overall covariance.\n",
    "        diff = self.mu - np.expand_dims(mu, axis=0)\n",
    "        diff_expand = np.expand_dims(self.mu, axis=1) * \\\n",
    "                np.expand_dims(diff, axis=2)\n",
    "        wts_expand = np.expand_dims(wts, axis=2)\n",
    "        sigma = np.sum((self.sigma + diff_expand) * wts_expand, axis=0)\n",
    "        return mu, sigma\n",
    "\n",
    "    def clusterwts(self, data):\n",
    "        \"\"\"\n",
    "        Compute cluster weights for specified points under GMM.\n",
    "        Args:\n",
    "            data: An N x D array of points\n",
    "        Returns:\n",
    "            A K x 1 array of average cluster log probabilities.\n",
    "        \"\"\"\n",
    "        # Compute probability of each point under each cluster.\n",
    "        logobs = self.estep(data)\n",
    "\n",
    "        # Renormalize to get cluster weights.\n",
    "        logwts = logobs - logsum(logobs, axis=1)\n",
    "\n",
    "        # Average the cluster probabilities.\n",
    "        logwts = logsum(logwts, axis=0) - np.log(data.shape[0])\n",
    "        return logwts.T\n",
    "\n",
    "    def update(self, data, K, max_iterations=100):\n",
    "        \"\"\"\n",
    "        Run EM to update clusters.\n",
    "        Args:\n",
    "            data: An N x D data matrix, where N = number of data points.\n",
    "            K: Number of clusters to use.\n",
    "        \"\"\"\n",
    "        # Constants.\n",
    "        N = data.shape[0]\n",
    "        Do = data.shape[1]\n",
    "\n",
    "        LOGGER.debug('Fitting GMM with %d clusters on %d points', K, N)\n",
    "\n",
    "        if (not self.warmstart or self.sigma is None or\n",
    "                K != self.sigma.shape[0]):\n",
    "            # Initialization.\n",
    "            LOGGER.debug('Initializing GMM.')\n",
    "            self.sigma = np.zeros((K, Do, Do))\n",
    "            self.mu = np.zeros((K, Do))\n",
    "            self.logmass = np.log(1.0 / K) * np.ones((K, 1))\n",
    "            self.mass = (1.0 / K) * np.ones((K, 1))\n",
    "            self.N = data.shape[0]\n",
    "            N = self.N\n",
    "\n",
    "            # Set initial cluster indices.\n",
    "            if not self.init_sequential:\n",
    "                cidx = np.random.randint(0, K, size=(1, N))\n",
    "            else:\n",
    "                raise NotImplementedError()\n",
    "\n",
    "            # Initialize.\n",
    "            for i in range(K):\n",
    "                cluster_idx = (cidx == i)[0]\n",
    "                mu = np.mean(data[cluster_idx, :], axis=0)\n",
    "                diff = (data[cluster_idx, :] - mu).T\n",
    "                sigma = (1.0 / K) * (diff.dot(diff.T))\n",
    "                self.mu[i, :] = mu\n",
    "                self.sigma[i, :, :] = sigma + np.eye(Do) * 2e-6\n",
    "\n",
    "        prevll = -float('inf')\n",
    "        for itr in range(max_iterations):\n",
    "            # E-step: compute cluster probabilities.\n",
    "            logobs = self.estep(data)\n",
    "\n",
    "            # Compute log-likelihood.\n",
    "            ll = np.sum(logsum(logobs, axis=1))\n",
    "            LOGGER.debug('GMM itr %d/%d. Log likelihood: %f',\n",
    "                         itr, max_iterations, ll)\n",
    "            if ll < prevll:\n",
    "                # TODO: Why does log-likelihood decrease sometimes?\n",
    "                LOGGER.debug('Log-likelihood decreased! Ending on itr=%d/%d',\n",
    "                             itr, max_iterations)\n",
    "                break\n",
    "            if np.abs(ll-prevll) < 1e-5*prevll:\n",
    "                LOGGER.debug('GMM converged on itr=%d/%d',\n",
    "                             itr, max_iterations)\n",
    "                break\n",
    "            prevll = ll\n",
    "\n",
    "            # Renormalize to get cluster weights.\n",
    "            logw = logobs - logsum(logobs, axis=1)\n",
    "            assert logw.shape == (N, K)\n",
    "\n",
    "            # Renormalize again to get weights for refitting clusters.\n",
    "            logwn = logw - logsum(logw, axis=0)\n",
    "            assert logwn.shape == (N, K)\n",
    "            w = np.exp(logwn)\n",
    "\n",
    "            # M-step: update clusters.\n",
    "            # Fit cluster mass.\n",
    "            self.logmass = logsum(logw, axis=0).T\n",
    "            self.logmass = self.logmass - logsum(self.logmass, axis=0)\n",
    "            assert self.logmass.shape == (K, 1)\n",
    "            self.mass = np.exp(self.logmass)\n",
    "            # Reboot small clusters.\n",
    "            w[:, (self.mass < (1.0 / K) * 1e-4)[:, 0]] = 1.0 / N\n",
    "            # Fit cluster means.\n",
    "            w_expand = np.expand_dims(w, axis=2)\n",
    "            data_expand = np.expand_dims(data, axis=1)\n",
    "            self.mu = np.sum(w_expand * data_expand, axis=0)\n",
    "            # Fit covariances.\n",
    "            wdata = data_expand * np.sqrt(w_expand)\n",
    "            assert wdata.shape == (N, K, Do)\n",
    "            for i in range(K):\n",
    "                # Compute weighted outer product.\n",
    "                XX = wdata[:, i, :].T.dot(wdata[:, i, :])\n",
    "                mu = self.mu[i, :]\n",
    "                self.sigma[i, :, :] = XX - np.outer(mu, mu)\n",
    "\n",
    "                if self.eigreg:  # Use eigenvalue regularization.\n",
    "                    raise NotImplementedError()\n",
    "                else:  # Use quick and dirty regularization.\n",
    "                    sigma = self.sigma[i, :, :]\n",
    "                    self.sigma[i, :, :] = 0.5 * (sigma + sigma.T) + \\\n",
    "                            1e-6 * np.eye(Do)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Gaussian mixture model (GMM) dynamics: `DynamicsPriorGMM`\n",
    "\n",
    "Optimizing the linear-Gaussian controllers $p_i(u_t | x_t)$ (that induce the trajectories $p_i (\\tau)$) requires fitting the system dynamics $p_i (x_{t+1} | x_t, u_t)$ at each iteration to samples generated on the physical system from the previous controller $\\hat{p_i}(u_t | x_t)$.\n",
    "\n",
    "![](trajectory_optimization.png)\n",
    "\n",
    "The linear-Gaussian dynamics are defined as $p_i (x_{t+1} | x_t, u_t) = \\mathcal{N} (f_{xt}x_t + f_{ut}u_t + f_{ct}, F_t)$, and the data that we obtain from the robot can be viewed as tuples $\\{x_t^i, u_t^i, x_{t+1}^i\\}$. A simple way to fit these linear-Gaussian dynamics is to use linear regression to determine $f_x$, $f_u$ and $f_c$, and fit $F_t$ based on errors however the sample complexity of linear regression scales with the dimensionality of the full state space $x_t$.\n",
    "\n",
    "Although this might be an issue for high-dimensional robotic systems, we can observe that the dynamics at nearby time steps are strongly correlated which means that we can dramatically reduce the sample complexity of the dynamics fitting by bringing in information from previous time steps. This implementation will fit a global model to all of the transitions $\\{x_t^i, u_t^i, x_{t+1}^i\\}$ for all t and all tuples from prior iterations and then use this model as **a prior for fitting the dynamics at each time step**.\n",
    "\n",
    "Below, the definition of GMM prior for dynamics estimation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import copy\n",
    "\n",
    "# DynamicsPriorGMM\n",
    "DYN_PRIOR_GMM = {\n",
    "    'min_samples_per_cluster': 20,\n",
    "    'max_clusters': 50,\n",
    "    'max_samples': 20,\n",
    "    'strength': 1.0,\n",
    "}\n",
    "\n",
    "# As defined in the code examples\n",
    "DYN_PRIOR_GMM_example = {\n",
    "    'min_samples_per_cluster': 40,\n",
    "    'max_clusters': 20,\n",
    "    'max_samples': 20,    \n",
    "    'strength': 1.0,\n",
    "}\n",
    "\n",
    "class DynamicsPriorGMM(object):\n",
    "    \"\"\"\n",
    "    A dynamics prior encoded as a GMM over [x_t, u_t, x_t+1] points.\n",
    "    See:\n",
    "        S. Levine*, C. Finn*, T. Darrell, P. Abbeel, \"End-to-end\n",
    "        training of Deep Visuomotor Policies\", arXiv:1504.00702,\n",
    "        Appendix A.3.\n",
    "    \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        \"\"\"\n",
    "        Hyperparameters:\n",
    "            min_samples_per_cluster: Minimum samples per cluster.\n",
    "            max_clusters: Maximum number of clusters to fit.\n",
    "            max_samples: Maximum number of trajectories to use for\n",
    "                fitting the GMM at any given time.\n",
    "            strength: Adjusts the strength of the prior.\n",
    "        \"\"\"\n",
    "        config = copy.deepcopy(DYN_PRIOR_GMM)\n",
    "        #config = copy.deepcopy(DYN_PRIOR_GMM_example)        \n",
    "        config.update(hyperparams)\n",
    "        self._hyperparams = config\n",
    "        self.X = None\n",
    "        self.U = None\n",
    "        self.gmm = GMM()\n",
    "        self._min_samp = self._hyperparams['min_samples_per_cluster']\n",
    "        self._max_samples = self._hyperparams['max_samples']\n",
    "        self._max_clusters = self._hyperparams['max_clusters']\n",
    "        self._strength = self._hyperparams['strength']\n",
    "\n",
    "        # Should we use copy.min_samples_per_cluster, etc. instead?\n",
    "        #self._min_samp = DYN_PRIOR_GMM_example.min_samples_per_cluster\n",
    "        #self._max_samples = DYN_PRIOR_GMM_example.max_samples\n",
    "        #self._max_clusters = DYN_PRIOR_GMM_example.max_clusters\n",
    "        #self._strength = DYN_PRIOR_GMM_example.strength\n",
    "        \n",
    "\n",
    "    def initial_state(self):\n",
    "        \"\"\" Return dynamics prior for initial time step. \"\"\"\n",
    "        # Compute mean and covariance.\n",
    "        mu0 = np.mean(self.X[:, 0, :], axis=0)\n",
    "        Phi = np.diag(np.var(self.X[:, 0, :], axis=0))\n",
    "\n",
    "        # Factor in multiplier.\n",
    "        n0 = self.X.shape[2] * self._strength\n",
    "        m = self.X.shape[2] * self._strength\n",
    "\n",
    "        # Multiply Phi by m (since it was normalized before).\n",
    "        Phi = Phi * m\n",
    "        return mu0, Phi, m, n0\n",
    "\n",
    "    def update(self, X, U):\n",
    "        \"\"\"\n",
    "        Update prior with additional data.\n",
    "        Args:\n",
    "            X: A N x T x dX matrix of sequential state data.\n",
    "            U: A N x T x dU matrix of sequential control data.\n",
    "        \"\"\"\n",
    "        # Constants.\n",
    "        T = X.shape[1] - 1\n",
    "\n",
    "        # Append data to dataset.\n",
    "        if self.X is None:\n",
    "            self.X = X\n",
    "        else:\n",
    "            self.X = np.concatenate([self.X, X], axis=0)\n",
    "\n",
    "        if self.U is None:\n",
    "            self.U = U\n",
    "        else:\n",
    "            self.U = np.concatenate([self.U, U], axis=0)\n",
    "\n",
    "        # Remove excess samples from dataset.\n",
    "        start = max(0, self.X.shape[0] - self._max_samples + 1)\n",
    "        self.X = self.X[start:, :]\n",
    "        self.U = self.U[start:, :]\n",
    "\n",
    "        # Compute cluster dimensionality.\n",
    "        Do = X.shape[2] + U.shape[2] + X.shape[2]  #TODO: Use Xtgt.\n",
    "\n",
    "        # Create dataset.\n",
    "        N = self.X.shape[0]\n",
    "        xux = np.reshape(\n",
    "            np.c_[self.X[:, :T, :], self.U[:, :T, :], self.X[:, 1:(T+1), :]],\n",
    "            [T * N, Do]\n",
    "        )\n",
    "\n",
    "        # Choose number of clusters.\n",
    "        K = int(max(2, min(self._max_clusters,\n",
    "                           np.floor(float(N * T) / self._min_samp))))\n",
    "        LOGGER.debug('Generating %d clusters for dynamics GMM.', K)\n",
    "\n",
    "        # Update GMM.\n",
    "        self.gmm.update(xux, K)\n",
    "\n",
    "    def eval(self, Dx, Du, pts):\n",
    "        \"\"\"\n",
    "        Evaluate prior.\n",
    "        Args:\n",
    "            pts: A N x Dx+Du+Dx matrix.\n",
    "        \"\"\"\n",
    "        # Construct query data point by rearranging entries and adding\n",
    "        # in reference.\n",
    "        assert pts.shape[1] == Dx + Du + Dx\n",
    "\n",
    "        # Perform query and fix mean.\n",
    "        mu0, Phi, m, n0 = self.gmm.inference(pts)\n",
    "\n",
    "        # Factor in multiplier.\n",
    "        n0 = n0 * self._strength\n",
    "        m = m * self._strength\n",
    "\n",
    "        # Multiply Phi by m (since it was normalized before).\n",
    "        Phi *= m\n",
    "        return mu0, Phi, m, n0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Linear regression dynamics with an arbitrary prior: `DynamicsLRPrior`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#regularization = 1e-6\n",
    "\n",
    "class DynamicsLRPrior(Dynamics):\n",
    "    \"\"\" Dynamics with linear regression, with arbitrary prior. \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "    #def __init__(self):\n",
    "        Dynamics.__init__(self, hyperparams)\n",
    "        #Dynamics.__init__(self)\n",
    "        self.Fm = None\n",
    "        self.fv = None\n",
    "        self.dyn_covar = None        \n",
    "        #self.prior = DynamicsPriorGMM() # Refer to the corresponding class for a deeper understanding\n",
    "        self.prior = self._hyperparams['prior']['type'](self._hyperparams['prior'])\n",
    "\n",
    "    def update_prior(self, samples):\n",
    "        \"\"\" Update dynamics prior. \"\"\"\n",
    "        X = samples.get_X()\n",
    "        U = samples.get_U()\n",
    "        self.prior.update(X, U)\n",
    "\n",
    "    def get_prior(self):\n",
    "        \"\"\" Return the dynamics prior. \"\"\"\n",
    "        return self.prior\n",
    "\n",
    "    #TODO: Merge this with DynamicsLR.fit - lots of duplicated code.\n",
    "    def fit(self, X, U):\n",
    "        \"\"\" Fit dynamics. \"\"\"\n",
    "        N, T, dX = X.shape\n",
    "        dU = U.shape[2]\n",
    "\n",
    "        if N == 1:\n",
    "            raise ValueError(\"Cannot fit dynamics on 1 sample\")\n",
    "\n",
    "        self.Fm = np.zeros([T, dX, dX+dU])\n",
    "        self.fv = np.zeros([T, dX])\n",
    "        self.dyn_covar = np.zeros([T, dX, dX])\n",
    "\n",
    "        it = slice(dX+dU)\n",
    "        ip = slice(dX+dU, dX+dU+dX)\n",
    "        # Fit dynamics with least squares regression.\n",
    "        dwts = (1.0 / N) * np.ones(N)\n",
    "        for t in range(T - 1):\n",
    "            Ys = np.c_[X[:, t, :], U[:, t, :], X[:, t+1, :]]\n",
    "            # Obtain Normal-inverse-Wishart prior.\n",
    "            mu0, Phi, mm, n0 = self.prior.eval(dX, dU, Ys)\n",
    "            sig_reg = np.zeros((dX+dU+dX, dX+dU+dX))\n",
    "            sig_reg[it, it] = self._hyperparams['regularization']\n",
    "            #sig_reg[it, it] = regularization\n",
    "            Fm, fv, dyn_covar = gauss_fit_joint_prior(Ys,\n",
    "                        mu0, Phi, mm, n0, dwts, dX+dU, dX, sig_reg)\n",
    "            self.Fm[t, :, :] = Fm\n",
    "            self.fv[t, :] = fv\n",
    "            self.dyn_covar[t, :, :] = dyn_covar\n",
    "        return self.Fm, self.fv, self.dyn_covar\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Algorithm\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `BundleType`\n",
    "General utility functions and classes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class BundleType(object):\n",
    "    \"\"\"\n",
    "    This class bundles many fields, similar to a record or a mutable\n",
    "    namedtuple.\n",
    "    \"\"\"\n",
    "    def __init__(self, variables):\n",
    "        for var, val in variables.items():\n",
    "            object.__setattr__(self, var, val)\n",
    "\n",
    "    # Freeze fields so new ones cannot be set.\n",
    "    def __setattr__(self, key, value):\n",
    "        if not hasattr(self, key):\n",
    "            raise AttributeError(\"%r has no attribute %s\" % (self, key))\n",
    "        object.__setattr__(self, key, value)\n",
    "\n",
    "\n",
    "def check_shape(value, expected_shape, name=''):\n",
    "    \"\"\"\n",
    "    Throws a ValueError if value.shape != expected_shape.\n",
    "    Args:\n",
    "        value: Matrix to shape check.\n",
    "        expected_shape: A tuple or list of integers.\n",
    "        name: An optional name to add to the exception message.\n",
    "    \"\"\"\n",
    "    if value.shape != tuple(expected_shape):\n",
    "        raise ValueError('Shape mismatch %s: Expected %s, got %s' %\n",
    "                         (name, str(expected_shape), str(value.shape)))\n",
    "\n",
    "\n",
    "def finite_differences(func, inputs, func_output_shape=(), epsilon=1e-5):\n",
    "    \"\"\"\n",
    "    Computes gradients via finite differences.\n",
    "    derivative = (func(x+epsilon) - func(x-epsilon)) / (2*epsilon)\n",
    "    Args:\n",
    "        func: Function to compute gradient of. Inputs and outputs can be\n",
    "            arbitrary dimension.\n",
    "        inputs: Vector value to compute gradient at.\n",
    "        func_output_shape: Shape of the output of func. Default is\n",
    "            empty-tuple, which works for scalar-valued functions.\n",
    "        epsilon: Difference to use for computing gradient.\n",
    "    Returns:\n",
    "        Gradient vector of each dimension of func with respect to each\n",
    "        dimension of input.\n",
    "    \"\"\"\n",
    "    gradient = np.zeros(inputs.shape+func_output_shape)\n",
    "    for idx, _ in np.ndenumerate(inputs):\n",
    "        test_input = np.copy(inputs)\n",
    "        test_input[idx] += epsilon\n",
    "        obj_d1 = func(test_input)\n",
    "        assert obj_d1.shape == func_output_shape\n",
    "        test_input = np.copy(inputs)\n",
    "        test_input[idx] -= epsilon\n",
    "        obj_d2 = func(test_input)\n",
    "        assert obj_d2.shape == func_output_shape\n",
    "        diff = (obj_d1 - obj_d2) / (2 * epsilon)\n",
    "        gradient[idx] += diff\n",
    "    return gradient"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `IterationData` and `TrajectoryInfo`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class IterationData(BundleType):\n",
    "    \"\"\" Collection of iteration variables. \"\"\"\n",
    "    def __init__(self):\n",
    "        variables = {\n",
    "            'sample_list': None,  # List of samples for the current iteration.\n",
    "            'traj_info': None,  # Current TrajectoryInfo object.\n",
    "            'pol_info': None,  # Current PolicyInfo object.\n",
    "            'traj_distr': None,  # Initial trajectory distribution.\n",
    "            'new_traj_distr': None, # Updated trajectory distribution.\n",
    "            'cs': None,  # Sample costs of the current iteration.\n",
    "            'step_mult': 1.0,  # KL step multiplier for the current iteration.\n",
    "            'eta': 1.0,  # Dual variable used in LQR backward pass.\n",
    "        }\n",
    "        BundleType.__init__(self, variables)\n",
    "\n",
    "\n",
    "class TrajectoryInfo(BundleType):\n",
    "    \"\"\" Collection of trajectory-related variables. \"\"\"\n",
    "    def __init__(self):\n",
    "        variables = {\n",
    "            'dynamics': None,  # Dynamics object for the current iteration.\n",
    "            'x0mu': None,  # Mean for the initial state, used by the dynamics.\n",
    "            'x0sigma': None,  # Covariance for the initial state distribution.\n",
    "            'cc': None,  # Cost estimate constant term.\n",
    "            'cv': None,  # Cost estimate vector term.\n",
    "            'Cm': None,  # Cost estimate matrix term.\n",
    "            'last_kl_step': float('inf'),  # KL step of the previous iteration.\n",
    "        }\n",
    "        BundleType.__init__(self, variables)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `PolicyInfo`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class PolicyInfo(BundleType):\n",
    "    \"\"\" Collection of policy-related variables. \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        T, dU, dX = hyperparams['T'], hyperparams['dU'], hyperparams['dX']\n",
    "        variables = {\n",
    "            'lambda_k': np.zeros((T, dU)),  # Dual variables.\n",
    "            'lambda_K': np.zeros((T, dU, dX)),  # Dual variables.\n",
    "            'pol_wt': hyperparams['init_pol_wt'] * np.ones(T),  # Policy weight.\n",
    "            'pol_mu': None,  # Mean of the current policy output.\n",
    "            'pol_sig': None,  # Covariance of the current policy output.\n",
    "            'pol_K': np.zeros((T, dU, dX)),  # Policy linearization.\n",
    "            'pol_k': np.zeros((T, dU)),  # Policy linearization.\n",
    "            'pol_S': np.zeros((T, dU, dU)),  # Policy linearization covariance.\n",
    "            'chol_pol_S': np.zeros((T, dU, dU)),  # Cholesky decomp of covar.\n",
    "            'prev_kl': None,  # Previous KL divergence.\n",
    "            'init_kl': None,  # The initial KL divergence, before the iteration.\n",
    "            'policy_samples': [],  # List of current policy samples.\n",
    "            'policy_prior': None,  # Current prior for policy linearization.\n",
    "        }\n",
    "        BundleType.__init__(self, variables)\n",
    "\n",
    "    def traj_distr(self):\n",
    "        \"\"\" Create a trajectory distribution object from policy info. \"\"\"\n",
    "        T, dU, dX = self.pol_K.shape\n",
    "        # Compute inverse policy covariances.\n",
    "        inv_pol_S = np.empty_like(self.chol_pol_S)\n",
    "        for t in range(T):\n",
    "            inv_pol_S[t, :, :] = np.linalg.solve(\n",
    "                self.chol_pol_S[t, :, :],\n",
    "                np.linalg.solve(self.chol_pol_S[t, :, :].T, np.eye(dU))\n",
    "            )\n",
    "        return LinearGaussianPolicy(self.pol_K, self.pol_k, self.pol_S,\n",
    "                self.chol_pol_S, inv_pol_S)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Algorithm superclass: `Algorithm`\n",
    "Base algorithm class"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "import random\n",
    "\n",
    "def extract_condition(hyperparams, m):\n",
    "    \"\"\"\n",
    "    Pull the relevant hyperparameters corresponding to the specified\n",
    "    condition, and return a new hyperparameter dictionary.\n",
    "    \"\"\"\n",
    "    return {var: val[m] if isinstance(val, list) else val\n",
    "            for var, val in hyperparams.items()}\n",
    "\n",
    "# Algorithm\n",
    "ALG = {\n",
    "    'inner_iterations': 1,  # Number of iterations.\n",
    "    'min_eta': 1e-5,  # Minimum initial lagrange multiplier in DGD for\n",
    "                      # trajectory optimization.\n",
    "    'kl_step':0.2,\n",
    "    'min_step_mult':0.01,\n",
    "    'max_step_mult':10.0,\n",
    "    'min_mult': 0.1,\n",
    "    'max_mult': 5.0,\n",
    "    # Trajectory settings.\n",
    "    'initial_state_var':1e-6,\n",
    "    'init_traj_distr': None,  # A list of initial LinearGaussianPolicy\n",
    "                              # objects for each condition.\n",
    "    # Trajectory optimization.\n",
    "    'traj_opt': None,\n",
    "    # Weight of maximum entropy term in trajectory optimization.\n",
    "    'max_ent_traj': 0.0,\n",
    "    # Dynamics hyperaparams.\n",
    "    'dynamics': None,\n",
    "    # Costs.\n",
    "    'cost': None,  # A list of Cost objects for each condition.\n",
    "    # Whether or not to sample with neural net policy (only for badmm/mdgps).\n",
    "    'sample_on_policy': False,\n",
    "    # Inidicates if the algorithm requires fitting of the dynamics.\n",
    "    'fit_dynamics': True,    \n",
    "}\n",
    "\n",
    "class Algorithm(object):\n",
    "    \"\"\" Algorithm superclass. \"\"\"\n",
    "    __metaclass__ = abc.ABCMeta\n",
    "\n",
    "    def __init__(self, hyperparams):\n",
    "        config = copy.deepcopy(ALG)\n",
    "        config.update(hyperparams)\n",
    "        self._hyperparams = config\n",
    "\n",
    "        if 'train_conditions' in hyperparams:\n",
    "            self._cond_idx = hyperparams['train_conditions']\n",
    "            self.M = len(self._cond_idx)\n",
    "        else:\n",
    "            self.M = hyperparams['conditions']\n",
    "            self._cond_idx = range(self.M)\n",
    "            self._hyperparams['train_conditions'] = self._cond_idx\n",
    "            self._hyperparams['test_conditions'] = self._cond_idx\n",
    "        self.iteration_count = 0\n",
    "\n",
    "        # Grab a few values from the agent.\n",
    "        agent = self._hyperparams['agent']\n",
    "        self.T = self._hyperparams['T'] = agent.T\n",
    "        self.dU = self._hyperparams['dU'] = agent.dU\n",
    "        self.dX = self._hyperparams['dX'] = agent.dX\n",
    "        self.dO = self._hyperparams['dO'] = agent.dO\n",
    "\n",
    "        init_traj_distr = config['init_traj_distr']\n",
    "        init_traj_distr['x0'] = agent.x0\n",
    "        init_traj_distr['dX'] = agent.dX\n",
    "        init_traj_distr['dU'] = agent.dU\n",
    "        del self._hyperparams['agent']  # Don't want to pickle this.\n",
    "\n",
    "        # IterationData objects for each condition.\n",
    "        self.cur = [IterationData() for _ in range(self.M)]\n",
    "        self.prev = [IterationData() for _ in range(self.M)]\n",
    "\n",
    "        if self._hyperparams['fit_dynamics']:\n",
    "            dynamics = self._hyperparams['dynamics']\n",
    "\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].traj_info = TrajectoryInfo()\n",
    "            if self._hyperparams['fit_dynamics']:\n",
    "                self.cur[m].traj_info.dynamics = dynamics['type'](dynamics)\n",
    "            init_traj_distr = extract_condition(\n",
    "                self._hyperparams['init_traj_distr'], self._cond_idx[m]\n",
    "            )\n",
    "            self.cur[m].traj_distr = init_traj_distr['type'](init_traj_distr)\n",
    "\n",
    "        self.traj_opt = hyperparams['traj_opt']['type'](\n",
    "            hyperparams['traj_opt']\n",
    "        )\n",
    "        if type(hyperparams['cost']) == list:\n",
    "            self.cost = [\n",
    "                hyperparams['cost'][i]['type'](hyperparams['cost'][i])\n",
    "                for i in range(self.M)\n",
    "            ]\n",
    "        else:\n",
    "            self.cost = [\n",
    "                hyperparams['cost']['type'](hyperparams['cost'])\n",
    "                for _ in range(self.M)\n",
    "            ]\n",
    "        self.base_kl_step = self._hyperparams['kl_step']\n",
    "\n",
    "    @abc.abstractmethod\n",
    "    def iteration(self, sample_list):\n",
    "        \"\"\" Run iteration of the algorithm. \"\"\"\n",
    "        raise NotImplementedError(\"Must be implemented in subclass\")\n",
    "\n",
    "    def _update_dynamics(self):\n",
    "        \"\"\"\n",
    "        Instantiate dynamics objects and update prior. Fit dynamics to\n",
    "        current samples.\n",
    "        \"\"\"\n",
    "        for m in range(self.M):\n",
    "            cur_data = self.cur[m].sample_list\n",
    "            X = cur_data.get_X()\n",
    "            U = cur_data.get_U()\n",
    "\n",
    "            # Update prior and fit dynamics.\n",
    "            self.cur[m].traj_info.dynamics.update_prior(cur_data)\n",
    "            self.cur[m].traj_info.dynamics.fit(X, U)\n",
    "\n",
    "            # Fit x0mu/x0sigma.\n",
    "            x0 = X[:, 0, :]\n",
    "            x0mu = np.mean(x0, axis=0)\n",
    "            self.cur[m].traj_info.x0mu = x0mu\n",
    "            self.cur[m].traj_info.x0sigma = np.diag(\n",
    "                np.maximum(np.var(x0, axis=0),\n",
    "                           self._hyperparams['initial_state_var'])\n",
    "            )\n",
    "\n",
    "            prior = self.cur[m].traj_info.dynamics.get_prior()\n",
    "            if prior:\n",
    "                mu0, Phi, priorm, n0 = prior.initial_state()\n",
    "                N = len(cur_data)\n",
    "                self.cur[m].traj_info.x0sigma += \\\n",
    "                        Phi + (N*priorm) / (N+priorm) * \\\n",
    "                        np.outer(x0mu-mu0, x0mu-mu0) / (N+n0)\n",
    "\n",
    "    def _update_trajectories(self):\n",
    "        \"\"\"\n",
    "        Compute new linear Gaussian controllers.\n",
    "        \"\"\"\n",
    "        if not hasattr(self, 'new_traj_distr'):\n",
    "            self.new_traj_distr = [\n",
    "                self.cur[cond].traj_distr for cond in range(self.M)\n",
    "            ]\n",
    "        for cond in range(self.M):\n",
    "            self.new_traj_distr[cond], self.cur[cond].eta = \\\n",
    "                    self.traj_opt.update(cond, self)\n",
    "\n",
    "    def _eval_cost(self, cond):\n",
    "        \"\"\"\n",
    "        Evaluate costs for all samples for a condition.\n",
    "        Args:\n",
    "            cond: Condition to evaluate cost on.\n",
    "        \"\"\"\n",
    "        # Constants.\n",
    "        T, dX, dU = self.T, self.dX, self.dU\n",
    "        N = len(self.cur[cond].sample_list)\n",
    "\n",
    "        # Compute cost.\n",
    "        cs = np.zeros((N, T))\n",
    "        cc = np.zeros((N, T))\n",
    "        cv = np.zeros((N, T, dX+dU))\n",
    "        Cm = np.zeros((N, T, dX+dU, dX+dU))\n",
    "        for n in range(N):\n",
    "            sample = self.cur[cond].sample_list[n]\n",
    "            # Get costs.\n",
    "            l, lx, lu, lxx, luu, lux = self.cost[cond].eval(sample)\n",
    "            cc[n, :] = l\n",
    "            cs[n, :] = l\n",
    "\n",
    "            # Assemble matrix and vector.\n",
    "            cv[n, :, :] = np.c_[lx, lu]\n",
    "            Cm[n, :, :, :] = np.concatenate(\n",
    "                (np.c_[lxx, np.transpose(lux, [0, 2, 1])], np.c_[lux, luu]),\n",
    "                axis=1\n",
    "            )\n",
    "\n",
    "            # Adjust for expanding cost around a sample.\n",
    "            X = sample.get_X()\n",
    "            U = sample.get_U()\n",
    "            yhat = np.c_[X, U]\n",
    "            rdiff = -yhat\n",
    "            rdiff_expand = np.expand_dims(rdiff, axis=2)\n",
    "            cv_update = np.sum(Cm[n, :, :, :] * rdiff_expand, axis=1)\n",
    "            cc[n, :] += np.sum(rdiff * cv[n, :, :], axis=1) + 0.5 * \\\n",
    "                    np.sum(rdiff * cv_update, axis=1)\n",
    "            cv[n, :, :] += cv_update\n",
    "\n",
    "        # Fill in cost estimate.\n",
    "        self.cur[cond].traj_info.cc = np.mean(cc, 0)  # Constant term (scalar).\n",
    "        self.cur[cond].traj_info.cv = np.mean(cv, 0)  # Linear term (vector).\n",
    "        self.cur[cond].traj_info.Cm = np.mean(Cm, 0)  # Quadratic term (matrix).\n",
    "\n",
    "        self.cur[cond].cs = cs  # True value of cost.\n",
    "\n",
    "    def _advance_iteration_variables(self):\n",
    "        \"\"\"\n",
    "        Move all 'cur' variables to 'prev', and advance iteration\n",
    "        counter.\n",
    "        \"\"\"\n",
    "        self.iteration_count += 1\n",
    "        self.prev = copy.deepcopy(self.cur)\n",
    "        # TODO: change IterationData to reflect new stuff better\n",
    "        for m in range(self.M):\n",
    "            self.prev[m].new_traj_distr = self.new_traj_distr[m]\n",
    "        self.cur = [IterationData() for _ in range(self.M)]\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].traj_info = TrajectoryInfo()\n",
    "            self.cur[m].traj_info.dynamics = copy.deepcopy(self.prev[m].traj_info.dynamics)\n",
    "            self.cur[m].step_mult = self.prev[m].step_mult\n",
    "            self.cur[m].eta = self.prev[m].eta\n",
    "            self.cur[m].traj_distr = self.new_traj_distr[m]\n",
    "        delattr(self, 'new_traj_distr')\n",
    "\n",
    "    def _set_new_mult(self, predicted_impr, actual_impr, m):\n",
    "        \"\"\"\n",
    "        Adjust step size multiplier according to the predicted versus\n",
    "        actual improvement.\n",
    "        \"\"\"\n",
    "        # Model improvement as I = predicted_dI * KL + penalty * KL^2,\n",
    "        # where predicted_dI = pred/KL and penalty = (act-pred)/(KL^2).\n",
    "        # Optimize I w.r.t. KL: 0 = predicted_dI + 2 * penalty * KL =>\n",
    "        # KL' = (-predicted_dI)/(2*penalty) = (pred/2*(pred-act)) * KL.\n",
    "        # Therefore, the new multiplier is given by pred/2*(pred-act).\n",
    "        new_mult = predicted_impr / (2.0 * max(1e-4,\n",
    "                                               predicted_impr - actual_impr))\n",
    "        new_mult = max(0.1, min(5.0, new_mult))\n",
    "        new_step = max(\n",
    "            min(new_mult * self.cur[m].step_mult,\n",
    "                self._hyperparams['max_step_mult']),\n",
    "            self._hyperparams['min_step_mult']\n",
    "        )\n",
    "        self.cur[m].step_mult = new_step\n",
    "\n",
    "        if new_mult > 1:\n",
    "            LOGGER.debug('Increasing step size multiplier to %f', new_step)\n",
    "        else:\n",
    "            LOGGER.debug('Decreasing step size multiplier to %f', new_step)\n",
    "\n",
    "    def _measure_ent(self, m):\n",
    "        \"\"\" Measure the entropy of the current trajectory. \"\"\"\n",
    "        ent = 0\n",
    "        for t in range(self.T):\n",
    "            ent = ent + np.sum(\n",
    "                np.log(np.diag(self.cur[m].traj_distr.chol_pol_covar[t, :, :]))\n",
    "            )\n",
    "        return ent\n",
    "\n",
    "    # For pickling.\n",
    "    def __getstate__(self):\n",
    "        state = self.__dict__.copy()\n",
    "        state['_random_state'] = random.getstate()\n",
    "        state['_np_random_state'] = np.random.get_state()\n",
    "        return state\n",
    "\n",
    "    # For unpickling.\n",
    "    def __setstate__(self, state):\n",
    "        self.__dict__ = state\n",
    "        random.setstate(state.pop('_random_state'))\n",
    "        np.random.set_state(state.pop('_np_random_state'))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `AlgorithmBADMM`\n",
    "BADMM-based Guided Policy Search (GPS) algorithm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "import scipy as sp\n",
    "\n",
    "# AlgorithmBADMM\n",
    "ALG_BADMM = {\n",
    "    'inner_iterations': 4,\n",
    "    'policy_dual_rate': 0.1,\n",
    "    'policy_dual_rate_covar': 0.0,\n",
    "    'fixed_lg_step': 0,\n",
    "    'lg_step_schedule': 10.0,\n",
    "    'ent_reg_schedule': 0.0,\n",
    "    'init_pol_wt': 0.01,\n",
    "    'policy_sample_mode': 'add',\n",
    "    'exp_step_increase': 2.0,\n",
    "    'exp_step_decrease': 0.5,\n",
    "    'exp_step_upper': 0.5,\n",
    "    'exp_step_lower': 1.0,\n",
    "}\n",
    "\n",
    "class AlgorithmBADMM(Algorithm):\n",
    "    \"\"\"\n",
    "    Sample-based joint policy learning and trajectory optimization with\n",
    "    BADMM-based guided policy search algorithm.\n",
    "    \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        config = copy.deepcopy(ALG_BADMM)\n",
    "        config.update(hyperparams)\n",
    "        Algorithm.__init__(self, config)\n",
    "\n",
    "        policy_prior = self._hyperparams['policy_prior']\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].pol_info = PolicyInfo(self._hyperparams)\n",
    "            self.cur[m].pol_info.policy_prior = \\\n",
    "                    policy_prior['type'](policy_prior)\n",
    "\n",
    "        self.policy_opt = self._hyperparams['policy_opt']['type'](\n",
    "            self._hyperparams['policy_opt'], self.dO, self.dU\n",
    "        )\n",
    "\n",
    "    def iteration(self, sample_lists):\n",
    "        \"\"\"\n",
    "        Run iteration of BADMM-based guided policy search.\n",
    "\n",
    "        Args:\n",
    "            sample_lists: List of SampleList objects for each condition.\n",
    "        \"\"\"\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].sample_list = sample_lists[m]\n",
    "\n",
    "        self._set_interp_values()\n",
    "        self._update_dynamics()  # Update dynamics model using all sample.\n",
    "        self._update_step_size()  # KL Divergence step size.\n",
    "\n",
    "        for m in range(self.M):\n",
    "            # save initial kl for debugging / visualization\n",
    "            self.cur[m].pol_info.init_kl = self._policy_kl(m)[0]\n",
    "\n",
    "        # Run inner loop to compute new policies.\n",
    "        for inner_itr in range(self._hyperparams['inner_iterations']):\n",
    "            #TODO: Could start from init controller.\n",
    "            if self.iteration_count > 0 or inner_itr > 0:\n",
    "                # Update the policy.\n",
    "                self._update_policy(inner_itr)\n",
    "            for m in range(self.M):\n",
    "                self._update_policy_fit(m)  # Update policy priors.\n",
    "            if self.iteration_count > 0 or inner_itr > 0:\n",
    "                step = (inner_itr == self._hyperparams['inner_iterations'] - 1)\n",
    "                # Update dual variables.\n",
    "                for m in range(self.M):\n",
    "                    self._policy_dual_step(m, step=step)\n",
    "            self._update_trajectories()\n",
    "\n",
    "        self._advance_iteration_variables()\n",
    "\n",
    "    def _set_interp_values(self):\n",
    "        \"\"\"\n",
    "        Use iteration-based interpolation to set values of some\n",
    "        schedule-based parameters.\n",
    "        \"\"\"\n",
    "        # Compute temporal interpolation value.\n",
    "        t = min((self.iteration_count + 1.0) /\n",
    "                (self._hyperparams['iterations'] - 1), 1)\n",
    "        # Perform iteration-based interpolation of entropy penalty.\n",
    "        if type(self._hyperparams['ent_reg_schedule']) in (int, float):\n",
    "            self.policy_opt.set_ent_reg(self._hyperparams['ent_reg_schedule'])\n",
    "        else:\n",
    "            sch = self._hyperparams['ent_reg_schedule']\n",
    "            self.policy_opt.set_ent_reg(\n",
    "                np.exp(np.interp(t, np.linspace(0, 1, num=len(sch)),\n",
    "                                 np.log(sch)))\n",
    "            )\n",
    "        # Perform iteration-based interpolation of Lagrange multiplier.\n",
    "        if type(self._hyperparams['lg_step_schedule']) in (int, float):\n",
    "            self._hyperparams['lg_step'] = self._hyperparams['lg_step_schedule']\n",
    "        else:\n",
    "            sch = self._hyperparams['lg_step_schedule']\n",
    "            self._hyperparams['lg_step'] = np.exp(\n",
    "                np.interp(t, np.linspace(0, 1, num=len(sch)), np.log(sch))\n",
    "            )\n",
    "\n",
    "    def _update_step_size(self):\n",
    "        \"\"\" Evaluate costs on samples, and adjust the step size. \"\"\"\n",
    "        # Evaluate cost function for all conditions and samples.\n",
    "        for m in range(self.M):\n",
    "            self._update_policy_fit(m, init=True)\n",
    "            self._eval_cost(m)\n",
    "            # Adjust step size relative to the previous iteration.\n",
    "            if self.iteration_count >= 1 and self.prev[m].sample_list:\n",
    "                self._stepadjust(m)\n",
    "\n",
    "    def _update_policy(self, inner_itr):\n",
    "        \"\"\" Compute the new policy. \"\"\"\n",
    "        dU, dO, T = self.dU, self.dO, self.T\n",
    "        # Compute target mean, cov, and weight for each sample.\n",
    "        obs_data, tgt_mu = np.zeros((0, T, dO)), np.zeros((0, T, dU))\n",
    "        tgt_prc, tgt_wt = np.zeros((0, T, dU, dU)), np.zeros((0, T))\n",
    "        for m in range(self.M):\n",
    "            samples = self.cur[m].sample_list\n",
    "            X = samples.get_X()\n",
    "            N = len(samples)\n",
    "            if inner_itr > 0:\n",
    "                traj, pol_info = self.new_traj_distr[m], self.cur[m].pol_info\n",
    "            else:\n",
    "                traj, pol_info = self.cur[m].traj_distr, self.cur[m].pol_info\n",
    "            mu = np.zeros((N, T, dU))\n",
    "            prc = np.zeros((N, T, dU, dU))\n",
    "            wt = np.zeros((N, T))\n",
    "            # Get time-indexed actions.\n",
    "            for t in range(T):\n",
    "                # Compute actions along this trajectory.\n",
    "                prc[:, t, :, :] = np.tile(traj.inv_pol_covar[t, :, :],\n",
    "                                          [N, 1, 1])\n",
    "                for i in range(N):\n",
    "                    mu[i, t, :] = \\\n",
    "                            (traj.K[t, :, :].dot(X[i, t, :]) + traj.k[t, :]) - \\\n",
    "                            np.linalg.solve(\n",
    "                                prc[i, t, :, :] / pol_info.pol_wt[t],\n",
    "                                pol_info.lambda_K[t, :, :].dot(X[i, t, :]) + \\\n",
    "                                        pol_info.lambda_k[t, :]\n",
    "                            )\n",
    "                wt[:, t].fill(pol_info.pol_wt[t])\n",
    "            tgt_mu = np.concatenate((tgt_mu, mu))\n",
    "            tgt_prc = np.concatenate((tgt_prc, prc))\n",
    "            tgt_wt = np.concatenate((tgt_wt, wt))\n",
    "            obs_data = np.concatenate((obs_data, samples.get_obs()))\n",
    "        self.policy_opt.update(obs_data, tgt_mu, tgt_prc, tgt_wt)\n",
    "\n",
    "    def _update_policy_fit(self, m, init=False):\n",
    "        \"\"\"\n",
    "        Re-estimate the local policy values in the neighborhood of the\n",
    "        trajectory.\n",
    "        Args:\n",
    "            m: Condition\n",
    "            init: Whether this is the initial fitting of the policy.\n",
    "        \"\"\"\n",
    "        dX, dU, T = self.dX, self.dU, self.T\n",
    "        # Choose samples to use.\n",
    "        samples = self.cur[m].sample_list\n",
    "        N = len(samples)\n",
    "        pol_info = self.cur[m].pol_info\n",
    "        X = samples.get_X()\n",
    "        obs = samples.get_obs().copy()\n",
    "        pol_mu, pol_sig = self.policy_opt.prob(obs)[:2]\n",
    "        pol_info.pol_mu, pol_info.pol_sig = pol_mu, pol_sig\n",
    "\n",
    "        # Update policy prior.\n",
    "        policy_prior = pol_info.policy_prior\n",
    "        if init:\n",
    "            samples = SampleList(self.cur[m].sample_list)\n",
    "            mode = self._hyperparams['policy_sample_mode']\n",
    "        else:\n",
    "            samples = SampleList([])\n",
    "            mode = 'add' # Don't replace with empty samples\n",
    "        policy_prior.update(samples, self.policy_opt, mode)\n",
    "\n",
    "        # Fit linearization and store in pol_info.\n",
    "        pol_info.pol_K, pol_info.pol_k, pol_info.pol_S = \\\n",
    "                policy_prior.fit(X, pol_mu, pol_sig)\n",
    "        for t in range(T):\n",
    "            pol_info.chol_pol_S[t, :, :] = \\\n",
    "                    sp.linalg.cholesky(pol_info.pol_S[t, :, :])\n",
    "\n",
    "    def _policy_dual_step(self, m, step=False):\n",
    "        \"\"\"\n",
    "        Update the dual variables for the specified condition.\n",
    "        Args:\n",
    "            m: Condition\n",
    "            step: Whether or not to update pol_wt.\n",
    "        \"\"\"\n",
    "        dU, T = self.dU, self.T\n",
    "        samples = self.cur[m].sample_list\n",
    "        N = len(samples)\n",
    "        X = samples.get_X()\n",
    "        if 'new_traj_distr' in dir(self):\n",
    "            traj, pol_info = self.new_traj_distr[m], self.cur[m].pol_info\n",
    "        else:\n",
    "            traj, pol_info = self.cur[m].traj_distr, self.cur[m].pol_info\n",
    "\n",
    "        # Compute trajectory action at each sampled state.\n",
    "        traj_mu = np.zeros((N, T, dU))\n",
    "        for i in range(N):\n",
    "            for t in range(T):\n",
    "                traj_mu[i, t, :] = traj.K[t, :, :].dot(X[i, t, :]) + \\\n",
    "                        traj.k[t, :]\n",
    "        # Compute policy action at each sampled state.\n",
    "        pol_mu = pol_info.pol_mu\n",
    "        # Compute the difference and increment based on pol_wt.\n",
    "        for t in range(T):\n",
    "            tU, pU = traj_mu[:, t, :], pol_mu[:, t, :]\n",
    "            # Increment mean term.\n",
    "            pol_info.lambda_k[t, :] -= self._hyperparams['policy_dual_rate'] * \\\n",
    "                    pol_info.pol_wt[t] * \\\n",
    "                    traj.inv_pol_covar[t, :, :].dot(np.mean(tU - pU, axis=0))\n",
    "            # Increment covariance term.\n",
    "            t_covar, p_covar = traj.K[t, :, :], pol_info.pol_K[t, :, :]\n",
    "            pol_info.lambda_K[t, :, :] -= \\\n",
    "                    self._hyperparams['policy_dual_rate_covar'] * \\\n",
    "                    pol_info.pol_wt[t] * \\\n",
    "                    traj.inv_pol_covar[t, :, :].dot(t_covar - p_covar)\n",
    "        # Compute KL divergence.\n",
    "        kl_m = self._policy_kl(m)[0]\n",
    "        if step:\n",
    "            lg_step = self._hyperparams['lg_step']\n",
    "            # Increment pol_wt based on change in KL divergence.\n",
    "            if self._hyperparams['fixed_lg_step'] == 1:\n",
    "                # Take fixed size step.\n",
    "                pol_info.pol_wt = np.array([\n",
    "                    max(wt + lg_step, 0) for wt in pol_info.pol_wt\n",
    "                ])\n",
    "            elif self._hyperparams['fixed_lg_step'] == 2:\n",
    "                # (In/De)crease based on change in constraint\n",
    "                # satisfaction.\n",
    "                if hasattr(pol_info, 'prev_kl'):\n",
    "                    kl_change = kl_m / pol_info.prev_kl\n",
    "                    for i in range(len(pol_info.pol_wt)):\n",
    "                        if kl_change[i] < 0.8:\n",
    "                            pol_info.pol_wt[i] *= 0.5\n",
    "                        elif kl_change[i] >= 0.95:\n",
    "                            pol_info.pol_wt[i] *= 2.0\n",
    "            elif self._hyperparams['fixed_lg_step'] == 3:\n",
    "                # (In/De)crease based on difference from average.\n",
    "                if hasattr(pol_info, 'prev_kl'):\n",
    "                    lower = np.mean(kl_m) - \\\n",
    "                            self._hyperparams['exp_step_lower'] * np.std(kl_m)\n",
    "                    upper = np.mean(kl_m) + \\\n",
    "                            self._hyperparams['exp_step_upper'] * np.std(kl_m)\n",
    "                    for i in range(len(pol_info.pol_wt)):\n",
    "                        if kl_m[i] < lower:\n",
    "                            pol_info.pol_wt[i] *= \\\n",
    "                                    self._hyperparams['exp_step_decrease']\n",
    "                        elif kl_m[i] >= upper:\n",
    "                            pol_info.pol_wt[i] *= \\\n",
    "                                    self._hyperparams['exp_step_increase']\n",
    "            else:\n",
    "                # Standard DGD step.\n",
    "                pol_info.pol_wt = np.array([\n",
    "                    max(pol_info.pol_wt[t] + lg_step * kl_m[t], 0)\n",
    "                    for t in range(T)\n",
    "                ])\n",
    "            pol_info.prev_kl = kl_m\n",
    "\n",
    "    def _advance_iteration_variables(self):\n",
    "        \"\"\"\n",
    "        Move all 'cur' variables to 'prev', reinitialize 'cur'\n",
    "        variables, and advance iteration counter.\n",
    "        \"\"\"\n",
    "        Algorithm._advance_iteration_variables(self)\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].traj_info.last_kl_step = \\\n",
    "                    self.prev[m].traj_info.last_kl_step\n",
    "            self.cur[m].pol_info = copy.deepcopy(self.prev[m].pol_info)\n",
    "\n",
    "    def _stepadjust(self, m):\n",
    "        \"\"\"\n",
    "        Calculate new step sizes.\n",
    "        Args:\n",
    "            m: Condition\n",
    "        \"\"\"\n",
    "\n",
    "        # Compute values under Laplace approximation. This is the policy\n",
    "        # that the previous samples were actually drawn from under the\n",
    "        # dynamics that were estimated from the previous samples.\n",
    "        prev_laplace_obj, prev_laplace_kl = self._estimate_cost(\n",
    "            self.prev[m].traj_distr, self.prev[m].traj_info, self.prev[m].pol_info, m\n",
    "        )\n",
    "        # This is the policy that we just used under the dynamics that\n",
    "        # were estimated from the previous samples (so this is the cost\n",
    "        # we thought we would have).\n",
    "        new_pred_laplace_obj, new_pred_laplace_kl = self._estimate_cost(\n",
    "            self.cur[m].traj_distr, self.prev[m].traj_info, self.prev[m].pol_info, m\n",
    "        )\n",
    "\n",
    "        # This is the actual cost we have under the current trajectory\n",
    "        # based on the latest samples.\n",
    "        new_actual_laplace_obj, new_actual_laplace_kl = self._estimate_cost(\n",
    "            self.cur[m].traj_distr, self.cur[m].traj_info, self.cur[m].pol_info, m\n",
    "        )\n",
    "\n",
    "        # Measure the entropy of the current trajectory (for printout).\n",
    "        ent = self._measure_ent(m)\n",
    "\n",
    "        # Compute actual objective values based on the samples.\n",
    "        prev_mc_obj = np.mean(np.sum(self.prev[m].cs, axis=1), axis=0)\n",
    "        new_mc_obj = np.mean(np.sum(self.cur[m].cs, axis=1), axis=0)\n",
    "\n",
    "        # Compute sample-based estimate of KL divergence between policy\n",
    "        # and trajectories.\n",
    "        new_mc_kl = self._policy_kl(m)[0]\n",
    "        if self.iteration_count >= 1 and self.prev[m].sample_list:\n",
    "            prev_mc_kl = self._policy_kl(m, prev=True)[0]\n",
    "        else:\n",
    "            prev_mc_kl = np.zeros_like(new_mc_kl)\n",
    "\n",
    "        # Compute full policy KL divergence objective terms by applying\n",
    "        # the Lagrange multipliers.\n",
    "        pol_wt = self.cur[m].pol_info.pol_wt\n",
    "        prev_laplace_kl_sum = np.sum(prev_laplace_kl * pol_wt)\n",
    "        new_pred_laplace_kl_sum = np.sum(new_pred_laplace_kl * pol_wt)\n",
    "        new_actual_laplace_kl_sum = np.sum(new_actual_laplace_kl * pol_wt)\n",
    "        prev_mc_kl_sum = np.sum(prev_mc_kl * pol_wt)\n",
    "        new_mc_kl_sum = np.sum(new_mc_kl * pol_wt)\n",
    "\n",
    "        LOGGER.debug(\n",
    "            'Trajectory step: ent: %f cost: %f -> %f KL: %f -> %f',\n",
    "            ent, prev_mc_obj, new_mc_obj, prev_mc_kl_sum, new_mc_kl_sum\n",
    "        )\n",
    "\n",
    "        # Compute predicted and actual improvement.\n",
    "        predicted_impr = np.sum(prev_laplace_obj) + prev_laplace_kl_sum - \\\n",
    "                np.sum(new_pred_laplace_obj) - new_pred_laplace_kl_sum\n",
    "        actual_impr = np.sum(prev_laplace_obj) + prev_laplace_kl_sum - \\\n",
    "                np.sum(new_actual_laplace_obj) - new_actual_laplace_kl_sum\n",
    "\n",
    "        # Print improvement details.\n",
    "        LOGGER.debug('Previous cost: Laplace: %f MC: %f',\n",
    "                     np.sum(prev_laplace_obj), prev_mc_obj)\n",
    "        LOGGER.debug('Predicted new cost: Laplace: %f MC: %f',\n",
    "                     np.sum(new_pred_laplace_obj), new_mc_obj)\n",
    "        LOGGER.debug('Actual new cost: Laplace: %f MC: %f',\n",
    "                     np.sum(new_actual_laplace_obj), new_mc_obj)\n",
    "        LOGGER.debug('Previous KL: Laplace: %f MC: %f',\n",
    "                     np.sum(prev_laplace_kl), np.sum(prev_mc_kl))\n",
    "        LOGGER.debug('Predicted new KL: Laplace: %f MC: %f',\n",
    "                     np.sum(new_pred_laplace_kl), np.sum(new_mc_kl))\n",
    "        LOGGER.debug('Actual new KL: Laplace: %f MC: %f',\n",
    "                     np.sum(new_actual_laplace_kl), np.sum(new_mc_kl))\n",
    "        LOGGER.debug('Previous w KL: Laplace: %f MC: %f',\n",
    "                     prev_laplace_kl_sum, prev_mc_kl_sum)\n",
    "        LOGGER.debug('Predicted w new KL: Laplace: %f MC: %f',\n",
    "                     new_pred_laplace_kl_sum, new_mc_kl_sum)\n",
    "        LOGGER.debug('Actual w new KL: Laplace %f MC: %f',\n",
    "                     new_actual_laplace_kl_sum, new_mc_kl_sum)\n",
    "        LOGGER.debug('Predicted/actual improvement: %f / %f',\n",
    "                     predicted_impr, actual_impr)\n",
    "\n",
    "        # Compute actual KL step taken at last iteration.\n",
    "        actual_step = self.cur[m].traj_info.last_kl_step / \\\n",
    "                (self._hyperparams['kl_step'] * self.T)\n",
    "        if actual_step < self.cur[m].step_mult:\n",
    "            self.cur[m].step_mult = max(actual_step,\n",
    "                                        self._hyperparams['min_step_mult'])\n",
    "\n",
    "        self._set_new_mult(predicted_impr, actual_impr, m)\n",
    "\n",
    "    def _policy_kl(self, m, prev=False):\n",
    "        \"\"\"\n",
    "        Monte-Carlo estimate of KL divergence between policy and\n",
    "        trajectory.\n",
    "        \"\"\"\n",
    "        dU, T = self.dU, self.T\n",
    "        if prev:\n",
    "            traj, pol_info = self.prev[m].traj_distr, self.cur[m].pol_info\n",
    "            samples = self.prev[m].sample_list\n",
    "        else:\n",
    "            traj, pol_info = self.cur[m].traj_distr, self.cur[m].pol_info\n",
    "            samples = self.cur[m].sample_list\n",
    "        N = len(samples)\n",
    "        X, obs = samples.get_X(), samples.get_obs()\n",
    "        kl, kl_m = np.zeros((N, T)), np.zeros(T)\n",
    "        kl_l, kl_lm = np.zeros((N, T)), np.zeros(T)\n",
    "        # Compute policy mean and covariance at each sample.\n",
    "        pol_mu, _, pol_prec, pol_det_sigma = self.policy_opt.prob(obs.copy())\n",
    "        # Compute KL divergence.\n",
    "        for t in range(T):\n",
    "            # Compute trajectory action at sample.\n",
    "            traj_mu = np.zeros((N, dU))\n",
    "            for i in range(N):\n",
    "                traj_mu[i, :] = traj.K[t, :, :].dot(X[i, t, :]) + traj.k[t, :]\n",
    "            diff = pol_mu[:, t, :] - traj_mu\n",
    "            tr_pp_ct = pol_prec[:, t, :, :] * traj.pol_covar[t, :, :]\n",
    "            k_ln_det_ct = 0.5 * dU + np.sum(\n",
    "                np.log(np.diag(traj.chol_pol_covar[t, :, :]))\n",
    "            )\n",
    "            ln_det_cp = np.log(pol_det_sigma[:, t])\n",
    "            # IMPORTANT: Note that this assumes that pol_prec does not\n",
    "            #            depend on state!!!!\n",
    "            #            (Only the last term makes this assumption.)\n",
    "            d_pp_d = np.sum(diff * (diff.dot(pol_prec[1, t, :, :])), axis=1)\n",
    "            kl[:, t] = 0.5 * np.sum(np.sum(tr_pp_ct, axis=1), axis=1) - \\\n",
    "                    k_ln_det_ct + 0.5 * ln_det_cp + 0.5 * d_pp_d\n",
    "            tr_pp_ct_m = np.mean(tr_pp_ct, axis=0)\n",
    "            kl_m[t] = 0.5 * np.sum(np.sum(tr_pp_ct_m, axis=0), axis=0) - \\\n",
    "                    k_ln_det_ct + 0.5 * np.mean(ln_det_cp) + \\\n",
    "                    0.5 * np.mean(d_pp_d)\n",
    "            # Compute trajectory action at sample with Lagrange\n",
    "            # multiplier.\n",
    "            traj_mu = np.zeros((N, dU))\n",
    "            for i in range(N):\n",
    "                traj_mu[i, :] = \\\n",
    "                        (traj.K[t, :, :] - pol_info.lambda_K[t, :, :]).dot(\n",
    "                            X[i, t, :]\n",
    "                        ) + (traj.k[t, :] - pol_info.lambda_k[t, :])\n",
    "            # Compute KL divergence with Lagrange multiplier.\n",
    "            diff_l = pol_mu[:, t, :] - traj_mu\n",
    "            d_pp_d_l = np.sum(diff_l * (diff_l.dot(pol_prec[1, t, :, :])),\n",
    "                              axis=1)\n",
    "            kl_l[:, t] = 0.5 * np.sum(np.sum(tr_pp_ct, axis=1), axis=1) - \\\n",
    "                    k_ln_det_ct + 0.5 * ln_det_cp + 0.5 * d_pp_d_l\n",
    "            kl_lm[t] = 0.5 * np.sum(np.sum(tr_pp_ct_m, axis=0), axis=0) - \\\n",
    "                    k_ln_det_ct + 0.5 * np.mean(ln_det_cp) + \\\n",
    "                    0.5 * np.mean(d_pp_d_l)\n",
    "        return kl_m, kl, kl_lm, kl_l\n",
    "\n",
    "    def _estimate_cost(self, traj_distr, traj_info, pol_info, m):\n",
    "        \"\"\"\n",
    "        Compute Laplace approximation to expected cost.\n",
    "        Args:\n",
    "            traj_distr: A linear Gaussian policy object.\n",
    "            traj_info: A TrajectoryInfo object.\n",
    "            pol_info: Policy linearization info.\n",
    "            m: Condition number.\n",
    "        \"\"\"\n",
    "        # Constants.\n",
    "        T, dU, dX = self.T, self.dU, self.dX\n",
    "\n",
    "        # Perform forward pass (note that we repeat this here, because\n",
    "        # traj_info may have different dynamics from the ones that were\n",
    "        # used to compute the distribution already saved in traj).\n",
    "        mu, sigma = self.traj_opt.forward(traj_distr, traj_info)\n",
    "\n",
    "        # Compute cost.\n",
    "        predicted_cost = np.zeros(T)\n",
    "        for t in range(T):\n",
    "            predicted_cost[t] = traj_info.cc[t] + 0.5 * \\\n",
    "                    (np.sum(sigma[t, :, :] * traj_info.Cm[t, :, :]) +\n",
    "                     mu[t, :].T.dot(traj_info.Cm[t, :, :]).dot(mu[t, :])) + \\\n",
    "                    mu[t, :].T.dot(traj_info.cv[t, :])\n",
    "\n",
    "        # Compute KL divergence.\n",
    "        predicted_kl = np.zeros(T)\n",
    "        for t in range(T):\n",
    "            inv_pS = np.linalg.solve(\n",
    "                pol_info.chol_pol_S[t, :, :],\n",
    "                np.linalg.solve(pol_info.chol_pol_S[t, :, :].T, np.eye(dU))\n",
    "            )\n",
    "            Ufb = pol_info.pol_K[t, :, :].dot(mu[t, :dX].T) + \\\n",
    "                    pol_info.pol_k[t, :]\n",
    "            diff = mu[t, dX:] - Ufb\n",
    "            Kbar = traj_distr.K[t, :, :] - pol_info.pol_K[t, :, :]\n",
    "            predicted_kl[t] = 0.5 * (diff).dot(inv_pS).dot(diff) + \\\n",
    "                    0.5 * np.sum(traj_distr.pol_covar[t, :, :] * inv_pS) + \\\n",
    "                    0.5 * np.sum(\n",
    "                        sigma[t, :dX, :dX] * Kbar.T.dot(inv_pS).dot(Kbar)\n",
    "                    ) + np.sum(\n",
    "                        np.log(np.diag(pol_info.chol_pol_S[t, :, :]))\n",
    "                    ) - np.sum(\n",
    "                        np.log(np.diag(traj_distr.chol_pol_covar[t, :, :]))\n",
    "                    ) + 0.5 * dU\n",
    "\n",
    "        return predicted_cost, predicted_kl\n",
    "\n",
    "    def compute_costs(self, m, eta, augment=True):\n",
    "        \"\"\" Compute cost estimates used in the LQR backward pass. \"\"\"\n",
    "        traj_info, traj_distr = self.cur[m].traj_info, self.cur[m].traj_distr\n",
    "        if not augment:  # Whether to augment cost with term to penalize KL\n",
    "            return traj_info.Cm, traj_info.cv\n",
    "\n",
    "        pol_info = self.cur[m].pol_info\n",
    "        multiplier = self._hyperparams['max_ent_traj']\n",
    "        T, dU, dX = traj_distr.T, traj_distr.dU, traj_distr.dX\n",
    "        Cm, cv = np.copy(traj_info.Cm), np.copy(traj_info.cv)\n",
    "\n",
    "        # Modify policy action via Lagrange multiplier.\n",
    "        cv[:, dX:] -= pol_info.lambda_k\n",
    "        Cm[:, dX:, :dX] -= pol_info.lambda_K\n",
    "        Cm[:, :dX, dX:] -= np.transpose(pol_info.lambda_K, [0, 2, 1])\n",
    "\n",
    "        #Pre-process the costs with KL-divergence terms.\n",
    "        TKLm = np.zeros((T, dX+dU, dX+dU))\n",
    "        TKLv = np.zeros((T, dX+dU))\n",
    "        PKLm = np.zeros((T, dX+dU, dX+dU))\n",
    "        PKLv = np.zeros((T, dX+dU))\n",
    "        fCm, fcv = np.zeros(Cm.shape), np.zeros(cv.shape)\n",
    "        for t in range(T):\n",
    "            K, k = traj_distr.K[t, :, :], traj_distr.k[t, :]\n",
    "            inv_pol_covar = traj_distr.inv_pol_covar[t, :, :]\n",
    "            # Trajectory KL-divergence terms.\n",
    "            TKLm[t, :, :] = np.vstack([\n",
    "                np.hstack([\n",
    "                    K.T.dot(inv_pol_covar).dot(K),\n",
    "                    -K.T.dot(inv_pol_covar)]),\n",
    "                np.hstack([-inv_pol_covar.dot(K), inv_pol_covar])\n",
    "            ])\n",
    "            TKLv[t, :] = np.concatenate([\n",
    "                K.T.dot(inv_pol_covar).dot(k), -inv_pol_covar.dot(k)\n",
    "            ])\n",
    "            # Policy KL-divergence terms.\n",
    "            inv_pol_S = np.linalg.solve(\n",
    "                pol_info.chol_pol_S[t, :, :],\n",
    "                np.linalg.solve(pol_info.chol_pol_S[t, :, :].T, np.eye(dU))\n",
    "            )\n",
    "            KB, kB = pol_info.pol_K[t, :, :], pol_info.pol_k[t, :]\n",
    "            PKLm[t, :, :] = np.vstack([\n",
    "                np.hstack([KB.T.dot(inv_pol_S).dot(KB), -KB.T.dot(inv_pol_S)]),\n",
    "                np.hstack([-inv_pol_S.dot(KB), inv_pol_S])\n",
    "            ])\n",
    "            PKLv[t, :] = np.concatenate([\n",
    "                KB.T.dot(inv_pol_S).dot(kB), -inv_pol_S.dot(kB)\n",
    "            ])\n",
    "            wt = pol_info.pol_wt[t]\n",
    "            fCm[t, :, :] = (Cm[t, :, :] + TKLm[t, :, :] * eta +\n",
    "                            PKLm[t, :, :] * wt) / (eta + wt + multiplier)\n",
    "            fcv[t, :] = (cv[t, :] + TKLv[t, :] * eta +\n",
    "                         PKLv[t, :] * wt) / (eta + wt + multiplier)\n",
    "\n",
    "        return fCm, fcv\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `AlgorithmMDGPS`\n",
    "MD-based GPS algorithm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "# AlgorithmMDGPS\n",
    "ALG_MDGPS = {\n",
    "    # TODO: remove need for init_pol_wt in MDGPS\n",
    "    'init_pol_wt': 0.01,\n",
    "    'policy_sample_mode': 'add',\n",
    "    # Whether to use 'laplace' or 'mc' cost in step adjusment\n",
    "    'step_rule': 'laplace',\n",
    "}\n",
    "\n",
    "class AlgorithmMDGPS(Algorithm):\n",
    "    \"\"\"\n",
    "    Sample-based joint policy learning and trajectory optimization with\n",
    "    (approximate) mirror descent guided policy search algorithm.\n",
    "    \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        config = copy.deepcopy(ALG_MDGPS)\n",
    "        config.update(hyperparams)\n",
    "        Algorithm.__init__(self, config)\n",
    "\n",
    "        policy_prior = self._hyperparams['policy_prior']\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].pol_info = PolicyInfo(self._hyperparams)\n",
    "            self.cur[m].pol_info.policy_prior = \\\n",
    "                    policy_prior['type'](policy_prior)\n",
    "\n",
    "        self.policy_opt = self._hyperparams['policy_opt']['type'](\n",
    "            self._hyperparams['policy_opt'], self.dO, self.dU\n",
    "        )\n",
    "\n",
    "    def iteration(self, sample_lists):\n",
    "        \"\"\"\n",
    "        Run iteration of MDGPS-based guided policy search.\n",
    "\n",
    "        Args:\n",
    "            sample_lists: List of SampleList objects for each condition.\n",
    "        \"\"\"\n",
    "        # Store the samples and evaluate the costs.\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].sample_list = sample_lists[m]\n",
    "            self._eval_cost(m)\n",
    "\n",
    "        # Update dynamics linearizations.\n",
    "        self._update_dynamics()\n",
    "\n",
    "        # On the first iteration, need to catch policy up to init_traj_distr.\n",
    "        if self.iteration_count == 0:\n",
    "            self.new_traj_distr = [\n",
    "                self.cur[cond].traj_distr for cond in range(self.M)\n",
    "            ]\n",
    "            self._update_policy()\n",
    "\n",
    "        # Update policy linearizations.\n",
    "        for m in range(self.M):\n",
    "            self._update_policy_fit(m)\n",
    "\n",
    "        # C-step\n",
    "        if self.iteration_count > 0:\n",
    "            self._stepadjust()\n",
    "        self._update_trajectories()\n",
    "\n",
    "        # S-step\n",
    "        self._update_policy()\n",
    "\n",
    "        # Prepare for next iteration\n",
    "        self._advance_iteration_variables()\n",
    "\n",
    "    def _update_policy(self):\n",
    "        \"\"\" Compute the new policy. \"\"\"\n",
    "        dU, dO, T = self.dU, self.dO, self.T\n",
    "        # Compute target mean, cov, and weight for each sample.\n",
    "        obs_data, tgt_mu = np.zeros((0, T, dO)), np.zeros((0, T, dU))\n",
    "        tgt_prc, tgt_wt = np.zeros((0, T, dU, dU)), np.zeros((0, T))\n",
    "        for m in range(self.M):\n",
    "            samples = self.cur[m].sample_list\n",
    "            X = samples.get_X()\n",
    "            N = len(samples)\n",
    "            traj, pol_info = self.new_traj_distr[m], self.cur[m].pol_info\n",
    "            mu = np.zeros((N, T, dU))\n",
    "            prc = np.zeros((N, T, dU, dU))\n",
    "            wt = np.zeros((N, T))\n",
    "            # Get time-indexed actions.\n",
    "            for t in range(T):\n",
    "                # Compute actions along this trajectory.\n",
    "                prc[:, t, :, :] = np.tile(traj.inv_pol_covar[t, :, :],\n",
    "                                          [N, 1, 1])\n",
    "                for i in range(N):\n",
    "                    mu[i, t, :] = (traj.K[t, :, :].dot(X[i, t, :]) + traj.k[t, :])\n",
    "                wt[:, t].fill(pol_info.pol_wt[t])\n",
    "            tgt_mu = np.concatenate((tgt_mu, mu))\n",
    "            tgt_prc = np.concatenate((tgt_prc, prc))\n",
    "            tgt_wt = np.concatenate((tgt_wt, wt))\n",
    "            obs_data = np.concatenate((obs_data, samples.get_obs()))\n",
    "        self.policy_opt.update(obs_data, tgt_mu, tgt_prc, tgt_wt)\n",
    "\n",
    "    def _update_policy_fit(self, m):\n",
    "        \"\"\"\n",
    "        Re-estimate the local policy values in the neighborhood of the\n",
    "        trajectory.\n",
    "        Args:\n",
    "            m: Condition\n",
    "        \"\"\"\n",
    "        dX, dU, T = self.dX, self.dU, self.T\n",
    "        # Choose samples to use.\n",
    "        samples = self.cur[m].sample_list\n",
    "        N = len(samples)\n",
    "        pol_info = self.cur[m].pol_info\n",
    "        X = samples.get_X()\n",
    "        obs = samples.get_obs().copy()\n",
    "        pol_mu, pol_sig = self.policy_opt.prob(obs)[:2]\n",
    "        pol_info.pol_mu, pol_info.pol_sig = pol_mu, pol_sig\n",
    "\n",
    "        # Update policy prior.\n",
    "        policy_prior = pol_info.policy_prior\n",
    "        samples = SampleList(self.cur[m].sample_list)\n",
    "        mode = self._hyperparams['policy_sample_mode']\n",
    "        policy_prior.update(samples, self.policy_opt, mode)\n",
    "\n",
    "        # Fit linearization and store in pol_info.\n",
    "        pol_info.pol_K, pol_info.pol_k, pol_info.pol_S = \\\n",
    "                policy_prior.fit(X, pol_mu, pol_sig)\n",
    "        for t in range(T):\n",
    "            pol_info.chol_pol_S[t, :, :] = \\\n",
    "                    sp.linalg.cholesky(pol_info.pol_S[t, :, :])\n",
    "\n",
    "    def _advance_iteration_variables(self):\n",
    "        \"\"\"\n",
    "        Move all 'cur' variables to 'prev', reinitialize 'cur'\n",
    "        variables, and advance iteration counter.\n",
    "        \"\"\"\n",
    "        Algorithm._advance_iteration_variables(self)\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].traj_info.last_kl_step = \\\n",
    "                    self.prev[m].traj_info.last_kl_step\n",
    "            self.cur[m].pol_info = copy.deepcopy(self.prev[m].pol_info)\n",
    "\n",
    "    def _stepadjust(self):\n",
    "        \"\"\"\n",
    "        Calculate new step sizes. This version uses the same step size\n",
    "        for all conditions.\n",
    "        \"\"\"\n",
    "        # Compute previous cost and previous expected cost.\n",
    "        prev_M = len(self.prev) # May be different in future.\n",
    "        prev_laplace = np.empty(prev_M)\n",
    "        prev_mc = np.empty(prev_M)\n",
    "        prev_predicted = np.empty(prev_M)\n",
    "        for m in range(prev_M):\n",
    "            prev_nn = self.prev[m].pol_info.traj_distr()\n",
    "            prev_lg = self.prev[m].new_traj_distr\n",
    "\n",
    "            # Compute values under Laplace approximation. This is the policy\n",
    "            # that the previous samples were actually drawn from under the\n",
    "            # dynamics that were estimated from the previous samples.\n",
    "            prev_laplace[m] = self.traj_opt.estimate_cost(\n",
    "                    prev_nn, self.prev[m].traj_info\n",
    "            ).sum()\n",
    "            # This is the actual cost that we experienced.\n",
    "            prev_mc[m] = self.prev[m].cs.mean(axis=0).sum()\n",
    "            # This is the policy that we just used under the dynamics that\n",
    "            # were estimated from the prev samples (so this is the cost\n",
    "            # we thought we would have).\n",
    "            prev_predicted[m] = self.traj_opt.estimate_cost(\n",
    "                    prev_lg, self.prev[m].traj_info\n",
    "            ).sum()\n",
    "\n",
    "        # Compute current cost.\n",
    "        cur_laplace = np.empty(self.M)\n",
    "        cur_mc = np.empty(self.M)\n",
    "        for m in range(self.M):\n",
    "            cur_nn = self.cur[m].pol_info.traj_distr()\n",
    "            # This is the actual cost we have under the current trajectory\n",
    "            # based on the latest samples.\n",
    "            cur_laplace[m] = self.traj_opt.estimate_cost(\n",
    "                    cur_nn, self.cur[m].traj_info\n",
    "            ).sum()\n",
    "            cur_mc[m] = self.cur[m].cs.mean(axis=0).sum()\n",
    "\n",
    "        # Compute predicted and actual improvement.\n",
    "        prev_laplace = prev_laplace.mean()\n",
    "        prev_mc = prev_mc.mean()\n",
    "        prev_predicted = prev_predicted.mean()\n",
    "        cur_laplace = cur_laplace.mean()\n",
    "        cur_mc = cur_mc.mean()\n",
    "        if self._hyperparams['step_rule'] == 'laplace':\n",
    "            predicted_impr = prev_laplace - prev_predicted\n",
    "            actual_impr = prev_laplace - cur_laplace\n",
    "        elif self._hyperparams['step_rule'] == 'mc':\n",
    "            predicted_impr = prev_mc - prev_predicted\n",
    "            actual_impr = prev_mc - cur_mc\n",
    "        LOGGER.debug('Previous cost: Laplace: %f, MC: %f',\n",
    "                     prev_laplace, prev_mc)\n",
    "        LOGGER.debug('Predicted cost: Laplace: %f', prev_predicted)\n",
    "        LOGGER.debug('Actual cost: Laplace: %f, MC: %f',\n",
    "                     cur_laplace, cur_mc)\n",
    "\n",
    "        for m in range(self.M):\n",
    "            self._set_new_mult(predicted_impr, actual_impr, m)\n",
    "\n",
    "    def compute_costs(self, m, eta, augment=True):\n",
    "        \"\"\" Compute cost estimates used in the LQR backward pass. \"\"\"\n",
    "        traj_info, traj_distr = self.cur[m].traj_info, self.cur[m].traj_distr\n",
    "        if not augment:  # Whether to augment cost with term to penalize KL\n",
    "            return traj_info.Cm, traj_info.cv\n",
    "\n",
    "        pol_info = self.cur[m].pol_info\n",
    "        multiplier = self._hyperparams['max_ent_traj']\n",
    "        T, dU, dX = traj_distr.T, traj_distr.dU, traj_distr.dX\n",
    "        Cm, cv = np.copy(traj_info.Cm), np.copy(traj_info.cv)\n",
    "\n",
    "        PKLm = np.zeros((T, dX+dU, dX+dU))\n",
    "        PKLv = np.zeros((T, dX+dU))\n",
    "        fCm, fcv = np.zeros(Cm.shape), np.zeros(cv.shape)\n",
    "        for t in range(T):\n",
    "            # Policy KL-divergence terms.\n",
    "            inv_pol_S = np.linalg.solve(\n",
    "                pol_info.chol_pol_S[t, :, :],\n",
    "                np.linalg.solve(pol_info.chol_pol_S[t, :, :].T, np.eye(dU))\n",
    "            )\n",
    "            KB, kB = pol_info.pol_K[t, :, :], pol_info.pol_k[t, :]\n",
    "            PKLm[t, :, :] = np.vstack([\n",
    "                np.hstack([KB.T.dot(inv_pol_S).dot(KB), -KB.T.dot(inv_pol_S)]),\n",
    "                np.hstack([-inv_pol_S.dot(KB), inv_pol_S])\n",
    "            ])\n",
    "            PKLv[t, :] = np.concatenate([\n",
    "                KB.T.dot(inv_pol_S).dot(kB), -inv_pol_S.dot(kB)\n",
    "            ])\n",
    "            fCm[t, :, :] = (Cm[t, :, :] + PKLm[t, :, :] * eta) / (eta + multiplier)\n",
    "            fcv[t, :] = (cv[t, :] + PKLv[t, :] * eta) / (eta + multiplier)\n",
    "\n",
    "        return fCm, fcv\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Trajectory optimization\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### iLQG-based trajectory optimization: `AlgorithmTrajOpt`\n",
    "Sample-based trajectory optimization algorithm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class AlgorithmTrajOpt(Algorithm):\n",
    "    \"\"\" Sample-based trajectory optimization. \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        Algorithm.__init__(self, hyperparams)\n",
    "\n",
    "    def iteration(self, sample_lists):\n",
    "        \"\"\"\n",
    "        Run iteration of LQR.\n",
    "        Args:\n",
    "            sample_lists: List of SampleList objects for each condition.\n",
    "        \"\"\"\n",
    "        for m in range(self.M):\n",
    "            self.cur[m].sample_list = sample_lists[m]\n",
    "\n",
    "        # Update dynamics model using all samples.\n",
    "        self._update_dynamics()\n",
    "\n",
    "        self._update_step_size()  # KL Divergence step size.\n",
    "\n",
    "        # Run inner loop to compute new policies.\n",
    "        for _ in range(self._hyperparams['inner_iterations']):\n",
    "            self._update_trajectories()\n",
    "\n",
    "        self._advance_iteration_variables()\n",
    "\n",
    "    def _update_step_size(self):\n",
    "        \"\"\" Evaluate costs on samples, and adjust the step size. \"\"\"\n",
    "        # Evaluate cost function for all conditions and samples.\n",
    "        for m in range(self.M):\n",
    "            self._eval_cost(m)\n",
    "\n",
    "        # Adjust step size relative to the previous iteration.\n",
    "        for m in range(self.M):\n",
    "            if self.iteration_count >= 1 and self.prev[m].sample_list:\n",
    "                self._stepadjust(m)\n",
    "\n",
    "    def _stepadjust(self, m):\n",
    "        \"\"\"\n",
    "        Calculate new step sizes.\n",
    "        Args:\n",
    "            m: Condition\n",
    "        \"\"\"\n",
    "        # Compute values under Laplace approximation. This is the policy\n",
    "        # that the previous samples were actually drawn from under the\n",
    "        # dynamics that were estimated from the previous samples.\n",
    "        previous_laplace_obj = self.traj_opt.estimate_cost(\n",
    "            self.prev[m].traj_distr, self.prev[m].traj_info\n",
    "        )\n",
    "        # This is the policy that we just used under the dynamics that\n",
    "        # were estimated from the previous samples (so this is the cost\n",
    "        # we thought we would have).\n",
    "        new_predicted_laplace_obj = self.traj_opt.estimate_cost(\n",
    "            self.cur[m].traj_distr, self.prev[m].traj_info\n",
    "        )\n",
    "\n",
    "        # This is the actual cost we have under the current trajectory\n",
    "        # based on the latest samples.\n",
    "        new_actual_laplace_obj = self.traj_opt.estimate_cost(\n",
    "            self.cur[m].traj_distr, self.cur[m].traj_info\n",
    "        )\n",
    "\n",
    "        # Measure the entropy of the current trajectory (for printout).\n",
    "        ent = self._measure_ent(m)\n",
    "\n",
    "        # Compute actual objective values based on the samples.\n",
    "        previous_mc_obj = np.mean(np.sum(self.prev[m].cs, axis=1), axis=0)\n",
    "        new_mc_obj = np.mean(np.sum(self.cur[m].cs, axis=1), axis=0)\n",
    "\n",
    "        LOGGER.debug('Trajectory step: ent: %f cost: %f -> %f',\n",
    "                     ent, previous_mc_obj, new_mc_obj)\n",
    "\n",
    "        # Compute predicted and actual improvement.\n",
    "        predicted_impr = np.sum(previous_laplace_obj) - \\\n",
    "                np.sum(new_predicted_laplace_obj)\n",
    "        actual_impr = np.sum(previous_laplace_obj) - \\\n",
    "                np.sum(new_actual_laplace_obj)\n",
    "\n",
    "        # Print improvement details.\n",
    "        LOGGER.debug('Previous cost: Laplace: %f MC: %f',\n",
    "                     np.sum(previous_laplace_obj), previous_mc_obj)\n",
    "        LOGGER.debug('Predicted new cost: Laplace: %f MC: %f',\n",
    "                     np.sum(new_predicted_laplace_obj), new_mc_obj)\n",
    "        LOGGER.debug('Actual new cost: Laplace: %f MC: %f',\n",
    "                     np.sum(new_actual_laplace_obj), new_mc_obj)\n",
    "        LOGGER.debug('Predicted/actual improvement: %f / %f',\n",
    "                     predicted_impr, actual_impr)\n",
    "\n",
    "        self._set_new_mult(predicted_impr, actual_impr, m)\n",
    "\n",
    "    def compute_costs(self, m, eta, augment=True):\n",
    "        \"\"\" Compute cost estimates used in the LQR backward pass. \"\"\"\n",
    "        traj_info, traj_distr = self.cur[m].traj_info, self.cur[m].traj_distr\n",
    "        if not augment:  # Whether to augment cost with term to penalize KL\n",
    "            return traj_info.Cm, traj_info.cv\n",
    "\n",
    "        multiplier = self._hyperparams['max_ent_traj']\n",
    "        fCm, fcv = traj_info.Cm / (eta + multiplier), traj_info.cv / (eta + multiplier)\n",
    "        K, ipc, k = traj_distr.K, traj_distr.inv_pol_covar, traj_distr.k\n",
    "\n",
    "        # Add in the trajectory divergence term.\n",
    "        for t in range(self.T - 1, -1, -1):\n",
    "            fCm[t, :, :] += eta / (eta + multiplier) * np.vstack([\n",
    "                np.hstack([\n",
    "                    K[t, :, :].T.dot(ipc[t, :, :]).dot(K[t, :, :]),\n",
    "                    -K[t, :, :].T.dot(ipc[t, :, :])\n",
    "                ]),\n",
    "                np.hstack([\n",
    "                    -ipc[t, :, :].dot(K[t, :, :]), ipc[t, :, :]\n",
    "                ])\n",
    "            ])\n",
    "            fcv[t, :] += eta / (eta + multiplier) * np.hstack([\n",
    "                K[t, :, :].T.dot(ipc[t, :, :]).dot(k[t, :]),\n",
    "                -ipc[t, :, :].dot(k[t, :])\n",
    "            ])\n",
    "\n",
    "        return fCm, fcv\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `TrajOpt`\n",
    "Base trajectory optimization class"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import abc\n",
    "\n",
    "class TrajOpt(object):\n",
    "    \"\"\" Trajectory optimization superclass. \"\"\"\n",
    "    __metaclass__ = abc.ABCMeta\n",
    "\n",
    "    def __init__(self, hyperparams):\n",
    "        self._hyperparams = hyperparams\n",
    "\n",
    "    @abc.abstractmethod\n",
    "    def update(self):\n",
    "        \"\"\" Update trajectory distributions. \"\"\"\n",
    "        raise NotImplementedError(\"Must be implemented in subclass.\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `TrajOptLQRPython`\n",
    "iLQG-based trajectory optimization"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "\"\"\" This file defines code for iLQG-based trajectory optimization. \"\"\"\n",
    "from numpy.linalg import LinAlgError\n",
    "import scipy as sp\n",
    "\n",
    "# Constants used in TrajOptLQR.\n",
    "DGD_MAX_ITER = 50\n",
    "DGD_MAX_LS_ITER = 20\n",
    "DGD_MAX_GD_ITER = 200\n",
    "\n",
    "ALPHA, BETA1, BETA2, EPS = 0.005, 0.9, 0.999, 1e-8  # Adam parameters\n",
    "\n",
    "def traj_distr_kl(new_mu, new_sigma, new_traj_distr, prev_traj_distr, tot=True):\n",
    "    \"\"\"\n",
    "    Compute KL divergence between new and previous trajectory\n",
    "    distributions.\n",
    "    Args:\n",
    "        new_mu: T x dX, mean of new trajectory distribution.\n",
    "        new_sigma: T x dX x dX, variance of new trajectory distribution.\n",
    "        new_traj_distr: A linear Gaussian policy object, new\n",
    "            distribution.\n",
    "        prev_traj_distr: A linear Gaussian policy object, previous\n",
    "            distribution.\n",
    "        tot: Whether or not to sum KL across all time steps.\n",
    "    Returns:\n",
    "        kl_div: The KL divergence between the new and previous\n",
    "            trajectories.\n",
    "    \"\"\"\n",
    "    # Constants.\n",
    "    T = new_mu.shape[0]\n",
    "    dU = new_traj_distr.dU\n",
    "\n",
    "    # Initialize vector of divergences for each time step.\n",
    "    kl_div = np.zeros(T)\n",
    "\n",
    "    # Step through trajectory.\n",
    "    for t in range(T):\n",
    "        # Fetch matrices and vectors from trajectory distributions.\n",
    "        mu_t = new_mu[t, :]\n",
    "        sigma_t = new_sigma[t, :, :]\n",
    "        K_prev = prev_traj_distr.K[t, :, :]\n",
    "        K_new = new_traj_distr.K[t, :, :]\n",
    "        k_prev = prev_traj_distr.k[t, :]\n",
    "        k_new = new_traj_distr.k[t, :]\n",
    "        chol_prev = prev_traj_distr.chol_pol_covar[t, :, :]\n",
    "        chol_new = new_traj_distr.chol_pol_covar[t, :, :]\n",
    "\n",
    "        # Compute log determinants and precision matrices.\n",
    "        logdet_prev = 2 * sum(np.log(np.diag(chol_prev)))\n",
    "        logdet_new = 2 * sum(np.log(np.diag(chol_new)))\n",
    "        prc_prev = sp.linalg.solve_triangular(\n",
    "            chol_prev, sp.linalg.solve_triangular(chol_prev.T, np.eye(dU),\n",
    "                                                  lower=True)\n",
    "        )\n",
    "        prc_new = sp.linalg.solve_triangular(\n",
    "            chol_new, sp.linalg.solve_triangular(chol_new.T, np.eye(dU),\n",
    "                                                 lower=True)\n",
    "        )\n",
    "\n",
    "        # Construct matrix, vector, and constants.\n",
    "        M_prev = np.r_[\n",
    "            np.c_[K_prev.T.dot(prc_prev).dot(K_prev), -K_prev.T.dot(prc_prev)],\n",
    "            np.c_[-prc_prev.dot(K_prev), prc_prev]\n",
    "        ]\n",
    "        M_new = np.r_[\n",
    "            np.c_[K_new.T.dot(prc_new).dot(K_new), -K_new.T.dot(prc_new)],\n",
    "            np.c_[-prc_new.dot(K_new), prc_new]\n",
    "        ]\n",
    "        v_prev = np.r_[K_prev.T.dot(prc_prev).dot(k_prev),\n",
    "                       -prc_prev.dot(k_prev)]\n",
    "        v_new = np.r_[K_new.T.dot(prc_new).dot(k_new), -prc_new.dot(k_new)]\n",
    "        c_prev = 0.5 * k_prev.T.dot(prc_prev).dot(k_prev)\n",
    "        c_new = 0.5 * k_new.T.dot(prc_new).dot(k_new)\n",
    "\n",
    "        # Compute KL divergence at timestep t.\n",
    "        kl_div[t] = max(\n",
    "            0,\n",
    "            -0.5 * mu_t.T.dot(M_new - M_prev).dot(mu_t) -\n",
    "            mu_t.T.dot(v_new - v_prev) - c_new + c_prev -\n",
    "            0.5 * np.sum(sigma_t * (M_new-M_prev)) - 0.5 * logdet_new +\n",
    "            0.5 * logdet_prev\n",
    "        )\n",
    "\n",
    "    # Add up divergences across time to get total divergence.\n",
    "    return np.sum(kl_div) if tot else kl_div\n",
    "\n",
    "def traj_distr_kl_alt(new_mu, new_sigma, new_traj_distr, prev_traj_distr, tot=True):\n",
    "    \"\"\"\n",
    "    This function computes the same quantity as the function above.\n",
    "    However, it is easier to modify and understand this function, i.e.,\n",
    "    passing in a different mu and sigma to this function will behave properly.\n",
    "    \"\"\"\n",
    "    T, dX, dU = new_mu.shape[0], new_traj_distr.dX, new_traj_distr.dU\n",
    "    kl_div = np.zeros(T)\n",
    "\n",
    "    for t in range(T):\n",
    "        K_prev = prev_traj_distr.K[t, :, :]\n",
    "        K_new = new_traj_distr.K[t, :, :]\n",
    "\n",
    "        k_prev = prev_traj_distr.k[t, :]\n",
    "        k_new = new_traj_distr.k[t, :]\n",
    "\n",
    "        sig_prev = prev_traj_distr.pol_covar[t, :, :]\n",
    "        sig_new = new_traj_distr.pol_covar[t, :, :]\n",
    "\n",
    "        chol_prev = prev_traj_distr.chol_pol_covar[t, :, :]\n",
    "        chol_new = new_traj_distr.chol_pol_covar[t, :, :]\n",
    "\n",
    "        inv_prev = prev_traj_distr.inv_pol_covar[t, :, :]\n",
    "        inv_new = new_traj_distr.inv_pol_covar[t, :, :]\n",
    "\n",
    "        logdet_prev = 2 * sum(np.log(np.diag(chol_prev)))\n",
    "        logdet_new = 2 * sum(np.log(np.diag(chol_new)))\n",
    "\n",
    "        K_diff, k_diff = K_prev - K_new, k_prev - k_new\n",
    "        mu, sigma = new_mu[t, :dX], new_sigma[t, :dX, :dX]\n",
    "\n",
    "        kl_div[t] = max(\n",
    "                0,\n",
    "                0.5 * (logdet_prev - logdet_new - new_traj_distr.dU +\n",
    "                       np.sum(np.diag(inv_prev.dot(sig_new))) +\n",
    "                       k_diff.T.dot(inv_prev).dot(k_diff) +\n",
    "                       mu.T.dot(K_diff.T).dot(inv_prev).dot(K_diff).dot(mu) +\n",
    "                       np.sum(np.diag(K_diff.T.dot(inv_prev).dot(K_diff).dot(sigma))) +\n",
    "                       2 * k_diff.T.dot(inv_prev).dot(K_diff).dot(mu))\n",
    "        )\n",
    "\n",
    "    return np.sum(kl_div) if tot else kl_div\n",
    "\n",
    "# TrajOptLQRPython\n",
    "TRAJ_OPT_LQR = {\n",
    "    # Dual variable updates for non-PD Q-function.\n",
    "    'del0': 1e-4,\n",
    "    'eta_error_threshold': 1e16,\n",
    "    'min_eta': 1e-8,\n",
    "    'max_eta': 1e16,\n",
    "    'cons_per_step': False,  # Whether or not to enforce separate KL constraints at each time step.\n",
    "    'use_prev_distr': False,  # Whether or not to measure expected KL under the previous traj distr.\n",
    "    'update_in_bwd_pass': True,  # Whether or not to update the TVLG controller during the bwd pass.\n",
    "}\n",
    "\n",
    "class TrajOptLQRPython(TrajOpt):\n",
    "    \"\"\" LQR trajectory optimization, Python implementation. \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        config = copy.deepcopy(TRAJ_OPT_LQR)\n",
    "        config.update(hyperparams)\n",
    "\n",
    "        TrajOpt.__init__(self, config)\n",
    "\n",
    "        self.cons_per_step = config['cons_per_step']\n",
    "        self._use_prev_distr = config['use_prev_distr']\n",
    "        self._update_in_bwd_pass = config['update_in_bwd_pass']\n",
    "\n",
    "    # TODO - Add arg and return spec on this function.\n",
    "    def update(self, m, algorithm):\n",
    "        \"\"\" Run dual gradient decent to optimize trajectories. \"\"\"\n",
    "        T = algorithm.T\n",
    "        eta = algorithm.cur[m].eta\n",
    "        if self.cons_per_step and type(eta) in (int, float):\n",
    "            eta = np.ones(T) * eta\n",
    "        step_mult = algorithm.cur[m].step_mult\n",
    "        traj_info = algorithm.cur[m].traj_info\n",
    "\n",
    "        if isinstance(algorithm, AlgorithmMDGPS):\n",
    "            # For MDGPS, constrain to previous NN linearization\n",
    "            prev_traj_distr = algorithm.cur[m].pol_info.traj_distr()\n",
    "        else:\n",
    "            # For BADMM/trajopt, constrain to previous LG controller\n",
    "            prev_traj_distr = algorithm.cur[m].traj_distr\n",
    "\n",
    "        # Set KL-divergence step size (epsilon).\n",
    "        kl_step = algorithm.base_kl_step * step_mult\n",
    "        if not self.cons_per_step:\n",
    "            kl_step *= T\n",
    "\n",
    "        # We assume at min_eta, kl_div > kl_step, opposite for max_eta.\n",
    "        if not self.cons_per_step:\n",
    "            min_eta = self._hyperparams['min_eta']\n",
    "            max_eta = self._hyperparams['max_eta']\n",
    "            LOGGER.debug(\"Running DGD for trajectory %d, eta: %f\", m, eta)\n",
    "        else:\n",
    "            min_eta = np.ones(T) * self._hyperparams['min_eta']\n",
    "            max_eta = np.ones(T) * self._hyperparams['max_eta']\n",
    "            LOGGER.debug(\"Running DGD for trajectory %d, avg eta: %f\", m,\n",
    "                         np.mean(eta[:-1]))\n",
    "\n",
    "        max_itr = (DGD_MAX_LS_ITER if self.cons_per_step else\n",
    "                   DGD_MAX_ITER)\n",
    "        for itr in range(max_itr):\n",
    "            if not self.cons_per_step:\n",
    "                LOGGER.debug(\"Iteration %d, bracket: (%.2e , %.2e , %.2e)\", itr,\n",
    "                             min_eta, eta, max_eta)\n",
    "\n",
    "            # Run fwd/bwd pass, note that eta may be updated.\n",
    "            # Compute KL divergence constraint violation.\n",
    "            traj_distr, eta = self.backward(prev_traj_distr, traj_info,\n",
    "                                            eta, algorithm, m)\n",
    "\n",
    "            if not self._use_prev_distr:\n",
    "                new_mu, new_sigma = self.forward(traj_distr, traj_info)\n",
    "                kl_div = traj_distr_kl(\n",
    "                        new_mu, new_sigma, traj_distr, prev_traj_distr,\n",
    "                        tot=(not self.cons_per_step)\n",
    "                )\n",
    "            else:\n",
    "                prev_mu, prev_sigma = self.forward(prev_traj_distr, traj_info)\n",
    "                kl_div = traj_distr_kl_alt(\n",
    "                        prev_mu, prev_sigma, traj_distr, prev_traj_distr,\n",
    "                        tot=(not self.cons_per_step)\n",
    "                )\n",
    "\n",
    "            con = kl_div - kl_step\n",
    "\n",
    "            # Convergence check - constraint satisfaction.\n",
    "            if self._conv_check(con, kl_step):\n",
    "                if not self.cons_per_step:\n",
    "                    LOGGER.debug(\"KL: %f / %f, converged iteration %d\", kl_div,\n",
    "                                 kl_step, itr)\n",
    "                else:\n",
    "                    LOGGER.debug(\n",
    "                            \"KL: %f / %f, converged iteration %d\",\n",
    "                            np.mean(kl_div[:-1]), np.mean(kl_step[:-1]), itr\n",
    "                    )\n",
    "                break\n",
    "\n",
    "            if not self.cons_per_step:\n",
    "                # Choose new eta (bisect bracket or multiply by constant)\n",
    "                if con < 0: # Eta was too big.\n",
    "                    max_eta = eta\n",
    "                    geom = np.sqrt(min_eta*max_eta)  # Geometric mean.\n",
    "                    new_eta = max(geom, 0.1*max_eta)\n",
    "                    LOGGER.debug(\"KL: %f / %f, eta too big, new eta: %f\",\n",
    "                                 kl_div, kl_step, new_eta)\n",
    "                else: # Eta was too small.\n",
    "                    min_eta = eta\n",
    "                    geom = np.sqrt(min_eta*max_eta)  # Geometric mean.\n",
    "                    new_eta = min(geom, 10.0*min_eta)\n",
    "                    LOGGER.debug(\"KL: %f / %f, eta too small, new eta: %f\",\n",
    "                                 kl_div, kl_step, new_eta)\n",
    "\n",
    "                # Logarithmic mean: log_mean(x,y) = (y - x)/(log(y) - log(x))\n",
    "                eta = new_eta\n",
    "            else:\n",
    "                for t in range(T):\n",
    "                    if con[t] < 0:\n",
    "                        max_eta[t] = eta[t]\n",
    "                        geom = np.sqrt(min_eta[t]*max_eta[t])\n",
    "                        eta[t] = max(geom, 0.1*max_eta[t])\n",
    "                    else:\n",
    "                        min_eta[t] = eta[t]\n",
    "                        geom = np.sqrt(min_eta[t]*max_eta[t])\n",
    "                        eta[t] = min(geom, 10.0*min_eta[t])\n",
    "                if itr % 10 == 0:\n",
    "                    LOGGER.debug(\"avg KL: %f / %f, avg new eta: %f\",\n",
    "                                 np.mean(kl_div[:-1]), np.mean(kl_step[:-1]),\n",
    "                                 np.mean(eta[:-1]))\n",
    "\n",
    "        if (self.cons_per_step and not self._conv_check(con, kl_step)):\n",
    "            m_b, v_b = np.zeros(T-1), np.zeros(T-1)\n",
    "\n",
    "            for itr in range(DGD_MAX_GD_ITER):\n",
    "                traj_distr, eta = self.backward(prev_traj_distr, traj_info,\n",
    "                                                eta, algorithm, m)\n",
    "\n",
    "                if not self._use_prev_distr:\n",
    "                    new_mu, new_sigma = self.forward(traj_distr, traj_info)\n",
    "                    kl_div = traj_distr_kl(\n",
    "                            new_mu, new_sigma, traj_distr, prev_traj_distr,\n",
    "                            tot=False\n",
    "                    )\n",
    "                else:\n",
    "                    prev_mu, prev_sigma = self.forward(prev_traj_distr,\n",
    "                                                       traj_info)\n",
    "                    kl_div = traj_distr_kl_alt(\n",
    "                            prev_mu, prev_sigma, traj_distr, prev_traj_distr,\n",
    "                            tot=False\n",
    "                    )\n",
    "\n",
    "                con = kl_div - kl_step\n",
    "                if self._conv_check(con, kl_step):\n",
    "                    LOGGER.debug(\n",
    "                            \"KL: %f / %f, converged iteration %d\",\n",
    "                            np.mean(kl_div[:-1]), np.mean(kl_step[:-1]), itr\n",
    "                    )\n",
    "                    break\n",
    "\n",
    "                m_b = (BETA1 * m_b + (1-BETA1) * con[:-1])\n",
    "                m_u = m_b / (1 - BETA1 ** (itr+1))\n",
    "                v_b = (BETA2 * v_b + (1-BETA2) * np.square(con[:-1]))\n",
    "                v_u = v_b / (1 - BETA2 ** (itr+1))\n",
    "                eta[:-1] = np.minimum(\n",
    "                        np.maximum(eta[:-1] + ALPHA * m_u / (np.sqrt(v_u) + EPS),\n",
    "                                   self._hyperparams['min_eta']),\n",
    "                        self._hyperparams['max_eta']\n",
    "                )\n",
    "\n",
    "                if itr % 10 == 0:\n",
    "                    LOGGER.debug(\"avg KL: %f / %f, avg new eta: %f\",\n",
    "                                 np.mean(kl_div[:-1]), np.mean(kl_step[:-1]),\n",
    "                                 np.mean(eta[:-1]))\n",
    "\n",
    "        if (np.mean(kl_div) > np.mean(kl_step) and\n",
    "            not self._conv_check(con, kl_step)):\n",
    "            LOGGER.warning(\n",
    "                    \"Final KL divergence after DGD convergence is too high.\"\n",
    "            )\n",
    "        return traj_distr, eta\n",
    "\n",
    "    def estimate_cost(self, traj_distr, traj_info):\n",
    "        \"\"\" Compute Laplace approximation to expected cost. \"\"\"\n",
    "        # Constants.\n",
    "        T = traj_distr.T\n",
    "\n",
    "        # Perform forward pass (note that we repeat this here, because\n",
    "        # traj_info may have different dynamics from the ones that were\n",
    "        # used to compute the distribution already saved in traj).\n",
    "        mu, sigma = self.forward(traj_distr, traj_info)\n",
    "\n",
    "        # Compute cost.\n",
    "        predicted_cost = np.zeros(T)\n",
    "        for t in range(T):\n",
    "            predicted_cost[t] = traj_info.cc[t] + 0.5 * \\\n",
    "                    np.sum(sigma[t, :, :] * traj_info.Cm[t, :, :]) + 0.5 * \\\n",
    "                    mu[t, :].T.dot(traj_info.Cm[t, :, :]).dot(mu[t, :]) + \\\n",
    "                    mu[t, :].T.dot(traj_info.cv[t, :])\n",
    "        return predicted_cost\n",
    "\n",
    "    def forward(self, traj_distr, traj_info):\n",
    "        \"\"\"\n",
    "        Perform LQR forward pass. Computes state-action marginals from\n",
    "        dynamics and policy.\n",
    "        Args:\n",
    "            traj_distr: A linear Gaussian policy object.\n",
    "            traj_info: A TrajectoryInfo object.\n",
    "        Returns:\n",
    "            mu: A T x dX mean action vector.\n",
    "            sigma: A T x dX x dX covariance matrix.\n",
    "        \"\"\"\n",
    "        # Compute state-action marginals from specified conditional\n",
    "        # parameters and current traj_info.\n",
    "        T = traj_distr.T\n",
    "        dU = traj_distr.dU\n",
    "        dX = traj_distr.dX\n",
    "\n",
    "        # Constants.\n",
    "        idx_x = slice(dX)\n",
    "\n",
    "        # Allocate space.\n",
    "        sigma = np.zeros((T, dX+dU, dX+dU))\n",
    "        mu = np.zeros((T, dX+dU))\n",
    "\n",
    "        # Pull out dynamics.\n",
    "        Fm = traj_info.dynamics.Fm\n",
    "        fv = traj_info.dynamics.fv\n",
    "        dyn_covar = traj_info.dynamics.dyn_covar\n",
    "\n",
    "        # Set initial covariance (initial mu is always zero).\n",
    "        sigma[0, idx_x, idx_x] = traj_info.x0sigma\n",
    "        mu[0, idx_x] = traj_info.x0mu\n",
    "\n",
    "        for t in range(T):\n",
    "            sigma[t, :, :] = np.vstack([\n",
    "                np.hstack([\n",
    "                    sigma[t, idx_x, idx_x],\n",
    "                    sigma[t, idx_x, idx_x].dot(traj_distr.K[t, :, :].T)\n",
    "                ]),\n",
    "                np.hstack([\n",
    "                    traj_distr.K[t, :, :].dot(sigma[t, idx_x, idx_x]),\n",
    "                    traj_distr.K[t, :, :].dot(sigma[t, idx_x, idx_x]).dot(\n",
    "                        traj_distr.K[t, :, :].T\n",
    "                    ) + traj_distr.pol_covar[t, :, :]\n",
    "                ])\n",
    "            ])\n",
    "            mu[t, :] = np.hstack([\n",
    "                mu[t, idx_x],\n",
    "                traj_distr.K[t, :, :].dot(mu[t, idx_x]) + traj_distr.k[t, :]\n",
    "            ])\n",
    "            if t < T - 1:\n",
    "                sigma[t+1, idx_x, idx_x] = \\\n",
    "                        Fm[t, :, :].dot(sigma[t, :, :]).dot(Fm[t, :, :].T) + \\\n",
    "                        dyn_covar[t, :, :]\n",
    "                mu[t+1, idx_x] = Fm[t, :, :].dot(mu[t, :]) + fv[t, :]\n",
    "        return mu, sigma\n",
    "\n",
    "    def backward(self, prev_traj_distr, traj_info, eta, algorithm, m):\n",
    "        \"\"\"\n",
    "        Perform LQR backward pass. This computes a new linear Gaussian\n",
    "        policy object.\n",
    "        Args:\n",
    "            prev_traj_distr: A linear Gaussian policy object from\n",
    "                previous iteration.\n",
    "            traj_info: A TrajectoryInfo object.\n",
    "            eta: Dual variable.\n",
    "            algorithm: Algorithm object needed to compute costs.\n",
    "            m: Condition number.\n",
    "        Returns:\n",
    "            traj_distr: A new linear Gaussian policy.\n",
    "            new_eta: The updated dual variable. Updates happen if the\n",
    "                Q-function is not PD.\n",
    "        \"\"\"\n",
    "        # Constants.\n",
    "        T = prev_traj_distr.T\n",
    "        dU = prev_traj_distr.dU\n",
    "        dX = prev_traj_distr.dX\n",
    "\n",
    "        if self._update_in_bwd_pass:\n",
    "            traj_distr = prev_traj_distr.nans_like()\n",
    "        else:\n",
    "            traj_distr = prev_traj_distr.copy()\n",
    "\n",
    "        # Store pol_wt if necessary\n",
    "        if type(algorithm) == AlgorithmBADMM:\n",
    "            pol_wt = algorithm.cur[m].pol_info.pol_wt\n",
    "\n",
    "        idx_x = slice(dX)\n",
    "        idx_u = slice(dX, dX+dU)\n",
    "\n",
    "        # Pull out dynamics.\n",
    "        Fm = traj_info.dynamics.Fm\n",
    "        fv = traj_info.dynamics.fv\n",
    "\n",
    "        # Non-SPD correction terms.\n",
    "        del_ = self._hyperparams['del0']\n",
    "        if self.cons_per_step:\n",
    "            del_ = np.ones(T) * del_\n",
    "        eta0 = eta\n",
    "\n",
    "        # Run dynamic programming.\n",
    "        fail = True\n",
    "        while fail:\n",
    "            fail = False  # Flip to true on non-symmetric PD.\n",
    "\n",
    "            # Allocate.\n",
    "            Vxx = np.zeros((T, dX, dX))\n",
    "            Vx = np.zeros((T, dX))\n",
    "            Qtt = np.zeros((T, dX+dU, dX+dU))\n",
    "            Qt = np.zeros((T, dX+dU))\n",
    "\n",
    "            if not self._update_in_bwd_pass:\n",
    "                new_K, new_k = np.zeros((T, dU, dX)), np.zeros((T, dU))\n",
    "                new_pS = np.zeros((T, dU, dU))\n",
    "                new_ipS, new_cpS = np.zeros((T, dU, dU)), np.zeros((T, dU, dU))\n",
    "\n",
    "            fCm, fcv = algorithm.compute_costs(\n",
    "                    m, eta, augment=(not self.cons_per_step)\n",
    "            )\n",
    "\n",
    "            # Compute state-action-state function at each time step.\n",
    "            for t in range(T - 1, -1, -1):\n",
    "                # Add in the cost.\n",
    "                Qtt[t] = fCm[t, :, :]  # (X+U) x (X+U)\n",
    "                Qt[t] = fcv[t, :]  # (X+U) x 1\n",
    "\n",
    "                # Add in the value function from the next time step.\n",
    "                if t < T - 1:\n",
    "                    if type(algorithm) == AlgorithmBADMM:\n",
    "                        multiplier = (pol_wt[t+1] + eta)/(pol_wt[t] + eta)\n",
    "                    else:\n",
    "                        multiplier = 1.0\n",
    "                    Qtt[t] += multiplier * \\\n",
    "                            Fm[t, :, :].T.dot(Vxx[t+1, :, :]).dot(Fm[t, :, :])\n",
    "                    Qt[t] += multiplier * \\\n",
    "                            Fm[t, :, :].T.dot(Vx[t+1, :] +\n",
    "                                            Vxx[t+1, :, :].dot(fv[t, :]))\n",
    "\n",
    "                # Symmetrize quadratic component.\n",
    "                Qtt[t] = 0.5 * (Qtt[t] + Qtt[t].T)\n",
    "\n",
    "                if not self.cons_per_step:\n",
    "                    inv_term = Qtt[t, idx_u, idx_u]\n",
    "                    k_term = Qt[t, idx_u]\n",
    "                    K_term = Qtt[t, idx_u, idx_x]\n",
    "                else:\n",
    "                    inv_term = (1.0 / eta[t]) * Qtt[t, idx_u, idx_u] + \\\n",
    "                            prev_traj_distr.inv_pol_covar[t]\n",
    "                    k_term = (1.0 / eta[t]) * Qt[t, idx_u] - \\\n",
    "                            prev_traj_distr.inv_pol_covar[t].dot(prev_traj_distr.k[t])\n",
    "                    K_term = (1.0 / eta[t]) * Qtt[t, idx_u, idx_x] - \\\n",
    "                            prev_traj_distr.inv_pol_covar[t].dot(prev_traj_distr.K[t])\n",
    "                # Compute Cholesky decomposition of Q function action\n",
    "                # component.\n",
    "                try:\n",
    "                    U = sp.linalg.cholesky(inv_term)\n",
    "                    L = U.T\n",
    "                except LinAlgError as e:\n",
    "                    # Error thrown when Qtt[idx_u, idx_u] is not\n",
    "                    # symmetric positive definite.\n",
    "                    LOGGER.debug('LinAlgError: %s', e)\n",
    "                    fail = t if self.cons_per_step else True\n",
    "                    break\n",
    "\n",
    "                if self._hyperparams['update_in_bwd_pass']:\n",
    "                    # Store conditional covariance, inverse, and Cholesky.\n",
    "                    traj_distr.inv_pol_covar[t, :, :] = inv_term\n",
    "                    traj_distr.pol_covar[t, :, :] = sp.linalg.solve_triangular(\n",
    "                        U, sp.linalg.solve_triangular(L, np.eye(dU), lower=True)\n",
    "                    )\n",
    "                    traj_distr.chol_pol_covar[t, :, :] = sp.linalg.cholesky(\n",
    "                        traj_distr.pol_covar[t, :, :]\n",
    "                    )\n",
    "\n",
    "                    # Compute mean terms.\n",
    "                    traj_distr.k[t, :] = -sp.linalg.solve_triangular(\n",
    "                        U, sp.linalg.solve_triangular(L, k_term, lower=True)\n",
    "                    )\n",
    "                    traj_distr.K[t, :, :] = -sp.linalg.solve_triangular(\n",
    "                        U, sp.linalg.solve_triangular(L, K_term, lower=True)\n",
    "                    )\n",
    "                else:\n",
    "                    # Store conditional covariance, inverse, and Cholesky.\n",
    "                    new_ipS[t, :, :] = inv_term\n",
    "                    new_pS[t, :, :] = sp.linalg.solve_triangular(\n",
    "                        U, sp.linalg.solve_triangular(L, np.eye(dU), lower=True)\n",
    "                    )\n",
    "                    new_cpS[t, :, :] = sp.linalg.cholesky(\n",
    "                        new_pS[t, :, :]\n",
    "                    )\n",
    "\n",
    "                    # Compute mean terms.\n",
    "                    new_k[t, :] = -sp.linalg.solve_triangular(\n",
    "                        U, sp.linalg.solve_triangular(L, k_term, lower=True)\n",
    "                    )\n",
    "                    new_K[t, :, :] = -sp.linalg.solve_triangular(\n",
    "                        U, sp.linalg.solve_triangular(L, K_term, lower=True)\n",
    "                    )\n",
    "\n",
    "                # Compute value function.\n",
    "                if (self.cons_per_step or\n",
    "                    not self._hyperparams['update_in_bwd_pass']):\n",
    "                    Vxx[t, :, :] = Qtt[t, idx_x, idx_x] + \\\n",
    "                            traj_distr.K[t].T.dot(Qtt[t, idx_u, idx_u]).dot(traj_distr.K[t]) + \\\n",
    "                            (2 * Qtt[t, idx_x, idx_u]).dot(traj_distr.K[t])\n",
    "                    Vx[t, :] = Qt[t, idx_x].T + \\\n",
    "                            Qt[t, idx_u].T.dot(traj_distr.K[t]) + \\\n",
    "                            traj_distr.k[t].T.dot(Qtt[t, idx_u, idx_u]).dot(traj_distr.K[t]) + \\\n",
    "                            Qtt[t, idx_x, idx_u].dot(traj_distr.k[t])\n",
    "                else:\n",
    "                    Vxx[t, :, :] = Qtt[t, idx_x, idx_x] + \\\n",
    "                            Qtt[t, idx_x, idx_u].dot(traj_distr.K[t, :, :])\n",
    "                    Vx[t, :] = Qt[t, idx_x] + \\\n",
    "                            Qtt[t, idx_x, idx_u].dot(traj_distr.k[t, :])\n",
    "                Vxx[t, :, :] = 0.5 * (Vxx[t, :, :] + Vxx[t, :, :].T)\n",
    "\n",
    "            if not self._hyperparams['update_in_bwd_pass']:\n",
    "                traj_distr.K, traj_distr.k = new_K, new_k\n",
    "                traj_distr.pol_covar = new_pS\n",
    "                traj_distr.inv_pol_covar = new_ipS\n",
    "                traj_distr.chol_pol_covar = new_cpS\n",
    "\n",
    "            # Increment eta on non-SPD Q-function.\n",
    "            if fail:\n",
    "                if not self.cons_per_step:\n",
    "                    old_eta = eta\n",
    "                    eta = eta0 + del_\n",
    "                    LOGGER.debug('Increasing eta: %f -> %f', old_eta, eta)\n",
    "                    del_ *= 2  # Increase del_ exponentially on failure.\n",
    "                else:\n",
    "                    old_eta = eta[fail]\n",
    "                    eta[fail] = eta0[fail] + del_[fail]\n",
    "                    LOGGER.debug('Increasing eta %d: %f -> %f',\n",
    "                                 fail, old_eta, eta[fail])\n",
    "                    del_[fail] *= 2  # Increase del_ exponentially on failure.\n",
    "                if self.cons_per_step:\n",
    "                    fail_check = (eta[fail] >= 1e16)\n",
    "                else:\n",
    "                    fail_check = (eta >= 1e16)\n",
    "                if fail_check:\n",
    "                    if np.any(np.isnan(Fm)) or np.any(np.isnan(fv)):\n",
    "                        raise ValueError('NaNs encountered in dynamics!')\n",
    "                    raise ValueError('Failed to find PD solution even for very \\\n",
    "                            large eta (check that dynamics and cost are \\\n",
    "                            reasonably well conditioned)!')\n",
    "        return traj_distr, eta\n",
    "\n",
    "    def _conv_check(self, con, kl_step):\n",
    "        \"\"\"Function that checks whether dual gradient descent has converged.\"\"\"\n",
    "        if self.cons_per_step:\n",
    "            return all([abs(con[t]) < (0.1*kl_step[t]) for t in range(con.size)])\n",
    "        return abs(con) < 0.1 * kl_step\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Cost"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, utilities and functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "RAMP_CONSTANT = 1\n",
    "RAMP_LINEAR = 2\n",
    "RAMP_QUADRATIC = 3\n",
    "RAMP_FINAL_ONLY = 4\n",
    "\n",
    "# CostAction\n",
    "COST_ACTION = {\n",
    "    'wu': np.array([]),  # Torque penalties, must be 1 x dU numpy array.\n",
    "}\n",
    "\n",
    "# CostState\n",
    "COST_STATE = {\n",
    "    'ramp_option': RAMP_CONSTANT,  # How target cost ramps over time.\n",
    "    'l1': 0.0,\n",
    "    'l2': 1.0,\n",
    "    'alpha': 1e-2,\n",
    "    'wp_final_multiplier': 1.0,  # Weight multiplier on final time step.\n",
    "    'data_types': {\n",
    "        'JointAngle': {\n",
    "            'target_state': None,  # Target state - must be set.\n",
    "            'wp': None,  # State weights - must be set.\n",
    "        },\n",
    "    },\n",
    "}\n",
    "\n",
    "# CostSum\n",
    "COST_SUM = {\n",
    "    'costs': [],  # A list of hyperparam dictionaries for each cost.\n",
    "    'weights': [],  # Weight multipliers for each cost.\n",
    "}\n",
    "\n",
    "def get_ramp_multiplier(ramp_option, T, wp_final_multiplier=1.0):\n",
    "    \"\"\"\n",
    "    Return a time-varying multiplier.\n",
    "    Returns:\n",
    "        A (T,) float vector containing weights for each time step.\n",
    "    \"\"\"\n",
    "    if ramp_option == RAMP_CONSTANT:\n",
    "        wpm = np.ones(T)\n",
    "    elif ramp_option == RAMP_LINEAR:\n",
    "        wpm = (np.arange(T, dtype=np.float32) + 1) / T\n",
    "    elif ramp_option == RAMP_QUADRATIC:\n",
    "        wpm = ((np.arange(T, dtype=np.float32) + 1) / T) ** 2\n",
    "    elif ramp_option == RAMP_FINAL_ONLY:\n",
    "        wpm = np.zeros(T)\n",
    "        wpm[T-1] = 1.0\n",
    "    else:\n",
    "        raise ValueError('Unknown cost ramp requested!')\n",
    "    wpm[-1] *= wp_final_multiplier\n",
    "    return wpm\n",
    "\n",
    "def evall1l2term(wp, d, Jd, Jdd, l1, l2, alpha):\n",
    "    \"\"\"\n",
    "    Evaluate and compute derivatives for combined l1/l2 norm penalty.\n",
    "    loss = (0.5 * l2 * d^2) + (l1 * sqrt(alpha + d^2))\n",
    "    Args:\n",
    "        wp: T x D matrix with weights for each dimension and time step.\n",
    "        d: T x D states to evaluate norm on.\n",
    "        Jd: T x D x Dx Jacobian - derivative of d with respect to state.\n",
    "        Jdd: T x D x Dx x Dx Jacobian - 2nd derivative of d with respect\n",
    "            to state.\n",
    "        l1: l1 loss weight.\n",
    "        l2: l2 loss weight.\n",
    "        alpha: Constant added in square root.\n",
    "    \"\"\"\n",
    "    # Get trajectory length.\n",
    "    T, _ = d.shape\n",
    "\n",
    "    # Compute scaled quantities.\n",
    "    sqrtwp = np.sqrt(wp)\n",
    "    dsclsq = d * sqrtwp\n",
    "    dscl = d * wp\n",
    "    dscls = d * (wp ** 2)\n",
    "\n",
    "    # Compute total cost.\n",
    "    l = 0.5 * np.sum(dsclsq ** 2, axis=1) * l2 + \\\n",
    "            np.sqrt(alpha + np.sum(dscl ** 2, axis=1)) * l1\n",
    "\n",
    "    # First order derivative terms.\n",
    "    d1 = dscl * l2 + (\n",
    "        dscls / np.sqrt(alpha + np.sum(dscl ** 2, axis=1, keepdims=True)) * l1\n",
    "    )\n",
    "    lx = np.sum(Jd * np.expand_dims(d1, axis=2), axis=1)\n",
    "\n",
    "    # Second order terms.\n",
    "    psq = np.expand_dims(\n",
    "        np.sqrt(alpha + np.sum(dscl ** 2, axis=1, keepdims=True)), axis=1\n",
    "    )\n",
    "    d2 = l1 * (\n",
    "        (np.expand_dims(np.eye(wp.shape[1]), axis=0) *\n",
    "         (np.expand_dims(wp ** 2, axis=1) / psq)) -\n",
    "        ((np.expand_dims(dscls, axis=1) *\n",
    "          np.expand_dims(dscls, axis=2)) / psq ** 3)\n",
    "    )\n",
    "    d2 += l2 * (\n",
    "        np.expand_dims(wp, axis=2) * np.tile(np.eye(wp.shape[1]), [T, 1, 1])\n",
    "    )\n",
    "\n",
    "    d1_expand = np.expand_dims(np.expand_dims(d1, axis=-1), axis=-1)\n",
    "    sec = np.sum(d1_expand * Jdd, axis=1)\n",
    "\n",
    "    Jd_expand_1 = np.expand_dims(np.expand_dims(Jd, axis=2), axis=4)\n",
    "    Jd_expand_2 = np.expand_dims(np.expand_dims(Jd, axis=1), axis=3)\n",
    "    d2_expand = np.expand_dims(np.expand_dims(d2, axis=-1), axis=-1)\n",
    "    lxx = np.sum(np.sum(Jd_expand_1 * Jd_expand_2 * d2_expand, axis=1), axis=1)\n",
    "\n",
    "    lxx += 0.5 * sec + 0.5 * np.transpose(sec, [0, 2, 1])\n",
    "\n",
    "    return l, lx, lxx\n",
    "\n",
    "def evallogl2term(wp, d, Jd, Jdd, l1, l2, alpha):\n",
    "    \"\"\"\n",
    "    Evaluate and compute derivatives for combined l1/l2 norm penalty.\n",
    "    loss = (0.5 * l2 * d^2) + (0.5 * l1 * log(alpha + d^2))\n",
    "    Args:\n",
    "        wp: T x D matrix with weights for each dimension and time step.\n",
    "        d: T x D states to evaluate norm on.\n",
    "        Jd: T x D x Dx Jacobian - derivative of d with respect to state.\n",
    "        Jdd: T x D x Dx x Dx Jacobian - 2nd derivative of d with respect\n",
    "            to state.\n",
    "        l1: l1 loss weight.\n",
    "        l2: l2 loss weight.\n",
    "        alpha: Constant added in square root.\n",
    "    \"\"\"\n",
    "    # Get trajectory length.\n",
    "    T, _ = d.shape\n",
    "\n",
    "    # Compute scaled quantities.\n",
    "    sqrtwp = np.sqrt(wp)\n",
    "    dsclsq = d * sqrtwp\n",
    "    dscl = d * wp\n",
    "    dscls = d * (wp ** 2)\n",
    "\n",
    "    # Compute total cost.\n",
    "    l = 0.5 * np.sum(dsclsq ** 2, axis=1) * l2 + \\\n",
    "            0.5 * np.log(alpha + np.sum(dscl ** 2, axis=1)) * l1\n",
    "    # First order derivative terms.\n",
    "    d1 = dscl * l2 + (\n",
    "        dscls / (alpha + np.sum(dscl ** 2, axis=1, keepdims=True)) * l1\n",
    "    )\n",
    "    lx = np.sum(Jd * np.expand_dims(d1, axis=2), axis=1)\n",
    "\n",
    "    # Second order terms.\n",
    "    psq = np.expand_dims(\n",
    "        alpha + np.sum(dscl ** 2, axis=1, keepdims=True), axis=1\n",
    "    )\n",
    "    #TODO: Need * 2.0 somewhere in following line, or * 0.0 which is\n",
    "    #      wrong but better.\n",
    "    d2 = l1 * (\n",
    "        (np.expand_dims(np.eye(wp.shape[1]), axis=0) *\n",
    "         (np.expand_dims(wp ** 2, axis=1) / psq)) -\n",
    "        ((np.expand_dims(dscls, axis=1) *\n",
    "          np.expand_dims(dscls, axis=2)) / psq ** 2)\n",
    "    )\n",
    "    d2 += l2 * (\n",
    "        np.expand_dims(wp, axis=2) * np.tile(np.eye(wp.shape[1]), [T, 1, 1])\n",
    "    )\n",
    "\n",
    "    d1_expand = np.expand_dims(np.expand_dims(d1, axis=-1), axis=-1)\n",
    "    sec = np.sum(d1_expand * Jdd, axis=1)\n",
    "\n",
    "    Jd_expand_1 = np.expand_dims(np.expand_dims(Jd, axis=2), axis=4)\n",
    "    Jd_expand_2 = np.expand_dims(np.expand_dims(Jd, axis=1), axis=3)\n",
    "    d2_expand = np.expand_dims(np.expand_dims(d2, axis=-1), axis=-1)\n",
    "    lxx = np.sum(np.sum(Jd_expand_1 * Jd_expand_2 * d2_expand, axis=1), axis=1)\n",
    "\n",
    "    lxx += 0.5 * sec + 0.5 * np.transpose(sec, [0, 2, 1])\n",
    "\n",
    "    return l, lx, lxx\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Cost(object):\n",
    "    \"\"\" Cost superclass. \"\"\"\n",
    "    __metaclass__ = abc.ABCMeta\n",
    "\n",
    "    def __init__(self, hyperparams):\n",
    "        self._hyperparams = hyperparams\n",
    "\n",
    "    @abc.abstractmethod\n",
    "    def eval(self, sample):\n",
    "        \"\"\"\n",
    "        Evaluate cost function and derivatives.\n",
    "        Args:\n",
    "            sample:  A single sample.\n",
    "        \"\"\"\n",
    "        raise NotImplementedError(\"Must be implemented in subclass.\")\n",
    "\n",
    "class CostAction(Cost):\n",
    "    \"\"\" Computes torque penalties. \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        config = copy.deepcopy(COST_ACTION)\n",
    "        config.update(hyperparams)\n",
    "        Cost.__init__(self, config)\n",
    "\n",
    "    def eval(self, sample):\n",
    "        \"\"\"\n",
    "        Evaluate cost function and derivatives on a sample.\n",
    "        Args:\n",
    "            sample: A single sample\n",
    "        \"\"\"\n",
    "        sample_u = sample.get_U()\n",
    "        T = sample.T\n",
    "        Du = sample.dU\n",
    "        Dx = sample.dX\n",
    "        l = 0.5 * np.sum(self._hyperparams['wu'] * (sample_u ** 2), axis=1)\n",
    "        lu = self._hyperparams['wu'] * sample_u\n",
    "        lx = np.zeros((T, Dx))\n",
    "        luu = np.tile(np.diag(self._hyperparams['wu']), [T, 1, 1])\n",
    "        lxx = np.zeros((T, Dx, Dx))\n",
    "        lux = np.zeros((T, Du, Dx))\n",
    "        return l, lx, lu, lxx, luu, lux\n",
    "\n",
    "class CostState(Cost):\n",
    "    \"\"\" Computes l1/l2 distance to a fixed target state. \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        config = copy.deepcopy(COST_STATE)\n",
    "        config.update(hyperparams)\n",
    "        Cost.__init__(self, config)\n",
    "\n",
    "    def eval(self, sample):\n",
    "        \"\"\"\n",
    "        Evaluate cost function and derivatives on a sample.\n",
    "        Args:\n",
    "            sample:  A single sample\n",
    "        \"\"\"\n",
    "        T = sample.T\n",
    "        Du = sample.dU\n",
    "        Dx = sample.dX\n",
    "\n",
    "        final_l = np.zeros(T)\n",
    "        final_lu = np.zeros((T, Du))\n",
    "        final_lx = np.zeros((T, Dx))\n",
    "        final_luu = np.zeros((T, Du, Du))\n",
    "        final_lxx = np.zeros((T, Dx, Dx))\n",
    "        final_lux = np.zeros((T, Du, Dx))\n",
    "\n",
    "        for data_type in self._hyperparams['data_types']:\n",
    "            config = self._hyperparams['data_types'][data_type]\n",
    "            wp = config['wp']\n",
    "            tgt = config['target_state']\n",
    "            x = sample.get(data_type)\n",
    "            _, dim_sensor = x.shape\n",
    "\n",
    "            wpm = get_ramp_multiplier(\n",
    "                self._hyperparams['ramp_option'], T,\n",
    "                wp_final_multiplier=self._hyperparams['wp_final_multiplier']\n",
    "            )\n",
    "            wp = wp * np.expand_dims(wpm, axis=-1)\n",
    "            # Compute state penalty.\n",
    "            dist = x - tgt\n",
    "\n",
    "            # Evaluate penalty term.\n",
    "            l, ls, lss = evall1l2term(\n",
    "                wp, dist, np.tile(np.eye(dim_sensor), [T, 1, 1]),\n",
    "                np.zeros((T, dim_sensor, dim_sensor, dim_sensor)),\n",
    "                self._hyperparams['l1'], self._hyperparams['l2'],\n",
    "                self._hyperparams['alpha']\n",
    "            )\n",
    "\n",
    "            final_l += l\n",
    "\n",
    "            sample.agent.pack_data_x(final_lx, ls, data_types=[data_type])\n",
    "            sample.agent.pack_data_x(final_lxx, lss,\n",
    "                                     data_types=[data_type, data_type])\n",
    "        return final_l, final_lx, final_lu, final_lxx, final_luu, final_lux\n",
    "\n",
    "class CostSum(Cost):\n",
    "    \"\"\" A wrapper cost function that adds other cost functions. \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        config = copy.deepcopy(COST_SUM)\n",
    "        config.update(hyperparams)\n",
    "        Cost.__init__(self, config)\n",
    "\n",
    "        self._costs = []\n",
    "        self._weights = self._hyperparams['weights']\n",
    "\n",
    "        for cost in self._hyperparams['costs']:\n",
    "            self._costs.append(cost['type'](cost))\n",
    "\n",
    "    def eval(self, sample):\n",
    "        \"\"\"\n",
    "        Evaluate cost function and derivatives.\n",
    "        Args:\n",
    "            sample:  A single sample\n",
    "        \"\"\"\n",
    "        l, lx, lu, lxx, luu, lux = self._costs[0].eval(sample)\n",
    "\n",
    "        # Compute weighted sum of each cost value and derivatives.\n",
    "        weight = self._weights[0]\n",
    "        l = l * weight\n",
    "        lx = lx * weight\n",
    "        lu = lu * weight\n",
    "        lxx = lxx * weight\n",
    "        luu = luu * weight\n",
    "        lux = lux * weight\n",
    "        for i in range(1, len(self._costs)):\n",
    "            pl, plx, plu, plxx, pluu, plux = self._costs[i].eval(sample)\n",
    "            weight = self._weights[i]\n",
    "            l = l + pl * weight\n",
    "            lx = lx + plx * weight\n",
    "            lu = lu + plu * weight\n",
    "            lxx = lxx + plxx * weight\n",
    "            luu = luu + pluu * weight\n",
    "            lux = lux + plux * weight\n",
    "        return l, lx, lu, lxx, luu, lux"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### GUI and Datalogger\n",
    "Code related to the custom GUI coded by the group at Berkeley"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def generate_experiment_info(config):\n",
    "    \"\"\"\n",
    "    Generate experiment info, to be displayed by GPS Trainig GUI.\n",
    "    Assumes config is the config created in hyperparams.py\n",
    "    \"\"\"\n",
    "    common = config['common']\n",
    "    algorithm = config['algorithm']\n",
    "\n",
    "    if type(algorithm['cost']) == list:\n",
    "        algorithm_cost_type = algorithm['cost'][0]['type'].__name__\n",
    "        if (algorithm_cost_type) == 'CostSum':\n",
    "            algorithm_cost_type += '(%s)' % ', '.join(\n",
    "                    map(lambda cost: cost['type'].__name__,\n",
    "                        algorithm['cost'][0]['costs']))\n",
    "    else:\n",
    "        algorithm_cost_type = algorithm['cost']['type'].__name__\n",
    "        if (algorithm_cost_type) == 'CostSum':\n",
    "            algorithm_cost_type += '(%s)' % ', '.join(\n",
    "                    map(lambda cost: cost['type'].__name__,\n",
    "                        algorithm['cost']['costs']))\n",
    "\n",
    "    if 'dynamics' in algorithm:        \n",
    "        alg_dyn = str(algorithm['dynamics']['type'].__name__)\n",
    "    else:\n",
    "        alg_dyn = 'None'       \n",
    "\n",
    "    return (\n",
    "        'exp_name:   ' + str(common['experiment_name'])              + '\\n' +\n",
    "        'alg_type:   ' + str(algorithm['type'].__name__)             + '\\n' +\n",
    "        'alg_dyn:    ' + alg_dyn + '\\n' +\n",
    "        'alg_cost:   ' + str(algorithm_cost_type)                    + '\\n' +\n",
    "        'iterations: ' + str(config['iterations'])                   + '\\n' +\n",
    "        'conditions: ' + str(algorithm['conditions'])                + '\\n' +\n",
    "        'samples:    ' + str(config['num_samples'])                  + '\\n'\n",
    "    )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `DataLogger`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import logging\n",
    "try:\n",
    "   import cPickle as pickle\n",
    "except:\n",
    "   import pickle\n",
    "\n",
    "\n",
    "LOGGER = logging.getLogger(__name__)\n",
    "\n",
    "\n",
    "class DataLogger(object):\n",
    "    \"\"\"\n",
    "    This class pickles data into files and unpickles data from files.\n",
    "    TODO: Handle logging text to terminal, GUI text, and/or log file at\n",
    "        DEBUG, INFO, WARN, ERROR, FATAL levels.\n",
    "    TODO: Handle logging data to terminal, GUI text/plots, and/or data\n",
    "          files.\n",
    "    \"\"\"\n",
    "    def __init__(self):\n",
    "        pass\n",
    "\n",
    "    def pickle(self, filename, data):\n",
    "        \"\"\" Pickle data into file specified by filename. \"\"\"\n",
    "        pickle.dump(data, open(filename, 'wb'))\n",
    "\n",
    "    def unpickle(self, filename):\n",
    "        \"\"\" Unpickle data from file specified by filename. \"\"\"\n",
    "        try:\n",
    "            return pickle.load(open(filename, 'rb'))\n",
    "        except IOError:\n",
    "            LOGGER.debug('Unpickle error. Cannot find file: %s', filename)\n",
    "            return None\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Agents"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `Agent`\n",
    "Base agent class"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from gps_pb2 import ACTION\n",
    "\n",
    "# Agent\n",
    "AGENT = {\n",
    "    'dH': 0,\n",
    "    'x0var': 0,\n",
    "    'noisy_body_idx': np.array([]),\n",
    "    'noisy_body_var': np.array([]),\n",
    "    'pos_body_idx': np.array([]),\n",
    "    'pos_body_offset': np.array([]),\n",
    "    'smooth_noise': True,\n",
    "    'smooth_noise_var': 2.0,\n",
    "    'smooth_noise_renormalize': True,\n",
    "}\n",
    "\n",
    "class Agent(object):\n",
    "    \"\"\"\n",
    "    Agent superclass. The agent interacts with the environment to\n",
    "    collect samples.\n",
    "    \"\"\"\n",
    "    __metaclass__ = abc.ABCMeta\n",
    "\n",
    "    def __init__(self, hyperparams):\n",
    "        config = copy.deepcopy(AGENT)\n",
    "        config.update(hyperparams)\n",
    "        self._hyperparams = config\n",
    "\n",
    "        # Store samples, along with size/index information for samples.\n",
    "        self._samples = [[] for _ in range(self._hyperparams['conditions'])]\n",
    "        self.T = self._hyperparams['T']\n",
    "        self.dU = self._hyperparams['sensor_dims'][ACTION]\n",
    "\n",
    "        self.x_data_types = self._hyperparams['state_include']\n",
    "        self.obs_data_types = self._hyperparams['obs_include']\n",
    "        if 'meta_include' in self._hyperparams:\n",
    "            self.meta_data_types = self._hyperparams['meta_include']\n",
    "        else:\n",
    "            self.meta_data_types = []\n",
    "\n",
    "        # List of indices for each data type in state X.\n",
    "        self._state_idx, i = [], 0\n",
    "        for sensor in self.x_data_types:\n",
    "            dim = self._hyperparams['sensor_dims'][sensor]\n",
    "            self._state_idx.append(list(range(i, i+dim)))\n",
    "            i += dim\n",
    "        self.dX = i\n",
    "\n",
    "        # List of indices for each data type in observation.\n",
    "        self._obs_idx, i = [], 0\n",
    "        for sensor in self.obs_data_types:\n",
    "            dim = self._hyperparams['sensor_dims'][sensor]\n",
    "            self._obs_idx.append(list(range(i, i+dim)))\n",
    "            i += dim\n",
    "        self.dO = i\n",
    "\n",
    "        # List of indices for each data type in meta data.\n",
    "        self._meta_idx, i = [], 0\n",
    "        for sensor in self.meta_data_types:\n",
    "            dim = self._hyperparams['sensor_dims'][sensor]\n",
    "            self._meta_idx.append(list(range(i, i+dim)))\n",
    "            i += dim\n",
    "        self.dM = i\n",
    "\n",
    "        self._x_data_idx = {d: i for d, i in zip(self.x_data_types,\n",
    "                                                 self._state_idx)}\n",
    "        self._obs_data_idx = {d: i for d, i in zip(self.obs_data_types,\n",
    "                                                   self._obs_idx)}\n",
    "        self._meta_data_idx = {d: i for d, i in zip(self.meta_data_types,\n",
    "                                                   self._meta_idx)}\n",
    "\n",
    "    @abc.abstractmethod\n",
    "    def sample(self, policy, condition, verbose=True, save=True, noisy=True):\n",
    "        \"\"\"\n",
    "        Draw a sample from the environment, using the specified policy\n",
    "        and under the specified condition, with or without noise.\n",
    "        \"\"\"\n",
    "        raise NotImplementedError(\"Must be implemented in subclass.\")\n",
    "\n",
    "    def reset(self, condition):\n",
    "        \"\"\" Reset environment to the specified condition. \"\"\"\n",
    "        pass  # May be overridden in subclass.\n",
    "\n",
    "    def get_samples(self, condition, start=0, end=None):\n",
    "        \"\"\"\n",
    "        Return the requested samples based on the start and end indices.\n",
    "        Args:\n",
    "            start: Starting index of samples to return.\n",
    "            end: End index of samples to return.\n",
    "        \"\"\"\n",
    "        return (SampleList(self._samples[condition][start:]) if end is None\n",
    "                else SampleList(self._samples[condition][start:end]))\n",
    "\n",
    "    def clear_samples(self, condition=None):\n",
    "        \"\"\"\n",
    "        Reset the samples for a given condition, defaulting to all conditions.\n",
    "        Args:\n",
    "            condition: Condition for which to reset samples.\n",
    "        \"\"\"\n",
    "        if condition is None:\n",
    "            self._samples = [[] for _ in range(self._hyperparams['conditions'])]\n",
    "        else:\n",
    "            self._samples[condition] = []\n",
    "\n",
    "    def delete_last_sample(self, condition):\n",
    "        \"\"\" Delete the last sample from the specified condition. \"\"\"\n",
    "        self._samples[condition].pop()\n",
    "\n",
    "    def get_idx_x(self, sensor_name):\n",
    "        \"\"\"\n",
    "        Return the indices corresponding to a certain state sensor name.\n",
    "        Args:\n",
    "            sensor_name: The name of the sensor.\n",
    "        \"\"\"\n",
    "        return self._x_data_idx[sensor_name]\n",
    "\n",
    "    def get_idx_obs(self, sensor_name):\n",
    "        \"\"\"\n",
    "        Return the indices corresponding to a certain observation sensor name.\n",
    "        Args:\n",
    "            sensor_name: The name of the sensor.\n",
    "        \"\"\"\n",
    "        return self._obs_data_idx[sensor_name]\n",
    "\n",
    "    def pack_data_obs(self, existing_mat, data_to_insert, data_types,\n",
    "                      axes=None):\n",
    "        \"\"\"\n",
    "        Update the observation matrix with new data.\n",
    "        Args:\n",
    "            existing_mat: Current observation matrix.\n",
    "            data_to_insert: New data to insert into the existing matrix.\n",
    "            data_types: Name of the sensors to insert data for.\n",
    "            axes: Which axes to insert data. Defaults to the last axes.\n",
    "        \"\"\"\n",
    "        num_sensor = len(data_types)\n",
    "        if axes is None:\n",
    "            # If axes not specified, assume indexing on last dimensions.\n",
    "            axes = list(range(-1, -num_sensor - 1, -1))\n",
    "        else:\n",
    "            # Make sure number of sensors and axes are consistent.\n",
    "            if num_sensor != len(axes):\n",
    "                raise ValueError(\n",
    "                    'Length of sensors (%d) must equal length of axes (%d)',\n",
    "                    num_sensor, len(axes)\n",
    "                )\n",
    "\n",
    "        # Shape checks.\n",
    "        insert_shape = list(existing_mat.shape)\n",
    "        for i in range(num_sensor):\n",
    "            # Make sure to slice along X.\n",
    "            if existing_mat.shape[axes[i]] != self.dO:\n",
    "                raise ValueError('Axes must be along an dX=%d dimensional axis',\n",
    "                                 self.dO)\n",
    "            insert_shape[axes[i]] = len(self._obs_data_idx[data_types[i]])\n",
    "        if tuple(insert_shape) != data_to_insert.shape:\n",
    "            raise ValueError('Data has shape %s. Expected %s',\n",
    "                             data_to_insert.shape, tuple(insert_shape))\n",
    "\n",
    "        # Actually perform the slice.\n",
    "        index = [slice(None) for _ in range(len(existing_mat.shape))]\n",
    "        for i in range(num_sensor):\n",
    "            index[axes[i]] = slice(self._obs_data_idx[data_types[i]][0],\n",
    "                                   self._obs_data_idx[data_types[i]][-1] + 1)\n",
    "        existing_mat[index] = data_to_insert\n",
    "\n",
    "    def pack_data_meta(self, existing_mat, data_to_insert, data_types,\n",
    "                       axes=None):\n",
    "        \"\"\"\n",
    "        Update the meta data matrix with new data.\n",
    "        Args:\n",
    "            existing_mat: Current meta data matrix.\n",
    "            data_to_insert: New data to insert into the existing matrix.\n",
    "            data_types: Name of the sensors to insert data for.\n",
    "            axes: Which axes to insert data. Defaults to the last axes.\n",
    "        \"\"\"\n",
    "        num_sensor = len(data_types)\n",
    "        if axes is None:\n",
    "            # If axes not specified, assume indexing on last dimensions.\n",
    "            axes = list(range(-1, -num_sensor - 1, -1))\n",
    "        else:\n",
    "            # Make sure number of sensors and axes are consistent.\n",
    "            if num_sensor != len(axes):\n",
    "                raise ValueError(\n",
    "                    'Length of sensors (%d) must equal length of axes (%d)',\n",
    "                    num_sensor, len(axes)\n",
    "                )\n",
    "\n",
    "        # Shape checks.\n",
    "        insert_shape = list(existing_mat.shape)\n",
    "        for i in range(num_sensor):\n",
    "            # Make sure to slice along X.\n",
    "            if existing_mat.shape[axes[i]] != self.dM:\n",
    "                raise ValueError('Axes must be along an dX=%d dimensional axis',\n",
    "                                 self.dM)\n",
    "            insert_shape[axes[i]] = len(self._meta_data_idx[data_types[i]])\n",
    "        if tuple(insert_shape) != data_to_insert.shape:\n",
    "            raise ValueError('Data has shape %s. Expected %s',\n",
    "                             data_to_insert.shape, tuple(insert_shape))\n",
    "\n",
    "        # Actually perform the slice.\n",
    "        index = [slice(None) for _ in range(len(existing_mat.shape))]\n",
    "        for i in range(num_sensor):\n",
    "            index[axes[i]] = slice(self._meta_data_idx[data_types[i]][0],\n",
    "                                   self._meta_data_idx[data_types[i]][-1] + 1)\n",
    "        existing_mat[index] = data_to_insert\n",
    "\n",
    "    def pack_data_x(self, existing_mat, data_to_insert, data_types, axes=None):\n",
    "        \"\"\"\n",
    "        Update the state matrix with new data.\n",
    "        Args:\n",
    "            existing_mat: Current state matrix.\n",
    "            data_to_insert: New data to insert into the existing matrix.\n",
    "            data_types: Name of the sensors to insert data for.\n",
    "            axes: Which axes to insert data. Defaults to the last axes.\n",
    "        \"\"\"\n",
    "        num_sensor = len(data_types)\n",
    "        if axes is None:\n",
    "            # If axes not specified, assume indexing on last dimensions.\n",
    "            axes = list(range(-1, -num_sensor - 1, -1))\n",
    "        else:\n",
    "            # Make sure number of sensors and axes are consistent.\n",
    "            if num_sensor != len(axes):\n",
    "                raise ValueError(\n",
    "                    'Length of sensors (%d) must equal length of axes (%d)',\n",
    "                    num_sensor, len(axes)\n",
    "                )\n",
    "\n",
    "        # Shape checks.\n",
    "        insert_shape = list(existing_mat.shape)\n",
    "        for i in range(num_sensor):\n",
    "            # Make sure to slice along X.\n",
    "            if existing_mat.shape[axes[i]] != self.dX:\n",
    "                raise ValueError('Axes must be along an dX=%d dimensional axis',\n",
    "                                 self.dX)\n",
    "            insert_shape[axes[i]] = len(self._x_data_idx[data_types[i]])\n",
    "        if tuple(insert_shape) != data_to_insert.shape:\n",
    "            raise ValueError('Data has shape %s. Expected %s',\n",
    "                             data_to_insert.shape, tuple(insert_shape))\n",
    "\n",
    "        # Actually perform the slice.\n",
    "        index = [slice(None) for _ in range(len(existing_mat.shape))]\n",
    "        for i in range(num_sensor):\n",
    "            index[axes[i]] = slice(self._x_data_idx[data_types[i]][0],\n",
    "                                   self._x_data_idx[data_types[i]][-1] + 1)\n",
    "        existing_mat[index] = data_to_insert\n",
    "\n",
    "    def unpack_data_x(self, existing_mat, data_types, axes=None):\n",
    "        \"\"\"\n",
    "        Returns the requested data from the state matrix.\n",
    "        Args:\n",
    "            existing_mat: State matrix to unpack from.\n",
    "            data_types: Names of the sensor to unpack.\n",
    "            axes: Which axes to unpack along. Defaults to the last axes.\n",
    "        \"\"\"\n",
    "        num_sensor = len(data_types)\n",
    "        if axes is None:\n",
    "            # If axes not specified, assume indexing on last dimensions.\n",
    "            axes = list(range(-1, -num_sensor - 1, -1))\n",
    "        else:\n",
    "            # Make sure number of sensors and axes are consistent.\n",
    "            if num_sensor != len(axes):\n",
    "                raise ValueError(\n",
    "                    'Length of sensors (%d) must equal length of axes (%d)',\n",
    "                    num_sensor, len(axes)\n",
    "                )\n",
    "\n",
    "        # Shape checks.\n",
    "        for i in range(num_sensor):\n",
    "            # Make sure to slice along X.\n",
    "            if existing_mat.shape[axes[i]] != self.dX:\n",
    "                raise ValueError('Axes must be along an dX=%d dimensional axis',\n",
    "                                 self.dX)\n",
    "\n",
    "        # Actually perform the slice.\n",
    "        index = [slice(None) for _ in range(len(existing_mat.shape))]\n",
    "        for i in range(num_sensor):\n",
    "            index[axes[i]] = slice(self._x_data_idx[data_types[i]][0],\n",
    "                                   self._x_data_idx[data_types[i]][-1] + 1)\n",
    "        return existing_mat[index]\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `AgentBox2D`\n",
    "An agent for the Box2D simulator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import scipy.ndimage as sp_ndimage\n",
    "\n",
    "def generate_noise(T, dU, hyperparams):\n",
    "    \"\"\"\n",
    "    Generate a T x dU gaussian-distributed noise vector. This will\n",
    "    approximately have mean 0 and variance 1, ignoring smoothing.\n",
    "\n",
    "    Args:\n",
    "        T: Number of time steps.\n",
    "        dU: Dimensionality of actions.\n",
    "    Hyperparams:\n",
    "        smooth: Whether or not to perform smoothing of noise.\n",
    "        var : If smooth=True, applies a Gaussian filter with this\n",
    "            variance.\n",
    "        renorm : If smooth=True, renormalizes data to have variance 1\n",
    "            after smoothing.\n",
    "    \"\"\"\n",
    "    smooth, var = hyperparams['smooth_noise'], hyperparams['smooth_noise_var']\n",
    "    renorm = hyperparams['smooth_noise_renormalize']\n",
    "    noise = np.random.randn(T, dU)\n",
    "    if smooth:\n",
    "        # Smooth noise. This violates the controller assumption, but\n",
    "        # might produce smoother motions.\n",
    "        for i in range(dU):\n",
    "            noise[:, i] = sp_ndimage.filters.gaussian_filter(noise[:, i], var)\n",
    "        if renorm:\n",
    "            variance = np.var(noise, axis=0)\n",
    "            noise = noise / np.sqrt(variance)\n",
    "    return noise\n",
    "\n",
    "\n",
    "def setup(value, n):\n",
    "    \"\"\" Go through various types of hyperparameters. \"\"\"\n",
    "    if not isinstance(value, list):\n",
    "        try:\n",
    "            return [value.copy() for _ in range(n)]\n",
    "        except AttributeError:\n",
    "            return [value for _ in range(n)]\n",
    "    assert len(value) == n, \\\n",
    "            'Number of elements must match number of conditions or 1.'\n",
    "    return value\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from copy import deepcopy\n",
    "from gps_pb2 import ACTION\n",
    "\n",
    "AGENT_BOX2D = {\n",
    "    'render': True,\n",
    "}\n",
    "\n",
    "class AgentBox2D(Agent):\n",
    "    \"\"\"\n",
    "    All communication between the algorithms and Box2D is done through\n",
    "    this class.\n",
    "    \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        config = deepcopy(AGENT_BOX2D)\n",
    "        config.update(hyperparams)\n",
    "        Agent.__init__(self, config)\n",
    "\n",
    "        self._setup_conditions()\n",
    "        self._setup_world(self._hyperparams[\"world\"],\n",
    "                          self._hyperparams[\"target_state\"],\n",
    "                          self._hyperparams[\"render\"])\n",
    "\n",
    "    def _setup_conditions(self):\n",
    "        \"\"\"\n",
    "        Helper method for setting some hyperparameters that may vary by\n",
    "        condition.\n",
    "        \"\"\"\n",
    "        conds = self._hyperparams['conditions']\n",
    "        for field in ('x0', 'x0var', 'pos_body_idx', 'pos_body_offset',\n",
    "                      'noisy_body_idx', 'noisy_body_var'):\n",
    "            self._hyperparams[field] = setup(self._hyperparams[field], conds)\n",
    "\n",
    "    def _setup_world(self, world, target, render):\n",
    "        \"\"\"\n",
    "        Helper method for handling setup of the Box2D world.\n",
    "        \"\"\"\n",
    "        self.x0 = self._hyperparams[\"x0\"]\n",
    "        self._worlds = [world(self.x0[i], target, render)\n",
    "                        for i in range(self._hyperparams['conditions'])]\n",
    "\n",
    "\n",
    "    def sample(self, policy, condition, verbose=False, save=True, noisy=True):\n",
    "        \"\"\"\n",
    "        Runs a trial and constructs a new sample containing information\n",
    "        about the trial.\n",
    "\n",
    "        Args:\n",
    "            policy: Policy to to used in the trial.\n",
    "            condition (int): Which condition setup to run.\n",
    "            verbose (boolean): Whether or not to plot the trial (not used here).\n",
    "            save (boolean): Whether or not to store the trial into the samples.\n",
    "            noisy (boolean): Whether or not to use noise during sampling.\n",
    "        \"\"\"\n",
    "        self._worlds[condition].run()\n",
    "        self._worlds[condition].reset_world()\n",
    "        b2d_X = self._worlds[condition].get_state()\n",
    "        new_sample = self._init_sample(b2d_X)\n",
    "        U = np.zeros([self.T, self.dU])\n",
    "        if noisy:\n",
    "            noise = generate_noise(self.T, self.dU, self._hyperparams)\n",
    "        else:\n",
    "            noise = np.zeros((self.T, self.dU))\n",
    "        for t in range(self.T):\n",
    "            X_t = new_sample.get_X(t=t)\n",
    "            obs_t = new_sample.get_obs(t=t)\n",
    "            U[t, :] = policy.act(X_t, obs_t, t, noise[t, :])\n",
    "            if (t+1) < self.T:\n",
    "                for _ in range(self._hyperparams['substeps']):\n",
    "                    self._worlds[condition].run_next(U[t, :])\n",
    "                b2d_X = self._worlds[condition].get_state()\n",
    "                self._set_sample(new_sample, b2d_X, t)\n",
    "        new_sample.set(ACTION, U)\n",
    "        if save:\n",
    "            self._samples[condition].append(new_sample)\n",
    "        return new_sample\n",
    "\n",
    "    def _init_sample(self, b2d_X):\n",
    "        \"\"\"\n",
    "        Construct a new sample and fill in the first time step.\n",
    "        \"\"\"\n",
    "        sample = Sample(self)\n",
    "        self._set_sample(sample, b2d_X, -1)\n",
    "        return sample\n",
    "\n",
    "    def _set_sample(self, sample, b2d_X, t):\n",
    "        for sensor in b2d_X.keys():\n",
    "            sample.set(sensor, np.array(b2d_X[sensor]), t=t+1)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Worlds"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `fwSettings`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class fwSettings(object):\n",
    "    \"\"\" This class contains the settings for Box2D's framwork. \"\"\"\n",
    "    backend = 'pygame'\n",
    "\n",
    "    # Physics options\n",
    "    hz = 20.0\n",
    "    velocityIterations = 8\n",
    "    positionIterations = 3\n",
    "    enableWarmStarting = True\n",
    "    enableContinuous = True\n",
    "    enableSubStepping = False\n",
    "\n",
    "    # Drawing\n",
    "    drawStats = True\n",
    "    drawShapes = True\n",
    "    drawJoints = True\n",
    "    drawCoreShapes = False\n",
    "    drawAABBs = False\n",
    "    drawOBBs = False\n",
    "    drawPairs = False\n",
    "    drawContactPoints = False\n",
    "    maxContactPoints = 100\n",
    "    drawContactNormals = False\n",
    "    drawFPS = True\n",
    "    drawMenu = True             # toggle by pressing F1\n",
    "    drawCOMs = False            # Centers of mass\n",
    "    pointSize = 2.5             # pixel radius for drawing points\n",
    "\n",
    "    # Miscellaneous testbed options\n",
    "    pause = False\n",
    "    singleStep = False\n",
    "    onlyInit = False\n",
    "\n",
    "#             text                  variable\n",
    "checkboxes = ((\"Warm Starting\", \"enableWarmStarting\"),\n",
    "              (\"Time of Impact\", \"enableContinuous\"),\n",
    "              (\"Sub-Stepping\", \"enableSubStepping\"),\n",
    "              (\"Draw\", None),\n",
    "              (\"Shapes\", \"drawShapes\"),\n",
    "              (\"Joints\", \"drawJoints\"),\n",
    "              (\"AABBs\", \"drawAABBs\"),\n",
    "              (\"Pairs\", \"drawPairs\"),\n",
    "              (\"Contact Points\", \"drawContactPoints\"),\n",
    "              (\"Contact Normals\", \"drawContactNormals\"),\n",
    "              (\"Center of Masses\", \"drawCOMs\"),\n",
    "              (\"Statistics\", \"drawStats\"),\n",
    "              (\"FPS\", \"drawFPS\"),\n",
    "              (\"Control\", None),\n",
    "              (\"Pause\" \"pause\"),\n",
    "              (\"Single Step\", \"singleStep\"))\n",
    "\n",
    "sliders = [\n",
    "    {'name' : 'hz', 'text' : 'Hertz', 'min' : 5, 'max' : 200},\n",
    "    {'name' : 'positionIterations', 'text' :\n",
    "     'Pos Iters', 'min' : 0, 'max' : 100},\n",
    "    {'name' : 'velocityIterations', 'text' :\n",
    "     'Vel Iters', 'min' : 1, 'max' : 500},\n",
    "]\n",
    "\n",
    "\n",
    "list_options = [i for i in dir(fwSettings) if not i.startswith('_')]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Framework: `fwQueryCallback` and `FrameworkBase`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "import Box2D as b2\n",
    "from time import time\n",
    "\n",
    "class fwQueryCallback(b2.b2QueryCallback):\n",
    "    \"\"\"\n",
    "    This callback for each fixture in the world.\n",
    "    \"\"\"\n",
    "    def __init__(self, p):\n",
    "        super(fwQueryCallback, self).__init__()\n",
    "        self.point = p\n",
    "        self.fixture = None\n",
    "\n",
    "    def ReportFixture(self, fixture):\n",
    "        \"\"\"\n",
    "        This method is called to query for a fixture.\n",
    "        \"\"\"\n",
    "        body = fixture.body\n",
    "        if body.type == b2.b2_dynamicBody:\n",
    "            inside = fixture.TestPoint(self.point)\n",
    "            if inside:\n",
    "                self.fixture = fixture\n",
    "                # We found the object, so stop the query\n",
    "                return False\n",
    "        # Continue the query\n",
    "        return True\n",
    "\n",
    "class FrameworkBase(b2.b2ContactListener):\n",
    "    \"\"\"\n",
    "    The base of the main Box2D GUI framework.\n",
    "\n",
    "    \"\"\"\n",
    "    name = \"None\"\n",
    "    description = None\n",
    "    TEXTLINE_START = 30\n",
    "    colors = {\n",
    "        'joint_line' : b2.b2Color(0.8, 0.8, 0.8),\n",
    "        'contact_add' : b2.b2Color(0.3, 0.95, 0.3),\n",
    "        'contact_persist' : b2.b2Color(0.3, 0.3, 0.95),\n",
    "        'contact_normal' : b2.b2Color(0.4, 0.9, 0.4),\n",
    "    }\n",
    "\n",
    "    def __reset(self):\n",
    "        \"\"\" Reset all of the variables to their starting values.\n",
    "        Not to be called except at initialization.\"\"\"\n",
    "        # Box2D-related\n",
    "        self.points = []\n",
    "        self.settings = fwSettings\n",
    "        self.using_contacts = False\n",
    "        self.stepCount = 0\n",
    "\n",
    "        # Box2D-callbacks\n",
    "        self.destructionListener = None\n",
    "        self.renderer = None\n",
    "\n",
    "    def __init__(self):\n",
    "        super(FrameworkBase, self).__init__()\n",
    "\n",
    "        self.__reset()\n",
    "\n",
    "        # Box2D Initialization\n",
    "        self.world = b2.b2World(gravity=(0, -10), doSleep=True)\n",
    "\n",
    "        self.world.contactListener = self\n",
    "        self.t_steps, self.t_draws = [], []\n",
    "\n",
    "    def __del__(self):\n",
    "        pass\n",
    "\n",
    "    def Step(self, settings, action=None):\n",
    "        \"\"\"\n",
    "        The main physics step.\n",
    "\n",
    "        Takes care of physics drawing\n",
    "        (callbacks are executed after the world.Step() )\n",
    "        and drawing additional information.\n",
    "        \"\"\"\n",
    "        assert action is None,\\\n",
    "            'action should only be used in subclass'\n",
    "\n",
    "        self.stepCount += 1\n",
    "        # Don't do anything if the setting's Hz are <= 0\n",
    "        if settings.hz > 0.0:\n",
    "            timeStep = 1.0 / settings.hz\n",
    "        else:\n",
    "            timeStep = 0.0\n",
    "\n",
    "        # Set the flags based on what the settings show\n",
    "        if self.renderer:\n",
    "            self.renderer.flags = dict(\n",
    "                drawShapes=settings.drawShapes,\n",
    "                drawJoints=settings.drawJoints,\n",
    "                drawAABBs=settings.drawAABBs,\n",
    "                drawPairs=settings.drawPairs,\n",
    "                drawCOMs=settings.drawCOMs,\n",
    "                convertVertices=isinstance(self.renderer, b2.b2DrawExtended)\n",
    "            )\n",
    "\n",
    "        # Set the other settings that aren't contained in the flags\n",
    "        self.world.warmStarting = settings.enableWarmStarting\n",
    "        self.world.continuousPhysics = settings.enableContinuous\n",
    "        self.world.subStepping = settings.enableSubStepping\n",
    "\n",
    "        # Reset the collision points\n",
    "        self.points = []\n",
    "\n",
    "        # Tell Box2D to step\n",
    "        t_step = time()\n",
    "        self.world.Step(timeStep, settings.velocityIterations,\n",
    "                        settings.positionIterations)\n",
    "        t_step = time()-t_step\n",
    "\n",
    "        # Update the debug draw settings so that the vertices will be properly\n",
    "        # converted to screen coordinates\n",
    "        t_draw = time()\n",
    "        if self.renderer:\n",
    "            self.renderer.StartDraw()\n",
    "\n",
    "        self.world.DrawDebugData()\n",
    "\n",
    "        if self.renderer:\n",
    "\n",
    "\n",
    "            # Draw each of the contact points in different colors.\n",
    "            if self.settings.drawContactPoints:\n",
    "                for point in self.points:\n",
    "                    if point['state'] == b2.b2_addState:\n",
    "                        self.renderer.DrawPoint(self.renderer.to_screen(\n",
    "                            point['position']), settings.pointSize,\n",
    "                                                self.colors['contact_add'])\n",
    "                    elif point['state'] == b2.b2_persistState:\n",
    "                        self.renderer.DrawPoint(self.renderer.to_screen(\n",
    "                            point['position']), settings.pointSize,\n",
    "                                                self.colors['contact_persist'])\n",
    "\n",
    "            if settings.drawContactNormals:\n",
    "                for point in self.points:\n",
    "                    p1 = self.renderer.to_screen(point['position'])\n",
    "                    p2 = self.renderer.axisScale * point['normal'] + p1\n",
    "                    self.renderer.DrawSegment(p1, p2,\n",
    "                                              self.colors['contact_normal'])\n",
    "\n",
    "            self.renderer.EndDraw()\n",
    "            t_draw = time()-t_draw\n",
    "\n",
    "            t_draw = max(b2.b2_epsilon, t_draw)\n",
    "            t_step = max(b2.b2_epsilon, t_step)\n",
    "\n",
    "\n",
    "            self.t_draws.append(1.0/t_draw)\n",
    "            self.t_steps.append(1.0/t_step)\n",
    "\n",
    "\n",
    "    def SimulationLoop(self, action):\n",
    "        \"\"\"\n",
    "        The main simulation loop. Don't override this, override Step instead.\n",
    "        \"\"\"\n",
    "\n",
    "        # Reset the text line to start the text from the top\n",
    "        self.textLine = self.TEXTLINE_START\n",
    "\n",
    "        # Draw the name of the test running\n",
    "        self.Print(self.name, (127, 127, 255))\n",
    "\n",
    "        if self.description:\n",
    "            # Draw the name of the test running\n",
    "            for s in self.description.split('\\n'):\n",
    "                self.Print(s, (127, 255, 127))\n",
    "\n",
    "        self.Step(self.settings, action)\n",
    "\n",
    "    def PreSolve(self, contact, old_manifold):\n",
    "        \"\"\"\n",
    "        This is a critical function when there are many contacts in the world.\n",
    "        It should be optimized as much as possible.\n",
    "        \"\"\"\n",
    "        if not (self.settings.drawContactPoints or\n",
    "                self.settings.drawContactNormals or self.using_contacts):\n",
    "            return\n",
    "        elif len(self.points) > self.settings.maxContactPoints:\n",
    "            return\n",
    "\n",
    "        manifold = contact.manifold\n",
    "        if manifold.pointCount == 0:\n",
    "            return\n",
    "\n",
    "        _, state2 = b2.b2GetPointStates(old_manifold, manifold)\n",
    "        if not state2:\n",
    "            return\n",
    "\n",
    "        worldManifold = contact.worldManifold\n",
    "\n",
    "        for i, _ in enumerate(state2):\n",
    "            self.points.append(\n",
    "                {\n",
    "                    'fixtureA' : contact.fixtureA,\n",
    "                    'fixtureB' : contact.fixtureB,\n",
    "                    'position' : worldManifold.points[i],\n",
    "                    'normal' : worldManifold.normal,\n",
    "                    'state' : state2[i]\n",
    "                })\n",
    "\n",
    "#print(fwSettings.backend.lower()) # pygame\n",
    "#print('gps.agent.box2d.'+'%s_framework' % (fwSettings.backend.lower())) # gps.agent.box2d.pygame_framework\n",
    "#print('%sFramework'%fwSettings.backend.capitalize()) # PygameFramework\n",
    "#print('%sFramework' %fwSettings.backend.capitalize()) # PygameFramework\n",
    "            \n",
    "#framework_module = __import__('gps.agent.box2d.'+'%s_framework' %\n",
    "#                              (fwSettings.backend.lower()),\n",
    "#                              fromlist=['%sFramework'\n",
    "#                                        %fwSettings.backend.capitalize()])\n",
    "#Framework = getattr(framework_module, '%sFramework' %\n",
    "#                    fwSettings.backend.capitalize())\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "import Box2D as b2\n",
    "import pygame\n",
    "\n",
    "GUIEnabled = False\n",
    "\n",
    "class PygameDraw(b2.b2DrawExtended):\n",
    "    \"\"\"\n",
    "    This debug draw class accepts callbacks from Box2D and\n",
    "    handles all of the rendering.\n",
    "    \"\"\"\n",
    "    surface = None\n",
    "    axisScale = 10.0\n",
    "    def __init__(self, test=None, **kwargs):\n",
    "        b2.b2DrawExtended.__init__(self, **kwargs)\n",
    "        self.flipX = False\n",
    "        self.flipY = True\n",
    "        self.convertVertices = True\n",
    "        self.test = test\n",
    "\n",
    "    def StartDraw(self):\n",
    "        \"\"\"\n",
    "        Called by renderer before drawing.\n",
    "        \"\"\"\n",
    "        self.zoom = self.test.viewZoom\n",
    "        self.center = self.test.viewCenter\n",
    "        self.offset = self.test.viewOffset\n",
    "        self.screenSize = self.test.screenSize\n",
    "\n",
    "    def EndDraw(self):\n",
    "        \"\"\"\n",
    "        Called by renderer when finished drawing.\n",
    "        \"\"\"\n",
    "\n",
    "        pass\n",
    "\n",
    "    def DrawPoint(self, p, size, color):\n",
    "        \"\"\"\n",
    "        Draw a single point at point p given a pixel size and color.\n",
    "        \"\"\"\n",
    "        self.DrawCircle(p, size/self.zoom, color, drawwidth=0)\n",
    "\n",
    "    def DrawAABB(self, aabb, color):\n",
    "        \"\"\"\n",
    "        Draw a wireframe around the AABB with the given color.\n",
    "        \"\"\"\n",
    "        points = [(aabb.lowerBound.x, aabb.lowerBound.y),\n",
    "                  (aabb.upperBound.x, aabb.lowerBound.y),\n",
    "                  (aabb.upperBound.x, aabb.upperBound.y),\n",
    "                  (aabb.lowerBound.x, aabb.upperBound.y)]\n",
    "\n",
    "        pygame.draw.aalines(self.surface, color, True, points)\n",
    "\n",
    "    def DrawSegment(self, p1, p2, color):\n",
    "        \"\"\"\n",
    "        Draw the line segment from p1-p2 with the specified color.\n",
    "        \"\"\"\n",
    "        pygame.draw.aaline(self.surface, color.bytes, p1, p2)\n",
    "\n",
    "    def DrawTransform(self, xf):\n",
    "        \"\"\"\n",
    "        Draw the transform xf on the screen\n",
    "        \"\"\"\n",
    "        p1 = xf.position\n",
    "        p2 = self.to_screen(p1 + self.axisScale * xf.R.col1)\n",
    "        p3 = self.to_screen(p1 + self.axisScale * xf.R.col2)\n",
    "        p1 = self.to_screen(p1)\n",
    "\n",
    "        pygame.draw.aaline(self.surface, (255, 0, 0), p1, p2)\n",
    "        pygame.draw.aaline(self.surface, (0, 255, 0), p1, p3)\n",
    "\n",
    "    def DrawCircle(self, center, radius, color, drawwidth=1):\n",
    "        \"\"\"\n",
    "        Draw a wireframe circle given the center, radius, and color.\n",
    "        \"\"\"\n",
    "        radius *= self.zoom\n",
    "        if radius < 1:\n",
    "            radius = 1\n",
    "        else: radius = int(radius)\n",
    "\n",
    "        pygame.draw.circle(self.surface, color.bytes, center, radius, drawwidth)\n",
    "\n",
    "    def DrawSolidCircle(self, center, radius, axis, color):\n",
    "        \"\"\"\n",
    "        Draw a solid circle given the center, radius, and color.\n",
    "        \"\"\"\n",
    "        radius *= self.zoom\n",
    "        if radius < 1:\n",
    "            radius = 1\n",
    "        else: radius = int(radius)\n",
    "\n",
    "        pygame.draw.circle(self.surface, (color/2).bytes+[127],\n",
    "                           center, radius, 0)\n",
    "        pygame.draw.circle(self.surface, color.bytes, center, radius, 1)\n",
    "        pygame.draw.aaline(self.surface, (255, 0, 0), center,\n",
    "                           (center[0] - radius*axis[0], center[1] +\n",
    "                            radius*axis[1]))\n",
    "\n",
    "    def DrawPolygon(self, vertices, color):\n",
    "        \"\"\"\n",
    "        Draw a wireframe polygon given the screen vertices with the given color.\n",
    "        \"\"\"\n",
    "        if not vertices:\n",
    "            return\n",
    "\n",
    "        if len(vertices) == 2:\n",
    "            pygame.draw.aaline(self.surface, color.bytes, vertices[0], vertices)\n",
    "        else:\n",
    "            pygame.draw.polygon(self.surface, color.bytes, vertices, 1)\n",
    "\n",
    "    def DrawSolidPolygon(self, vertices, color):\n",
    "        \"\"\"\n",
    "        Draw a filled polygon given the screen vertices with the given color.\n",
    "        \"\"\"\n",
    "        if not vertices:\n",
    "            return\n",
    "\n",
    "        if len(vertices) == 2:\n",
    "            pygame.draw.aaline(self.surface, color.bytes, vertices[0],\n",
    "                               vertices[1])\n",
    "        else:\n",
    "            pygame.draw.polygon(self.surface, (color/2).bytes+[127],\n",
    "                                vertices, 0)\n",
    "            pygame.draw.polygon(self.surface, color.bytes, vertices, 1)\n",
    "\n",
    "class PygameFramework(FrameworkBase):\n",
    "    \"\"\"\n",
    "    This class is the framework for running the simulation\n",
    "    \"\"\"\n",
    "\n",
    "    def __reset(self):\n",
    "        # Screen/rendering-related\n",
    "        self._viewZoom = 10.0\n",
    "        self._viewCenter = None\n",
    "        self._viewOffset = None\n",
    "        self.screenSize = None\n",
    "        self.rMouseDown = False\n",
    "        self.textLine = 30\n",
    "        self.font = None\n",
    "        self.fps = 0\n",
    "\n",
    "        # GUI-related (PGU)\n",
    "        self.gui_app = None\n",
    "        self.gui_table = None\n",
    "\n",
    "    def __init__(self):\n",
    "        super(PygameFramework, self).__init__()\n",
    "\n",
    "        self.__reset()\n",
    "        print('Initializing pygame framework...')\n",
    "        # Pygame Initialization\n",
    "        pygame.init()\n",
    "        caption = \"Python Box2D Testbed - \" + self.name\n",
    "        pygame.display.set_caption(caption)\n",
    "\n",
    "        self.screen = pygame.display.set_mode((640, 480))\n",
    "        self.screenSize = b2.b2Vec2(*self.screen.get_size())\n",
    "\n",
    "        self.renderer = PygameDraw(surface=self.screen, test=self)\n",
    "        self.world.renderer = self.renderer\n",
    "\n",
    "\n",
    "        self.font = pygame.font.Font(None, 15)\n",
    "\n",
    "        self.viewCenter = (0, 20.0)\n",
    "        self.groundbody = self.world.CreateBody()\n",
    "\n",
    "    def setCenter(self, value):\n",
    "        \"\"\"\n",
    "        Updates the view offset based on the center of the screen.\n",
    "        \"\"\"\n",
    "        self._viewCenter = b2.b2Vec2(*value)\n",
    "        self._viewCenter *= self._viewZoom\n",
    "        self._viewOffset = self._viewCenter - self.screenSize/2\n",
    "\n",
    "    def setZoom(self, zoom):\n",
    "        \"\"\"\n",
    "        Tells the display the zoom.\n",
    "        \"\"\"\n",
    "        self._viewZoom = zoom\n",
    "\n",
    "    viewZoom = property(lambda self: self._viewZoom, setZoom,\n",
    "                        doc='Zoom factor for the display')\n",
    "    viewCenter = property(lambda self: self._viewCenter/self._viewZoom,\n",
    "                          setCenter, doc='Screen center in camera coordinates')\n",
    "    viewOffset = property(lambda self: self._viewOffset,\n",
    "                          doc='Offset of the top-left corner of the screen')\n",
    "\n",
    "\n",
    "    def run(self):\n",
    "        \"\"\"\n",
    "        Begins the draw loopn and tells the GUI to paint itself.\n",
    "        \"\"\"\n",
    "\n",
    "        # If any of the test constructors update the settings, reflect\n",
    "        # those changes on the GUI before running\n",
    "        if GUIEnabled:\n",
    "            self.gui_table.updateGUI(self.settings)\n",
    "        self.clock = pygame.time.Clock()\n",
    "        self.screen.fill((0, 0, 0))\n",
    "\n",
    "            # Run the simulation loop\n",
    "        self.SimulationLoop([0, 0, 0])\n",
    "\n",
    "        if GUIEnabled and self.settings.drawMenu:\n",
    "            self.gui_app.paint(self.screen)\n",
    "\n",
    "        pygame.display.flip()\n",
    "        self.clock.tick(self.settings.hz)\n",
    "        self.fps = self.clock.get_fps()\n",
    "\n",
    "    def run_next(self, action):\n",
    "        \"\"\"\n",
    "        Updates the screen and tells the GUI to paint itself.\n",
    "        \"\"\"\n",
    "        self.screen.fill((0, 0, 0))\n",
    "\n",
    "        # Run the simulation loop\n",
    "        self.SimulationLoop(action)\n",
    "        if GUIEnabled and self.settings.drawMenu:\n",
    "            self.gui_app.paint(self.screen)\n",
    "\n",
    "        pygame.display.flip()\n",
    "        self.clock.tick(self.settings.hz)\n",
    "        self.fps = self.clock.get_fps()\n",
    "\n",
    "\n",
    "\n",
    "    def Step(self, settings):\n",
    "        \"\"\"\n",
    "        Updates the simulation\n",
    "        \"\"\"\n",
    "        if GUIEnabled:\n",
    "            self.gui_table.updateSettings(self.settings)\n",
    "\n",
    "        super(PygameFramework, self).Step(settings)\n",
    "\n",
    "        if GUIEnabled:\n",
    "            self.gui_table.updateGUI(self.settings)\n",
    "\n",
    "    def ConvertScreenToWorld(self, x, y):\n",
    "        \"\"\"\n",
    "        Converts the display screen to the simulation's coordinates.\n",
    "        \"\"\"\n",
    "        return b2.b2Vec2((x + self.viewOffset.x) / self.viewZoom,\n",
    "                         ((self.screenSize.y - y + self.viewOffset.y)\n",
    "                          / self.viewZoom))\n",
    "\n",
    "    def DrawStringAt(self, x, y, s, color=(229, 153, 153, 255)):\n",
    "        \"\"\"\n",
    "        Draw some text, str, at screen coordinates (x, y).\n",
    "        \"\"\"\n",
    "        self.screen.blit(self.font.render(s, True, color), (x, y))\n",
    "\n",
    "    def Print(self, s, color=(229, 153, 153, 255)):\n",
    "        \"\"\"\n",
    "        Draw some text at the top status lines\n",
    "        and advance to the next line.\n",
    "        \"\"\"\n",
    "        self.screen.blit(self.font.render(s, True, color), (5, self.textLine))\n",
    "        self.textLine += 15\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `ArmWorld`"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "import Box2D as b2\n",
    "#from framework import Framework\n",
    "from gps_pb2 import JOINT_ANGLES, JOINT_VELOCITIES, END_EFFECTOR_POINTS\n",
    "\n",
    "#class ArmWorld(Framework):\n",
    "class ArmWorld(PygameFramework):\n",
    "    \"\"\" This class defines the 2 Link Arm and its environment.\"\"\"\n",
    "    name = \"2 Link Arm\"\n",
    "    def __init__(self, x0, target, render):\n",
    "        self.render = render\n",
    "        if self.render:\n",
    "            super(ArmWorld, self).__init__()\n",
    "        else:\n",
    "            self.world = b2.b2World(gravity=(0, -10), doSleep=True)\n",
    "\n",
    "        self.world.gravity = (0.0, 0.0)\n",
    "\n",
    "        fixture_length = 5\n",
    "        self.x0 = x0\n",
    "\n",
    "        rectangle_fixture = b2.b2FixtureDef(\n",
    "            shape=b2.b2PolygonShape(box=(.5, fixture_length)),\n",
    "            density=.5,\n",
    "            friction=1,\n",
    "        )\n",
    "        square_fixture = b2.b2FixtureDef(\n",
    "            shape=b2.b2PolygonShape(box=(1, 1)),\n",
    "            density=100.0,\n",
    "            friction=1,\n",
    "        )\n",
    "        self.base = self.world.CreateBody(\n",
    "            position=(0, 15),\n",
    "            fixtures=square_fixture,\n",
    "        )\n",
    "\n",
    "        self.body1 = self.world.CreateDynamicBody(\n",
    "            position=(0, 2),\n",
    "            fixtures=rectangle_fixture,\n",
    "            angle=b2.b2_pi,\n",
    "        )\n",
    "\n",
    "        self.body2 = self.world.CreateDynamicBody(\n",
    "            fixtures=rectangle_fixture,\n",
    "            position=(0, 2),\n",
    "            angle=b2.b2_pi,\n",
    "        )\n",
    "        self.target1 = self.world.CreateDynamicBody(\n",
    "            fixtures=rectangle_fixture,\n",
    "            position=(0, 0),\n",
    "            angle=b2.b2_pi,\n",
    "        )\n",
    "        self.target2 = self.world.CreateDynamicBody(\n",
    "            fixtures=rectangle_fixture,\n",
    "            position=(0, 0),\n",
    "            angle=b2.b2_pi,\n",
    "        )\n",
    "\n",
    "        self.joint1 = self.world.CreateRevoluteJoint(\n",
    "            bodyA=self.base,\n",
    "            bodyB=self.body1,\n",
    "            localAnchorA=(0, 0),\n",
    "            localAnchorB=(0, fixture_length),\n",
    "            enableMotor=True,\n",
    "            maxMotorTorque=400,\n",
    "            enableLimit=False,\n",
    "        )\n",
    "\n",
    "        self.joint2 = self.world.CreateRevoluteJoint(\n",
    "            bodyA=self.body1,\n",
    "            bodyB=self.body2,\n",
    "            localAnchorA=(0, -(fixture_length - 0.5)),\n",
    "            localAnchorB=(0, fixture_length - 0.5),\n",
    "            enableMotor=True,\n",
    "            maxMotorTorque=400,\n",
    "            enableLimit=False,\n",
    "        )\n",
    "\n",
    "        self.set_joint_angles(self.body1, self.body2, x0[0], x0[1])\n",
    "        self.set_joint_angles(self.target1, self.target2, target[0], target[1])\n",
    "        self.target1.active = False\n",
    "        self.target2.active = False\n",
    "\n",
    "        self.joint1.motorSpeed = x0[2]\n",
    "        self.joint2.motorSpeed = x0[3]\n",
    "\n",
    "    def set_joint_angles(self, body1, body2, angle1, angle2):\n",
    "        \"\"\" Converts the given absolute angle of the arms to joint angles\"\"\"\n",
    "        pos = self.base.GetWorldPoint((0, 0))\n",
    "        body1.angle = angle1 + np.pi\n",
    "        new_pos = body1.GetWorldPoint((0, 5))\n",
    "        body1.position += pos - new_pos\n",
    "        body2.angle = angle2 + body1.angle\n",
    "        pos = body1.GetWorldPoint((0, -4.5))\n",
    "        new_pos = body2.GetWorldPoint((0, 4.5))\n",
    "        body2.position += pos - new_pos\n",
    "\n",
    "\n",
    "    def run(self):\n",
    "        \"\"\"Initiates the first time step\n",
    "        \"\"\"\n",
    "        if self.render:\n",
    "            super(ArmWorld, self).run()\n",
    "        else:\n",
    "            self.run_next(None)\n",
    "\n",
    "    def run_next(self, action):\n",
    "        \"\"\"Moves forward in time one step. Calls the renderer if applicable.\"\"\"\n",
    "        if self.render:\n",
    "            super(ArmWorld, self).run_next(action)\n",
    "        else:\n",
    "            if action is not None:\n",
    "                self.joint1.motorSpeed = action[0]\n",
    "                self.joint2.motorSpeed = action[1]\n",
    "            self.world.Step(1.0 / fwSettings.hz, fwSettings.velocityIterations,\n",
    "                            fwSettings.positionIterations)\n",
    "\n",
    "    def Step(self, settings, action):\n",
    "        \"\"\"Moves forward in time one step. Called by the renderer\"\"\"\n",
    "        self.joint1.motorSpeed = action[0]\n",
    "        self.joint2.motorSpeed = action[1]\n",
    "\n",
    "        super(ArmWorld, self).Step(settings)\n",
    "\n",
    "    def reset_world(self):\n",
    "        \"\"\"Returns the world to its intial state\"\"\"\n",
    "        self.world.ClearForces()\n",
    "        self.joint1.motorSpeed = 0\n",
    "        self.joint2.motorSpeed = 0\n",
    "        self.body1.linearVelocity = (0, 0)\n",
    "        self.body1.angularVelocity = 0\n",
    "        self.body2.linearVelocity = (0, 0)\n",
    "        self.body2.angularVelocity = 0\n",
    "        self.set_joint_angles(self.body1, self.body2, self.x0[0], self.x0[1])\n",
    "\n",
    "\n",
    "    def get_state(self):\n",
    "        \"\"\"Retrieves the state of the point mass\"\"\"\n",
    "        state = {JOINT_ANGLES: np.array([self.joint1.angle,\n",
    "                                         self.joint2.angle]),\n",
    "                 JOINT_VELOCITIES: np.array([self.joint1.speed,\n",
    "                                             self.joint2.speed]),\n",
    "                 END_EFFECTOR_POINTS: np.append(np.array(self.body2.position),[0])}\n",
    "\n",
    "        return state"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Pendulum agent: `Pendulum`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "\"\"\" This file defines an agent for the Box2D simulator. \"\"\"\n",
    "from copy import deepcopy\n",
    "\n",
    "#from gps.agent.agent import Agent\n",
    "#from gps.agent.agent_utils import generate_noise, setup\n",
    "#from gps.agent.config import AGENT_BOX2D\n",
    "from gps_pb2 import ACTION\n",
    "#from gps.sample.sample import Sample\n",
    "\n",
    "class Pendulum(Agent):\n",
    "    \"\"\"\n",
    "    All communication between the algorithms and Box2D is done through\n",
    "    this class.\n",
    "    \"\"\"\n",
    "    def __init__(self, hyperparams):\n",
    "        config = deepcopy(AGENT_BOX2D)\n",
    "        config.update(hyperparams)\n",
    "        Agent.__init__(self, config)\n",
    "\n",
    "        self._setup_conditions()\n",
    "        self._setup_world(self._hyperparams[\"world\"],\n",
    "                          self._hyperparams[\"target_state\"],\n",
    "                          self._hyperparams[\"render\"])\n",
    "\n",
    "    def _setup_conditions(self):\n",
    "        \"\"\"\n",
    "        Helper method for setting some hyperparameters that may vary by\n",
    "        condition.\n",
    "        \"\"\"\n",
    "        conds = self._hyperparams['conditions']\n",
    "        for field in ('x0', 'x0var', 'pos_body_idx', 'pos_body_offset',\n",
    "                      'noisy_body_idx', 'noisy_body_var'):\n",
    "            self._hyperparams[field] = setup(self._hyperparams[field], conds)\n",
    "\n",
    "    def _setup_world(self, world, target, render):\n",
    "        \"\"\"\n",
    "        Helper method for handling setup of the Box2D world.\n",
    "        \"\"\"\n",
    "        self.x0 = self._hyperparams[\"x0\"]\n",
    "\n",
    "        self._worlds = [world(self.x0[i], target, render)\n",
    "                        for i in range(self._hyperparams['conditions'])]\n",
    "\n",
    "\n",
    "    def sample(self, policy, condition, verbose=False, save=True, noisy=True):\n",
    "        \"\"\"\n",
    "        Runs a trial and constructs a new sample containing information\n",
    "        about the trial.\n",
    "\n",
    "        Args:\n",
    "            policy: Policy to to used in the trial.\n",
    "            condition (int): Which condition setup to run.\n",
    "            verbose (boolean): Whether or not to plot the trial (not used here).\n",
    "            save (boolean): Whether or not to store the trial into the samples.\n",
    "            noisy (boolean): Whether or not to use noise during sampling.\n",
    "        \"\"\"\n",
    "        self._worlds[condition].run()\n",
    "        self._worlds[condition].reset_world()\n",
    "        b2d_X = self._worlds[condition].get_state()\n",
    "        new_sample = self._init_sample(b2d_X)\n",
    "        U = np.zeros([self.T, self.dU])\n",
    "        if noisy:\n",
    "            noise = generate_noise(self.T, self.dU, self._hyperparams)\n",
    "        else:\n",
    "            noise = np.zeros((self.T, self.dU))\n",
    "        for t in range(self.T):\n",
    "            X_t = new_sample.get_X(t=t)\n",
    "            obs_t = new_sample.get_obs(t=t)\n",
    "            U[t, :] = policy.act(X_t, obs_t, t, noise[t, :])\n",
    "            if (t+1) < self.T:\n",
    "                for _ in range(self._hyperparams['substeps']):\n",
    "                    self._worlds[condition].run_next(U[t, :])\n",
    "                b2d_X = self._worlds[condition].get_state()\n",
    "                self._set_sample(new_sample, b2d_X, t)\n",
    "        new_sample.set(ACTION, U)\n",
    "        if save:\n",
    "            self._samples[condition].append(new_sample)\n",
    "        return new_sample\n",
    "\n",
    "    def _init_sample(self, b2d_X):\n",
    "        \"\"\"\n",
    "        Construct a new sample and fill in the first time step.\n",
    "        \"\"\"\n",
    "        sample = Sample(self)\n",
    "        self._set_sample(sample, b2d_X, -1)\n",
    "        return sample\n",
    "\n",
    "    def _set_sample(self, sample, b2d_X, t):\n",
    "        # print(\"b2d_X: \", b2d_X)\n",
    "        for sensor in b2d_X.keys():\n",
    "            sample.set(sensor, np.array(b2d_X[sensor]), t=t+1)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `PendulumWorld`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:68: MatplotlibDeprecationWarning: The get_axis_bgcolor function was deprecated in version 2.0. Use get_facecolor instead.\n",
      "  color, alpha = self._ax.get_axis_bgcolor(), self._ax.get_alpha()\n",
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:69: MatplotlibDeprecationWarning: The set_axis_bgcolor function was deprecated in version 2.0. Use set_facecolor instead.\n",
      "  self._ax.set_axis_bgcolor(mpl.rcParams['figure.facecolor'])\n",
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:71: MatplotlibDeprecationWarning: The set_axis_bgcolor function was deprecated in version 2.0. Use set_facecolor instead.\n",
      "  self._ax.set_axis_bgcolor(ColorConverter().to_rgba(color, alpha))\n"
     ]
    }
   ],
   "source": [
    "import gym\n",
    "from gps_pb2 import JOINT_ANGLES, JOINT_VELOCITIES, END_EFFECTOR_POINTS\n",
    "import math\n",
    "#import cv2\n",
    "import time\n",
    "\n",
    "class PendulumWorld():\n",
    "    \"\"\"\n",
    "    State\n",
    "    0 \tcos(theta) \t-1.0 \t1.0\n",
    "    1 \tsin(theta) \t-1.0 \t1.0\n",
    "    2 \ttheta dot \t-8.0 \t8.0\n",
    "    Action\n",
    "    Num \tObservation \tMin \tMax\n",
    "    0 \t    Joint effort \t-2.0 \t2.0\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self, x0, target, render):\n",
    "        self.env = gym.make('Pendulum-v0')\n",
    "        self.render = render\n",
    "        print(\"self.x0: \", x0)\n",
    "        print(\"target: \", target)\n",
    "        self.max = -999\n",
    "        self.min = 999\n",
    "        self.target = target\n",
    "\n",
    "    def run(self):\n",
    "        self.observation = self.env.reset()\n",
    "        # self.x0 = self.observation\n",
    "        if self.render:\n",
    "            self.env.render()\n",
    "\n",
    "    def run_next(self, action):\n",
    "\n",
    "        # action[0] = action[0]*-2.0\n",
    "        if action[0] > self.max:\n",
    "            self.max = action[0]\n",
    "        if action[0] < self.min:\n",
    "            self.min = action[0]\n",
    "        if action is not None:\n",
    "            # if(action[0]>2.0):\n",
    "            #     action=[2.0]\n",
    "            # if(action[0]<-2.0):\n",
    "            #     action=[-2.0]\n",
    "            print(\"action: \", action)\n",
    "            self.observation, self.reward, self.done, self.info = self.env.step(action)\n",
    "\n",
    "    def reset_world(self):\n",
    "        print(\"Reset!\")\n",
    "        print(\"min: \", self.min, \" self.max: \", self.max)\n",
    "        self.run()\n",
    "\n",
    "    def get_state(self):\n",
    "        joint_angle = math.atan2(self.observation[1],self.observation[0])+np.pi\n",
    "        joint_vel = self.observation[2]\n",
    "        end_effector_pose = [self.observation[0], self.observation[1], 0.0]\n",
    "\n",
    "        if self.render:\n",
    "            self.env.render()\n",
    "            #img = np.zeros((500, 500, 3), np.uint8)\n",
    "            #cv2.line(img,(250,250),(250+int(end_effector_pose[0]*50),250+int(end_effector_pose[1]*50)),(255,0,0),5)\n",
    "            #cv2.circle(img,(250+int(end_effector_pose[0]), 250+int(end_effector_pose[1])), 5, (0,0,255), -1)\n",
    "            #cv2.circle(img,(250+int(math.cos(self.target)*50), 250+int(math.sin(self.target)*50)), 5, (0,255,255), -1)\n",
    "            #cv2.imshow(\"image\", img)\n",
    "\n",
    "        state =  {JOINT_ANGLES: np.array( [ joint_angle ] ),\n",
    "                  JOINT_VELOCITIES: np.array( [ joint_vel ] ),\n",
    "                  END_EFFECTOR_POINTS: np.array( end_effector_pose ) }\n",
    "        print(\"state: \", state)\n",
    "        angle = math.atan2(self.observation[1],self.observation[0])\n",
    "        print(\"cos(angle): \", math.cos(angle), \"  sin(angle) \", math.sin(angle))\n",
    "        print(\"theta dot: \", self.observation[2])\n",
    "        print(\"Angle: \", (math.atan2(self.observation[1],self.observation[0])*180/3.1416 +360)%360)\n",
    "        return state\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Policy"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `guess_dynamics`\n",
    "Initial guess at the model using position-velocity assumption."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def guess_dynamics(gains, acc, dX, dU, dt):\n",
    "    \"\"\"\n",
    "    Initial guess at the model using position-velocity assumption.\n",
    "    Note: This code assumes joint positions occupy the first dU state\n",
    "          indices and joint velocities occupy the next dU.\n",
    "    Args:\n",
    "        gains: dU dimensional joint gains.\n",
    "        acc: dU dimensional joint acceleration.\n",
    "        dX: Dimensionality of the state.\n",
    "        dU: Dimensionality of the action.\n",
    "        dt: Length of a time step.\n",
    "    Returns:\n",
    "        Fd: A dX by dX+dU transition matrix.\n",
    "        fc: A dX bias vector.\n",
    "    \"\"\"\n",
    "    #TODO: Use packing instead of assuming which indices are the joint\n",
    "    #      angles.\n",
    "    Fd = np.vstack([\n",
    "        np.hstack([\n",
    "            np.eye(dU), dt * np.eye(dU), np.zeros((dU, dX - dU*2)),\n",
    "            dt ** 2 * np.diag(gains)\n",
    "        ]),\n",
    "        np.hstack([\n",
    "            np.zeros((dU, dU)), np.eye(dU), np.zeros((dU, dX - dU*2)),\n",
    "            dt * np.diag(gains)\n",
    "        ]),\n",
    "        np.zeros((dX - dU*2, dX+dU))\n",
    "    ])\n",
    "    fc = np.hstack([acc * dt ** 2, acc * dt, np.zeros((dX - dU*2))])\n",
    "    return Fd, fc\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `Policy`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class Policy(object):\n",
    "    \"\"\" Computes actions from states/observations. \"\"\"\n",
    "    __metaclass__ = abc.ABCMeta\n",
    "\n",
    "    @abc.abstractmethod\n",
    "    def act(self, x, obs, t, noise):\n",
    "        \"\"\"\n",
    "        Args:\n",
    "            x: State vector.\n",
    "            obs: Observation vector.\n",
    "            t: Time step.\n",
    "            noise: A dU-dimensional noise vector.\n",
    "        Returns:\n",
    "            A dU dimensional action vector.\n",
    "        \"\"\"\n",
    "        raise NotImplementedError(\"Must be implemented in subclass.\")\n",
    "\n",
    "    def set_meta_data(self, meta):\n",
    "        \"\"\"\n",
    "        Set meta data for policy (e.g., domain image, multi modal observation sizes)\n",
    "        Args:\n",
    "            meta: meta data.\n",
    "        \"\"\"\n",
    "        return\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### `LinearGaussianPolicy`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class LinearGaussianPolicy(Policy):\n",
    "    \"\"\"\n",
    "    Time-varying linear Gaussian policy.\n",
    "    U = K*x + k + noise, where noise ~ N(0, chol_pol_covar)\n",
    "    \"\"\"\n",
    "    def __init__(self, K, k, pol_covar, chol_pol_covar, inv_pol_covar):\n",
    "        Policy.__init__(self)\n",
    "\n",
    "        # Assume K has the correct shape, and make sure others match.\n",
    "        self.T = K.shape[0]\n",
    "        self.dU = K.shape[1]\n",
    "        self.dX = K.shape[2]\n",
    "\n",
    "        check_shape(k, (self.T, self.dU))\n",
    "        check_shape(pol_covar, (self.T, self.dU, self.dU))\n",
    "        check_shape(chol_pol_covar, (self.T, self.dU, self.dU))\n",
    "        check_shape(inv_pol_covar, (self.T, self.dU, self.dU))\n",
    "\n",
    "        self.K = K\n",
    "        self.k = k\n",
    "        self.pol_covar = pol_covar\n",
    "        self.chol_pol_covar = chol_pol_covar\n",
    "        self.inv_pol_covar = inv_pol_covar\n",
    "\n",
    "    def act(self, x, obs, t, noise=None):\n",
    "        \"\"\"\n",
    "        Return an action for a state.\n",
    "        Args:\n",
    "            x: State vector.\n",
    "            obs: Observation vector.\n",
    "            t: Time step.\n",
    "            noise: Action noise. This will be scaled by the variance.\n",
    "        \"\"\"\n",
    "        u = self.K[t].dot(x) + self.k[t]\n",
    "        u += self.chol_pol_covar[t].T.dot(noise)\n",
    "        return u\n",
    "\n",
    "    def fold_k(self, noise):\n",
    "        \"\"\"\n",
    "        Fold noise into k.\n",
    "        Args:\n",
    "            noise: A T x Du noise vector with mean 0 and variance 1.\n",
    "        Returns:\n",
    "            k: A T x dU bias vector.\n",
    "        \"\"\"\n",
    "        k = np.zeros_like(self.k)\n",
    "        for i in range(self.T):\n",
    "            scaled_noise = self.chol_pol_covar[i].T.dot(noise[i])\n",
    "            k[i] = scaled_noise + self.k[i]\n",
    "        return k\n",
    "\n",
    "    def nans_like(self):\n",
    "        \"\"\"\n",
    "        Returns:\n",
    "            A new linear Gaussian policy object with the same dimensions\n",
    "            but all values filled with NaNs.\n",
    "        \"\"\"\n",
    "        policy = LinearGaussianPolicy(\n",
    "            np.zeros_like(self.K), np.zeros_like(self.k),\n",
    "            np.zeros_like(self.pol_covar), np.zeros_like(self.chol_pol_covar),\n",
    "            np.zeros_like(self.inv_pol_covar)\n",
    "        )\n",
    "        policy.K.fill(np.nan)\n",
    "        policy.k.fill(np.nan)\n",
    "        policy.pol_covar.fill(np.nan)\n",
    "        policy.chol_pol_covar.fill(np.nan)\n",
    "        policy.inv_pol_covar.fill(np.nan)\n",
    "        return policy\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Initializations for linear Gaussian controllers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Initial Linear Gaussian Trajectory Distributions, PD-based initializer.\n",
    "# Note, PD is the default initializer type.\n",
    "INIT_LG_PD = {\n",
    "    'init_var': 10.0,\n",
    "    'pos_gains': 10.0, # position gains\n",
    "    'vel_gains_mult': 0.01,  # velocity gains multiplier on pos_gains\n",
    "    'init_action_offset': None,\n",
    "}\n",
    "\n",
    "# Initial Linear Gaussian Trajectory distribution, LQR-based initializer.\n",
    "INIT_LG_LQR = {\n",
    "    'init_var': 1.0,\n",
    "    'stiffness': 1.0,\n",
    "    'stiffness_vel': 0.5,\n",
    "    'final_weight': 1.0,\n",
    "    # Parameters for guessing dynamics\n",
    "    'init_acc': [],  # dU vector of accelerations, default zeros.\n",
    "    'init_gains': [],  # dU vector of gains, default ones.\n",
    "}\n",
    "\n",
    "def init_lqr(hyperparams):\n",
    "    \"\"\"\n",
    "    Return initial gains for a time-varying linear Gaussian controller\n",
    "    that tries to hold the initial position.\n",
    "    \"\"\"\n",
    "    config = copy.deepcopy(INIT_LG_LQR)\n",
    "    config.update(hyperparams)\n",
    "\n",
    "    x0, dX, dU = config['x0'], config['dX'], config['dU']\n",
    "    dt, T = config['dt'], config['T']\n",
    "\n",
    "    #TODO: Use packing instead of assuming which indices are the joint\n",
    "    #      angles.\n",
    "\n",
    "    # Notation notes:\n",
    "    # L = loss, Q = q-function (dX+dU dimensional),\n",
    "    # V = value function (dX dimensional), F = dynamics\n",
    "    # Vectors are lower-case, matrices are upper case.\n",
    "    # Derivatives: x = state, u = action, t = state+action (trajectory).\n",
    "    # The time index is denoted by _t after the above.\n",
    "    # Ex. Ltt_t = Loss, 2nd derivative (w.r.t. trajectory),\n",
    "    # indexed by time t.\n",
    "\n",
    "    # Constants.\n",
    "    idx_x = slice(dX)  # Slices out state.\n",
    "    idx_u = slice(dX, dX+dU)  # Slices out actions.\n",
    "\n",
    "    if len(config['init_acc']) == 0:\n",
    "        config['init_acc'] = np.zeros(dU)\n",
    "\n",
    "    if len(config['init_gains']) == 0:\n",
    "        config['init_gains'] = np.ones(dU)\n",
    "\n",
    "    # Set up simple linear dynamics model.\n",
    "    Fd, fc = guess_dynamics(config['init_gains'], config['init_acc'],\n",
    "                            dX, dU, dt)\n",
    "\n",
    "    # Setup a cost function based on stiffness.\n",
    "    # Ltt = (dX+dU) by (dX+dU) - Hessian of loss with respect to\n",
    "    # trajectory at a single timestep.\n",
    "    Ltt = np.diag(np.hstack([\n",
    "        config['stiffness'] * np.ones(dU),\n",
    "        config['stiffness'] * config['stiffness_vel'] * np.ones(dU),\n",
    "        np.zeros(dX - dU*2), np.ones(dU)\n",
    "    ]))\n",
    "    Ltt = Ltt / config['init_var']  # Cost function - quadratic term.\n",
    "    lt = -Ltt.dot(np.r_[x0, np.zeros(dU)])  # Cost function - linear term.\n",
    "\n",
    "    # Perform dynamic programming.\n",
    "    K = np.zeros((T, dU, dX))  # Controller gains matrix.\n",
    "    k = np.zeros((T, dU))  # Controller bias term.\n",
    "    PSig = np.zeros((T, dU, dU))  # Covariance of noise.\n",
    "    cholPSig = np.zeros((T, dU, dU))  # Cholesky decomposition.\n",
    "    invPSig = np.zeros((T, dU, dU))  # Inverse of covariance.\n",
    "    vx_t = np.zeros(dX)  # Vx = dV/dX. Derivative of value function.\n",
    "    Vxx_t = np.zeros((dX, dX))  # Vxx = ddV/dXdX.\n",
    "\n",
    "    #TODO: A lot of this code is repeated with traj_opt_lqr_python.py\n",
    "    #      backward pass.\n",
    "    for t in range(T - 1, -1, -1):\n",
    "        # Compute Q function at this step.\n",
    "        if t == (T - 1):\n",
    "            Ltt_t = config['final_weight'] * Ltt\n",
    "            lt_t = config['final_weight'] * lt\n",
    "        else:\n",
    "            Ltt_t = Ltt\n",
    "            lt_t = lt\n",
    "        # Qtt = (dX+dU) by (dX+dU) 2nd Derivative of Q-function with\n",
    "        # respect to trajectory (dX+dU).\n",
    "        Qtt_t = Ltt_t + Fd.T.dot(Vxx_t).dot(Fd)\n",
    "        # Qt = (dX+dU) 1st Derivative of Q-function with respect to\n",
    "        # trajectory (dX+dU).\n",
    "        qt_t = lt_t + Fd.T.dot(vx_t + Vxx_t.dot(fc))\n",
    "\n",
    "        # Compute preceding value function.\n",
    "        U = sp.linalg.cholesky(Qtt_t[idx_u, idx_u])\n",
    "        L = U.T\n",
    "\n",
    "        invPSig[t, :, :] = Qtt_t[idx_u, idx_u]\n",
    "        PSig[t, :, :] = sp.linalg.solve_triangular(\n",
    "            U, sp.linalg.solve_triangular(L, np.eye(dU), lower=True)\n",
    "        )\n",
    "        cholPSig[t, :, :] = sp.linalg.cholesky(PSig[t, :, :])\n",
    "        K[t, :, :] = -sp.linalg.solve_triangular(\n",
    "            U, sp.linalg.solve_triangular(L, Qtt_t[idx_u, idx_x], lower=True)\n",
    "        )\n",
    "        k[t, :] = -sp.linalg.solve_triangular(\n",
    "            U, sp.linalg.solve_triangular(L, qt_t[idx_u], lower=True)\n",
    "        )\n",
    "        Vxx_t = Qtt_t[idx_x, idx_x] + Qtt_t[idx_x, idx_u].dot(K[t, :, :])\n",
    "        vx_t = qt_t[idx_x] + Qtt_t[idx_x, idx_u].dot(k[t, :])\n",
    "        Vxx_t = 0.5 * (Vxx_t + Vxx_t.T)\n",
    "\n",
    "    return LinearGaussianPolicy(K, k, PSig, cholPSig, invPSig)\n",
    "\n",
    "\n",
    "#TODO: Fix docstring\n",
    "def init_pd(hyperparams):\n",
    "    \"\"\"\n",
    "    This function initializes the linear-Gaussian controller as a\n",
    "    proportional-derivative (PD) controller with Gaussian noise. The\n",
    "    position gains are controlled by the variable pos_gains, velocity\n",
    "    gains are controlled by pos_gains*vel_gans_mult.\n",
    "    \"\"\"\n",
    "    config = copy.deepcopy(INIT_LG_PD)\n",
    "    config.update(hyperparams)\n",
    "\n",
    "    dU, dQ, dX = config['dU'], config['dQ'], config['dX']\n",
    "    x0, T = config['x0'], config['T']\n",
    "\n",
    "    # Choose initialization mode.\n",
    "    Kp = 1.0\n",
    "    Kv = config['vel_gains_mult']\n",
    "    if dU < dQ:\n",
    "        K = -config['pos_gains'] * np.tile(\n",
    "            [np.eye(dU) * Kp, np.zeros((dU, dQ-dU)),\n",
    "             np.eye(dU) * Kv, np.zeros((dU, dQ-dU))],\n",
    "            [T, 1, 1]\n",
    "        )\n",
    "    else:\n",
    "        K = -config['pos_gains'] * np.tile(\n",
    "            np.hstack([\n",
    "                np.eye(dU) * Kp, np.eye(dU) * Kv,\n",
    "                np.zeros((dU, dX - dU*2))\n",
    "            ]), [T, 1, 1]\n",
    "        )\n",
    "    k = np.tile(-K[0, :, :].dot(x0), [T, 1])\n",
    "    PSig = config['init_var'] * np.tile(np.eye(dU), [T, 1, 1])\n",
    "    cholPSig = np.sqrt(config['init_var']) * np.tile(np.eye(dU), [T, 1, 1])\n",
    "    invPSig = (1.0 / config['init_var']) * np.tile(np.eye(dU), [T, 1, 1])\n",
    "\n",
    "    return LinearGaussianPolicy(K, k, PSig, cholPSig, invPSig)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Run"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### GPS main class and code: `GPSMain`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "import matplotlib as mpl\n",
    "# mpl.use('Qt4Agg')\n",
    "\n",
    "qt_found=False\n",
    "try:\n",
    "     import PyQt4\n",
    "     mpl.use('Qt4Agg')\n",
    "     qt_found = True\n",
    "except ImportError:\n",
    "     qt_found = False\n",
    "if qt_found == False:\n",
    "   try:\n",
    "      import PyQt5\n",
    "      mpl.use('Qt5Agg')\n",
    "      qt_found = True\n",
    "   except ImportError:\n",
    "      qt_found = False\n",
    "\n",
    "import logging\n",
    "import imp\n",
    "import os\n",
    "import os.path\n",
    "import sys\n",
    "import argparse\n",
    "import threading\n",
    "import time\n",
    "import traceback\n",
    "\n",
    "# Add gps/python to path so that imports work.\n",
    "#sys.path.append('/'.join(str.split(__file__, '/')[:-2]))\n",
    "\n",
    "from gps.gui.gps_training_gui import GPSTrainingGUI\n",
    "#from gps.utility.data_logger import DataLogger\n",
    "#from gps.sample.sample_list import SampleList\n",
    "\n",
    "\n",
    "class GPSMain(object):\n",
    "    \"\"\" Main class to run algorithms and experiments. \"\"\"\n",
    "    def __init__(self, config, quit_on_end=False):\n",
    "        \"\"\"\n",
    "        Initialize GPSMain\n",
    "        Args:\n",
    "            config: Hyperparameters for experiment\n",
    "            quit_on_end: When true, quit automatically on completion\n",
    "        \"\"\"\n",
    "        self._quit_on_end = quit_on_end\n",
    "        self._hyperparams = config\n",
    "        self._conditions = config['common']['conditions']\n",
    "        if 'test_conditions' in config['common']:\n",
    "            self._train_idx = config['common']['train_conditions']\n",
    "            self._test_idx = config['common']['test_conditions']\n",
    "        else:\n",
    "            self._train_idx = range(self._conditions)\n",
    "            config['common']['train_conditions'] = config['common']['conditions']\n",
    "            self._hyperparams=config\n",
    "            self._test_idx = self._train_idx\n",
    "\n",
    "        self._data_files_dir = config['common']['data_files_dir']\n",
    "\n",
    "        self.agent = config['agent']['type'](config['agent'])\n",
    "        self.data_logger = DataLogger()\n",
    "        self.gui = GPSTrainingGUI(config['common']) if config['gui_on'] else None\n",
    "\n",
    "        config['algorithm']['agent'] = self.agent\n",
    "        self.algorithm = config['algorithm']['type'](config['algorithm'])\n",
    "\n",
    "    def run(self, itr_load=None):\n",
    "        \"\"\"\n",
    "        Run training by iteratively sampling and taking an iteration.\n",
    "        Args:\n",
    "            itr_load: If specified, loads algorithm state from that\n",
    "                iteration, and resumes training at the next iteration.\n",
    "        Returns: None\n",
    "        \"\"\"\n",
    "        try:\n",
    "            itr_start = self._initialize(itr_load)\n",
    "\n",
    "            for itr in range(itr_start, self._hyperparams['iterations']):\n",
    "                for cond in self._train_idx:\n",
    "                    for i in range(self._hyperparams['num_samples']):\n",
    "                        self._take_sample(itr, cond, i)\n",
    "\n",
    "                traj_sample_lists = [\n",
    "                    self.agent.get_samples(cond, -self._hyperparams['num_samples'])\n",
    "                    for cond in self._train_idx\n",
    "                ]\n",
    "\n",
    "                # Clear agent samples.\n",
    "                self.agent.clear_samples()\n",
    "\n",
    "                self._take_iteration(itr, traj_sample_lists)\n",
    "                pol_sample_lists = self._take_policy_samples()\n",
    "                self._log_data(itr, traj_sample_lists, pol_sample_lists)\n",
    "        except Exception as e:\n",
    "            traceback.print_exception(*sys.exc_info())\n",
    "        finally:\n",
    "            self._end()\n",
    "\n",
    "    def test_policy(self, itr, N):\n",
    "        \"\"\"\n",
    "        Take N policy samples of the algorithm state at iteration itr,\n",
    "        for testing the policy to see how it is behaving.\n",
    "        (Called directly from the command line --policy flag).\n",
    "        Args:\n",
    "            itr: the iteration from which to take policy samples\n",
    "            N: the number of policy samples to take\n",
    "        Returns: None\n",
    "        \"\"\"\n",
    "        algorithm_file = self._data_files_dir + 'algorithm_itr_%02d.pkl' % itr\n",
    "        self.algorithm = self.data_logger.unpickle(algorithm_file)\n",
    "        if self.algorithm is None:\n",
    "            print(\"Error: cannot find '%s.'\" % algorithm_file)\n",
    "            os._exit(1) # called instead of sys.exit(), since t\n",
    "        traj_sample_lists = self.data_logger.unpickle(self._data_files_dir +\n",
    "            ('traj_sample_itr_%02d.pkl' % itr))\n",
    "\n",
    "        pol_sample_lists = self._take_policy_samples(N)\n",
    "        self.data_logger.pickle(\n",
    "            self._data_files_dir + ('pol_sample_itr_%02d.pkl' % itr),\n",
    "            copy.copy(pol_sample_lists)\n",
    "        )\n",
    "\n",
    "        if self.gui:\n",
    "            self.gui.update(itr, self.algorithm, self.agent,\n",
    "                traj_sample_lists, pol_sample_lists)\n",
    "            self.gui.set_status_text(('Took %d policy sample(s) from ' +\n",
    "                'algorithm state at iteration %d.\\n' +\n",
    "                'Saved to: data_files/pol_sample_itr_%02d.pkl.\\n') % (N, itr, itr))\n",
    "\n",
    "    def _initialize(self, itr_load):\n",
    "        \"\"\"\n",
    "        Initialize from the specified iteration.\n",
    "        Args:\n",
    "            itr_load: If specified, loads algorithm state from that\n",
    "                iteration, and resumes training at the next iteration.\n",
    "        Returns:\n",
    "            itr_start: Iteration to start from.\n",
    "        \"\"\"\n",
    "        if itr_load is None:\n",
    "            if self.gui:\n",
    "                self.gui.set_status_text('Press \\'go\\' to begin.')\n",
    "            return 0\n",
    "        else:\n",
    "            algorithm_file = self._data_files_dir + 'algorithm_itr_%02d.pkl' % itr_load\n",
    "            self.algorithm = self.data_logger.unpickle(algorithm_file)\n",
    "            if self.algorithm is None:\n",
    "                print(\"Error: cannot find '%s.'\" % algorithm_file)\n",
    "                os._exit(1) # called instead of sys.exit(), since this is in a thread\n",
    "\n",
    "            if self.gui:\n",
    "                traj_sample_lists = self.data_logger.unpickle(self._data_files_dir +\n",
    "                    ('traj_sample_itr_%02d.pkl' % itr_load))\n",
    "                if self.algorithm.cur[0].pol_info:\n",
    "                    pol_sample_lists = self.data_logger.unpickle(self._data_files_dir +\n",
    "                        ('pol_sample_itr_%02d.pkl' % itr_load))\n",
    "                else:\n",
    "                    pol_sample_lists = None\n",
    "                self.gui.set_status_text(\n",
    "                    ('Resuming training from algorithm state at iteration %d.\\n' +\n",
    "                    'Press \\'go\\' to begin.') % itr_load)\n",
    "            return itr_load + 1\n",
    "\n",
    "    def _take_sample(self, itr, cond, i):\n",
    "        \"\"\"\n",
    "        Collect a sample from the agent.\n",
    "        Args:\n",
    "            itr: Iteration number.\n",
    "            cond: Condition number.\n",
    "            i: Sample number.\n",
    "        Returns: None\n",
    "        \"\"\"\n",
    "        if self.algorithm._hyperparams['sample_on_policy'] \\\n",
    "                and self.algorithm.iteration_count > 0:\n",
    "            pol = self.algorithm.policy_opt.policy\n",
    "        else:\n",
    "            pol = self.algorithm.cur[cond].traj_distr\n",
    "        if self.gui:\n",
    "            self.gui.set_image_overlays(cond)   # Must call for each new cond.\n",
    "            redo = True\n",
    "            while redo:\n",
    "                while self.gui.mode in ('wait', 'request', 'process'):\n",
    "                    if self.gui.mode in ('wait', 'process'):\n",
    "                        time.sleep(0.01)\n",
    "                        continue\n",
    "                    # 'request' mode.\n",
    "                    if self.gui.request == 'reset':\n",
    "                        try:\n",
    "                            self.agent.reset(cond)\n",
    "                        except NotImplementedError:\n",
    "                            self.gui.err_msg = 'Agent reset unimplemented.'\n",
    "                    elif self.gui.request == 'fail':\n",
    "                        self.gui.err_msg = 'Cannot fail before sampling.'\n",
    "                    self.gui.process_mode()  # Complete request.\n",
    "\n",
    "                self.gui.set_status_text(\n",
    "                    'Sampling: iteration %d, condition %d, sample %d.' %\n",
    "                    (itr, cond, i)\n",
    "                )\n",
    "                self.agent.sample(\n",
    "                    pol, cond,\n",
    "                    verbose=(i < self._hyperparams['verbose_trials'])\n",
    "                )\n",
    "\n",
    "                if self.gui.mode == 'request' and self.gui.request == 'fail':\n",
    "                    redo = True\n",
    "                    self.gui.process_mode()\n",
    "                    self.agent.delete_last_sample(cond)\n",
    "                else:\n",
    "                    redo = False\n",
    "        else:\n",
    "            self.agent.sample(\n",
    "                pol, cond,\n",
    "                verbose=(i < self._hyperparams['verbose_trials'])\n",
    "            )\n",
    "\n",
    "    def _take_iteration(self, itr, sample_lists):\n",
    "        \"\"\"\n",
    "        Take an iteration of the algorithm.\n",
    "        Args:\n",
    "            itr: Iteration number.\n",
    "        Returns: None\n",
    "        \"\"\"\n",
    "        if self.gui:\n",
    "            self.gui.set_status_text('Calculating.')\n",
    "            self.gui.start_display_calculating()\n",
    "        self.algorithm.iteration(sample_lists)\n",
    "        if self.gui:\n",
    "            self.gui.stop_display_calculating()\n",
    "\n",
    "    def _take_policy_samples(self, N=None):\n",
    "        \"\"\"\n",
    "        Take samples from the policy to see how it's doing.\n",
    "        Args:\n",
    "            N  : number of policy samples to take per condition\n",
    "        Returns: None\n",
    "        \"\"\"\n",
    "        if 'verbose_policy_trials' not in self._hyperparams:\n",
    "            # AlgorithmTrajOpt\n",
    "            return None\n",
    "        verbose = self._hyperparams['verbose_policy_trials']\n",
    "        if self.gui:\n",
    "            self.gui.set_status_text('Taking policy samples.')\n",
    "        pol_samples = [[None] for _ in range(len(self._test_idx))]\n",
    "        # Since this isn't noisy, just take one sample.\n",
    "        # TODO: Make this noisy? Add hyperparam?\n",
    "        # TODO: Take at all conditions for GUI?\n",
    "        for cond in range(len(self._test_idx)):\n",
    "            pol_samples[cond][0] = self.agent.sample(\n",
    "                self.algorithm.policy_opt.policy, self._test_idx[cond],\n",
    "                verbose=verbose, save=False, noisy=False)\n",
    "        return [SampleList(samples) for samples in pol_samples]\n",
    "\n",
    "    def _log_data(self, itr, traj_sample_lists, pol_sample_lists=None):\n",
    "        \"\"\"\n",
    "        Log data and algorithm, and update the GUI.\n",
    "        Args:\n",
    "            itr: Iteration number.\n",
    "            traj_sample_lists: trajectory samples as SampleList object\n",
    "            pol_sample_lists: policy samples as SampleList object\n",
    "        Returns: None\n",
    "        \"\"\"\n",
    "        if self.gui:\n",
    "            self.gui.set_status_text('Logging data and updating GUI.')\n",
    "            self.gui.update(itr, self.algorithm, self.agent,\n",
    "                traj_sample_lists, pol_sample_lists)\n",
    "            self.gui.save_figure(\n",
    "                self._data_files_dir + ('figure_itr_%02d.png' % itr)\n",
    "            )\n",
    "        if 'no_sample_logging' in self._hyperparams['common']:\n",
    "            return\n",
    "        self.data_logger.pickle(\n",
    "            self._data_files_dir + ('algorithm_itr_%02d.pkl' % itr),\n",
    "            copy.copy(self.algorithm)\n",
    "        )\n",
    "        self.data_logger.pickle(\n",
    "            self._data_files_dir + ('traj_sample_itr_%02d.pkl' % itr),\n",
    "            copy.copy(traj_sample_lists)\n",
    "        )\n",
    "        if pol_sample_lists:\n",
    "            self.data_logger.pickle(\n",
    "                self._data_files_dir + ('pol_sample_itr_%02d.pkl' % itr),\n",
    "                copy.copy(pol_sample_lists)\n",
    "            )\n",
    "\n",
    "    def _end(self):\n",
    "        \"\"\" Finish running and exit. \"\"\"\n",
    "        if self.gui:\n",
    "            self.gui.set_status_text('Training complete.')\n",
    "            self.gui.end_mode()\n",
    "            if self._quit_on_end:\n",
    "                # Quit automatically (for running sequential expts)\n",
    "                os._exit(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [],
   "source": [
    "from gps_pb2 import JOINT_ANGLES, JOINT_VELOCITIES, END_EFFECTOR_POINTS, ACTION\n",
    "from datetime import datetime\n",
    "import os\n",
    "\n",
    "SENSOR_DIMS = {\n",
    "    JOINT_ANGLES: 1,\n",
    "    JOINT_VELOCITIES: 1,\n",
    "    END_EFFECTOR_POINTS: 3,\n",
    "    ACTION: 1\n",
    "}\n",
    "\n",
    "#BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2])\n",
    "BASE_DIR = '.'\n",
    "#EXP_DIR = BASE_DIR + '/../experiments/box2d_arm_example/'\n",
    "EXP_DIR = BASE_DIR + 'experiments/'\n",
    "\n",
    "\n",
    "common = {\n",
    "    'experiment_name': 'box2d_arm_example' + '_' + \\\n",
    "            datetime.strftime(datetime.now(), '%m-%d-%y_%H-%M'),\n",
    "    'experiment_dir': EXP_DIR,\n",
    "    'data_files_dir': EXP_DIR + 'data_files/',\n",
    "    'log_filename': EXP_DIR + 'log.txt',\n",
    "    'conditions': 1,\n",
    "}\n",
    "\n",
    "if not os.path.exists(common['data_files_dir']):\n",
    "    os.makedirs(common['data_files_dir'])\n",
    "\n",
    "agent = {\n",
    "    'type': Pendulum,\n",
    "    'target_state' : np.array([0]),\n",
    "    'world' : PendulumWorld,\n",
    "    'render' : True,\n",
    "    'x0': np.array([0, 0, 0, 0, 0]),\n",
    "    'rk': 0,\n",
    "    'dt': 0.05,\n",
    "    'substeps': 1,\n",
    "    'conditions': common['conditions'],\n",
    "    'pos_body_idx': np.array([]),\n",
    "    'pos_body_offset': np.array([]),\n",
    "    'T': 100,\n",
    "    'sensor_dims': SENSOR_DIMS,\n",
    "    'state_include': [JOINT_ANGLES, JOINT_VELOCITIES, END_EFFECTOR_POINTS],\n",
    "    'obs_include': [],\n",
    "}\n",
    "\n",
    "algorithm = {\n",
    "    'type': AlgorithmTrajOpt,\n",
    "    'conditions': common['conditions'],\n",
    "}\n",
    "\n",
    "algorithm['init_traj_distr'] = {\n",
    "    'type': init_lqr,\n",
    "    'init_gains': np.zeros(SENSOR_DIMS[ACTION]),\n",
    "    'init_acc': np.zeros(SENSOR_DIMS[ACTION]),\n",
    "    'init_var': 0.1,\n",
    "    'stiffness': 0.01,\n",
    "    'dt': agent['dt'],\n",
    "    'T': agent['T'],\n",
    "}\n",
    "\n",
    "action_cost = {\n",
    "    'type': CostAction,\n",
    "    'wu': np.array([1])\n",
    "}\n",
    "\n",
    "state_cost = {\n",
    "    'type': CostState,\n",
    "    'data_types' : {\n",
    "        JOINT_ANGLES: {\n",
    "            'wp': np.array([1]),\n",
    "            'target_state': agent[\"target_state\"],\n",
    "        },\n",
    "    },\n",
    "}\n",
    "\n",
    "algorithm['cost'] = {\n",
    "    'type': CostSum,\n",
    "    'costs': [action_cost, state_cost],\n",
    "    'weights': [1e-5, 1.0],\n",
    "}\n",
    "\n",
    "algorithm['dynamics'] = {\n",
    "    'type': DynamicsLRPrior,\n",
    "    'regularization': 1e-6,\n",
    "    'prior': {\n",
    "        'type': DynamicsPriorGMM,\n",
    "        'max_clusters': 20,\n",
    "        'min_samples_per_cluster': 40,\n",
    "        'max_samples': 20,\n",
    "    },\n",
    "}\n",
    "\n",
    "algorithm['traj_opt'] = {\n",
    "    'type': TrajOptLQRPython,\n",
    "}\n",
    "\n",
    "algorithm['policy_opt'] = {}\n",
    "\n",
    "config_gps = {\n",
    "    'iterations': 10,\n",
    "    'num_samples': 5,\n",
    "    'verbose_trials': 5,\n",
    "    'common': common,\n",
    "    'agent': agent,\n",
    "    'gui_on': True,\n",
    "    'algorithm': algorithm,\n",
    "}\n",
    "\n",
    "common['info'] = generate_experiment_info(config_gps)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:68: MatplotlibDeprecationWarning: The get_axis_bgcolor function was deprecated in version 2.0. Use get_facecolor instead.\n",
      "  color, alpha = self._ax.get_axis_bgcolor(), self._ax.get_alpha()\n",
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:69: MatplotlibDeprecationWarning: The set_axis_bgcolor function was deprecated in version 2.0. Use set_facecolor instead.\n",
      "  self._ax.set_axis_bgcolor(mpl.rcParams['figure.facecolor'])\n",
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:71: MatplotlibDeprecationWarning: The set_axis_bgcolor function was deprecated in version 2.0. Use set_facecolor instead.\n",
      "  self._ax.set_axis_bgcolor(ColorConverter().to_rgba(color, alpha))\n",
      "[2017-05-19 18:01:22,441] Making new env: Pendulum-v0\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "self.x0:  [0 0 0 0 0]\n",
      "target:  [0]\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:64: MatplotlibDeprecationWarning: The set_axis_bgcolor function was deprecated in version 2.0. Use set_facecolor instead.\n",
      "  self._ax.set_axis_bgcolor(ColorConverter().to_rgba(color, alpha))\n",
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:68: MatplotlibDeprecationWarning: The get_axis_bgcolor function was deprecated in version 2.0. Use get_facecolor instead.\n",
      "  color, alpha = self._ax.get_axis_bgcolor(), self._ax.get_alpha()\n",
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:69: MatplotlibDeprecationWarning: The set_axis_bgcolor function was deprecated in version 2.0. Use set_facecolor instead.\n",
      "  self._ax.set_axis_bgcolor(mpl.rcParams['figure.facecolor'])\n",
      "/Users/Victor/basic_reinforcement_learning/tutorial10/gps/gui/textbox.py:71: MatplotlibDeprecationWarning: The set_axis_bgcolor function was deprecated in version 2.0. Use set_facecolor instead.\n",
      "  self._ax.set_axis_bgcolor(ColorConverter().to_rgba(color, alpha))\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  999  self.max:  -999\n",
      "state:  {1: array([ 2.75493736]), 2: array([-0.37335662]), 3: array([ 0.9261755 , -0.37709274,  0.        ])}\n",
      "cos(angle):  0.9261755030787623   sin(angle)  -0.37709274389306613\n",
      "theta dot:  -0.373356621013\n",
      "Angle:  337.8463354265376\n",
      "action:  [ 0.82617292]\n",
      "state:  {1: array([ 2.72832485]), 2: array([-0.53225024]), 3: array([ 0.91581335, -0.40160417,  0.        ])}\n",
      "cos(angle):  0.9158133510572014   sin(angle)  -0.40160416584664455\n",
      "theta dot:  -0.532250241284\n",
      "Angle:  336.32155436862064\n",
      "action:  [ 0.82503172]\n",
      "state:  {1: array([ 2.69283992]), 2: array([-0.70969861]), 3: array([ 0.90098892, -0.4338421 ,  0.        ])}\n",
      "cos(angle):  0.9009889191894476   sin(angle)  -0.43384209972964943\n",
      "theta dot:  -0.709698607471\n",
      "Angle:  334.2884223762469\n",
      "action:  [ 0.80778155]\n",
      "state:  {1: array([ 2.64714427]), 2: array([-0.91391295]), 3: array([ 0.88023061, -0.47454617,  0.        ])}\n",
      "cos(angle):  0.880230611813601   sin(angle)  -0.4745461727021448\n",
      "theta dot:  -0.913912950003\n",
      "Angle:  331.6702607547716\n",
      "action:  [ 0.74356446]\n",
      "state:  {1: array([ 2.58922988]), 2: array([-1.15828791]), 3: array([ 0.85128715, -0.52470009,  0.        ])}\n",
      "cos(angle):  0.8512871497306074   sin(angle)  -0.5247000940571085\n",
      "theta dot:  -1.15828790997\n",
      "Angle:  328.3520180791388\n",
      "action:  [ 0.61318479]\n",
      "state:  {1: array([ 2.51623811]), 2: array([-1.45983526]), 3: array([ 0.81075563, -0.58538476,  0.        ])}\n",
      "cos(angle):  0.8107556276129901   sin(angle)  -0.5853847557750941\n",
      "theta dot:  -1.45983526264\n",
      "Angle:  324.169907892049\n",
      "action:  [ 0.44223935]\n",
      "state:  {1: array([ 2.42461122]), 2: array([-1.83253793]), 3: array([ 0.75379269, -0.6571123 ,  0.        ])}\n",
      "cos(angle):  0.7537926949731187   sin(angle)  -0.6571122986257089\n",
      "theta dot:  -1.83253792757\n",
      "Angle:  318.9200857160603\n",
      "action:  [ 0.28884889]\n",
      "state:  {1: array([ 2.31050898]), 2: array([-2.28204482]), 3: array([ 0.67407568, -0.73866229,  0.        ])}\n",
      "cos(angle):  0.6740756838147861   sin(angle)  -0.7386622858178347\n",
      "theta dot:  -2.28204481795\n",
      "Angle:  312.3825241672996\n",
      "action:  [ 0.20052806]\n",
      "state:  {1: array([ 2.17021086]), 2: array([-2.80596232]), 3: array([ 0.56415917, -0.82566605,  0.        ])}\n",
      "cos(angle):  0.5641591694356836   sin(angle)  -0.8256660532816156\n",
      "theta dot:  -2.80596232277\n",
      "Angle:  304.34405303637556\n",
      "action:  [ 0.19097558]\n",
      "state:  {1: array([ 2.00038258]), 2: array([-3.39656552]), 3: array([ 0.41649469, -0.90913815,  0.        ])}\n",
      "cos(angle):  0.4164946878219316   sin(angle)  -0.9091381495769011\n",
      "theta dot:  -3.39656552499\n",
      "Angle:  294.613632319242\n",
      "action:  [ 0.24337024]\n",
      "state:  {1: array([ 1.7982869]), 2: array([-4.0419136]), 3: array([ 0.22553347, -0.97423542,  0.        ])}\n",
      "cos(angle):  0.22553346664245943   sin(angle)  -0.9742354209451813\n",
      "theta dot:  -4.04191360058\n",
      "Angle:  283.0344298729676\n",
      "action:  [ 0.31984917]\n",
      "state:  {1: array([ 1.56205626]), 2: array([-4.72461279]), 3: array([-0.00873995, -0.99996181,  0.        ])}\n",
      "cos(angle):  -0.008739952055113572   sin(angle)  -0.9999618058896421\n",
      "theta dot:  -4.72461279012\n",
      "Angle:  269.49944288827686\n",
      "action:  [ 0.37744128]\n",
      "state:  {1: array([ 1.29115787]), 2: array([-5.41796795]), 3: array([-0.27600817, -0.96115529,  0.        ])}\n",
      "cos(angle):  -0.2760081714482012   sin(angle)  -0.9611552888549385\n",
      "theta dot:  -5.4179679526\n",
      "Angle:  253.97814432277363\n",
      "action:  [ 0.39379625]\n",
      "state:  {1: array([ 0.98716962]), 2: array([-6.07976498]), 3: array([-0.55105393, -0.83446963,  0.        ])}\n",
      "cos(angle):  -0.5510539335428816   sin(angle)  -0.8344696293616188\n",
      "theta dot:  -6.07976498147\n",
      "Angle:  236.56094135828596\n",
      "action:  [ 0.37917393]\n",
      "state:  {1: array([ 0.65473256]), 2: array([-6.64874111]), 3: array([-0.79321081, -0.60894713,  0.        ])}\n",
      "cos(angle):  -0.7932108126601465   sin(angle)  -0.6089471296254134\n",
      "theta dot:  -6.64874111468\n",
      "Angle:  217.51374565159324\n",
      "action:  [ 0.35367311]\n",
      "state:  {1: array([ 0.30211254]), 2: array([-7.0524005]), 3: array([-0.95471006, -0.29753773,  0.        ])}\n",
      "cos(angle):  -0.9547100607028622   sin(angle)  -0.29753772868787104\n",
      "theta dot:  -7.05240049582\n",
      "Angle:  197.31015370405015\n",
      "action:  [ 0.31297442]\n",
      "state:  {1: array([ 6.22386746]), 2: array([-7.22860763]), 3: array([-0.99824121,  0.05928307,  0.        ])}\n",
      "cos(angle):  -0.9982412124184482   sin(angle)  0.05928306528298505\n",
      "theta dot:  -7.2286076286\n",
      "Angle:  176.60092485090058\n",
      "action:  [ 0.22696957]\n",
      "state:  {1: array([ 5.86636247]), 2: array([-7.15009989]), 3: array([-0.91437985,  0.40485737,  0.        ])}\n",
      "cos(angle):  -0.9143798498623188   sin(angle)  0.4048573701512216\n",
      "theta dot:  -7.15009989407\n",
      "Angle:  156.11744539883944\n",
      "action:  [ 0.07768023]\n",
      "state:  {1: array([ 5.52462223]), 2: array([-6.83480483]), 3: array([-0.72582519,  0.6878792 ,  0.        ])}\n",
      "cos(angle):  -0.7258251860718228   sin(angle)  0.6878792039768349\n",
      "theta dot:  -6.83480483212\n",
      "Angle:  136.53721765211833\n",
      "action:  [-0.1020521]\n",
      "state:  {1: array([ 5.20791206]), 2: array([-6.33420324]), 3: array([-0.47549188,  0.87972011,  0.        ])}\n",
      "cos(angle):  -0.4754918829999016   sin(angle)  0.8797201084442755\n",
      "theta dot:  -6.33420324438\n",
      "Angle:  118.39110446156707\n",
      "action:  [-0.23390831]\n",
      "state:  {1: array([ 4.92243709]), 2: array([-5.70949941]), 3: array([-0.20850696,  0.97802088,  0.        ])}\n",
      "cos(angle):  -0.20850695518806803   sin(angle)  0.9780208840501317\n",
      "theta dot:  -5.70949941004\n",
      "Angle:  102.03463174373746\n",
      "action:  [-0.24938902]\n",
      "state:  {1: array([ 4.67176749]), 2: array([-5.0133921]), 3: array([ 0.04061032,  0.99917506,  0.        ])}\n",
      "cos(angle):  0.040610321686967245   sin(angle)  0.9991750606237533\n",
      "theta dot:  -5.01339209927\n",
      "Angle:  87.67235491237398\n",
      "action:  [-0.14995562]\n",
      "state:  {1: array([ 4.45744228]), 2: array([-4.28650415]), 3: array([ 0.25219383,  0.96767674,  0.        ])}\n",
      "cos(angle):  0.2521938315961831   sin(angle)  0.9676767390533039\n",
      "theta dot:  -4.28650414751\n",
      "Angle:  75.39245380225628\n",
      "action:  [-0.00884319]\n",
      "state:  {1: array([ 4.27933863]), 2: array([-3.56207307]), 3: array([ 0.41964153,  0.90768992,  0.        ])}\n",
      "cos(angle):  0.41964152540491967   sin(angle)  0.9076899196068182\n",
      "theta dot:  -3.5620730711\n",
      "Angle:  65.1878900003909\n",
      "action:  [ 0.09160887]\n",
      "state:  {1: array([ 4.13596041]), 2: array([-2.8675643]), 3: array([ 0.54503308,  0.83841454,  0.        ])}\n",
      "cos(angle):  0.5450330789247855   sin(angle)  0.8384145411893621\n",
      "theta dot:  -2.86756430098\n",
      "Angle:  56.97294261408638\n",
      "action:  [ 0.12483212]\n",
      "state:  {1: array([ 4.02495898]), 2: array([-2.22002858]), 3: array([ 0.63455298,  0.77287937,  0.        ])}\n",
      "cos(angle):  0.6345529778865909   sin(angle)  0.7728793685014885\n",
      "theta dot:  -2.22002857753\n",
      "Angle:  50.61304409174113\n",
      "action:  [ 0.13138124]\n",
      "state:  {1: array([ 3.94392589]), 2: array([-1.62066187]), 3: array([ 0.69503105,  0.71897972,  0.        ])}\n",
      "cos(angle):  0.6950310531865129   sin(angle)  0.7189797181467963\n",
      "theta dot:  -1.62066186562\n",
      "Angle:  45.970200702832585\n",
      "action:  [ 0.1716777]\n",
      "state:  {1: array([ 3.89114212]), 2: array([-1.05567542]), 3: array([ 0.7319959 ,  0.68130904,  0.        ])}\n",
      "cos(angle):  0.7319958965904658   sin(angle)  0.6813090395515974\n",
      "theta dot:  -1.05567542146\n",
      "Angle:  42.94592046564259\n",
      "action:  [ 0.25925608]\n",
      "state:  {1: array([ 3.86585186]), 2: array([-0.50580523]), 3: array([ 0.74899046,  0.66258078,  0.        ])}\n",
      "cos(angle):  0.7489904648008208   sin(angle)  0.6625807751794873\n",
      "theta dot:  -0.5058052292\n",
      "Angle:  41.49689860964463\n",
      "action:  [ 0.33722007]\n",
      "state:  {1: array([ 3.86793753]), 2: array([ 0.04171336]), 3: array([ 0.74760691,  0.66414148,  0.        ])}\n",
      "cos(angle):  0.7476069131356557   sin(angle)  0.6641414784756151\n",
      "theta dot:  0.041713362682\n",
      "Angle:  41.61639831175131\n",
      "action:  [ 0.32491691]\n",
      "state:  {1: array([ 3.89736538]), 2: array([ 0.58855701]), 3: array([ 0.72774179,  0.68585122,  0.        ])}\n",
      "cos(angle):  0.7277417879950542   sin(angle)  0.6858512156479433\n",
      "theta dot:  0.588557008025\n",
      "Angle:  43.3024859970792\n",
      "action:  [ 0.19000632]\n",
      "state:  {1: array([ 3.95393769]), 2: array([ 1.13144637]), 3: array([ 0.68779806,  0.72590208,  0.        ])}\n",
      "cos(angle):  0.6877980558052608   sin(angle)  0.7259020832250749\n",
      "theta dot:  1.13144636728\n",
      "Angle:  46.54383349692176\n",
      "action:  [-0.018967]\n",
      "state:  {1: array([ 4.03758909]), 2: array([ 1.67302788]), 3: array([ 0.62474108,  0.78083198,  0.        ])}\n",
      "cos(angle):  0.6247410786689471   sin(angle)  0.7808319823262623\n",
      "theta dot:  1.67302787919\n",
      "Angle:  51.336694113383544\n",
      "action:  [-0.1987572]\n",
      "state:  {1: array([ 4.149031]), 2: array([ 2.22883829]), 3: array([ 0.53402826,  0.84546663,  0.        ])}\n",
      "cos(angle):  0.5340282617061842   sin(angle)  0.8454666260113827\n",
      "theta dot:  2.22883828551\n",
      "Angle:  57.72183053101264\n",
      "action:  [-0.25435797]\n",
      "state:  {1: array([ 4.29027023]), 2: array([ 2.82478456]), 3: array([ 0.40969414,  0.91222295,  0.        ])}\n",
      "cos(angle):  0.4096941435320218   sin(angle)  0.9122229490401801\n",
      "theta dot:  2.82478455928\n",
      "Angle:  65.81422327149534\n",
      "action:  [-0.16329659]\n",
      "state:  {1: array([ 4.46449309]), 2: array([ 3.48445728]), 3: array([ 0.24536471,  0.96943084,  0.        ])}\n",
      "cos(angle):  0.24536471072610894   sin(angle)  0.9694308426753777\n",
      "theta dot:  3.4844572829\n",
      "Angle:  75.79643473893373\n",
      "action:  [ 0.00278089]\n",
      "state:  {1: array([ 4.67509047]), 2: array([ 4.21194755]), 3: array([ 0.03728986,  0.99930449,  0.        ])}\n",
      "cos(angle):  0.0372898608630986   sin(angle)  0.9993044912722101\n",
      "theta dot:  4.21194754816\n",
      "Angle:  87.8627474246415\n",
      "action:  [ 0.12421565]\n",
      "state:  {1: array([ 4.92409339]), 2: array([ 4.98005826]), 3: array([-0.21012656,  0.97767419,  0.        ])}\n",
      "cos(angle):  -0.21012655684918657   sin(angle)  0.9776741942522087\n",
      "theta dot:  4.98005826445\n",
      "Angle:  102.12953007680818\n",
      "action:  [ 0.11490285]\n",
      "state:  {1: array([ 5.21062085]), 2: array([ 5.73054934]), 3: array([-0.47787311,  0.87842888,  0.        ])}\n",
      "cos(angle):  -0.4778731114127775   sin(angle)  0.8784288755435303\n",
      "theta dot:  5.73054933711\n",
      "Angle:  118.5463062526448\n",
      "action:  [-0.02529811]\n",
      "state:  {1: array([ 5.52989967]), 2: array([ 6.38557628]), 3: array([-0.7294453 ,  0.68403914,  0.        ])}\n",
      "cos(angle):  -0.7294453030374383   sin(angle)  0.6840391435266112\n",
      "theta dot:  6.3855762778\n",
      "Angle:  136.83959199883623\n",
      "action:  [-0.22019486]\n",
      "state:  {1: array([ 5.87317849]), 2: array([ 6.86557641]), 3: array([-0.9171181 ,  0.39861558,  0.        ])}\n",
      "cos(angle):  -0.917118103910706   sin(angle)  0.39861558358803717\n",
      "theta dot:  6.86557640622\n",
      "Angle:  156.50797360566503\n",
      "action:  [-0.36940414]\n",
      "state:  {1: array([ 6.22863486]), 2: array([ 7.10912747]), 3: array([-0.99851249,  0.0545234 ,  0.        ])}\n",
      "cos(angle):  -0.9985124932764629   sin(angle)  0.05452339654516978\n",
      "theta dot:  7.1091274729\n",
      "Angle:  176.87407599173196\n",
      "action:  [-0.40753663]\n",
      "state:  {1: array([ 0.29989403]), 2: array([ 7.08888953]), 3: array([-0.9553678 , -0.29541897,  0.        ])}\n",
      "cos(angle):  -0.9553678003325137   sin(angle)  -0.29541896704141096\n",
      "theta dot:  7.08888952566\n",
      "Angle:  197.1830428998821\n",
      "action:  [-0.34420704]\n",
      "state:  {1: array([ 0.64067874]), 2: array([ 6.81569425]), 3: array([-0.80169023, -0.59773972,  0.        ])}\n",
      "cos(angle):  -0.8016902319723929   sin(angle)  -0.5977397192424566\n",
      "theta dot:  6.81569424509\n",
      "Angle:  216.70852297557138\n",
      "action:  [-0.25389361]\n",
      "state:  {1: array([ 0.95714401]), 2: array([ 6.32930541]), 3: array([-0.57585725, -0.81755026,  0.        ])}\n",
      "cos(angle):  -0.5758572452180057   sin(angle)  -0.8175502633660695\n",
      "theta dot:  6.32930541385\n",
      "Angle:  234.84060494800963\n",
      "action:  [-0.21104622]\n",
      "state:  {1: array([ 1.2413683]), 2: array([ 5.68448578]), 3: array([-0.32350186, -0.94622753,  0.        ])}\n",
      "cos(angle):  -0.32350186348458115   sin(angle)  -0.9462275330606288\n",
      "theta dot:  5.68448578348\n",
      "Angle:  251.1254190717977\n",
      "action:  [-0.23151347]\n",
      "state:  {1: array([ 1.48837271]), 2: array([ 4.94008811]), 3: array([-0.08233033, -0.9966051 ,  0.        ])}\n",
      "cos(angle):  -0.08233032544851923   sin(angle)  -0.9966050960694215\n",
      "theta dot:  4.94008811345\n",
      "Angle:  265.27769594377645\n",
      "action:  [-0.28136984]\n",
      "state:  {1: array([ 1.69589415]), 2: array([ 4.15042882]), 3: array([ 0.12477179, -0.99218547,  0.        ])}\n",
      "cos(angle):  0.1247717903442294   sin(angle)  -0.9921854667018136\n",
      "theta dot:  4.15042881571\n",
      "Angle:  277.16777085510415\n",
      "action:  [-0.32438484]\n",
      "state:  {1: array([ 1.86377575]), 2: array([ 3.35763199]), 3: array([ 0.28880596, -0.95738765,  0.        ])}\n",
      "cos(angle):  0.28880596293465605   sin(angle)  -0.9573876517761163\n",
      "theta dot:  3.35763199005\n",
      "Angle:  286.7866554713637\n",
      "action:  [-0.34394095]\n",
      "state:  {1: array([ 1.99317575]), 2: array([ 2.58800011]), 3: array([ 0.40993192, -0.91211612,  0.        ])}\n",
      "cos(angle):  0.4099319240228356   sin(angle)  -0.9121161207143179\n",
      "theta dot:  2.58800010875\n",
      "Angle:  294.2007123146186\n",
      "action:  [-0.33758286]\n",
      "state:  {1: array([ 2.08583953]), 2: array([ 1.85327559]), 3: array([ 0.49257245, -0.87027144,  0.        ])}\n",
      "cos(angle):  0.49257244973264275   sin(angle)  -0.8702714414275485\n",
      "theta dot:  1.85327558914\n",
      "Angle:  299.5099433759365\n",
      "action:  [-0.31369509]\n",
      "state:  {1: array([ 2.14351542]), 2: array([ 1.15351774]), 3: array([ 0.54191926, -0.84043056,  0.        ])}\n",
      "cos(angle):  0.5419192575832028   sin(angle)  -0.8404305552872707\n",
      "theta dot:  1.15351774436\n",
      "Angle:  302.81452056565513\n",
      "action:  [-0.285339]\n",
      "state:  {1: array([ 2.16753512]), 2: array([ 0.48039398]), 3: array([ 0.56194788, -0.82717264,  0.        ])}\n",
      "cos(angle):  0.5619478838600056   sin(angle)  -0.8271726396740052\n",
      "theta dot:  0.48039397802\n",
      "Angle:  304.19074471964734\n",
      "action:  [-0.25335219]\n",
      "state:  {1: array([ 2.1586357]), 2: array([-0.17798833]), 3: array([ 0.55456437, -0.83214083,  0.        ])}\n",
      "cos(angle):  0.5545643742745344   sin(angle)  -0.8321408262941401\n",
      "theta dot:  -0.177988330859\n",
      "Angle:  303.6808469039708\n",
      "action:  [-0.20378161]\n",
      "state:  {1: array([ 2.11700264]), 2: array([-0.83266119]), 3: array([ 0.51944927, -0.85450129,  0.        ])}\n",
      "cos(angle):  0.5194492660628713   sin(angle)  -0.8545012931451562\n",
      "theta dot:  -0.832661192119\n",
      "Angle:  301.29545387841864\n",
      "action:  [-0.13365413]\n",
      "state:  {1: array([ 2.04232338]), 2: array([-1.49358528]), 3: array([ 0.45424723, -0.89087567,  0.        ])}\n",
      "cos(angle):  0.454247227115354   sin(angle)  -0.8908756684622227\n",
      "theta dot:  -1.49358528174\n",
      "Angle:  297.01665723476975\n",
      "action:  [-0.06981071]\n",
      "state:  {1: array([ 1.9337127]), 2: array([-2.17221364]), 3: array([ 0.35500215, -0.93486548,  0.        ])}\n",
      "cos(angle):  0.35500215191702406   sin(angle)  -0.9348654834436247\n",
      "theta dot:  -2.17221363898\n",
      "Angle:  290.7937381009367\n",
      "action:  [-0.05023603]\n",
      "state:  {1: array([ 1.78966779]), 2: array([-2.88089816]), 3: array([ 0.21712815, -0.97614311,  0.        ])}\n",
      "cos(angle):  0.21712814633474908   sin(angle)  -0.9761431083961182\n",
      "theta dot:  -2.88089815577\n",
      "Angle:  282.5405921237534\n",
      "action:  [-0.08991724]\n",
      "state:  {1: array([ 1.60834313]), 2: array([-3.62649307]), 3: array([ 0.03753799, -0.9992952 ,  0.        ])}\n",
      "cos(angle):  0.03753798628901168   sin(angle)  -0.9992952014221653\n",
      "theta dot:  -3.62649307291\n",
      "Angle:  272.15147904244867\n",
      "action:  [-0.17174204]\n",
      "state:  {1: array([ 1.38825685]), 2: array([-4.40172578]), 3: array([-0.18152745, -0.98338588,  0.        ])}\n",
      "cos(angle):  -0.18152744617280372   sin(angle)  -0.9833858786285168\n",
      "theta dot:  -4.40172578066\n",
      "Angle:  259.54149303978573\n",
      "action:  [-0.26644466]\n",
      "state:  {1: array([ 1.12929525]), 2: array([-5.17923189]), 3: array([-0.42729709, -0.90411128,  0.        ])}\n",
      "cos(angle):  -0.42729708551690543   sin(angle)  -0.9041112767291195\n",
      "theta dot:  -5.17923188907\n",
      "Angle:  244.70412131786105\n",
      "action:  [-0.35404085]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "state:  {1: array([ 0.83377418]), 2: array([-5.91042147]), 3: array([-0.67208588, -0.74047321,  0.        ])}\n",
      "cos(angle):  -0.6720858762824378   sin(angle)  -0.7404732101174679\n",
      "theta dot:  -5.91042147458\n",
      "Angle:  227.7720506305608\n",
      "action:  [-0.42445102]\n",
      "state:  {1: array([ 0.50730198]), 2: array([-6.52944403]), 3: array([-0.87405844, -0.48582079,  0.        ])}\n",
      "cos(angle):  -0.874058443712963   sin(angle)  -0.48582078688964403\n",
      "theta dot:  -6.52944403487\n",
      "Angle:  209.06661508375234\n",
      "action:  [-0.4677728]\n",
      "state:  {1: array([ 0.1591032]), 2: array([-6.96397554]), 3: array([-0.98736976, -0.1584328 ,  0.        ])}\n",
      "cos(angle):  -0.987369763205865   sin(angle)  -0.15843279555317505\n",
      "theta dot:  -6.96397554452\n",
      "Angle:  189.11634136950764\n",
      "action:  [-0.47606157]\n",
      "state:  {1: array([ 6.08457804]), 2: array([-7.15420938]), 3: array([-0.98034232,  0.19730417,  0.        ])}\n",
      "cos(angle):  -0.9803423198677262   sin(angle)  0.19730417095531638\n",
      "theta dot:  -7.15420937652\n",
      "Angle:  168.62024731032795\n",
      "action:  [-0.45311295]\n",
      "state:  {1: array([ 5.73086813]), 2: array([-7.07419819]), 3: array([-0.85131107,  0.52466128,  0.        ])}\n",
      "cos(angle):  -0.851311073974232   sin(angle)  0.5246612767575282\n",
      "theta dot:  -7.07419819075\n",
      "Angle:  148.35420971269548\n",
      "action:  [-0.40954971]\n",
      "state:  {1: array([ 5.39376139]), 2: array([-6.74213469]), 3: array([-0.62985958,  0.77670902,  0.        ])}\n",
      "cos(angle):  -0.6298595818029349   sin(angle)  0.7767090235159058\n",
      "theta dot:  -6.74213469037\n",
      "Angle:  129.0394617456392\n",
      "action:  [-0.3468126]\n",
      "state:  {1: array([ 5.08318015]), 2: array([-6.21162481]), 3: array([-0.36235295,  0.93204095,  0.        ])}\n",
      "cos(angle):  -0.36235294959562775   sin(angle)  0.932040953992553\n",
      "theta dot:  -6.21162481282\n",
      "Angle:  111.24450907330362\n",
      "action:  [-0.25715925]\n",
      "state:  {1: array([ 4.80562175]), 2: array([-5.55116799]), 3: array([-0.09309776,  0.99565697,  0.        ])}\n",
      "cos(angle):  -0.09309776234204241   sin(angle)  0.9956569723789939\n",
      "theta dot:  -5.55116798536\n",
      "Angle:  95.3416214147037\n",
      "action:  [-0.14066046]\n",
      "state:  {1: array([ 4.56434554]), 2: array([-4.82552433]), 3: array([ 0.14750326,  0.98906157,  0.        ])}\n",
      "cos(angle):  0.14750326166905103   sin(angle)  0.9890615692650238\n",
      "theta dot:  -4.82552432551\n",
      "Angle:  81.51754485194795\n",
      "action:  [-0.01779881]\n",
      "state:  {1: array([ 4.36002564]), 2: array([-4.08639797]), 3: array([ 0.34511691,  0.93855971,  0.        ])}\n",
      "cos(angle):  0.34511690724776733   sin(angle)  0.9385597052568024\n",
      "theta dot:  -4.08639797067\n",
      "Angle:  69.81090437066234\n",
      "action:  [ 0.07410969]\n",
      "state:  {1: array([ 4.19145755]), 2: array([-3.37136174]), 3: array([ 0.49768823,  0.86735599,  0.        ])}\n",
      "cos(angle):  0.49768823453845357   sin(angle)  0.8673559945039853\n",
      "theta dot:  -3.37136173892\n",
      "Angle:  60.15268701317251\n",
      "action:  [ 0.10002751]\n",
      "state:  {1: array([ 4.05616552]), 2: array([-2.70584062]), 3: array([ 0.61012905,  0.79230205,  0.        ])}\n",
      "cos(angle):  0.6101290498338721   sin(angle)  0.7923020525966196\n",
      "theta dot:  -2.70584061688\n",
      "Angle:  52.40104277076313\n",
      "action:  [ 0.05047879]\n",
      "state:  {1: array([ 3.95096341]), 2: array([-2.10404226]), 3: array([ 0.68995405,  0.72385317,  0.        ])}\n",
      "cos(angle):  0.689954051538027   sin(angle)  0.723853166578873\n",
      "theta dot:  -2.10404225849\n",
      "Angle:  46.37341979954499\n",
      "action:  [-0.05135576]\n",
      "state:  {1: array([ 3.87252062]), 2: array([-1.56885575]), 3: array([ 0.74455525,  0.66756084,  0.        ])}\n",
      "cos(angle):  0.74455524914067   sin(angle)  0.6675608444007743\n",
      "theta dot:  -1.56885574828\n",
      "Angle:  41.878989657401064\n",
      "action:  [-0.16572158]\n",
      "state:  {1: array([ 3.81786845]), 2: array([-1.09304335]), 3: array([ 0.77990907,  0.62589283,  0.        ])}\n",
      "cos(angle):  0.7799090729078684   sin(angle)  0.6258928326766406\n",
      "theta dot:  -1.0930433525\n",
      "Angle:  38.74765843367135\n",
      "action:  [-0.25708229]\n",
      "state:  {1: array([ 3.78475915]), 2: array([-0.66218607]), 3: array([ 0.80020072,  0.59973228,  0.        ])}\n",
      "cos(angle):  0.8002007234934296   sin(angle)  0.5997322753700953\n",
      "theta dot:  -0.662186070839\n",
      "Angle:  36.850639514155375\n",
      "action:  [-0.30371225]\n",
      "state:  {1: array([ 3.77186196]), 2: array([-0.2579437]), 3: array([ 0.80786882,  0.58936235,  0.        ])}\n",
      "cos(angle):  0.8078688165128222   sin(angle)  0.5893623463593277\n",
      "theta dot:  -0.257943702313\n",
      "Angle:  36.11168696742351\n",
      "action:  [-0.30027975]\n",
      "state:  {1: array([ 3.77881377]), 2: array([ 0.1390361]), 3: array([ 0.8037522 ,  0.59496421,  0.        ])}\n",
      "cos(angle):  0.8037521964498645   sin(angle)  0.5949642062359872\n",
      "theta dot:  0.139036095241\n",
      "Angle:  36.509995108870896\n",
      "action:  [-0.25617222]\n",
      "state:  {1: array([ 3.80615544]), 2: array([ 0.54683342]), 3: array([ 0.7871865 ,  0.61671502,  0.        ])}\n",
      "cos(angle):  0.787186497121387   sin(angle)  0.616715022315624\n",
      "theta dot:  0.546833416892\n",
      "Angle:  38.076553789807406\n",
      "action:  [-0.18644868]\n",
      "state:  {1: array([ 3.85522556]), 2: array([ 0.98140238]), 3: array([ 0.75598883,  0.65458452,  0.        ])}\n",
      "cos(angle):  0.7559888269158057   sin(angle)  0.6545845198127314\n",
      "theta dot:  0.981402381078\n",
      "Angle:  40.888057937281644\n",
      "action:  [-0.10240283]\n",
      "state:  {1: array([ 3.92807458]), 2: array([ 1.45698035]), 3: array([ 0.70634003,  0.7078727 ,  0.        ])}\n",
      "cos(angle):  0.7063400331084612   sin(angle)  0.7078726987448647\n",
      "theta dot:  1.45698034685\n",
      "Angle:  45.06198941220737\n",
      "action:  [-0.00691855]\n",
      "state:  {1: array([ 4.02741693]), 2: array([ 1.98684709]), 3: array([ 0.63265137,  0.77443673,  0.        ])}\n",
      "cos(angle):  0.6326513666696177   sin(angle)  0.7744367296629627\n",
      "theta dot:  1.98684708775\n",
      "Angle:  50.75387373540701\n",
      "action:  [ 0.10714998]\n",
      "state:  {1: array([ 4.15660429]), 2: array([ 2.58374713]), 3: array([ 0.52761005,  0.84948669,  0.        ])}\n",
      "cos(angle):  0.52761004962446   sin(angle)  0.8494866894397315\n",
      "theta dot:  2.58374713196\n",
      "Angle:  58.15574672612115\n",
      "action:  [ 0.24360487]\n",
      "state:  {1: array([ 4.31947443]), 2: array([ 3.25740288]), 3: array([ 0.38288249,  0.92379705,  0.        ])}\n",
      "cos(angle):  0.3828824901882302   sin(angle)  0.9237970549353682\n",
      "theta dot:  3.25740287968\n",
      "Angle:  67.4874967634114\n",
      "action:  [ 0.37286629]\n",
      "state:  {1: array([ 4.51978346]), 2: array([ 4.00618061]), 3: array([ 0.19141688,  0.98150883,  0.        ])}\n",
      "cos(angle):  0.19141688197685144   sin(angle)  0.9815088269059327\n",
      "theta dot:  4.00618061375\n",
      "Angle:  78.96433198234115\n",
      "action:  [ 0.43268998]\n",
      "state:  {1: array([ 4.76014425]), 2: array([ 4.80721573]), 3: array([-0.04773712,  0.99885993,  0.        ])}\n",
      "cos(angle):  -0.04773711783134061   sin(angle)  0.9988599339152395\n",
      "theta dot:  4.80721573055\n",
      "Angle:  92.7359584067687\n",
      "action:  [ 0.38720647]\n",
      "state:  {1: array([ 5.04086633]), 2: array([ 5.61444165]), 3: array([-0.32260216,  0.94653465,  0.        ])}\n",
      "cos(angle):  -0.32260216177588497   sin(angle)  0.9465346508277052\n",
      "theta dot:  5.61444165132\n",
      "Angle:  108.82011134217464\n",
      "action:  [ 0.27713049]\n",
      "state:  {1: array([ 5.35916194]), 2: array([ 6.36591221]), 3: array([-0.60261427,  0.79803261,  0.        ])}\n",
      "cos(angle):  -0.6026142651542047   sin(angle)  0.7980326105070256\n",
      "theta dot:  6.36591221239\n",
      "Angle:  127.0570638222801\n",
      "action:  [ 0.1881553]\n",
      "state:  {1: array([ 5.70879494]), 2: array([ 6.99265997]), 3: array([-0.83952369,  0.54332309,  0.        ])}\n",
      "cos(angle):  -0.839523685518152   sin(angle)  0.5433230912210699\n",
      "theta dot:  6.99265996581\n",
      "Angle:  147.08951215824078\n",
      "action:  [ 0.17442827]\n",
      "state:  {1: array([ 6.08011077]), 2: array([ 7.42631652]), 3: array([-0.97945113,  0.20168164,  0.        ])}\n",
      "cos(angle):  -0.9794511294453878   sin(angle)  0.20168164276441766\n",
      "theta dot:  7.42631652413\n",
      "Angle:  168.36429211660504\n",
      "action:  [ 0.22577346]\n",
      "state:  {1: array([ 0.17749765]), 2: array([ 7.61144378]), 3: array([-0.98428861, -0.17656709,  0.        ])}\n",
      "cos(angle):  -0.984288607222562   sin(angle)  -0.17656709119161781\n",
      "theta dot:  7.61144377561\n",
      "Angle:  190.17026317852626\n",
      "action:  [ 0.29786219]\n",
      "state:  {1: array([ 0.55368254]), 2: array([ 7.52369779]), 3: array([-0.85059393, -0.52582313,  0.        ])}\n",
      "cos(angle):  -0.8505939312047771   sin(angle)  -0.5258231301470133\n",
      "theta dot:  7.52369778521\n",
      "Angle:  211.72401924769747\n",
      "action:  [ 0.35446743]\n",
      "state:  {1: array([ 0.91280756]), 2: array([ 7.18250055]), 3: array([-0.61152675, -0.79122376,  0.        ])}\n",
      "cos(angle):  -0.6115267513821642   sin(angle)  -0.7912237561802457\n",
      "theta dot:  7.18250055218\n",
      "Angle:  232.30031953087098\n",
      "action:  [ 0.38501391]\n",
      "state:  {1: array([ 1.2451493]), 2: array([ 6.64683482]), 3: array([-0.31992187, -0.94744393,  0.        ])}\n",
      "cos(angle):  -0.31992186977081166   sin(angle)  -0.9474439282840689\n",
      "theta dot:  6.64683482143\n",
      "Angle:  251.34205412243907\n",
      "action:  [ 0.39900862]\n",
      "state:  {1: array([ 1.54495446]), 2: array([ 5.99610317]), 3: array([-0.02583899, -0.99966612,  0.        ])}\n",
      "cos(angle):  -0.02583898737902229   sin(angle)  -0.9996661176268938\n",
      "theta dot:  5.99610316862\n",
      "Angle:  268.5195842082404\n",
      "action:  [ 0.40947278]\n",
      "state:  {1: array([ 1.81034319]), 2: array([ 5.3077745]), 3: array([ 0.23726245, -0.97144559,  0.        ])}\n",
      "cos(angle):  0.23726245125129847   sin(angle)  -0.9714455873728725\n",
      "theta dot:  5.30777449665\n",
      "Angle:  283.72520251414505\n",
      "action:  [ 0.41767759]\n",
      "state:  {1: array([ 2.04243529]), 2: array([ 4.64184194]), 3: array([ 0.45434692, -0.89082483,  0.        ])}\n",
      "cos(angle):  0.4543469199481527   sin(angle)  -0.8908248292080924\n",
      "theta dot:  4.64184194467\n",
      "Angle:  297.02306904776697\n",
      "action:  [ 0.41328603]\n",
      "state:  {1: array([ 2.2442211]), 2: array([ 4.03571623]), 3: array([ 0.62366675, -0.78169034,  0.        ])}\n",
      "cos(angle):  0.6236667488823507   sin(angle)  -0.7816903391615626\n",
      "theta dot:  4.03571622724\n",
      "Angle:  308.5845173687311\n",
      "action:  [ 0.39288142]\n",
      "state:  {1: array([ 2.41964013]), 2: array([ 3.50838069]), 3: array([ 0.75051683, -0.66085133,  0.        ])}\n",
      "cos(angle):  0.7505168334464304   sin(angle)  -0.6608513317785953\n",
      "theta dot:  3.50838068613\n",
      "Angle:  318.6352641777496\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -0.476061568881  self.max:  0.82617291766\n",
      "state:  {1: array([ 4.05646642]), 2: array([ 0.00827646]), 3: array([ 0.60989062,  0.7924856 ,  0.        ])}\n",
      "cos(angle):  0.6098906188705335   sin(angle)  0.7924856042943099\n",
      "theta dot:  0.00827645734779\n",
      "Angle:  52.418283005814885\n",
      "action:  [ 0.09828865]\n",
      "state:  {1: array([ 4.08733562]), 2: array([ 0.61738396]), 3: array([ 0.58514055,  0.8109319 ,  0.        ])}\n",
      "cos(angle):  0.5851405473730771   sin(angle)  0.8109318959197102\n",
      "theta dot:  0.617383958596\n",
      "Angle:  54.1869536282241\n",
      "action:  [ 0.0295393]\n",
      "state:  {1: array([ 4.14883631]), 2: array([ 1.23001378]), 3: array([ 0.53419286,  0.84536264,  0.        ])}\n",
      "cos(angle):  0.5341928606501832   sin(angle)  0.8453626367603277\n",
      "theta dot:  1.23001377525\n",
      "Angle:  57.71067529147666\n",
      "action:  [-0.03190904]\n",
      "state:  {1: array([ 4.24179878]), 2: array([ 1.8592494]), 3: array([ 0.45341241,  0.89130084,  0.        ])}\n",
      "cos(angle):  0.4534124138226318   sin(angle)  0.8913008375355285\n",
      "theta dot:  1.8592493967\n",
      "Angle:  63.037020010828485\n",
      "action:  [-0.02292504]\n",
      "state:  {1: array([ 4.36801309]), 2: array([ 2.52428627]), 3: array([ 0.33760928,  0.94128634,  0.        ])}\n",
      "cos(angle):  0.33760927755107456   sin(angle)  0.9412863409778353\n",
      "theta dot:  2.52428626857\n",
      "Angle:  70.2685505739484\n",
      "action:  [ 0.0517538]\n",
      "state:  {1: array([ 4.52991379]), 2: array([ 3.23801409]), 3: array([ 0.18146422,  0.98339755,  0.        ])}\n",
      "cos(angle):  0.18146421872721688   sin(angle)  0.9833975479538887\n",
      "theta dot:  3.23801409414\n",
      "Angle:  79.54475596204338\n",
      "action:  [ 0.15631413]\n",
      "state:  {1: array([ 4.72986426]), 2: array([ 3.99900937]), 3: array([-0.01747439,  0.99984731,  0.        ])}\n",
      "cos(angle):  -0.017474393192488207   sin(angle)  0.9998473111343323\n",
      "theta dot:  3.99900937399\n",
      "Angle:  91.00104714038196\n",
      "action:  [ 0.28060493]\n",
      "state:  {1: array([ 4.96941354]), 2: array([ 4.7909856]), 3: array([-0.25420399,  0.96715063,  0.        ])}\n",
      "cos(angle):  -0.2542039851626712   sin(angle)  0.9671506262870414\n",
      "theta dot:  4.79098559751\n",
      "Angle:  104.726177767315\n",
      "action:  [ 0.43494129]\n",
      "state:  {1: array([ 5.24849303]), 2: array([ 5.58158976]), 3: array([-0.51079051,  0.85970521,  0.        ])}\n",
      "cos(angle):  -0.5107905085520352   sin(angle)  0.8597052148109567\n",
      "theta dot:  5.58158976075\n",
      "Angle:  120.71621718886456\n",
      "action:  [ 0.59896527]\n",
      "state:  {1: array([ 5.5643037]), 2: array([ 6.31621346]), 3: array([-0.75254271,  0.65854344,  0.        ])}\n",
      "cos(angle):  -0.752542712537106   sin(angle)  0.6585434426120229\n",
      "theta dot:  6.31621346187\n",
      "Angle:  138.81079356931969\n",
      "action:  [ 0.70904822]\n",
      "state:  {1: array([ 5.91012762]), 2: array([ 6.91647828]), 3: array([-0.93121728,  0.3644645 ,  0.        ])}\n",
      "cos(angle):  -0.9312172813134363   sin(angle)  0.36446450442150385\n",
      "theta dot:  6.91647827712\n",
      "Angle:  158.6249979537231\n",
      "action:  [ 0.71444439]\n",
      "state:  {1: array([ 6.27497728]), 2: array([ 7.29699331]), 3: array([-0.99996631,  0.00820793,  0.        ])}\n",
      "cos(angle):  -0.9999663143631844   sin(angle)  0.00820793146347163\n",
      "theta dot:  7.29699331353\n",
      "Angle:  179.52929507039426\n",
      "action:  [ 0.63130314]\n",
      "state:  {1: array([ 0.36168421]), 2: array([ 7.39784473]), 3: array([-0.93530219, -0.35384998,  0.        ])}\n",
      "cos(angle):  -0.9353021917356032   sin(angle)  -0.3538499825273088\n",
      "theta dot:  7.39784473311\n",
      "Angle:  200.7233513810964\n",
      "action:  [ 0.52522622]\n",
      "state:  {1: array([ 0.72224627]), 2: array([ 7.21124118]), 3: array([-0.75032268, -0.66107177,  0.        ])}\n",
      "cos(angle):  -0.7503226763822408   sin(angle)  -0.6610717671377224\n",
      "theta dot:  7.21124117859\n",
      "Angle:  221.38198730142835\n",
      "action:  [ 0.44901978]\n",
      "state:  {1: array([ 1.06138579]), 2: array([ 6.78279032]), 3: array([-0.48766271, -0.87303212,  0.        ])}\n",
      "cos(angle):  -0.487662713106565   sin(angle)  -0.8730321175337962\n",
      "theta dot:  6.78279032005\n",
      "Angle:  240.81320479582865\n",
      "action:  [ 0.41033716]\n",
      "state:  {1: array([ 1.37086413]), 2: array([ 6.18956681]), 3: array([-0.19860288, -0.98008005,  0.        ])}\n",
      "cos(angle):  -0.19860288040474836   sin(angle)  -0.9800800456569541\n",
      "theta dot:  6.18956680568\n",
      "Angle:  258.54496608023607\n",
      "action:  [ 0.38996439]\n",
      "state:  {1: array([ 1.6465142]), 2: array([ 5.51300143]), 3: array([ 0.07564554, -0.99713477,  0.        ])}\n",
      "cos(angle):  0.07564554278938085   sin(angle)  -0.9971347711598969\n",
      "theta dot:  5.51300143016\n",
      "Angle:  274.3385148679456\n",
      "action:  [ 0.37048796]\n",
      "state:  {1: array([ 1.88755038]), 2: array([ 4.82072355]), 3: array([ 0.31148374, -0.95025148,  0.        ])}\n",
      "cos(angle):  0.3114837383609773   sin(angle)  -0.9502514828910662\n",
      "theta dot:  4.82072354518\n",
      "Angle:  288.14883824029823\n",
      "action:  [ 0.34379834]\n",
      "state:  {1: array([ 2.09553061]), 2: array([ 4.15960468]), 3: array([ 0.50098306, -0.86545709,  0.        ])}\n",
      "cos(angle):  0.5009830569422777   sin(angle)  -0.8654570911701923\n",
      "theta dot:  4.15960468439\n",
      "Angle:  300.06520001756616\n",
      "action:  [ 0.30280416]\n",
      "state:  {1: array([ 2.27332724]), 2: array([ 3.55593249]), 3: array([ 0.64615137, -0.76320928,  0.        ])}\n",
      "cos(angle):  0.6461513678323448   sin(angle)  -0.7632092831251398\n",
      "theta dot:  3.55593249007\n",
      "Angle:  310.2521723917265\n",
      "action:  [ 0.24228916]\n",
      "state:  {1: array([ 2.42432068]), 2: array([ 3.0198689]), 3: array([ 0.75360175, -0.65733127,  0.        ])}\n",
      "cos(angle):  0.7536017487314894   sin(angle)  -0.6573312744034328\n",
      "theta dot:  3.01986890169\n",
      "Angle:  318.9034392987879\n",
      "action:  [ 0.17310336]\n",
      "state:  {1: array([ 2.55196248]), 2: array([ 2.55283595]), 3: array([ 0.83114638, -0.55605368,  0.        ])}\n",
      "cos(angle):  0.8311463783246779   sin(angle)  -0.5560536824783839\n",
      "theta dot:  2.55283594961\n",
      "Angle:  326.21675848216205\n",
      "action:  [ 0.12035361]\n",
      "state:  {1: array([ 2.65965491]), 2: array([ 2.15384873]), 3: array([ 0.88609845, -0.46349707,  0.        ])}\n",
      "cos(angle):  0.8860984506457942   sin(angle)  -0.46349707201138085\n",
      "theta dot:  2.15384872939\n",
      "Angle:  332.3870661484718\n",
      "action:  [ 0.09606683]\n",
      "state:  {1: array([ 2.75068671]), 2: array([ 1.82063595]), 3: array([ 0.92456425, -0.38102617,  0.        ])}\n",
      "cos(angle):  0.9245642519561053   sin(angle)  -0.38102617233576946\n",
      "theta dot:  1.82063594985\n",
      "Angle:  337.6027917496578\n",
      "action:  [ 0.09035842]\n",
      "state:  {1: array([ 2.82810772]), 2: array([ 1.54842008]), 3: array([ 0.95126468, -0.3083756 ,  0.        ])}\n",
      "cos(angle):  0.9512646789303104   sin(angle)  -0.3083755999096125\n",
      "theta dot:  1.54842008398\n",
      "Angle:  342.0386781628966\n",
      "action:  [ 0.1001227]\n",
      "state:  {1: array([ 2.89471556]), 2: array([ 1.33215679]), 3: array([ 0.96968031, -0.24437694,  0.        ])}\n",
      "cos(angle):  0.9696803141836255   sin(angle)  -0.24437693893807833\n",
      "theta dot:  1.33215678859\n",
      "Angle:  345.85501732044474\n",
      "action:  [ 0.14220935]\n",
      "state:  {1: array([ 2.95322583]), 2: array([ 1.17020549]), 3: array([ 0.98231137, -0.18725486,  0.        ])}\n",
      "cos(angle):  0.9823113652796344   sin(angle)  -0.18725485746025583\n",
      "theta dot:  1.17020548648\n",
      "Angle:  349.2074012580216\n",
      "action:  [ 0.21685247]\n",
      "state:  {1: array([ 3.00634044]), 2: array([ 1.06229221]), 3: array([ 0.99086735, -0.13484022,  0.        ])}\n",
      "cos(angle):  0.9908673543093042   sin(angle)  -0.1348402245773857\n",
      "theta dot:  1.06229221448\n",
      "Angle:  352.25063716656945\n",
      "action:  [ 0.27930839]\n",
      "state:  {1: array([ 3.05649336]), 2: array([ 1.0030583]), 3: array([ 0.99638124, -0.08499662,  0.        ])}\n",
      "cos(angle):  0.9963812394658653   sin(angle)  -0.08499662134735725\n",
      "theta dot:  1.00305830386\n",
      "Angle:  355.1241808178113\n",
      "action:  [ 0.27446375]\n",
      "state:  {1: array([ 3.10551738]), 2: array([ 0.9804804]), 3: array([ 0.99934936, -0.03606745,  0.        ])}\n",
      "cos(angle):  0.9993493577472419   sin(angle)  -0.036067453059165075\n",
      "theta dot:  0.980480400559\n",
      "Angle:  357.93304369183426\n",
      "action:  [ 0.19333481]\n",
      "state:  {1: array([ 3.15463888]), 2: array([ 0.98243003]), 3: array([ 0.9999149 ,  0.01304585,  0.        ])}\n",
      "cos(angle):  0.9999148992242293   sin(angle)  0.013045854107701187\n",
      "theta dot:  0.982430032458\n",
      "Angle:  0.7474918367685746\n",
      "action:  [ 0.07888875]\n",
      "state:  {1: array([ 3.20484126]), 2: array([ 1.00404774]), 3: array([ 0.99800047,  0.06320645,  0.        ])}\n",
      "cos(angle):  0.998000473313182   sin(angle)  0.06320644956540981\n",
      "theta dot:  1.00404773517\n",
      "Angle:  3.6238699932786744\n",
      "action:  [-0.01615063]\n",
      "state:  {1: array([ 3.25729276]), 2: array([ 1.04902998]), 3: array([ 0.99331421,  0.11544215,  0.        ])}\n",
      "cos(angle):  0.9933142055810715   sin(angle)  0.11544214564380202\n",
      "theta dot:  1.04902997711\n",
      "Angle:  6.629112479276785\n",
      "action:  [-0.0617174]\n",
      "state:  {1: array([ 3.31361046]), 2: array([ 1.12635398]), 3: array([ 0.98524138,  0.17117072,  0.        ])}\n",
      "cos(angle):  0.9852413831141431   sin(angle)  0.17117072471462585\n",
      "theta dot:  1.12635397581\n",
      "Angle:  9.855871386302908\n",
      "action:  [-0.05286436]\n",
      "state:  {1: array([ 3.37595058]), 2: array([ 1.24680236]), 3: array([ 0.97266364,  0.23221851,  0.        ])}\n",
      "cos(angle):  0.9726636430908598   sin(angle)  0.23221851220179776\n",
      "theta dot:  1.24680236486\n",
      "Angle:  13.427688703498916\n",
      "action:  [-0.00676424]\n",
      "state:  {1: array([ 3.44694816]), 2: array([ 1.41995161]), 3: array([ 0.95374014,  0.30063226,  0.        ])}\n",
      "cos(angle):  0.9537401358298141   sin(angle)  0.3006322559329382\n",
      "theta dot:  1.41995161243\n",
      "Angle:  17.49554091633894\n",
      "action:  [ 0.03762794]\n",
      "state:  {1: array([ 3.52950166]), 2: array([ 1.65106999]), 3: array([ 0.92570201,  0.37825361,  0.        ])}\n",
      "cos(angle):  0.9257020085491143   sin(angle)  0.3782536072110027\n",
      "theta dot:  1.65106999486\n",
      "Angle:  22.22549697495191\n",
      "action:  [ 0.03502675]\n",
      "state:  {1: array([ 3.62650237]), 2: array([ 1.94001421]), 3: array([ 0.88471704,  0.46612849,  0.        ])}\n",
      "cos(angle):  0.8847170358171361   sin(angle)  0.46612848715235183\n",
      "theta dot:  1.94001421204\n",
      "Angle:  27.783215305852934\n",
      "action:  [-0.03339339]\n",
      "state:  {1: array([ 3.74073245]), 2: array([ 2.28460157]), 3: array([ 0.82582102,  0.56393231,  0.        ])}\n",
      "cos(angle):  0.8258210170825359   sin(angle)  0.5639323077681984\n",
      "theta dot:  2.28460156843\n",
      "Angle:  34.328101388051664\n",
      "action:  [-0.13620011]\n",
      "state:  {1: array([ 3.87508849]), 2: array([ 2.68712078]), 3: array([ 0.74283859,  0.66947056,  0.        ])}\n",
      "cos(angle):  0.7428385873005399   sin(angle)  0.6694705618750821\n",
      "theta dot:  2.68712078294\n",
      "Angle:  42.02611738197055\n",
      "action:  [-0.20141649]\n",
      "state:  {1: array([ 4.03303905]), 2: array([ 3.15901123]), 3: array([ 0.62828741,  0.77798131,  0.        ])}\n",
      "cos(angle):  0.6282874144032871   sin(angle)  0.7779813139802474\n",
      "theta dot:  3.15901123081\n",
      "Angle:  51.075996767396646\n",
      "action:  [-0.1610068]\n",
      "state:  {1: array([ 4.21895636]), 2: array([ 3.7183462]), 3: array([ 0.47365182,  0.88071218,  0.        ])}\n",
      "cos(angle):  0.47365182303846803   sin(angle)  0.880712183708353\n",
      "theta dot:  3.71834619627\n",
      "Angle:  61.72824904854957\n",
      "action:  [-0.00067318]\n",
      "state:  {1: array([ 4.43789533]), 2: array([ 4.37877936]), 3: array([ 0.27105958,  0.96256257,  0.        ])}\n",
      "cos(angle):  0.27105958032198485   sin(angle)  0.9625625714288237\n",
      "theta dot:  4.37877935658\n",
      "Angle:  74.272498542182\n",
      "action:  [ 0.22326709]\n",
      "state:  {1: array([ 4.6946049]), 2: array([ 5.13419135]), 3: array([ 0.01778315,  0.99984187,  0.        ])}\n",
      "cos(angle):  0.01778314741146806   sin(angle)  0.9998418673311005\n",
      "theta dot:  5.13419134805\n",
      "Angle:  88.98083892047055\n",
      "action:  [ 0.41271649]\n",
      "state:  {1: array([ 4.99190391]), 2: array([ 5.94598022]), 3: array([-0.27588943,  0.96118938,  0.        ])}\n",
      "cos(angle):  -0.2758894333286745   sin(angle)  0.9611893781027664\n",
      "theta dot:  5.94598022223\n",
      "Angle:  106.01477767782734\n",
      "action:  [ 0.48199736]\n",
      "state:  {1: array([ 5.3288625]), 2: array([ 6.73917186]), 3: array([-0.57816143,  0.8159224 ,  0.        ])}\n",
      "cos(angle):  -0.578161426893628   sin(angle)  0.8159224009992151\n",
      "theta dot:  6.73917185973\n",
      "Angle:  125.3210377801991\n",
      "action:  [ 0.40766059]\n",
      "state:  {1: array([ 5.69947564]), 2: array([ 7.41226275]), 3: array([-0.83442391,  0.55112316,  0.        ])}\n",
      "cos(angle):  -0.8344239110334979   sin(angle)  0.5511231592807194\n",
      "theta dot:  7.41226274921\n",
      "Angle:  146.55555673324318\n",
      "action:  [ 0.24025977]\n",
      "state:  {1: array([ 6.09255784]), 2: array([ 7.86164408]), 3: array([-0.98188554,  0.18947503,  0.        ])}\n",
      "cos(angle):  -0.9818855393014498   sin(angle)  0.18947503189259807\n",
      "theta dot:  7.86164408419\n",
      "Angle:  169.0774553701482\n",
      "action:  [ 0.06808677]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "state:  {1: array([ 0.2100707]), 2: array([ 8.]), 3: array([-0.97801617, -0.20852905,  0.        ])}\n",
      "cos(angle):  -0.9780161735803161   sin(angle)  -0.20852904885247292\n",
      "theta dot:  8.0\n",
      "Angle:  192.03655744157305\n",
      "action:  [-0.03644375]\n",
      "state:  {1: array([ 0.60197754]), 2: array([ 7.83813665]), 3: array([-0.8242174, -0.5662735,  0.       ])}\n",
      "cos(angle):  -0.8242174014110956   sin(angle)  -0.5662734985951055\n",
      "theta dot:  7.83813665015\n",
      "Angle:  214.49111239808076\n",
      "action:  [-0.05488935]\n",
      "state:  {1: array([ 0.97223744]), 2: array([ 7.40519812]), 3: array([-0.56345248, -0.82614847,  0.        ])}\n",
      "cos(angle):  -0.5634524841810717   sin(angle)  -0.8261484721708194\n",
      "theta dot:  7.40519812394\n",
      "Angle:  235.70539273785414\n",
      "action:  [-0.02929067]\n",
      "state:  {1: array([ 1.3112971]), 2: array([ 6.78119317]), 3: array([-0.25659658, -0.9665186 ,  0.        ])}\n",
      "cos(angle):  -0.2565965774652616   sin(angle)  -0.9665185960099857\n",
      "theta dot:  6.78119316997\n",
      "Angle:  255.13203474501142\n",
      "action:  [-0.02453573]\n",
      "state:  {1: array([ 1.61392829]), 2: array([ 6.05262386]), 3: array([ 0.04311859, -0.99906996,  0.        ])}\n",
      "cos(angle):  0.04311859409012108   sin(angle)  -0.9990699609355149\n",
      "theta dot:  6.05262386359\n",
      "Angle:  272.47148431602545\n",
      "action:  [-0.06736213]\n",
      "state:  {1: array([ 1.87858915]), 2: array([ 5.29321707]), 3: array([ 0.30295592, -0.95300457,  0.        ])}\n",
      "cos(angle):  0.3029559235482812   sin(angle)  -0.9530045689224202\n",
      "theta dot:  5.29321707376\n",
      "Angle:  287.6353987748616\n",
      "action:  [-0.12825184]\n",
      "state:  {1: array([ 2.10655044]), 2: array([ 4.55922587]), 3: array([ 0.51048963, -0.85988391,  0.        ])}\n",
      "cos(angle):  0.5104896346762239   sin(angle)  -0.8598839066339918\n",
      "theta dot:  4.55922587057\n",
      "Angle:  300.6965882436316\n",
      "action:  [-0.16978029]\n",
      "state:  {1: array([ 2.30099274]), 2: array([ 3.8888459]), 3: array([ 0.66701598, -0.74504341,  0.        ])}\n",
      "cos(angle):  0.6670159807104271   sin(angle)  -0.7450434091225202\n",
      "theta dot:  3.88884589742\n",
      "Angle:  311.83728504678896\n",
      "action:  [-0.1938891]\n",
      "state:  {1: array([ 2.46604173]), 2: array([ 3.30097998]), 3: array([ 0.78036256, -0.62532733,  0.        ])}\n",
      "cos(angle):  0.7803625645362691   sin(angle)  -0.6253273285811017\n",
      "theta dot:  3.30097997572\n",
      "Angle:  321.2938739764654\n",
      "action:  [-0.22447593]\n",
      "state:  {1: array([ 2.60595739]), 2: array([ 2.79831309]), 3: array([ 0.85994457, -0.51038744,  0.        ])}\n",
      "cos(angle):  0.8599445715121948   sin(angle)  -0.5103874351183693\n",
      "theta dot:  2.79831308941\n",
      "Angle:  329.3104317192385\n",
      "action:  [-0.25381918]\n",
      "state:  {1: array([ 2.72482987]), 2: array([ 2.37744964]), 3: array([ 0.91440416, -0.40480245,  0.        ])}\n",
      "cos(angle):  0.9144041625880945   sin(angle)  -0.40480245483638744\n",
      "theta dot:  2.37744963617\n",
      "Angle:  336.1213073003173\n",
      "action:  [-0.23165427]\n",
      "state:  {1: array([ 2.82678485]), 2: array([ 2.03909965]), 3: array([ 0.95085591, -0.30963372,  0.        ])}\n",
      "cos(angle):  0.9508559079132758   sin(angle)  -0.30963372294764646\n",
      "theta dot:  2.03909965441\n",
      "Angle:  341.96288385037207\n",
      "action:  [-0.11953543]\n",
      "state:  {1: array([ 2.91623206]), 2: array([ 1.78894405]), 3: array([ 0.97471359, -0.22345786,  0.        ])}\n",
      "cos(angle):  0.9747135919498442   sin(angle)  -0.22345785658202458\n",
      "theta dot:  1.78894404739\n",
      "Angle:  347.0878190510733\n",
      "action:  [ 0.05027456]\n",
      "state:  {1: array([ 2.99767665]), 2: array([ 1.62889184]), 3: array([ 0.98966195, -0.14341973,  0.        ])}\n",
      "cos(angle):  0.9896619533793106   sin(angle)  -0.1434197267932388\n",
      "theta dot:  1.62889183859\n",
      "Angle:  351.75423952068087\n",
      "action:  [ 0.18441174]\n",
      "state:  {1: array([ 3.07512609]), 2: array([ 1.5489888]), 3: array([ 0.99779191, -0.06641764,  0.        ])}\n",
      "cos(angle):  0.9977919108889372   sin(angle)  -0.06641763745123158\n",
      "theta dot:  1.54898880418\n",
      "Angle:  356.19175519346265\n",
      "action:  [ 0.20418045]\n",
      "state:  {1: array([ 3.15161622]), 2: array([ 1.52980264]), 3: array([ 0.99994976,  0.0100234 ,  0.        ])}\n",
      "cos(angle):  0.9999497644812086   sin(angle)  0.010023398324690838\n",
      "theta dot:  1.5298026439\n",
      "Angle:  0.5743066943240365\n",
      "action:  [ 0.10944018]\n",
      "state:  {1: array([ 3.22930303]), 2: array([ 1.55373622]), 3: array([ 0.99615591,  0.08759796,  0.        ])}\n",
      "cos(angle):  0.9961559102335409   sin(angle)  0.08759795948985152\n",
      "theta dot:  1.55373622003\n",
      "Angle:  5.025422679881672\n",
      "action:  [-0.03191544]\n",
      "state:  {1: array([ 3.3100354]), 2: array([ 1.61464737]), 3: array([ 0.98584703,  0.16764734,  0.        ])}\n",
      "cos(angle):  0.9858470316764947   sin(angle)  0.1676473385850323\n",
      "theta dot:  1.61464737361\n",
      "Angle:  9.651035858662055\n",
      "action:  [-0.14077082]\n",
      "state:  {1: array([ 3.39599876]), 2: array([ 1.71926725]), 3: array([ 0.96781293,  0.25167068,  0.        ])}\n",
      "cos(angle):  0.9678129314158177   sin(angle)  0.25167067724373793\n",
      "theta dot:  1.7192672543\n",
      "Angle:  14.57636221743627\n",
      "action:  [-0.16456893]\n",
      "state:  {1: array([ 3.49016551]), 2: array([ 1.88333492]), 3: array([ 0.93986112,  0.34155684,  0.        ])}\n",
      "cos(angle):  0.9398611210428685   sin(angle)  0.34155683736684667\n",
      "theta dot:  1.88333492276\n",
      "Angle:  19.971706724953492\n",
      "action:  [-0.08936843]\n",
      "state:  {1: array([ 3.59647037]), 2: array([ 2.12609729]), 3: array([ 0.89831476,  0.43935247,  0.        ])}\n",
      "cos(angle):  0.8983147593568086   sin(angle)  0.4393524702579002\n",
      "theta dot:  2.12609728575\n",
      "Angle:  26.062512547371455\n",
      "action:  [ 0.05172581]\n",
      "state:  {1: array([ 3.7196389]), 2: array([ 2.46337051]), 3: array([ 0.83753176,  0.54638865,  0.        ])}\n",
      "cos(angle):  0.8375317575481637   sin(angle)  0.546388648398083\n",
      "theta dot:  2.46337051053\n",
      "Angle:  33.11953272650362\n",
      "action:  [ 0.1883365]\n",
      "state:  {1: array([ 3.86470952]), 2: array([ 2.90141247]), 3: array([ 0.74974687,  0.66172474,  0.        ])}\n",
      "cos(angle):  0.7497468655550033   sin(angle)  0.6617247445807417\n",
      "theta dot:  2.90141247222\n",
      "Angle:  41.431447753884356\n",
      "action:  [ 0.25280987]\n",
      "state:  {1: array([ 4.0364909]), 2: array([ 3.43562751]), 3: array([ 0.6255982 ,  0.78014543,  0.        ])}\n",
      "cos(angle):  0.6255982043624339   sin(angle)  0.7801454266343542\n",
      "theta dot:  3.43562751085\n",
      "Angle:  51.273772555785115\n",
      "action:  [ 0.2186115]\n",
      "state:  {1: array([ 4.23916731]), 2: array([ 4.05352831]), 3: array([ 0.45575627,  0.89010461,  0.        ])}\n",
      "cos(angle):  0.45575626697562194   sin(angle)  0.8901046147012415\n",
      "theta dot:  4.05352830635\n",
      "Angle:  62.88624860528671\n",
      "action:  [ 0.10783079]\n",
      "state:  {1: array([ 4.47603138]), 2: array([ 4.73728139]), 3: array([ 0.23416305,  0.97219734,  0.        ])}\n",
      "cos(angle):  0.23416305456168954   sin(angle)  0.9721973379300826\n",
      "theta dot:  4.73728138573\n",
      "Angle:  76.45752835813028\n",
      "action:  [-0.02860381]\n",
      "state:  {1: array([ 4.74913832]), 2: array([ 5.46213882]), 3: array([-0.03674107,  0.99932482,  0.        ])}\n",
      "cos(angle):  -0.036741071149658786   sin(angle)  0.9993248189106362\n",
      "theta dot:  5.4621388172\n",
      "Angle:  92.10536683368412\n",
      "action:  [-0.14441744]\n",
      "state:  {1: array([ 5.05863681]), 2: array([ 6.18996981]), 3: array([-0.33937072,  0.94065271,  0.        ])}\n",
      "cos(angle):  -0.33937071872150393   sin(angle)  0.9406527070467877\n",
      "theta dot:  6.18996981473\n",
      "Angle:  109.83828265127585\n",
      "action:  [-0.22252652]\n",
      "state:  {1: array([ 5.40174083]), 2: array([ 6.86208037]), 3: array([-0.63603717,  0.77165842,  0.        ])}\n",
      "cos(angle):  -0.6360371665365743   sin(angle)  0.7716584236461921\n",
      "theta dot:  6.86208036663\n",
      "Angle:  129.49664886585026\n",
      "action:  [-0.2700626]\n",
      "state:  {1: array([ 5.77175657]), 2: array([ 7.40031479]), 3: array([-0.87204614,  0.48942367,  0.        ])}\n",
      "cos(angle):  -0.8720461408781591   sin(angle)  0.48942366941077736\n",
      "theta dot:  7.40031479434\n",
      "Angle:  150.696939529549\n",
      "action:  [-0.3001804]\n",
      "state:  {1: array([ 6.15787435]), 2: array([ 7.72235549]), 3: array([-0.99215885,  0.12498326,  0.        ])}\n",
      "cos(angle):  -0.9921588502364234   sin(angle)  0.12498326246957374\n",
      "theta dot:  7.72235548653\n",
      "Angle:  172.81980666054892\n",
      "action:  [-0.32937406]\n",
      "state:  {1: array([ 0.26302338]), 2: array([ 7.76668682]), 3: array([-0.96560831, -0.26000114,  0.        ])}\n",
      "cos(angle):  -0.9656083103528167   sin(angle)  -0.2600011364928594\n",
      "theta dot:  7.7666868244\n",
      "Angle:  195.0705152572211\n",
      "action:  [-0.37993762]\n",
      "state:  {1: array([ 0.63875815]), 2: array([ 7.51469533]), 3: array([-0.80283677, -0.5961989 ,  0.        ])}\n",
      "cos(angle):  -0.8028367685402704   sin(angle)  -0.5961988955707787\n",
      "theta dot:  7.51469532928\n",
      "Angle:  216.5984812501872\n",
      "action:  [-0.46285938]\n",
      "state:  {1: array([ 0.98866401]), 2: array([ 6.99811725]), 3: array([-0.54980629, -0.83529219,  0.        ])}\n",
      "cos(angle):  -0.549806293897346   sin(angle)  -0.8352921879144238\n",
      "theta dot:  6.9981172499\n",
      "Angle:  236.6465635168933\n",
      "action:  [-0.55868933]\n",
      "state:  {1: array([ 1.30305624]), 2: array([ 6.28784471]), 3: array([-0.26455271, -0.96437123,  0.        ])}\n",
      "cos(angle):  -0.26455271469549546   sin(angle)  -0.9643712257980553\n",
      "theta dot:  6.28784470922\n",
      "Angle:  254.65986959754383\n",
      "action:  [-0.62891022]\n",
      "state:  {1: array([ 1.57656773]), 2: array([ 5.47022976]), 3: array([ 0.00577137, -0.99998335,  0.        ])}\n",
      "cos(angle):  0.005771373272210737   sin(angle)  -0.99998334548659\n",
      "theta dot:  5.47022975616\n",
      "Angle:  270.3308868516424\n",
      "action:  [-0.64816511]\n",
      "state:  {1: array([ 1.80771861]), 2: array([ 4.62301748]), 3: array([ 0.234712  , -0.97206496,  0.        ])}\n",
      "cos(angle):  0.2347119984156574   sin(angle)  -0.9720649555455275\n",
      "theta dot:  4.62301748105\n",
      "Angle:  283.5748253955203\n",
      "action:  [-0.61741687]\n",
      "state:  {1: array([ 1.99778642]), 2: array([ 3.80135623]), 3: array([ 0.41413301, -0.91021637,  0.        ])}\n",
      "cos(angle):  0.4141330140963496   sin(angle)  -0.910216373526357\n",
      "theta dot:  3.80135623395\n",
      "Angle:  294.46488336138873\n",
      "action:  [-0.54609262]\n",
      "state:  {1: array([ 2.14962542]), 2: array([ 3.03678006]), 3: array([ 0.54704414, -0.83710376,  0.        ])}\n",
      "cos(angle):  0.5470441423035084   sin(angle)  -0.8371037608154792\n",
      "theta dot:  3.03678006113\n",
      "Angle:  303.16459705828527\n",
      "action:  [-0.43812243]\n",
      "state:  {1: array([ 2.26678711]), 2: array([ 2.34323388]), 3: array([ 0.6411461 , -0.76741884,  0.        ])}\n",
      "cos(angle):  0.6411461034318657   sin(angle)  -0.767418838740707\n",
      "theta dot:  2.34323387644\n",
      "Angle:  309.8774519373278\n",
      "action:  [-0.30588396]\n",
      "state:  {1: array([ 2.35287647]), 2: array([ 1.72178715]), 3: array([ 0.7047567 , -0.70944908,  0.        ])}\n",
      "cos(angle):  0.7047567002871152   sin(angle)  -0.7094490773835831\n",
      "theta dot:  1.72178715407\n",
      "Angle:  314.80999726029114\n",
      "action:  [-0.18400052]\n",
      "state:  {1: array([ 2.41098149]), 2: array([ 1.16210027]), 3: array([ 0.74476669, -0.66732494,  0.        ])}\n",
      "cos(angle):  0.744766693982099   sin(angle)  -0.6673249369946955\n",
      "theta dot:  1.16210026792\n",
      "Angle:  318.1391615114126\n",
      "action:  [-0.11084137]\n",
      "state:  {1: array([ 2.4432305]), 2: array([ 0.64498036]), 3: array([ 0.76589629, -0.64296413,  0.        ])}\n",
      "cos(angle):  0.765896293024382   sin(angle)  -0.6429641267843098\n",
      "theta dot:  0.644980359347\n",
      "Angle:  319.9868898135897\n",
      "action:  [-0.09584136]\n",
      "state:  {1: array([ 2.45064956]), 2: array([ 0.14838106]), 3: array([ 0.77064536, -0.63726426,  0.        ])}\n",
      "cos(angle):  0.7706453559439018   sin(angle)  -0.637264258657346\n",
      "theta dot:  0.148381060448\n",
      "Angle:  320.4119692457378\n",
      "action:  [-0.11166279]\n",
      "state:  {1: array([ 2.43333373]), 2: array([-0.34631655]), 3: array([ 0.75949562, -0.65051242,  0.        ])}\n",
      "cos(angle):  0.7594956179000313   sin(angle)  -0.6505124183216257\n",
      "theta dot:  -0.346316551588\n",
      "Angle:  319.4198477266749\n",
      "action:  [-0.11398233]\n",
      "state:  {1: array([ 2.39076882]), 2: array([-0.85129821]), 3: array([ 0.73112706, -0.68224132,  0.        ])}\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "cos(angle):  0.7311270627069166   sin(angle)  -0.6822413196058682\n",
      "theta dot:  -0.851298214956\n",
      "Angle:  316.9810636884141\n",
      "action:  [-0.0644751]\n",
      "state:  {1: array([ 2.3221363]), 2: array([-1.37265047]), 3: array([ 0.68261859, -0.73077484,  0.        ])}\n",
      "cos(angle):  0.6826185876050707   sin(angle)  -0.7307748380014589\n",
      "theta dot:  -1.37265046994\n",
      "Angle:  313.04871895024013\n",
      "action:  [ 0.05619948]\n",
      "state:  {1: array([ 2.22652121]), 2: array([-1.91230168]), 3: array([ 0.60973395, -0.79260615,  0.        ])}\n",
      "cos(angle):  0.6097339512801937   sin(angle)  -0.7926061497719044\n",
      "theta dot:  -1.9123016768\n",
      "Angle:  307.57039099911884\n",
      "action:  [ 0.24359276]\n",
      "state:  {1: array([ 2.10301034]), 2: array([-2.47021738]), 3: array([ 0.50744237, -0.8616857 ,  0.        ])}\n",
      "cos(angle):  0.5074423689067143   sin(angle)  -0.8616856980583709\n",
      "theta dot:  -2.47021737588\n",
      "Angle:  300.49375604149145\n",
      "action:  [ 0.47128793]\n",
      "state:  {1: array([ 1.95072092]), 2: array([-3.04578846]), 3: array([ 0.37085044, -0.9286926 ,  0.        ])}\n",
      "cos(angle):  0.37085044015930557   sin(angle)  -0.928692603089768\n",
      "theta dot:  -3.04578845974\n",
      "Angle:  291.76823524391943\n",
      "action:  [ 0.69732506]\n",
      "state:  {1: array([ 1.76883546]), 2: array([-3.63770915]), 3: array([ 0.19674717, -0.98045426,  0.        ])}\n",
      "cos(angle):  0.19674717178742807   sin(angle)  -0.9804542571653448\n",
      "theta dot:  -3.63770915269\n",
      "Angle:  281.34699053605567\n",
      "action:  [ 0.87599211]\n",
      "state:  {1: array([ 1.55675291]), 2: array([-4.24165103]), 3: array([-0.01404295, -0.99990139,  0.        ])}\n",
      "cos(angle):  -0.014042954902333636   sin(angle)  -0.9999013928471202\n",
      "theta dot:  -4.241651029\n",
      "Angle:  269.1955838448795\n",
      "Reset!\n",
      "min:  -0.648165106663  self.max:  0.875992110438\n",
      "state:  {1: array([ 5.89087357]), 2: array([-0.66062138]), 3: array([-0.92402769,  0.38232554,  0.        ])}\n",
      "cos(angle):  -0.9240276938958076   sin(angle)  0.3823255431090049\n",
      "theta dot:  -0.660621380372\n",
      "Angle:  157.52182490640928\n",
      "action:  [ 0.06572871]\n",
      "state:  {1: array([ 5.87267268]), 2: array([-0.36401792]), 3: array([-0.91691636,  0.39907942,  0.        ])}\n",
      "cos(angle):  -0.9169163623959729   sin(angle)  0.39907942113135453\n",
      "theta dot:  -0.364017916875\n",
      "Angle:  156.47899282979904\n",
      "action:  [ 0.14631658]\n",
      "state:  {1: array([ 5.87053463]), 2: array([-0.04276086]), 3: array([-0.91606102,  0.40103891,  0.        ])}\n",
      "cos(angle):  -0.9160610182739275   sin(angle)  0.4010389143199635\n",
      "theta dot:  -0.0427608646529\n",
      "Angle:  156.35649226261148\n",
      "action:  [ 0.21592546]\n",
      "state:  {1: array([ 5.88505499]), 2: array([ 0.29040714]), 3: array([-0.92178747,  0.38769557,  0.        ])}\n",
      "cos(angle):  -0.9217874720452375   sin(angle)  0.38769557178339104\n",
      "theta dot:  0.290407139928\n",
      "Angle:  157.18844549006008\n",
      "action:  [ 0.17750059]\n",
      "state:  {1: array([ 5.91544518]), 2: array([ 0.60780391]), 3: array([-0.93314217,  0.35950757,  0.        ])}\n",
      "cos(angle):  -0.9331421706255678   sin(angle)  0.35950756515016996\n",
      "theta dot:  0.607803908011\n",
      "Angle:  158.92967135334698\n",
      "action:  [ 0.02313674]\n",
      "state:  {1: array([ 5.95949044]), 2: array([ 0.88090509]), 3: array([-0.94806666,  0.31807171,  0.        ])}\n",
      "cos(angle):  -0.9480666592344871   sin(angle)  0.31807170519862177\n",
      "theta dot:  0.880905093229\n",
      "Angle:  161.45327265174797\n",
      "action:  [-0.15390372]\n",
      "state:  {1: array([ 6.0143091]), 2: array([ 1.09637331]), 3: array([-0.96407004,  0.26564819,  0.        ])}\n",
      "cos(angle):  -0.9640700398340956   sin(angle)  0.26564818518914324\n",
      "theta dot:  1.09637331427\n",
      "Angle:  164.59414349096062\n",
      "action:  [-0.23979915]\n",
      "state:  {1: array([ 6.07729108]), 2: array([ 1.25963958]), 3: array([-0.97887856,  0.20444258,  0.        ])}\n",
      "cos(angle):  -0.9788785585547363   sin(angle)  0.20444257776158484\n",
      "theta dot:  1.25963958008\n",
      "Angle:  168.20273663481623\n",
      "action:  [-0.18681215]\n",
      "state:  {1: array([ 6.14653857]), 2: array([ 1.38494969]), 3: array([-0.99067835,  0.13622188,  0.        ])}\n",
      "cos(angle):  -0.9906783526593962   sin(angle)  0.13622188360195694\n",
      "theta dot:  1.38494969143\n",
      "Angle:  172.17031596473498\n",
      "action:  [-0.0311709]\n",
      "state:  {1: array([ 6.22066059]), 2: array([ 1.48244047]), 3: array([-0.99804597,  0.06248399,  0.        ])}\n",
      "cos(angle):  -0.9980459666813307   sin(angle)  0.062483985077202694\n",
      "theta dot:  1.48244046883\n",
      "Angle:  176.4171851458625\n",
      "action:  [ 0.14444589]\n",
      "state:  {1: array([ 0.0150238]), 2: array([ 1.55097034]), 3: array([-0.99988714, -0.01502324,  0.        ])}\n",
      "cos(angle):  -0.999887144816602   sin(angle)  -0.015023236352517066\n",
      "theta dot:  1.55097034077\n",
      "Angle:  180.86121932419414\n",
      "action:  [ 0.25381844]\n",
      "state:  {1: array([ 0.09391259]), 2: array([ 1.57777568]), 3: array([-0.99559345, -0.0937746 ,  0.        ])}\n",
      "cos(angle):  -0.9955934532274594   sin(angle)  -0.09377460152206771\n",
      "theta dot:  1.57777567921\n",
      "Angle:  185.38120312635291\n",
      "action:  [ 0.24111639]\n",
      "state:  {1: array([ 0.17109319]), 2: array([ 1.54361219]), 3: array([-0.98539923, -0.17025968,  0.        ])}\n",
      "cos(angle):  -0.9853992287499471   sin(angle)  -0.17025968395075025\n",
      "theta dot:  1.54361218723\n",
      "Angle:  189.8033159621804\n",
      "action:  [ 0.11258197]\n",
      "state:  {1: array([ 0.24273343]), 2: array([ 1.43280472]), 3: array([-0.9706846 , -0.24035682,  0.        ])}\n",
      "cos(angle):  -0.9706846032343794   sin(angle)  -0.24035682025629163\n",
      "theta dot:  1.43280471925\n",
      "Angle:  193.90798952764527\n",
      "action:  [-0.05171452]\n",
      "state:  {1: array([ 0.30497243]), 2: array([ 1.24477993]), 3: array([-0.95385523, -0.30026687,  0.        ])}\n",
      "cos(angle):  -0.9538552321548331   sin(angle)  -0.30026687478116765\n",
      "theta dot:  1.24477992602\n",
      "Angle:  197.47401299790945\n",
      "action:  [-0.13526651]\n",
      "state:  {1: array([ 0.35493692]), 2: array([ 0.99928979]), 3: array([-0.93766841, -0.34753121,  0.        ])}\n",
      "cos(angle):  -0.9376684140726532   sin(angle)  -0.3475312147857733\n",
      "theta dot:  0.999289793362\n",
      "Angle:  200.33676068706626\n",
      "action:  [-0.06468793]\n",
      "state:  {1: array([ 0.39138383]), 2: array([ 0.72893819]), 3: array([-0.92438206, -0.38146796,  0.        ])}\n",
      "cos(angle):  -0.9243820596560334   sin(angle)  -0.3814679642985367\n",
      "theta dot:  0.728938193128\n",
      "Angle:  202.42500990343797\n",
      "action:  [ 0.12570873]\n",
      "state:  {1: array([ 0.4144685]), 2: array([ 0.46169353]), 3: array([-0.91533049, -0.40270349,  0.        ])}\n",
      "cos(angle):  -0.9153304855122946   sin(angle)  -0.40270349178002796\n",
      "theta dot:  0.461693529034\n",
      "Angle:  203.74766134261182\n",
      "action:  [ 0.30439942]\n",
      "state:  {1: array([ 0.42473479]), 2: array([ 0.20532582]), 3: array([-0.91114805, -0.41207915,  0.        ])}\n",
      "cos(angle):  -0.9111480508494939   sin(angle)  -0.4120791543298059\n",
      "theta dot:  0.20532582269\n",
      "Angle:  204.3358751203724\n",
      "action:  [ 0.33654802]\n",
      "state:  {1: array([ 0.42207223]), 2: array([-0.05325134]), 3: array([-0.91224201, -0.4096517 ,  0.        ])}\n",
      "cos(angle):  -0.9122420082209228   sin(angle)  -0.40965170381320015\n",
      "theta dot:  -0.0532513396234\n",
      "Angle:  204.18332162641698\n",
      "action:  [ 0.19219302]\n",
      "state:  {1: array([ 0.40548917]), 2: array([-0.33166117]), 3: array([-0.91890955, -0.39446831,  0.        ])}\n",
      "cos(angle):  -0.9189095455290666   sin(angle)  -0.3944683094185948\n",
      "theta dot:  -0.331661165061\n",
      "Angle:  203.23318459893255\n",
      "action:  [-0.03649805]\n",
      "state:  {1: array([ 0.37383981]), 2: array([-0.6329871]), 3: array([-0.93093194, -0.36519272,  0.        ])}\n",
      "cos(angle):  -0.9309319399799209   sin(angle)  -0.3651927205260546\n",
      "theta dot:  -0.632987103943\n",
      "Angle:  201.41981436227425\n",
      "action:  [-0.22937143]\n",
      "state:  {1: array([ 0.32677545]), 2: array([-0.94128736]), 3: array([-0.94708232, -0.32099078,  0.        ])}\n",
      "cos(angle):  -0.9470823177331169   sin(angle)  -0.32099078403790243\n",
      "theta dot:  -0.941287359519\n",
      "Angle:  198.723231017587\n",
      "action:  [-0.33275652]\n",
      "state:  {1: array([ 0.26517825]), 2: array([-1.23194393]), 3: array([-0.9650458 , -0.26208129,  0.        ])}\n",
      "cos(angle):  -0.9650458004035558   sin(angle)  -0.2620812910595876\n",
      "theta dot:  -1.23194392615\n",
      "Angle:  195.19397989226061\n",
      "action:  [-0.35810591]\n",
      "state:  {1: array([ 0.19106721]), 2: array([-1.48222078]), 3: array([-0.98180212, -0.18990679,  0.        ])}\n",
      "cos(angle):  -0.9818021237822101   sin(angle)  -0.18990679223435325\n",
      "theta dot:  -1.48222078076\n",
      "Angle:  190.94774006959713\n",
      "action:  [-0.33058496]\n",
      "state:  {1: array([ 0.10735528]), 2: array([-1.67423862]), 3: array([-0.99424295, -0.10714918,  0.        ])}\n",
      "cos(angle):  -0.9942429544245001   sin(angle)  -0.10714918374510173\n",
      "theta dot:  -1.67423861872\n",
      "Angle:  186.15141094798946\n",
      "action:  [-0.27725518]\n",
      "state:  {1: array([ 0.01754584]), 2: array([-1.79618878]), 3: array([-0.99984608, -0.01754494,  0.        ])}\n",
      "cos(angle):  -0.9998460756953891   sin(angle)  -0.01754493991241668\n",
      "theta dot:  -1.7961887834\n",
      "Angle:  181.0057211559746\n",
      "action:  [-0.24476296]\n",
      "state:  {1: array([ 6.20842805]), 2: array([-1.84606193]), 3: array([-0.99720698,  0.07468764,  0.        ])}\n",
      "cos(angle):  -0.9972069774351808   sin(angle)  0.07468764392180798\n",
      "theta dot:  -1.84606193247\n",
      "Angle:  175.71631381579982\n",
      "action:  [-0.28151141]\n",
      "state:  {1: array([ 6.11681441]), 2: array([-1.83227291]), 3: array([-0.98619225,  0.16560446,  0.        ])}\n",
      "cos(angle):  -0.9861922546379536   sin(angle)  0.16560445915527053\n",
      "theta dot:  -1.83227291161\n",
      "Angle:  170.46725085283424\n",
      "action:  [-0.3909368]\n",
      "state:  {1: array([ 6.0284789]), 2: array([-1.76671009]), 3: array([-0.96773731,  0.2519613 ,  0.        ])}\n",
      "cos(angle):  -0.967737311629462   sin(angle)  0.25196129798082406\n",
      "theta dot:  -1.7667100865\n",
      "Angle:  165.40601110924126\n",
      "action:  [-0.51759406]\n",
      "state:  {1: array([ 5.94570999]), 2: array([-1.65537822]), 3: array([-0.94359361,  0.33110588,  0.        ])}\n",
      "cos(angle):  -0.9435936090961679   sin(angle)  0.33110587562420024\n",
      "theta dot:  -1.65537822246\n",
      "Angle:  160.6637129165449\n",
      "action:  [-0.58581456]\n",
      "state:  {1: array([ 5.87096394]), 2: array([-1.494921]), 3: array([-0.9162331 ,  0.40064561,  0.        ])}\n",
      "cos(angle):  -0.9162331029988156   sin(angle)  0.4006456052038533\n",
      "theta dot:  -1.49492099959\n",
      "Angle:  156.38108973208205\n",
      "action:  [-0.55320637]\n",
      "state:  {1: array([ 5.80709305]), 2: array([-1.27741775]), 3: array([-0.88879266,  0.45830951,  0.        ])}\n",
      "cos(angle):  -0.8887926610159788   sin(angle)  0.4583095086555978\n",
      "theta dot:  -1.27741775193\n",
      "Angle:  152.7215659966157\n",
      "action:  [-0.43099879]\n",
      "state:  {1: array([ 5.75717628]), 2: array([-0.99833544]), 3: array([-0.86481776,  0.50208588,  0.        ])}\n",
      "cos(angle):  -0.8648177637477319   sin(angle)  0.5020858845918417\n",
      "theta dot:  -0.99833543868\n",
      "Angle:  149.86155232583616\n",
      "action:  [-0.26430118]\n",
      "state:  {1: array([ 5.72410547]), 2: array([-0.6614162]), 3: array([-0.84774353,  0.53040636,  0.        ])}\n",
      "cos(angle):  -0.8477435300884528   sin(angle)  0.5304063604380782\n",
      "theta dot:  -0.661416202188\n",
      "Angle:  147.9667389123856\n",
      "action:  [-0.10698112]\n",
      "state:  {1: array([ 5.71012254]), 2: array([-0.2796586]), 3: array([-0.84024426,  0.54220806,  0.        ])}\n",
      "cos(angle):  -0.8402442617030548   sin(angle)  0.5422080603191808\n",
      "theta dot:  -0.279658600531\n",
      "Angle:  147.16557791010007\n",
      "action:  [-0.00787835]\n",
      "state:  {1: array([ 5.71641332]), 2: array([ 0.12581569]), 3: array([-0.84363853,  0.53691157,  0.        ])}\n",
      "cos(angle):  -0.8436385274636509   sin(angle)  0.536911570911787\n",
      "theta dot:  0.125815691556\n",
      "Angle:  147.52601247338112\n",
      "action:  [ 0.01139678]\n",
      "state:  {1: array([ 5.74292377]), 2: array([ 0.53020889]), 3: array([-0.85757419,  0.5143603 ,  0.        ])}\n",
      "cos(angle):  -0.8575741858204758   sin(angle)  0.5143602976653117\n",
      "theta dot:  0.530208886758\n",
      "Angle:  149.0449454950333\n",
      "action:  [-0.02500392]\n",
      "state:  {1: array([ 5.7885352]), 2: array([ 0.91222852]), 3: array([-0.88013486,  0.47472373,  0.        ])}\n",
      "cos(angle):  -0.8801348637871406   sin(angle)  0.47472373181292665\n",
      "theta dot:  0.91222852195\n",
      "Angle:  151.6582815968767\n",
      "action:  [-0.07557542]\n",
      "state:  {1: array([ 5.85138195]), 2: array([ 1.25693501]), 3: array([-0.9082125 ,  0.41850932,  0.        ])}\n",
      "cos(angle):  -0.9082125031943471   sin(angle)  0.41850931774747624\n",
      "theta dot:  1.25693500744\n",
      "Angle:  155.25912672896948\n",
      "action:  [-0.12704803]\n",
      "state:  {1: array([ 5.92896994]), 2: array([ 1.55175979]), 3: array([-0.93791893,  0.34685455,  0.        ])}\n",
      "cos(angle):  -0.9379189292388411   sin(angle)  0.34685455478552646\n",
      "theta dot:  1.55175979155\n",
      "Angle:  159.70458067725394\n",
      "action:  [-0.18937535]\n",
      "state:  {1: array([ 6.01814466]), 2: array([ 1.78349441]), 3: array([-0.96508185,  0.2619485 ,  0.        ])}\n",
      "cos(angle):  -0.9650818529762463   sin(angle)  0.26194850077054255\n",
      "theta dot:  1.78349440552\n",
      "Angle:  164.81390384051292\n",
      "action:  [-0.26141306]\n",
      "state:  {1: array([ 6.11518185]), 2: array([ 1.94074382]), 3: array([-0.98592058,  0.16721425,  0.        ])}\n",
      "cos(angle):  -0.9859205815268325   sin(angle)  0.16721425454126965\n",
      "theta dot:  1.9407438222\n",
      "Angle:  170.3737123456691\n",
      "action:  [-0.32434708]\n",
      "state:  {1: array([ 6.21605697]), 2: array([ 2.01750245]), 3: array([-0.99774774,  0.06707793,  0.        ])}\n",
      "cos(angle):  -0.9977477390602288   sin(angle)  0.06707793378005475\n",
      "theta dot:  2.01750245175\n",
      "Angle:  176.15341761232992\n",
      "action:  [-0.35162053]\n",
      "state:  {1: array([ 0.03362505]), 2: array([ 2.01506782]), 3: array([-0.99943473, -0.03361872,  0.        ])}\n",
      "cos(angle):  -0.9994347311704668   sin(angle)  -0.033618716962681164\n",
      "theta dot:  2.01506782258\n",
      "Angle:  181.92699003111827\n",
      "action:  [-0.31729655]\n",
      "state:  {1: array([ 0.13073802]), 2: array([ 1.9422593]), 3: array([-0.99146595, -0.1303659 ,  0.        ])}\n",
      "cos(angle):  -0.9914659513465574   sin(angle)  -0.1303658978432087\n",
      "theta dot:  1.94225930272\n",
      "Angle:  187.491140058003\n",
      "action:  [-0.22537722]\n",
      "state:  {1: array([ 0.22127193]), 2: array([ 1.8106783]), 3: array([-0.97561909, -0.21947072,  0.        ])}\n",
      "cos(angle):  -0.9756190865565971   sin(angle)  -0.21947072229905998\n",
      "theta dot:  1.81067829678\n",
      "Angle:  192.6783391511353\n",
      "action:  [-0.13737423]\n",
      "state:  {1: array([ 0.30254539]), 2: array([ 1.62546912]), 3: array([-0.95458118, -0.29795095,  0.        ])}\n",
      "cos(angle):  -0.9545811812620492   sin(angle)  -0.2979509496214955\n",
      "theta dot:  1.62546912045\n",
      "Angle:  197.33495427847157\n",
      "action:  [-0.1288795]\n",
      "state:  {1: array([ 0.37167909]), 2: array([ 1.38267398]), 3: array([-0.93171885, -0.36318038,  0.        ])}\n",
      "cos(angle):  -0.9317188474533814   sin(angle)  -0.36318038121592217\n",
      "theta dot:  1.38267398325\n",
      "Angle:  201.29601419992377\n",
      "action:  [-0.20187512]\n",
      "state:  {1: array([ 0.42567946]), 2: array([ 1.08000743]), 3: array([-0.91075837, -0.4129397 ,  0.        ])}\n",
      "cos(angle):  -0.9107583673826829   sin(angle)  -0.4129397004920088\n",
      "theta dot:  1.08000742928\n",
      "Angle:  204.39000034185744\n",
      "action:  [-0.2760257]\n",
      "state:  {1: array([ 0.4621244]), 2: array([ 0.7288988]), 3: array([-0.89510735, -0.44585068,  0.        ])}\n",
      "cos(angle):  -0.895107353138451   sin(angle)  -0.44585067719750776\n",
      "theta dot:  0.728898798222\n",
      "Angle:  206.4781367004016\n",
      "action:  [-0.27624836]\n",
      "state:  {1: array([ 0.47977808]), 2: array([ 0.35307354]), 3: array([-0.88709738, -0.46158232,  0.        ])}\n",
      "cos(angle):  -0.8870973806843897   sin(angle)  -0.4615823189669368\n",
      "theta dot:  0.353073536432\n",
      "Angle:  207.4896155098911\n",
      "action:  [-0.19709422]\n",
      "state:  {1: array([ 0.47864421]), 2: array([-0.02267734]), 3: array([-0.88762018, -0.46057617,  0.        ])}\n",
      "cos(angle):  -0.8876201831829421   sin(angle)  -0.4605761722085505\n",
      "theta dot:  -0.022677335731\n",
      "Angle:  207.42464988040953\n",
      "action:  [-0.08662819]\n",
      "state:  {1: array([ 0.45958902]), 2: array([-0.38110369]), 3: array([-0.89623487, -0.44357981,  0.        ])}\n",
      "cos(angle):  -0.896234873487151   sin(angle)  -0.4435798141771901\n",
      "theta dot:  -0.381103692997\n",
      "Angle:  206.3328707751861\n",
      "action:  [-0.01126807]\n",
      "state:  {1: array([ 0.42381509]), 2: array([-0.71547876]), 3: array([-0.91152666, -0.41124099,  0.        ])}\n",
      "cos(angle):  -0.9115266576919154   sin(angle)  -0.4112409905602866\n",
      "theta dot:  -0.715478764614\n",
      "Angle:  204.28317989107543\n",
      "action:  [-0.03739238]\n",
      "state:  {1: array([ 0.37233917]), 2: array([-1.02951836]), 3: array([-0.93147892, -0.36379531,  0.        ])}\n",
      "cos(angle):  -0.9314789162823356   sin(angle)  -0.36379531129673137\n",
      "theta dot:  -1.02951836483\n",
      "Angle:  201.33383392614164\n",
      "action:  [-0.19548289]\n",
      "state:  {1: array([ 0.3057548]), 2: array([-1.33168728]), 3: array([-0.95362002, -0.30101306,  0.        ])}\n",
      "cos(angle):  -0.9536200182926049   sin(angle)  -0.30101305737727047\n",
      "theta dot:  -1.33168728166\n",
      "Angle:  197.51883980374805\n",
      "action:  [-0.43589265]\n",
      "state:  {1: array([ 0.22461326]), 2: array([-1.62283097]), 3: array([-0.97488032, -0.22272935,  0.        ])}\n",
      "cos(angle):  -0.9748803192762303   sin(angle)  -0.22272934940836872\n",
      "theta dot:  -1.62283097161\n",
      "Angle:  192.8697823984486\n",
      "action:  [-0.64767113]\n",
      "state:  {1: array([ 0.13026182]), 2: array([-1.88702865]), 3: array([-0.99152792, -0.12989375,  0.        ])}\n",
      "cos(angle):  -0.9915279185220486   sin(angle)  -0.12989375193339253\n",
      "theta dot:  -1.88702865333\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  187.46385615704136\n",
      "action:  [-0.74845573]\n",
      "state:  {1: array([ 0.02542596]), 2: array([-2.09671733]), 3: array([-0.99967678, -0.02542322,  0.        ])}\n",
      "cos(angle):  -0.9996767777758322   sin(angle)  -0.02542321725764513\n",
      "theta dot:  -2.09671732645\n",
      "Angle:  181.45721752129379\n",
      "action:  [-0.74627703]\n",
      "state:  {1: array([ 6.19722495]), 2: array([-2.22772629]), 3: array([-0.99630768,  0.08585453,  0.        ])}\n",
      "cos(angle):  -0.996307682869586   sin(angle)  0.08585453427185062\n",
      "theta dot:  -2.22772629399\n",
      "Angle:  175.07442488264144\n",
      "action:  [-0.70534184]\n",
      "state:  {1: array([ 6.08376812]), 2: array([-2.26913667]), 3: array([-0.9801822 ,  0.19809811,  0.        ])}\n",
      "cos(angle):  -0.9801821975809841   sin(angle)  0.19809810585998214\n",
      "theta dot:  -2.26913666864\n",
      "Angle:  168.57384237126098\n",
      "action:  [-0.67300636]\n",
      "state:  {1: array([ 5.97269241]), 2: array([-2.22151404]), 3: array([-0.95218309,  0.305528  ,  0.        ])}\n",
      "cos(angle):  -0.9521830927742417   sin(angle)  0.30552799844675416\n",
      "theta dot:  -2.22151404302\n",
      "Angle:  162.20968831370578\n",
      "action:  [-0.64830721]\n",
      "state:  {1: array([ 5.86821171]), 2: array([-2.08961413]), 3: array([-0.91512696,  0.40316577,  0.        ])}\n",
      "cos(angle):  -0.9151269644549502   sin(angle)  0.4031657710265942\n",
      "theta dot:  -2.08961412568\n",
      "Angle:  156.22339880163247\n",
      "action:  [-0.59641877]\n",
      "state:  {1: array([ 5.77437658]), 2: array([-1.87670261]), 3: array([-0.87332544,  0.48713723,  0.        ])}\n",
      "cos(angle):  -0.8733254389048246   sin(angle)  0.4871372268280217\n",
      "theta dot:  -1.87670261332\n",
      "Angle:  150.84705441663357\n",
      "action:  [-0.48353542]\n",
      "state:  {1: array([ 5.69518258]), 2: array([-1.58388001]), 3: array([-0.83205022,  0.5547003 ,  0.        ])}\n",
      "cos(angle):  -0.8320502237624033   sin(angle)  0.554700302088375\n",
      "theta dot:  -1.58388000549\n",
      "Angle:  146.30958304873297\n",
      "action:  [-0.31890878]\n",
      "state:  {1: array([ 5.63439802]), 2: array([-1.2156911]), 3: array([-0.79681713,  0.60422054,  0.        ])}\n",
      "cos(angle):  -0.7968171312648384   sin(angle)  0.604220538647002\n",
      "theta dot:  -1.21569109584\n",
      "Angle:  142.8268927436203\n",
      "action:  [-0.16358121]\n",
      "state:  {1: array([ 5.59504488]), 2: array([-0.78706287]), 3: array([-0.77242837,  0.6351019 ,  0.        ])}\n",
      "cos(angle):  -0.7724283667487885   sin(angle)  0.6351018959519796\n",
      "theta dot:  -0.787062873772\n",
      "Angle:  140.57212897230977\n",
      "action:  [-0.08324548]\n",
      "state:  {1: array([ 5.57888371]), 2: array([-0.32322327]), 3: array([-0.76206396,  0.64750176,  0.        ])}\n",
      "cos(angle):  -0.7620639573946029   sin(angle)  0.6475017566308812\n",
      "theta dot:  -0.323223273166\n",
      "Angle:  139.64616466797645\n",
      "action:  [-0.09375354]\n",
      "state:  {1: array([ 5.58630071]), 2: array([ 0.14834001]), 3: array([-0.76684547,  0.64183177,  0.        ])}\n",
      "cos(angle):  -0.7668454731054221   sin(angle)  0.6418317695297744\n",
      "theta dot:  0.148340012689\n",
      "Angle:  140.07112650723082\n",
      "action:  [-0.152051]\n",
      "state:  {1: array([ 5.61664602]), 2: array([ 0.60690619]), 3: array([-0.78596602,  0.61826969,  0.        ])}\n",
      "cos(angle):  -0.785966024765173   sin(angle)  0.6182696886592708\n",
      "theta dot:  0.606906190579\n",
      "Angle:  141.80978060552889\n",
      "action:  [-0.18820907]\n",
      "state:  {1: array([ 5.66876488]), 2: array([ 1.0423771]), 3: array([-0.8171077 ,  0.57648505,  0.        ])}\n",
      "cos(angle):  -0.8171076987686564   sin(angle)  0.5764850463047508\n",
      "theta dot:  1.04237709719\n",
      "Angle:  144.7959640390401\n",
      "action:  [-0.15786311]\n",
      "state:  {1: array([ 5.74131795]), 2: array([ 1.45106142]), 3: array([-0.85674711,  0.51573674,  0.        ])}\n",
      "cos(angle):  -0.8567471109846196   sin(angle)  0.5157367427472157\n",
      "theta dot:  1.45106141519\n",
      "Angle:  148.95293906344364\n",
      "action:  [-0.08492971]\n",
      "state:  {1: array([ 5.83257418]), 2: array([ 1.82512452]), 3: array([-0.90018111,  0.43551574,  0.        ])}\n",
      "cos(angle):  -0.900181113146033   sin(angle)  0.4355157443022799\n",
      "theta dot:  1.82512451576\n",
      "Angle:  154.18152342869757\n",
      "action:  [-0.03725489]\n",
      "state:  {1: array([ 5.93988283]), 2: array([ 2.14617309]), 3: array([-0.94164819,  0.33659869,  0.        ])}\n",
      "cos(angle):  -0.9416481931356613   sin(angle)  0.3365986933491339\n",
      "theta dot:  2.14617309055\n",
      "Angle:  160.32984206090282\n",
      "action:  [-0.05153155]\n",
      "state:  {1: array([ 6.05942745]), 2: array([ 2.39089238]), 3: array([-0.97507048,  0.22189536,  0.        ])}\n",
      "cos(angle):  -0.9750704847810003   sin(angle)  0.22189535756510329\n",
      "theta dot:  2.39089237761\n",
      "Angle:  167.17922816942246\n",
      "action:  [-0.10190786]\n",
      "state:  {1: array([ 6.18652884]), 2: array([ 2.54202772]), 3: array([-0.9953324 ,  0.09650604,  0.        ])}\n",
      "cos(angle):  -0.9953323988068582   sin(angle)  0.09650604066785395\n",
      "theta dot:  2.54202771605\n",
      "Angle:  174.46158411684144\n",
      "action:  [-0.13496972]\n",
      "state:  {1: array([ 0.03305162]), 2: array([ 2.59416179]), 3: array([-0.99945385, -0.0330456 ,  0.        ])}\n",
      "cos(angle):  -0.9994538450178907   sin(angle)  -0.03304559999385977\n",
      "theta dot:  2.59416178904\n",
      "Angle:  181.89413466720785\n",
      "action:  [-0.10394896]\n",
      "state:  {1: array([ 0.16074088]), 2: array([ 2.55378525]), 3: array([-0.98710898, -0.16004958,  0.        ])}\n",
      "cos(angle):  -0.9871089768355218   sin(angle)  -0.16004957935192854\n",
      "theta dot:  2.55378524519\n",
      "Angle:  189.21017337574924\n",
      "action:  [ 0.01062532]\n",
      "state:  {1: array([ 0.28250797]), 2: array([ 2.43534186]), 3: array([-0.96035932, -0.27876508,  0.        ])}\n",
      "cos(angle):  -0.960359324179207   sin(angle)  -0.27876507755107477\n",
      "theta dot:  2.43534185916\n",
      "Angle:  196.1868975712077\n",
      "action:  [ 0.17895288]\n",
      "state:  {1: array([ 0.39516352]), 2: array([ 2.25311098]), 3: array([-0.92293363, -0.38495911,  0.        ])}\n",
      "cos(angle):  -0.922933627540098   sin(angle)  -0.3849591136155575\n",
      "theta dot:  2.25311098303\n",
      "Angle:  202.6415699824926\n",
      "action:  [ 0.33612151]\n",
      "state:  {1: array([ 0.49590402]), 2: array([ 2.01480987]), 3: array([-0.87953891, -0.47582696,  0.        ])}\n",
      "cos(angle):  -0.879538914407669   sin(angle)  -0.475826962290473\n",
      "theta dot:  2.01480987366\n",
      "Angle:  208.41356159915395\n",
      "action:  [ 0.43766763]\n",
      "state:  {1: array([ 0.58208351]), 2: array([ 1.7235898]), 3: array([-0.83531902, -0.54976552,  0.        ])}\n",
      "cos(angle):  -0.8353190244869547   sin(angle)  -0.5497655203176736\n",
      "theta dot:  1.72358979642\n",
      "Angle:  213.35127109996492\n",
      "action:  [ 0.48257242]\n",
      "state:  {1: array([ 0.65126608]), 2: array([ 1.38365152]), 3: array([-0.79531695, -0.60619383,  0.        ])}\n",
      "cos(angle):  -0.7953169456158583   sin(angle)  -0.6061938271017463\n",
      "theta dot:  1.38365151897\n",
      "Angle:  217.3151314484172\n",
      "action:  [ 0.48322785]\n",
      "state:  {1: array([ 0.7013406]), 2: array([ 1.00149033]), 3: array([-0.76397786, -0.64524245,  0.        ])}\n",
      "cos(angle):  -0.7639778636852659   sin(angle)  -0.6452424535001531\n",
      "theta dot:  1.0014903257\n",
      "Angle:  220.1841831836238\n",
      "action:  [ 0.45196304]\n",
      "state:  {1: array([ 0.73060824]), 2: array([ 0.58535294]), 3: array([-0.74476864, -0.66732276,  0.        ])}\n",
      "cos(angle):  -0.7447686447171061   sin(angle)  -0.6673227598742942\n",
      "theta dot:  0.585352942239\n",
      "Angle:  221.8610919180752\n",
      "action:  [ 0.42057366]\n",
      "state:  {1: array([ 0.73800559]), 2: array([ 0.14794692]), 3: array([-0.7398119 , -0.67281376,  0.        ])}\n",
      "cos(angle):  -0.7398118952855326   sin(angle)  -0.6728137629344603\n",
      "theta dot:  0.147946921649\n",
      "Angle:  222.284927637085\n",
      "action:  [ 0.42958861]\n",
      "state:  {1: array([ 0.72339434]), 2: array([-0.29222511]), 3: array([-0.74956323, -0.66193275,  0.        ])}\n",
      "cos(angle):  -0.7495632299089499   sin(angle)  -0.661932749128235\n",
      "theta dot:  -0.292225109451\n",
      "Angle:  221.44776632276816\n",
      "action:  [ 0.47682705]\n",
      "state:  {1: array([ 0.6875368]), 2: array([-0.71715061]), 3: array([-0.77281159, -0.63463552,  0.        ])}\n",
      "cos(angle):  -0.7728115892725842   sin(angle)  -0.634635523340746\n",
      "theta dot:  -0.717150614118\n",
      "Angle:  219.39328595382764\n",
      "action:  [ 0.50817093]\n",
      "state:  {1: array([ 0.63169172]), 2: array([-1.11690162]), 3: array([-0.80702968, -0.59051087,  0.        ])}\n",
      "cos(angle):  -0.8070296824381871   sin(angle)  -0.590510873450878\n",
      "theta dot:  -1.11690161674\n",
      "Angle:  216.19360599755223\n",
      "action:  [ 0.48193203]\n",
      "state:  {1: array([ 0.55731698]), 2: array([-1.48749497]), 3: array([-0.84867725, -0.52891108,  0.        ])}\n",
      "cos(angle):  -0.8486772454934963   sin(angle)  -0.5289110823017151\n",
      "theta dot:  -1.48749496727\n",
      "Angle:  211.93225677887517\n",
      "action:  [ 0.41661783]\n",
      "state:  {1: array([ 0.46623269]), 2: array([-1.82168561]), 3: array([-0.89326812, -0.44952427,  0.        ])}\n",
      "cos(angle):  -0.8932681181153644   sin(angle)  -0.44952426982159194\n",
      "theta dot:  -1.82168560524\n",
      "Angle:  206.71352414353623\n",
      "action:  [ 0.35646828]\n",
      "state:  {1: array([ 0.36096477]), 2: array([-2.10535857]), 3: array([-0.93555653, -0.35317699,  0.        ])}\n",
      "cos(angle):  -0.9355565257349121   sin(angle)  -0.3531769912590864\n",
      "theta dot:  -2.10535856597\n",
      "Angle:  200.68213023796443\n",
      "action:  [ 0.31889277]\n",
      "state:  {1: array([ 0.2448444]), 2: array([-2.32240739]), 3: array([-0.97017506, -0.24240537,  0.        ])}\n",
      "cos(angle):  -0.9701750557273198   sin(angle)  -0.24240536554394168\n",
      "theta dot:  -2.32240739416\n",
      "Angle:  194.02893869625433\n",
      "action:  [ 0.30096505]\n",
      "state:  {1: array([ 0.12189106]), 2: array([-2.45906666]), 3: array([-0.99258048, -0.12158946,  0.        ])}\n",
      "cos(angle):  -0.9925804773480287   sin(angle)  -0.12158945672861388\n",
      "theta dot:  -2.45906666055\n",
      "Angle:  186.98424811025853\n",
      "action:  [ 0.3024271]\n",
      "state:  {1: array([ 6.27983164]), 2: array([-2.50489469]), 3: array([-0.99999438,  0.00335366,  0.        ])}\n",
      "cos(angle):  -0.9999943764524456   sin(angle)  0.0033536641878037527\n",
      "theta dot:  -2.5048946883\n",
      "Angle:  179.80742836796094\n",
      "action:  [ 0.31336844]\n",
      "state:  {1: array([ 6.15706293]), 2: array([-2.45537417]), 3: array([-0.99205711,  0.12578828,  0.        ])}\n",
      "cos(angle):  -0.9920571099799473   sin(angle)  0.1257882766327402\n",
      "theta dot:  -2.45537417423\n",
      "Angle:  172.7733159513375\n",
      "action:  [ 0.30765584]\n",
      "state:  {1: array([ 6.0413187]), 2: array([-2.31488459]), 3: array([-0.97089259,  0.23951532,  0.        ])}\n",
      "cos(angle):  -0.9708925851240263   sin(angle)  0.23951531924114\n",
      "theta dot:  -2.31488459147\n",
      "Angle:  166.14167560145518\n",
      "action:  [ 0.27235352]\n",
      "state:  {1: array([ 5.93659894]), 2: array([-2.09439507]), 3: array([-0.94053777,  0.33968914,  0.        ])}\n",
      "cos(angle):  -0.940537766185567   sin(angle)  0.33968913785793\n",
      "theta dot:  -2.09439507423\n",
      "Angle:  160.14168971271692\n",
      "action:  [ 0.22508793]\n",
      "state:  {1: array([ 5.84630569]), 2: array([-1.80586503]), 3: array([-0.90607635,  0.42311422,  0.        ])}\n",
      "cos(angle):  -0.9060763518994395   sin(angle)  0.42311422160996104\n",
      "theta dot:  -1.80586503125\n",
      "Angle:  154.9682795773639\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -0.748455727785  self.max:  0.875992110438\n",
      "state:  {1: array([ 3.83598801]), 2: array([ 0.38504727]), 3: array([ 0.76844077,  0.63992092,  0.        ])}\n",
      "cos(angle):  0.7684407662848791   sin(angle)  0.6399209237956732\n",
      "theta dot:  0.385047267767\n",
      "Angle:  39.78583020249988\n",
      "action:  [-0.06229991]\n",
      "state:  {1: array([ 3.87877016]), 2: array([ 0.85564297]), 3: array([ 0.74036879,  0.6722009 ,  0.        ])}\n",
      "cos(angle):  0.7403687888718228   sin(angle)  0.6722009048375868\n",
      "theta dot:  0.855642973478\n",
      "Angle:  42.23706102797223\n",
      "action:  [-0.04099634]\n",
      "state:  {1: array([ 3.94645237]), 2: array([ 1.3536442]), 3: array([ 0.69321235,  0.7207334 ,  0.        ])}\n",
      "cos(angle):  0.6932123498024221   sin(angle)  0.7207334029177532\n",
      "theta dot:  1.35364420066\n",
      "Angle:  46.114956942763456\n",
      "action:  [-0.04332342]\n",
      "state:  {1: array([ 4.04083716]), 2: array([ 1.88769574]), 3: array([ 0.62220159,  0.78285706,  0.        ])}\n",
      "cos(angle):  0.6222015925721358   sin(angle)  0.7828570611552903\n",
      "theta dot:  1.88769574048\n",
      "Angle:  51.522794243611884\n",
      "action:  [-0.1075695]\n",
      "state:  {1: array([ 4.16377231]), 2: array([ 2.45870311]), 3: array([ 0.52150741,  0.85324676,  0.        ])}\n",
      "cos(angle):  0.5215074056258873   sin(angle)  0.8532467555621623\n",
      "theta dot:  2.45870311129\n",
      "Angle:  58.56644334014027\n",
      "action:  [-0.22873795]\n",
      "state:  {1: array([ 4.31698869]), 2: array([ 3.06432749]), 3: array([ 0.38517763,  0.92284245,  0.        ])}\n",
      "cos(angle):  0.3851776286687733   sin(angle)  0.9228424537119544\n",
      "theta dot:  3.06432748517\n",
      "Angle:  67.34507440918543\n",
      "action:  [-0.3579278]\n",
      "state:  {1: array([ 4.50212719]), 2: array([ 3.70277016]), 3: array([ 0.20871593,  0.97797631,  0.        ])}\n",
      "cos(angle):  0.208715928499121   sin(angle)  0.9779763091152821\n",
      "theta dot:  3.70277015506\n",
      "Angle:  77.95270472351649\n",
      "action:  [-0.42515601]\n",
      "state:  {1: array([ 4.72075114]), 2: array([ 4.37247899]), 3: array([-0.00836206,  0.99996504,  0.        ])}\n",
      "cos(angle):  -0.008362064778126335   sin(angle)  0.9999650373251289\n",
      "theta dot:  4.37247898507\n",
      "Angle:  90.47890502450934\n",
      "action:  [-0.38634211]\n",
      "state:  {1: array([ 4.97397621]), 2: array([ 5.06450145]), 3: array([-0.25861412,  0.96598071,  0.        ])}\n",
      "cos(angle):  -0.25861411501103376   sin(angle)  0.9659807138432215\n",
      "theta dot:  5.06450144705\n",
      "Angle:  104.98759900955781\n",
      "action:  [-0.26588461]\n",
      "state:  {1: array([ 5.26143143]), 2: array([ 5.74910429]), 3: array([-0.52187065,  0.85302463,  0.        ])}\n",
      "cos(angle):  -0.5218706538633191   sin(angle)  0.8530246307324729\n",
      "theta dot:  5.74910429166\n",
      "Angle:  121.45753109032387\n",
      "action:  [-0.1433206]\n",
      "state:  {1: array([ 5.57980016]), 2: array([ 6.36737468]), 3: array([-0.76265704,  0.64680309,  0.        ])}\n",
      "cos(angle):  -0.7626570396558353   sin(angle)  0.6468030920329599\n",
      "theta dot:  6.36737467503\n",
      "Angle:  139.69867320747085\n",
      "action:  [-0.09237871]\n",
      "state:  {1: array([ 5.92173117]), 2: array([ 6.83862019]), 3: array([-0.93538358,  0.35363478,  0.        ])}\n",
      "cos(angle):  -0.9353835802611994   sin(angle)  0.353634780215041\n",
      "theta dot:  6.83862018745\n",
      "Angle:  159.2898311165003\n",
      "action:  [-0.13029129]\n",
      "state:  {1: array([ 6.2759463]), 2: array([ 7.08430258]), 3: array([-0.9999738 ,  0.00723894,  0.        ])}\n",
      "cos(angle):  -0.9999737985137302   sin(angle)  0.007238942327554871\n",
      "theta dot:  7.08430257902\n",
      "Angle:  179.58481558658673\n",
      "action:  [-0.21178235]\n",
      "state:  {1: array([ 0.34565922]), 2: array([ 7.05796443]), 3: array([-0.9408523 , -0.33881698,  0.        ])}\n",
      "cos(angle):  -0.9408523034558319   sin(angle)  -0.33881697578759995\n",
      "theta dot:  7.0579644326\n",
      "Angle:  199.8051888362306\n",
      "action:  [-0.26818823]\n",
      "state:  {1: array([ 0.68384039]), 2: array([ 6.76362347]), 3: array([-0.77515218, -0.63177456,  0.        ])}\n",
      "cos(angle):  -0.7751521805957363   sin(angle)  -0.6317745617842452\n",
      "theta dot:  6.7636234659\n",
      "Angle:  219.18149746657008\n",
      "action:  [-0.26408354]\n",
      "state:  {1: array([ 0.99634939]), 2: array([ 6.25018001]), 3: array([-0.54337058, -0.83949295,  0.        ])}\n",
      "cos(angle):  -0.5433705811197725   sin(angle)  -0.8394929490910336\n",
      "theta dot:  6.25018001324\n",
      "Angle:  237.08690239372024\n",
      "action:  [-0.21993475]\n",
      "state:  {1: array([ 1.27572789]), 2: array([ 5.58757009]), 3: array([-0.29080533, -0.95678224,  0.        ])}\n",
      "cos(angle):  -0.2908053256017841   sin(angle)  -0.9567822440877759\n",
      "theta dot:  5.58757008946\n",
      "Angle:  253.0940741549625\n",
      "action:  [-0.18202599]\n",
      "state:  {1: array([ 1.51786187]), 2: array([ 4.84267951]), 3: array([-0.05290974, -0.9985993 ,  0.        ])}\n",
      "cos(angle):  -0.0529097394594996   sin(angle)  -0.9985992987531725\n",
      "theta dot:  4.84267950798\n",
      "Angle:  266.9672965804247\n",
      "action:  [-0.18167558]\n",
      "state:  {1: array([ 1.7211858]), 2: array([ 4.0664787]), 3: array([ 0.14982323, -0.9887128 ,  0.        ])}\n",
      "cos(angle):  0.14982322563264544   sin(angle)  -0.9887128000896062\n",
      "theta dot:  4.06647869617\n",
      "Angle:  278.61687267716553\n",
      "action:  [-0.22244319]\n",
      "state:  {1: array([ 1.88576469]), 2: array([ 3.29157762]), 3: array([ 0.30978639, -0.95080618,  0.        ])}\n",
      "cos(angle):  0.3097863867498772   sin(angle)  -0.9508061813978996\n",
      "theta dot:  3.29157761792\n",
      "Angle:  288.0465258988728\n",
      "action:  [-0.28880624]\n",
      "state:  {1: array([ 2.01252229]), 2: array([ 2.53515205]), 3: array([ 0.4275004 , -0.90401516,  0.        ])}\n",
      "cos(angle):  0.42750039605806367   sin(angle)  -0.9040151610289502\n",
      "theta dot:  2.53515204602\n",
      "Angle:  295.30918454865514\n",
      "action:  [-0.3589311]\n",
      "state:  {1: array([ 2.10268734]), 2: array([ 1.80330101]), 3: array([ 0.50716401, -0.86184956,  0.        ])}\n",
      "cos(angle):  0.5071640145733424   sin(angle)  -0.8618495589845426\n",
      "theta dot:  1.80330101049\n",
      "Angle:  300.47524932280027\n",
      "action:  [-0.41620833]\n",
      "state:  {1: array([ 2.15741147]), 2: array([ 1.09448259]), 3: array([ 0.55354522, -0.83281912,  0.        ])}\n",
      "cos(angle):  0.5535452242517206   sin(angle)  -0.832819118841608\n",
      "theta dot:  1.09448259173\n",
      "Angle:  303.61070365358506\n",
      "action:  [-0.44913569]\n",
      "state:  {1: array([ 2.17753636]), 2: array([ 0.4024979]), 3: array([ 0.5701924 , -0.82151119,  0.        ])}\n",
      "cos(angle):  0.5701923979273095   sin(angle)  -0.8215111863790443\n",
      "theta dot:  0.402497899273\n",
      "Angle:  304.76377250176967\n",
      "action:  [-0.43426523]\n",
      "state:  {1: array([ 2.1635976]), 2: array([-0.27877528]), 3: array([ 0.55868653, -0.8293789 ,  0.        ])}\n",
      "cos(angle):  0.5586865281743133   sin(angle)  -0.8293789020927239\n",
      "theta dot:  -0.278775275082\n",
      "Angle:  303.9651420345748\n",
      "action:  [-0.33526664]\n",
      "state:  {1: array([ 2.11604263]), 2: array([-0.95109945]), 3: array([ 0.51862869, -0.85499958,  0.        ])}\n",
      "cos(angle):  0.5186286926968213   sin(angle)  -0.8549995784277242\n",
      "theta dot:  -0.951099447193\n",
      "Angle:  301.2404491950233\n",
      "action:  [-0.14255661]\n",
      "state:  {1: array([ 2.035356]), 2: array([-1.61373262]), 3: array([ 0.44802918, -0.89401893,  0.        ])}\n",
      "cos(angle):  0.44802918060463454   sin(angle)  -0.8940189334274413\n",
      "theta dot:  -1.61373262241\n",
      "Angle:  296.6174565792647\n",
      "action:  [ 0.09028243]\n",
      "state:  {1: array([ 1.92182077]), 2: array([-2.27070446]), 3: array([ 0.34385996, -0.93902094,  0.        ])}\n",
      "cos(angle):  0.3438599640306868   sin(angle)  -0.9390209396689804\n",
      "theta dot:  -2.27070445791\n",
      "Angle:  290.1123826929804\n",
      "action:  [ 0.27106635]\n",
      "state:  {1: array([ 1.77510526]), 2: array([-2.93431021]), 3: array([ 0.20289052, -0.97920143,  0.        ])}\n",
      "cos(angle):  0.20289051708330025   sin(angle)  -0.9792014287559384\n",
      "theta dot:  -2.93431021079\n",
      "Angle:  281.7062228072268\n",
      "action:  [ 0.33298635]\n",
      "state:  {1: array([ 1.5941671]), 2: array([-3.61876333]), 3: array([ 0.02336864, -0.99972692,  0.        ])}\n",
      "cos(angle):  0.02336864162565717   sin(angle)  -0.9997269160068522\n",
      "theta dot:  -3.6187633293\n",
      "Angle:  271.33925375843046\n",
      "action:  [ 0.26578844]\n",
      "state:  {1: array([ 1.37773258]), 2: array([-4.32869025]), 3: array([-0.19186661, -0.98142101,  0.        ])}\n",
      "cos(angle):  -0.19186661292783902   sin(angle)  -0.9814210120247063\n",
      "theta dot:  -4.32869024973\n",
      "Angle:  258.93849865032945\n",
      "action:  [ 0.09493894]\n",
      "state:  {1: array([ 1.12520682]), 2: array([-5.05051517]), 3: array([-0.4309899 , -0.90235675,  0.        ])}\n",
      "cos(angle):  -0.4309898961168934   sin(angle)  -0.9023567528672622\n",
      "theta dot:  -5.05051516809\n",
      "Angle:  244.46987230933922\n",
      "action:  [-0.14356044]\n",
      "state:  {1: array([ 0.83776599]), 2: array([-5.7488168]), 3: array([-0.6691247 , -0.74315014,  0.        ])}\n",
      "cos(angle):  -0.6691247027482187   sin(angle)  -0.7431501410698298\n",
      "theta dot:  -5.74881679883\n",
      "Angle:  228.000763832932\n",
      "action:  [-0.40243666]\n",
      "state:  {1: array([ 0.51943874]), 2: array([-6.3665449]), 3: array([-0.86809792, -0.49639299,  0.        ])}\n",
      "cos(angle):  -0.8680979219893463   sin(angle)  -0.4963929872971402\n",
      "theta dot:  -6.36654490312\n",
      "Angle:  209.76199883163218\n",
      "action:  [-0.61453141]\n",
      "state:  {1: array([ 0.17788777]), 2: array([-6.83101936]), 3: array([-0.98421965, -0.17695107,  0.        ])}\n",
      "cos(angle):  -0.9842196490563425   sin(angle)  -0.17695107349606568\n",
      "theta dot:  -6.83101935544\n",
      "Angle:  190.1926156514271\n",
      "action:  [-0.70932655]\n",
      "state:  {1: array([ 6.1075665]), 2: array([-7.07013164]), 3: array([-0.98461861,  0.17471746,  0.        ])}\n",
      "cos(angle):  -0.9846186105097521   sin(angle)  0.1747174628875008\n",
      "theta dot:  -7.07013164264\n",
      "Angle:  169.9373859941188\n",
      "action:  [-0.65173552]\n",
      "state:  {1: array([ 5.7557238]), 2: array([-7.03685387]), 3: array([-0.86408758,  0.50334148,  0.        ])}\n",
      "cos(angle):  -0.8640875836994747   sin(angle)  0.5033414821931562\n",
      "theta dot:  -7.03685387346\n",
      "Angle:  149.77833173478064\n",
      "action:  [-0.46131853]\n",
      "state:  {1: array([ 5.41929653]), 2: array([-6.72854554]), 3: array([-0.64948546,  0.76037401,  0.        ])}\n",
      "cos(angle):  -0.6494854587047907   sin(angle)  0.7603740125300362\n",
      "theta dot:  -6.72854554188\n",
      "Angle:  130.50251371945717\n",
      "action:  [-0.19921843]\n",
      "state:  {1: array([ 5.10988914]), 2: array([-6.1881478]), 3: array([-0.38711462,  0.9220316 ,  0.        ])}\n",
      "cos(angle):  -0.38711461925926843   sin(angle)  0.922031600085242\n",
      "theta dot:  -6.18814779692\n",
      "Angle:  112.77481758620058\n",
      "action:  [ 0.04641688]\n",
      "state:  {1: array([ 4.83540606]), 2: array([-5.48966156]), 3: array([-0.12270704,  0.99244294,  0.        ])}\n",
      "cos(angle):  -0.12270703888057016   sin(angle)  0.9924429367017341\n",
      "theta dot:  -5.48966156492\n",
      "Angle:  97.04813243078729\n",
      "action:  [ 0.17758409]\n",
      "state:  {1: array([ 4.59947147]), 2: array([-4.71869175]), 3: array([ 0.11267771,  0.99363159,  0.        ])}\n",
      "cos(angle):  0.112677705945871   sin(angle)  0.9936315889618123\n",
      "theta dot:  -4.71869174941\n",
      "Angle:  83.5301079385996\n",
      "action:  [ 0.13453763]\n",
      "state:  {1: array([ 4.4018071]), 2: array([-3.95328741]), 3: array([ 0.30561273,  0.9521559 ,  0.        ])}\n",
      "cos(angle):  0.3056127288234261   sin(angle)  0.9521559010903093\n",
      "theta dot:  -3.95328741269\n",
      "Angle:  72.20480022463641\n",
      "action:  [-0.0510692]\n",
      "state:  {1: array([ 4.23946556]), 2: array([-3.24683087]), 3: array([ 0.45549078,  0.8902405 ,  0.        ])}\n",
      "cos(angle):  0.45549077811450744   sin(angle)  0.8902405018042262\n",
      "theta dot:  -3.24683086665\n",
      "Angle:  62.90333670289772\n",
      "action:  [-0.26702831]\n",
      "state:  {1: array([ 4.10850532]), 2: array([-2.61920474]), 3: array([ 0.56784353,  0.82313652,  0.        ])}\n",
      "cos(angle):  0.5678435303656837   sin(angle)  0.8231365166373297\n",
      "theta dot:  -2.61920473667\n",
      "Angle:  55.39988539464355\n",
      "action:  [-0.39962481]\n",
      "state:  {1: array([ 4.00541552]), 2: array([-2.06179607]), 3: array([ 0.64953558,  0.7603312 ,  0.        ])}\n",
      "cos(angle):  0.6495355795531826   sin(angle)  0.7603311981594015\n",
      "theta dot:  -2.06179607034\n",
      "Angle:  49.493288554491926\n",
      "action:  [-0.40313838]\n",
      "state:  {1: array([ 3.9278146]), 2: array([-1.55201843]), 3: array([ 0.70652404,  0.70768904,  0.        ])}\n",
      "cos(angle):  0.7065240422538357   sin(angle)  0.7076890402693122\n",
      "theta dot:  -1.55201842834\n",
      "Angle:  45.047093668111074\n",
      "action:  [-0.3048576]\n",
      "state:  {1: array([ 3.87446558]), 2: array([-1.06698029]), 3: array([ 0.74325546,  0.66900771,  0.        ])}\n",
      "cos(angle):  0.7432554618996312   sin(angle)  0.6690077117316107\n",
      "theta dot:  -1.06698028746\n",
      "Angle:  41.99042745116026\n",
      "action:  [-0.16880367]\n",
      "state:  {1: array([ 3.84493833]), 2: array([-0.59054505]), 3: array([ 0.76268257,  0.64677299,  0.        ])}\n",
      "cos(angle):  0.7626825680387435   sin(angle)  0.6467729898579775\n",
      "theta dot:  -0.59054505423\n",
      "Angle:  40.29864444629868\n",
      "action:  [-0.06305309]\n",
      "state:  {1: array([ 3.83919216]), 2: array([-0.11492327]), 3: array([ 0.76638642,  0.64237984,  0.        ])}\n",
      "cos(angle):  0.7663864198478599   sin(angle)  0.6423798373803304\n",
      "theta dot:  -0.11492327487\n",
      "Angle:  39.96941428528959\n",
      "action:  [-0.0353416]\n",
      "state:  {1: array([ 3.85727018]), 2: array([ 0.36156036]), 3: array([ 0.75464887,  0.65612886,  0.        ])}\n",
      "cos(angle):  0.7546488682657575   sin(angle)  0.6561288635818511\n",
      "theta dot:  0.361560362425\n",
      "Angle:  41.00520600346596\n",
      "action:  [-0.08623991]\n",
      "state:  {1: array([ 3.89930623]), 2: array([ 0.84072102]), 3: array([ 0.72640928,  0.68726237,  0.        ])}\n",
      "cos(angle):  0.7264092785359437   sin(angle)  0.6872623662451551\n",
      "theta dot:  0.840721023964\n",
      "Angle:  43.41368869243729\n",
      "action:  [-0.16867282]\n",
      "state:  {1: array([ 3.96584958]), 2: array([ 1.33086688]), 3: array([ 0.6791026 ,  0.73404336,  0.        ])}\n",
      "cos(angle):  0.6791026032543813   sin(angle)  0.7340433599271383\n",
      "theta dot:  1.33086687526\n",
      "Angle:  47.22633252912834\n",
      "action:  [-0.23711607]\n",
      "state:  {1: array([ 4.05814118]), 2: array([ 1.84583198]), 3: array([ 0.60856254,  0.79350591,  0.        ])}\n",
      "cos(angle):  0.608562543453254   sin(angle)  0.7935059109456629\n",
      "theta dot:  1.84583198415\n",
      "Angle:  52.51423928279377\n",
      "action:  [-0.29688053]\n",
      "state:  {1: array([ 4.17796264]), 2: array([ 2.39642934]), 3: array([ 0.50934745,  0.86056096,  0.        ])}\n",
      "cos(angle):  0.5093474512630235   sin(angle)  0.8605609646572763\n",
      "theta dot:  2.39642933735\n",
      "Angle:  59.37948757542148\n",
      "action:  [-0.38439015]\n",
      "state:  {1: array([ 4.32717222]), 2: array([ 2.98419154]), 3: array([ 0.37576002,  0.926717  ,  0.        ])}\n",
      "cos(angle):  0.37576002079166915   sin(angle)  0.9267170046862442\n",
      "theta dot:  2.9841915387\n",
      "Angle:  67.92854660531509\n",
      "action:  [-0.50241924]\n",
      "state:  {1: array([ 4.50736554]), 2: array([ 3.60386641]), 3: array([ 0.20359011,  0.97905621,  0.        ])}\n",
      "cos(angle):  0.2035901087778326   sin(angle)  0.9790562126904819\n",
      "theta dot:  3.60386640619\n",
      "Angle:  78.25283921281323\n",
      "action:  [-0.59514236]\n",
      "state:  {1: array([ 4.7198099]), 2: array([ 4.24888721]), 3: array([-0.00742085,  0.99997247,  0.        ])}\n",
      "cos(angle):  -0.007420852712240226   sin(angle)  0.9999724650934261\n",
      "theta dot:  4.24888721115\n",
      "Angle:  90.4249759903771\n",
      "action:  [-0.58851654]\n",
      "state:  {1: array([ 4.96533936]), 2: array([ 4.91058908]), 3: array([-0.25026153,  0.96817827,  0.        ])}\n",
      "cos(angle):  -0.250261533107428   sin(angle)  0.9681782713151126\n",
      "theta dot:  4.91058907946\n",
      "Angle:  104.49274455262605\n",
      "action:  [-0.45012396]\n",
      "state:  {1: array([ 5.24379956]), 2: array([ 5.56920419]), 3: array([-0.5067499 ,  0.86209312,  0.        ])}\n",
      "cos(angle):  -0.5067498999193409   sin(angle)  0.862093115000774\n",
      "theta dot:  5.56920418896\n",
      "Angle:  120.44730200762228\n",
      "action:  [-0.21640896]\n",
      "state:  {1: array([ 5.5529652]), 2: array([ 6.18331268]), 3: array([-0.7450276 ,  0.66703364,  0.        ])}\n",
      "cos(angle):  -0.7450276006478284   sin(angle)  0.6670336380370483\n",
      "theta dot:  6.18331268184\n",
      "Angle:  138.16114658889603\n",
      "action:  [ 0.03153231]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "state:  {1: array([ 5.88738109]), 2: array([ 6.68831776]), 3: array([-0.9226868 ,  0.38555036,  0.        ])}\n",
      "cos(angle):  -0.9226867952400399   sin(angle)  0.38555035713855135\n",
      "theta dot:  6.68831775715\n",
      "Angle:  157.32172075948347\n",
      "action:  [ 0.21972038]\n",
      "state:  {1: array([ 6.23790302]), 2: array([ 7.01043858]), 3: array([-0.99897493,  0.04526682,  0.        ])}\n",
      "cos(angle):  -0.9989749322126138   sin(angle)  0.04526681798849646\n",
      "theta dot:  7.01043858164\n",
      "Angle:  177.40510095898924\n",
      "action:  [ 0.31477825]\n",
      "state:  {1: array([ 0.30929798]), 2: array([ 7.09160543]), 3: array([-0.95254749, -0.30439   ,  0.        ])}\n",
      "cos(angle):  -0.9525474924319984   sin(angle)  -0.30439000420761514\n",
      "theta dot:  7.09160543234\n",
      "Angle:  197.72184834844438\n",
      "action:  [ 0.31983493]\n",
      "state:  {1: array([ 0.65486239]), 2: array([ 6.91128817]), 3: array([-0.79313175, -0.60905011,  0.        ])}\n",
      "cos(angle):  -0.793131747897791   sin(angle)  -0.6090501050624613\n",
      "theta dot:  6.91128816843\n",
      "Angle:  217.52118420147255\n",
      "action:  [ 0.25436162]\n",
      "state:  {1: array([ 0.97949513]), 2: array([ 6.49265483]), 3: array([-0.55744177, -0.83021604,  0.        ])}\n",
      "cos(angle):  -0.5574417688223081   sin(angle)  -0.8302160407823113\n",
      "theta dot:  6.49265483305\n",
      "Angle:  236.12122669492942\n",
      "action:  [ 0.14267046]\n",
      "state:  {1: array([ 1.2740648]), 2: array([ 5.89139337]), 3: array([-0.29239614, -0.95629728,  0.        ])}\n",
      "cos(angle):  -0.2923961431923392   sin(angle)  -0.956297284031616\n",
      "theta dot:  5.89139337156\n",
      "Angle:  252.99878600993287\n",
      "action:  [ 0.0088664]\n",
      "state:  {1: array([ 1.53283982]), 2: array([ 5.17550037]), 3: array([-0.0379474 , -0.99927974,  0.        ])}\n",
      "cos(angle):  -0.03794739630402626   sin(angle)  -0.9992797381683195\n",
      "theta dot:  5.17550036857\n",
      "Angle:  267.825467738066\n",
      "action:  [-0.12569464]\n",
      "state:  {1: array([ 1.75319914]), 2: array([ 4.40718637]), 3: array([ 0.18139304, -0.98341068,  0.        ])}\n",
      "cos(angle):  0.18139304264542908   sin(angle)  -0.983410679258586\n",
      "theta dot:  4.40718636882\n",
      "Angle:  280.4510971369073\n",
      "action:  [-0.24008504]\n",
      "state:  {1: array([ 1.93487992]), 2: array([ 3.6336156]), 3: array([ 0.3560931 , -0.93445048,  0.        ])}\n",
      "cos(angle):  0.356093103384544   sin(angle)  -0.9344504811502664\n",
      "theta dot:  3.63361560271\n",
      "Angle:  290.8606147153366\n",
      "action:  [-0.3111289]\n",
      "state:  {1: array([ 2.07918534]), 2: array([ 2.88610841]), 3: array([ 0.48677063, -0.87352982,  0.        ])}\n",
      "cos(angle):  0.486770630733574   sin(angle)  -0.8735298237926616\n",
      "theta dot:  2.886108407\n",
      "Angle:  299.1286869279055\n",
      "action:  [-0.31963318]\n",
      "state:  {1: array([ 2.18833614]), 2: array([ 2.18301606]), 3: array([ 0.57903111, -0.81530545,  0.        ])}\n",
      "cos(angle):  0.5790311105047397   sin(angle)  -0.8153054477112538\n",
      "theta dot:  2.18301606175\n",
      "Angle:  305.3825526510346\n",
      "action:  [-0.26337174]\n",
      "state:  {1: array([ 2.2649377]), 2: array([ 1.53203122]), 3: array([ 0.63972573, -0.76860327,  0.        ])}\n",
      "cos(angle):  0.6397257320454932   sin(angle)  -0.7686032707182932\n",
      "theta dot:  1.53203121564\n",
      "Angle:  309.77148852471953\n",
      "action:  [-0.16327217]\n",
      "state:  {1: array([ 2.3114921]), 2: array([ 0.93108794]), 3: array([ 0.67480155, -0.73799923,  0.        ])}\n",
      "cos(angle):  0.6748015526855424   sin(angle)  -0.7379992306860361\n",
      "theta dot:  0.931087937484\n",
      "Angle:  312.43885274592947\n",
      "action:  [-0.05474048]\n",
      "state:  {1: array([ 2.32996097]), 2: array([ 0.36937744]), 3: array([ 0.68831571, -0.72541125,  0.        ])}\n",
      "cos(angle):  0.6883157073822888   sin(angle)  -0.725411253683605\n",
      "theta dot:  0.369377442622\n",
      "Angle:  313.4970386969097\n",
      "action:  [ 0.02735492]\n",
      "state:  {1: array([ 2.32143208]), 2: array([-0.17057776]), 3: array([ 0.6821038 , -0.73125537,  0.        ])}\n",
      "cos(angle):  0.6821037965005341   sin(angle)  -0.7312553663389814\n",
      "theta dot:  -0.170577759957\n",
      "Angle:  313.0083705534129\n",
      "action:  [ 0.05674168]\n",
      "state:  {1: array([ 2.28590668]), 2: array([-0.71050803]), 3: array([ 0.65570074, -0.75502089,  0.        ])}\n",
      "cos(angle):  0.6557007391582564   sin(angle)  -0.7550208875702157\n",
      "theta dot:  -0.710508033045\n",
      "Angle:  310.9729197330011\n",
      "action:  [ 0.02116198]\n",
      "state:  {1: array([ 2.22222671]), 2: array([-1.2735994]), 3: array([ 0.60632449, -0.79521734,  0.        ])}\n",
      "cos(angle):  0.6063244905560541   sin(angle)  -0.7952173364256727\n",
      "theta dot:  -1.27359940125\n",
      "Angle:  307.32433474088657\n",
      "action:  [-0.06718306]\n",
      "state:  {1: array([ 2.12822222]), 2: array([-1.88008986]), 3: array([ 0.52900351, -0.84861963,  0.        ])}\n",
      "cos(angle):  0.5290035121105443   sin(angle)  -0.8486196345682259\n",
      "theta dot:  -1.88008986206\n",
      "Angle:  301.938286625753\n",
      "action:  [-0.17457984]\n",
      "state:  {1: array([ 2.00108514]), 2: array([-2.54274156]), 3: array([ 0.4171333 , -0.90884531,  0.        ])}\n",
      "cos(angle):  0.4171333044835815   sin(angle)  -0.9088453148311916\n",
      "theta dot:  -2.54274156438\n",
      "Angle:  294.6538856582711\n",
      "action:  [-0.27843586]\n",
      "state:  {1: array([ 1.83777809]), 2: array([-3.26614093]), 3: array([ 0.26382134, -0.96457156,  0.        ])}\n",
      "cos(angle):  0.263821338815994   sin(angle)  -0.9645715635375824\n",
      "theta dot:  -3.26614092947\n",
      "Angle:  285.2971030108164\n",
      "action:  [-0.39040699]\n",
      "state:  {1: array([ 1.63537156]), 2: array([-4.04813065]), 3: array([ 0.06453036, -0.99791574,  0.        ])}\n",
      "cos(angle):  0.06453036216741098   sin(angle)  -0.9979157441179806\n",
      "theta dot:  -4.04813065007\n",
      "Angle:  273.700090071349\n",
      "action:  [-0.52865254]\n",
      "state:  {1: array([ 1.39157829]), 2: array([-4.87586534]), 3: array([-0.17826019, -0.98398339,  0.        ])}\n",
      "cos(angle):  -0.17826018718841546   sin(angle)  -0.9839833868839205\n",
      "theta dot:  -4.87586533845\n",
      "Angle:  259.73179746693097\n",
      "action:  [-0.67781627]\n",
      "state:  {1: array([ 1.10580203]), 2: array([-5.71552532]), 3: array([-0.44841771, -0.89382412,  0.        ])}\n",
      "cos(angle):  -0.4484177072728305   sin(angle)  -0.8938241212924264\n",
      "theta dot:  -5.71552531884\n",
      "Angle:  243.3580618323735\n",
      "action:  [-0.79518029]\n",
      "state:  {1: array([ 0.7805435]), 2: array([-6.50517045]), 3: array([-0.7105312, -0.7036657,  0.       ])}\n",
      "cos(angle):  -0.7105311982677713   sin(angle)  -0.703665699240886\n",
      "theta dot:  -6.50517045387\n",
      "Angle:  224.72216481019802\n",
      "action:  [-0.85140204]\n",
      "state:  {1: array([ 0.422512]), 2: array([-7.16063003]), 3: array([-0.91206177, -0.41005285,  0.        ])}\n",
      "cos(angle):  -0.9120617655685404   sin(angle)  -0.41005284511632994\n",
      "theta dot:  -7.16063003392\n",
      "Angle:  204.20851880012427\n",
      "action:  [-0.84309179]\n",
      "state:  {1: array([ 0.04278033]), 2: array([-7.59463344]), 3: array([-0.99908506, -0.04276728,  0.        ])}\n",
      "cos(angle):  -0.9990850612386197   sin(angle)  -0.04276728200182315\n",
      "theta dot:  -7.59463343579\n",
      "Angle:  182.45154753640466\n",
      "action:  [-0.76400507]\n",
      "state:  {1: array([ 5.93890015]), 2: array([-7.74130966]), 3: array([-0.94131697,  0.33752387,  0.        ])}\n",
      "cos(angle):  -0.9413169710342637   sin(angle)  0.3375238658863684\n",
      "theta dot:  -7.74130965756\n",
      "Angle:  160.27353899753075\n",
      "action:  [-0.59469851]\n",
      "state:  {1: array([ 5.56003158]), 2: array([-7.57737153]), 3: array([-0.74972247,  0.66175238,  0.        ])}\n",
      "cos(angle):  -0.749722472832733   sin(angle)  0.6617523809776372\n",
      "theta dot:  -7.57737153496\n",
      "Angle:  138.56601932135953\n",
      "action:  [-0.34730006]\n",
      "state:  {1: array([ 5.20337396]), 2: array([-7.13315226]), 3: array([-0.47149474,  0.88186887,  0.        ])}\n",
      "cos(angle):  -0.471494743631454   sin(angle)  0.8818688716174925\n",
      "theta dot:  -7.13315225844\n",
      "Angle:  118.13109115546268\n",
      "action:  [-0.10209794]\n",
      "state:  {1: array([ 4.8790207]), 2: array([-6.4870653]), 3: array([-0.16586167,  0.98614903,  0.        ])}\n",
      "cos(angle):  -0.1658616695398904   sin(angle)  0.9861490285841386\n",
      "theta dot:  -6.48706529628\n",
      "Angle:  99.54706146787066\n",
      "action:  [ 0.03303312]\n",
      "state:  {1: array([ 4.59189577]), 2: array([-5.74249856]), 3: array([ 0.12020185,  0.99274947,  0.        ])}\n",
      "cos(angle):  0.12020185432284197   sin(angle)  0.9927494720307588\n",
      "theta dot:  -5.74249855627\n",
      "Angle:  83.09605338078188\n",
      "action:  [ 0.00534603]\n",
      "state:  {1: array([ 4.34203904]), 2: array([-4.99713455]), 3: array([ 0.36194166,  0.93220075,  0.        ])}\n",
      "cos(angle):  0.36194166455687127   sin(angle)  0.9322007463297813\n",
      "theta dot:  -4.9971345474\n",
      "Angle:  68.780350895874\n",
      "action:  [-0.14370147]\n",
      "state:  {1: array([ 4.12606208]), 2: array([-4.31953921]), 3: array([ 0.55330515,  0.83297864,  0.        ])}\n",
      "cos(angle):  0.553305145287905   sin(angle)  0.8329786409013921\n",
      "theta dot:  -4.31953920748\n",
      "Angle:  56.40581153144808\n",
      "action:  [-0.30666581]\n",
      "state:  {1: array([ 3.93902183]), 2: array([-3.7408051]), 3: array([ 0.6985486 ,  0.71556261,  0.        ])}\n",
      "cos(angle):  0.698548601501168   sin(angle)  0.715562611754389\n",
      "theta dot:  -3.74080509873\n",
      "Angle:  45.68921938460102\n",
      "action:  [-0.36875337]\n",
      "state:  {1: array([ 3.77604952]), 2: array([-3.25944615]), 3: array([ 0.80539375,  0.59274017,  0.        ])}\n",
      "cos(angle):  0.8053937511756395   sin(angle)  0.592740166993289\n",
      "theta dot:  -3.25944614554\n",
      "Angle:  36.35161583551246\n",
      "action:  [-0.26339404]\n",
      "state:  {1: array([ 3.63332952]), 2: array([-2.85440013]), 3: array([ 0.88151412,  0.47215767,  0.        ])}\n",
      "cos(angle):  0.8815141159007861   sin(angle)  0.4721576680174277\n",
      "theta dot:  -2.85440012608\n",
      "Angle:  28.174380944148993\n",
      "action:  [-0.00315389]\n",
      "state:  {1: array([ 3.50829177]), 2: array([-2.50075496]), 3: array([ 0.93351592,  0.35853596,  0.        ])}\n",
      "cos(angle):  0.9335159154558708   sin(angle)  0.35853596136313515\n",
      "theta dot:  -2.50075495849\n",
      "Angle:  21.010262461073523\n",
      "action:  [ 0.3298017]\n",
      "state:  {1: array([ 3.39917263]), 2: array([-2.18238273]), 3: array([ 0.96700929,  0.25474112,  0.        ])}\n",
      "cos(angle):  0.9670092884103819   sin(angle)  0.254741115896211\n",
      "theta dot:  -2.18238273289\n",
      "Angle:  14.758211087244433\n",
      "action:  [ 0.63183501]\n",
      "state:  {1: array([ 3.30434505]), 2: array([-1.89655164]), 3: array([ 0.98678504,  0.16203484,  0.        ])}\n",
      "cos(angle):  0.9867850377929906   sin(angle)  0.1620348394262357\n",
      "theta dot:  -1.8965516446\n",
      "Angle:  9.325003549230019\n",
      "action:  [ 0.8349791]\n",
      "state:  {1: array([ 3.22185612]), 2: array([-1.64977865]), 3: array([ 0.99678062,  0.08017731,  0.        ])}\n",
      "cos(angle):  0.9967806171614525   sin(angle)  0.08017731132455098\n",
      "theta dot:  -1.64977865032\n",
      "Angle:  4.598746911569322\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -0.851402037412  self.max:  0.875992110438\n",
      "state:  {1: array([ 1.28855844]), 2: array([ 0.99850649]), 3: array([-0.27850569, -0.96043458,  0.        ])}\n",
      "cos(angle):  -0.27850568601456865   sin(angle)  -0.9604345802070824\n",
      "theta dot:  0.998506487886\n",
      "Angle:  253.82920866170315\n",
      "action:  [-0.36033797]\n",
      "state:  {1: array([ 1.29976493]), 2: array([ 0.22412986]), 3: array([-0.26772532, -0.96349528,  0.        ])}\n",
      "cos(angle):  -0.2677253200740566   sin(angle)  -0.9634952791743424\n",
      "theta dot:  0.224129857256\n",
      "Angle:  254.47129190441385\n",
      "action:  [-0.30606442]\n",
      "state:  {1: array([ 1.27254487]), 2: array([-0.54440127]), 3: array([-0.29384931, -0.95585176,  0.        ])}\n",
      "cos(angle):  -0.29384930702248213   sin(angle)  -0.9558517587797843\n",
      "theta dot:  -0.544401265725\n",
      "Angle:  252.9117008070357\n",
      "action:  [-0.23401466]\n",
      "state:  {1: array([ 1.20772526]), 2: array([-1.29639228]), 3: array([-0.35514677, -0.93481055,  0.        ])}\n",
      "cos(angle):  -0.3551467715672422   sin(angle)  -0.9348105533450962\n",
      "theta dot:  -1.2963922844\n",
      "Angle:  249.19781916723696\n",
      "action:  [-0.17257526]\n",
      "state:  {1: array([ 1.10655593]), 2: array([-2.02338649]), 3: array([-0.44774372, -0.89416193,  0.        ])}\n",
      "cos(angle):  -0.4477437196924942   sin(angle)  -0.8941619324685709\n",
      "theta dot:  -2.0233864882\n",
      "Angle:  243.40125741724384\n",
      "action:  [-0.11764313]\n",
      "state:  {1: array([ 0.97097321]), 2: array([-2.71165441]), 3: array([-0.56449647, -0.82543548,  0.        ])}\n",
      "cos(angle):  -0.5644964743350815   sin(angle)  -0.8254354792854877\n",
      "theta dot:  -2.71165440715\n",
      "Angle:  235.6329579315189\n",
      "action:  [-0.06504673]\n",
      "state:  {1: array([ 0.80394881]), 2: array([-3.34048803]), 3: array([-0.69386858, -0.72010165,  0.        ])}\n",
      "cos(angle):  -0.6938685809311227   sin(angle)  -0.7201016542104525\n",
      "theta dot:  -3.3404880268\n",
      "Angle:  226.06318703733228\n",
      "action:  [-0.04407831]\n",
      "state:  {1: array([ 0.60959001]), 2: array([-3.88717601]), 3: array([-0.81988282, -0.57253137,  0.        ])}\n",
      "cos(angle):  -0.8198828185011958   sin(angle)  -0.5725313650155205\n",
      "theta dot:  -3.8871760137\n",
      "Angle:  214.92727408746686\n",
      "action:  [-0.08846468]\n",
      "state:  {1: array([ 0.3930978]), 2: array([-4.32984424]), 3: array([-0.92372688, -0.38305177,  0.        ])}\n",
      "cos(angle):  -0.9237268767036735   sin(angle)  -0.38305176837508054\n",
      "theta dot:  -4.32984424009\n",
      "Angle:  202.52321304824144\n",
      "action:  [-0.18087057]\n",
      "state:  {1: array([ 0.16088462]), 2: array([-4.64426365]), 3: array([-0.98708596, -0.16019146,  0.        ])}\n",
      "cos(angle):  -0.9870859616789258   sin(angle)  -0.1601914612468164\n",
      "theta dot:  -4.6442636525\n",
      "Angle:  189.21840884893794\n",
      "action:  [-0.25149782]\n",
      "state:  {1: array([ 6.20396333]), 2: array([-4.80213192]), 3: array([-0.99686358,  0.07913914,  0.        ])}\n",
      "cos(angle):  -0.9968635798316131   sin(angle)  0.07913913826483729\n",
      "theta dot:  -4.80213192202\n",
      "Angle:  175.4605045944519\n",
      "action:  [-0.23493972]\n",
      "state:  {1: array([ 5.9650624]), 2: array([-4.77801853]), 3: array([-0.94982422,  0.3127842 ,  0.        ])}\n",
      "cos(angle):  -0.949824216312297   sin(angle)  0.31278420373594773\n",
      "theta dot:  -4.77801852635\n",
      "Angle:  161.77252180316043\n",
      "action:  [-0.11802237]\n",
      "state:  {1: array([ 5.73700571]), 2: array([-4.56113373]), 3: array([-0.85451517,  0.51942643,  0.        ])}\n",
      "cos(angle):  -0.8545151737221129   sin(angle)  0.5194264316326876\n",
      "theta dot:  -4.56113372924\n",
      "Angle:  148.70586673467528\n",
      "action:  [ 0.0637714]\n",
      "state:  {1: array([ 5.5289058]), 2: array([-4.1619982]), 3: array([-0.7287651 ,  0.68476377,  0.        ])}\n",
      "cos(angle):  -0.7287651028046561   sin(angle)  0.6847637730882957\n",
      "theta dot:  -4.16199819536\n",
      "Angle:  136.78264806958748\n",
      "action:  [ 0.25691577]\n",
      "state:  {1: array([ 5.3484114]), 2: array([-3.609888]), 3: array([-0.59400033,  0.8044648 ,  0.        ])}\n",
      "cos(angle):  -0.594000328913376   sin(angle)  0.8044647967753475\n",
      "theta dot:  -3.60988800071\n",
      "Angle:  126.44110490484115\n",
      "action:  [ 0.41544512]\n",
      "state:  {1: array([ 5.20120027]), 2: array([-2.94422263]), 3: array([-0.46957672,  0.88289167,  0.        ])}\n",
      "cos(angle):  -0.46957672014500784   sin(angle)  0.8828916716663812\n",
      "theta dot:  -2.94422263478\n",
      "Angle:  118.00654808252591\n",
      "action:  [ 0.52303037]\n",
      "state:  {1: array([ 5.09102031]), 2: array([-2.20359932]), 3: array([-0.36964908,  0.92917143,  0.        ])}\n",
      "cos(angle):  -0.3696490834020908   sin(angle)  0.9291714347417241\n",
      "theta dot:  -2.20359932487\n",
      "Angle:  111.69371579202675\n",
      "action:  [ 0.60009531]\n",
      "state:  {1: array([ 5.02018498]), 2: array([-1.41670645]), 3: array([-0.30295896,  0.9530036 ,  0.        ])}\n",
      "cos(angle):  -0.3029589573837045   sin(angle)  0.9530036044742847\n",
      "theta dot:  -1.41670645278\n",
      "Angle:  107.63516025504236\n",
      "action:  [ 0.66928984]\n",
      "state:  {1: array([ 4.99010697]), 2: array([-0.60156027]), 3: array([-0.27416179,  0.96168358,  0.        ])}\n",
      "cos(angle):  -0.27416179261583207   sin(angle)  0.9616835817823207\n",
      "theta dot:  -0.601560273803\n",
      "Angle:  105.91182104437576\n",
      "action:  [ 0.70612516]\n",
      "state:  {1: array([ 5.00138803]), 2: array([ 0.22562119]), 3: array([-0.28499293,  0.95852962,  0.        ])}\n",
      "cos(angle):  -0.2849929270270178   sin(angle)  0.9585296195447343\n",
      "theta dot:  0.225621187006\n",
      "Angle:  106.55817662212314\n",
      "action:  [ 0.64610163]\n",
      "state:  {1: array([ 5.05345971]), 2: array([ 1.04143365]), 3: array([-0.33449634,  0.94239705,  0.        ])}\n",
      "cos(angle):  -0.3344963379108834   sin(angle)  0.9423970500400604\n",
      "theta dot:  1.04143364658\n",
      "Angle:  109.54165727504846\n",
      "action:  [ 0.45725343]\n",
      "state:  {1: array([ 5.14430068]), 2: array([ 1.81681945]), 3: array([-0.41860771,  0.90816716,  0.        ])}\n",
      "cos(angle):  -0.41860771300726707   sin(angle)  0.9081671556551831\n",
      "theta dot:  1.81681944815\n",
      "Angle:  114.74644942980302\n",
      "action:  [ 0.19558573]\n",
      "state:  {1: array([ 5.27066482]), 2: array([ 2.52728267]), 3: array([-0.5297246,  0.8481697,  0.       ])}\n",
      "cos(angle):  -0.5297246037566712   sin(angle)  0.8481697024622123\n",
      "theta dot:  2.52728267491\n",
      "Angle:  121.98656404471683\n",
      "action:  [-0.02926611]\n",
      "state:  {1: array([ 5.42861582]), 2: array([ 3.15902003]), 3: array([-0.6565433 ,  0.75428834,  0.        ])}\n",
      "cos(angle):  -0.6565433012030117   sin(angle)  0.7542883358805512\n",
      "theta dot:  3.15902003457\n",
      "Angle:  131.0364686510111\n",
      "action:  [-0.14983465]\n",
      "state:  {1: array([ 5.61372887]), 2: array([ 3.70226109]), 3: array([-0.7841591 ,  0.62055984,  0.        ])}\n",
      "cos(angle):  -0.7841590976045122   sin(angle)  0.6205598356678244\n",
      "theta dot:  3.70226108951\n",
      "Angle:  141.64264060337837\n",
      "action:  [-0.19537601]\n",
      "state:  {1: array([ 5.8206476]), 2: array([ 4.13837456]), 3: array([-0.894923  ,  0.44622059,  0.        ])}\n",
      "cos(angle):  -0.8949230042774982   sin(angle)  0.4462205916527575\n",
      "theta dot:  4.13837456466\n",
      "Angle:  153.4981827099199\n",
      "action:  [-0.24528655]\n",
      "state:  {1: array([ 6.04245995]), 2: array([ 4.43624703]), 3: array([-0.9711653 ,  0.23840713,  0.        ])}\n",
      "cos(angle):  -0.9711653009378776   sin(angle)  0.2384071271045429\n",
      "theta dot:  4.4362470265\n",
      "Angle:  166.20706456581365\n",
      "action:  [-0.33568804]\n",
      "state:  {1: array([ 6.27069491]), 2: array([ 4.56469917]), 3: array([-0.999922  ,  0.01249007,  0.        ])}\n",
      "cos(angle):  -0.9999219960292254   sin(angle)  0.012490070333264481\n",
      "theta dot:  4.56469916532\n",
      "Angle:  179.28393383239313\n",
      "action:  [-0.42664869]\n",
      "state:  {1: array([ 0.21301308]), 2: array([ 4.51006941]), 3: array([-0.97739837, -0.21140583,  0.        ])}\n",
      "cos(angle):  -0.9773983706422293   sin(angle)  -0.21140583025525866\n",
      "theta dot:  4.51006941499\n",
      "Angle:  192.2051425930892\n",
      "action:  [-0.45973552]\n",
      "state:  {1: array([ 0.42714081]), 2: array([ 4.28255471]), 3: array([-0.91015394, -0.4142702 ,  0.        ])}\n",
      "cos(angle):  -0.9101539449493391   sin(angle)  -0.4142701974474574\n",
      "theta dot:  4.28255471364\n",
      "Angle:  204.47372943507816\n",
      "action:  [-0.42198139]\n",
      "state:  {1: array([ 0.62256855]), 2: array([ 3.90855486]), 3: array([-0.81238335, -0.58312373,  0.        ])}\n",
      "cos(angle):  -0.8123833532424953   sin(angle)  -0.58312373247406\n",
      "theta dot:  3.90855485709\n",
      "Angle:  215.67088811658112\n",
      "action:  [-0.34030676]\n",
      "state:  {1: array([ 0.79357686]), 2: array([ 3.42016604]), 3: array([-0.70129999, -0.71286628,  0.        ])}\n",
      "cos(angle):  -0.7012999870094436   sin(angle)  -0.7128662765347749\n",
      "theta dot:  3.42016604299\n",
      "Angle:  225.46891917937708\n",
      "action:  [-0.24245727]\n",
      "state:  {1: array([ 0.93603424]), 2: array([ 2.84914775]), 3: array([-0.59298596, -0.8052128 ,  0.        ])}\n",
      "cos(angle):  -0.5929859574866764   sin(angle)  -0.8052128005835536\n",
      "theta dot:  2.84914774524\n",
      "Angle:  233.63110714321488\n",
      "action:  [-0.14527274]\n",
      "state:  {1: array([ 1.04720661]), 2: array([ 2.22344723]), 3: array([-0.49999216, -0.86602993,  0.        ])}\n",
      "cos(angle):  -0.4999921589101882   sin(angle)  -0.8660299307924231\n",
      "theta dot:  2.22344723375\n",
      "Angle:  240.00079937130064\n",
      "action:  [-0.05704686]\n",
      "state:  {1: array([ 1.12547499]), 2: array([ 1.56536776]), 3: array([-0.4307479, -0.9024723,  0.       ])}\n",
      "cos(angle):  -0.4307478973014563   sin(angle)  -0.9024722981733977\n",
      "theta dot:  1.56536775613\n",
      "Angle:  244.48523717534368\n",
      "action:  [ 0.02618074]\n",
      "state:  {1: array([ 1.17009703]), 2: array([ 0.89244064]), 3: array([-0.39006235, -0.92078845,  0.        ])}\n",
      "cos(angle):  -0.3900623463927076   sin(angle)  -0.9207884479763065\n",
      "theta dot:  0.892440643682\n",
      "Angle:  247.041885314234\n",
      "action:  [ 0.10902177]\n",
      "state:  {1: array([ 1.18100715]), 2: array([ 0.21820257]), 3: array([-0.37999341, -0.92498919,  0.        ])}\n",
      "cos(angle):  -0.3799934107516145   sin(angle)  -0.9249891933343626\n",
      "theta dot:  0.218202573037\n",
      "Angle:  247.66698817816606\n",
      "action:  [ 0.17412845]\n",
      "state:  {1: array([ 1.15853615]), 2: array([-0.44942006]), 3: array([-0.40068116, -0.91621755,  0.        ])}\n",
      "cos(angle):  -0.4006811622205805   sin(angle)  -0.9162175539911712\n",
      "theta dot:  -0.449420055087\n",
      "Angle:  246.37949756962718\n",
      "action:  [ 0.18596201]\n",
      "state:  {1: array([ 1.10310171]), 2: array([-1.10868892]), 3: array([-0.45082968, -0.89260999,  0.        ])}\n",
      "cos(angle):  -0.45082968143840574   sin(angle)  -0.8926099922889871\n",
      "theta dot:  -1.10868891965\n",
      "Angle:  243.20334520241647\n",
      "action:  [ 0.12338032]\n",
      "state:  {1: array([ 1.01511974]), 2: array([-1.75963937]), 3: array([-0.52751821, -0.84954372,  0.        ])}\n",
      "cos(angle):  -0.5275182136322476   sin(angle)  -0.8495437212328995\n",
      "theta dot:  -1.75963936658\n",
      "Angle:  238.1623615319254\n",
      "action:  [ 0.00711714]\n",
      "state:  {1: array([ 0.89533326]), 2: array([-2.39572959]), 3: array([-0.62525877, -0.7804175 ,  0.        ])}\n",
      "cos(angle):  -0.6252587711148211   sin(angle)  -0.7804174966926253\n",
      "theta dot:  -2.39572958675\n",
      "Angle:  231.299117872417\n",
      "action:  [-0.10163894]\n",
      "state:  {1: array([ 0.74551883]), 2: array([-2.99628855]), 3: array([-0.73473605, -0.6783531 ,  0.        ])}\n",
      "cos(angle):  -0.7347360513409151   sin(angle)  -0.6783531048502396\n",
      "theta dot:  -2.99628855032\n",
      "Angle:  222.7154035380271\n",
      "action:  [-0.13843083]\n",
      "state:  {1: array([ 0.56922793]), 2: array([-3.525818]), 3: array([-0.84231736, -0.53898188,  0.        ])}\n",
      "cos(angle):  -0.8423173579935214   sin(angle)  -0.5389818813492843\n",
      "theta dot:  -3.52581800318\n",
      "Angle:  212.61470261218665\n",
      "action:  [-0.0868835]\n",
      "state:  {1: array([ 0.37207358]), 2: array([-3.94308694]), 3: array([-0.9315755 , -0.36354791,  0.        ])}\n",
      "cos(angle):  -0.93157550218779   sin(angle)  -0.36354791118031043\n",
      "theta dot:  -3.94308693924\n",
      "Angle:  201.3186170337622\n",
      "action:  [ 0.00296238]\n",
      "state:  {1: array([ 0.16130841]), 2: array([-4.21530352]), 3: array([-0.98701799, -0.16060977,  0.        ])}\n",
      "cos(angle):  -0.9870179853261694   sin(angle)  -0.1606097650912603\n",
      "theta dot:  -4.21530351512\n",
      "Angle:  189.24269023338394\n",
      "action:  [ 0.05248539]\n",
      "state:  {1: array([ 6.22809931]), 2: array([-4.32788803]), 3: array([-0.99848315,  0.05505814,  0.        ])}\n",
      "cos(angle):  -0.9984831502378267   sin(angle)  0.055058139190727044\n",
      "theta dot:  -4.32788803061\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  176.84339147377887\n",
      "action:  [ 0.01468791]\n",
      "state:  {1: array([ 6.01387975]), 2: array([-4.28439124]), 3: array([-0.96395589,  0.26606209,  0.        ])}\n",
      "cos(angle):  -0.9639558939087225   sin(angle)  0.26606208786415947\n",
      "theta dot:  -4.28439123933\n",
      "Angle:  164.569543385565\n",
      "action:  [-0.09495017]\n",
      "state:  {1: array([ 5.80892539]), 2: array([-4.0990872]), 3: array([-0.88963095,  0.45668017,  0.        ])}\n",
      "cos(angle):  -0.8896309467825003   sin(angle)  0.45668017093680785\n",
      "theta dot:  -4.0990871985\n",
      "Angle:  152.8265510292673\n",
      "action:  [-0.2134723]\n",
      "state:  {1: array([ 5.6194955]), 2: array([-3.78859792]), 3: array([-0.78772457,  0.61602759,  0.        ])}\n",
      "cos(angle):  -0.7877245729292409   sin(angle)  0.6160275945146004\n",
      "theta dot:  -3.78859791517\n",
      "Angle:  141.97304286893393\n",
      "action:  [-0.27192633]\n",
      "state:  {1: array([ 5.45112719]), 2: array([-3.36736617]), 3: array([-0.67335558,  0.73931878,  0.        ])}\n",
      "cos(angle):  -0.6733555801367576   sin(angle)  0.7393187828661535\n",
      "theta dot:  -3.36736616858\n",
      "Angle:  132.32627195052964\n",
      "action:  [-0.23329547]\n",
      "state:  {1: array([ 5.30873362]), 2: array([-2.8478714]), 3: array([-0.56162181,  0.82739407,  0.        ])}\n",
      "cos(angle):  -0.5616218059266239   sin(angle)  0.827394070022089\n",
      "theta dot:  -2.84787140168\n",
      "Angle:  124.16774043312125\n",
      "action:  [-0.10705848]\n",
      "state:  {1: array([ 5.19656439]), 2: array([-2.24338462]), 3: array([-0.4654787 ,  0.88505908,  0.        ])}\n",
      "cos(angle):  -0.46547870263815144   sin(angle)  0.8850590812992675\n",
      "theta dot:  -2.2433846213\n",
      "Angle:  117.7409319305545\n",
      "action:  [ 0.06160339]\n",
      "state:  {1: array([ 5.1180469]), 2: array([-1.5703498]), 3: array([-0.39462337,  0.91884297,  0.        ])}\n",
      "cos(angle):  -0.39462336640144713   sin(angle)  0.918842967372548\n",
      "theta dot:  -1.57034980123\n",
      "Angle:  113.24222165202542\n",
      "action:  [ 0.21614717]\n",
      "state:  {1: array([ 5.07560712]), 2: array([-0.8487955]), 3: array([-0.35528425,  0.93475831,  0.        ])}\n",
      "cos(angle):  -0.3552842510164673   sin(angle)  0.9347583115328089\n",
      "theta dot:  -0.84879550028\n",
      "Angle:  110.81060734640948\n",
      "action:  [ 0.30589352]\n",
      "state:  {1: array([ 5.07051498]), 2: array([-0.10184274]), 3: array([-0.35051975,  0.93655534,  0.        ])}\n",
      "cos(angle):  -0.3505197480683532   sin(angle)  0.9365553407108937\n",
      "theta dot:  -0.101842738061\n",
      "Angle:  110.5188500754183\n",
      "action:  [ 0.29113968]\n",
      "state:  {1: array([ 5.10272722]), 2: array([ 0.64424472]), 3: array([-0.38050123,  0.92478041,  0.        ])}\n",
      "cos(angle):  -0.3805012338389764   sin(angle)  0.9247804123396086\n",
      "theta dot:  0.644244719572\n",
      "Angle:  112.36447092980683\n",
      "action:  [ 0.15997105]\n",
      "state:  {1: array([ 5.1708185]), 2: array([ 1.36182569]), 3: array([-0.44254033,  0.8967486 ,  0.        ])}\n",
      "cos(angle):  -0.44254033064687964   sin(angle)  0.8967486023133521\n",
      "theta dot:  1.36182568597\n",
      "Angle:  116.2658050187307\n",
      "action:  [-0.04241494]\n",
      "state:  {1: array([ 5.27221975]), 2: array([ 2.0280249]), 3: array([-0.53104281,  0.84734499,  0.        ])}\n",
      "cos(angle):  -0.5310428078883604   sin(angle)  0.8473449924263705\n",
      "theta dot:  2.0280248966\n",
      "Angle:  122.07565479890116\n",
      "action:  [-0.227508]\n",
      "state:  {1: array([ 5.40369012]), 2: array([ 2.62940744]), 3: array([-0.63754014,  0.77041714,  0.        ])}\n",
      "cos(angle):  -0.6375401424219351   sin(angle)  0.7704171381794531\n",
      "theta dot:  2.62940744086\n",
      "Angle:  129.60833463331005\n",
      "action:  [-0.33378622]\n",
      "state:  {1: array([ 5.56154774]), 2: array([ 3.15715236]), 3: array([-0.75072493,  0.66061492,  0.        ])}\n",
      "cos(angle):  -0.7507249341252602   sin(angle)  0.6606149205722074\n",
      "theta dot:  3.15715236223\n",
      "Angle:  138.652888764978\n",
      "action:  [-0.36659385]\n",
      "state:  {1: array([ 5.74142896]), 2: array([ 3.59762447]), 3: array([-0.85680436,  0.51564163,  0.        ])}\n",
      "cos(angle):  -0.8568043589502002   sin(angle)  0.5156416298980683\n",
      "theta dot:  3.59762447495\n",
      "Angle:  148.95929959848468\n",
      "action:  [-0.35629344]\n",
      "state:  {1: array([ 5.93797455]), 2: array([ 3.93091168]), 3: array([-0.94100415,  0.33839501,  0.        ])}\n",
      "cos(angle):  -0.9410041532039993   sin(angle)  0.3383950112705921\n",
      "theta dot:  3.93091168189\n",
      "Angle:  160.2205057154472\n",
      "action:  [-0.30617567]\n",
      "state:  {1: array([ 6.14491363]), 2: array([ 4.13878159]), 3: array([-0.99045569,  0.1378315 ,  0.        ])}\n",
      "cos(angle):  -0.9904556921795404   sin(angle)  0.13783149795727875\n",
      "theta dot:  4.13878159016\n",
      "Angle:  172.07721386143044\n",
      "action:  [-0.20594854]\n",
      "state:  {1: array([ 0.07229147]), 2: array([ 4.21126293]), 3: array([-0.99738811, -0.07222852,  0.        ])}\n",
      "cos(angle):  -0.9973881097846478   sin(angle)  -0.07222851556142816\n",
      "theta dot:  4.21126293293\n",
      "Angle:  184.14240710819655\n",
      "action:  [-0.08082525]\n",
      "state:  {1: array([ 0.27953985]), 2: array([ 4.14496776]), 3: array([-0.9611825 , -0.27591339,  0.        ])}\n",
      "cos(angle):  -0.9611825006132135   sin(angle)  -0.27591339314163366\n",
      "theta dot:  4.14496775937\n",
      "Angle:  196.0168372820897\n",
      "action:  [-0.00580564]\n",
      "state:  {1: array([ 0.47639795]), 2: array([ 3.93716187]), 3: array([-0.88865252, -0.45858118,  0.        ])}\n",
      "cos(angle):  -0.8886525179461793   sin(angle)  -0.4585811840316996\n",
      "theta dot:  3.93716186858\n",
      "Angle:  207.29594882309482\n",
      "action:  [-0.05789491]\n",
      "state:  {1: array([ 0.65562503]), 2: array([ 3.58454174]), 3: array([-0.79266703, -0.60965481,  0.        ])}\n",
      "cos(angle):  -0.7926670278263583   sin(angle)  -0.6096548064248548\n",
      "theta dot:  3.58454174403\n",
      "Angle:  217.5648804809285\n",
      "action:  [-0.24481554]\n",
      "state:  {1: array([ 0.81015395]), 2: array([ 3.09057831]), 3: array([-0.68938692, -0.72439331,  0.        ])}\n",
      "cos(angle):  -0.6893869210397235   sin(angle)  -0.72439331381465\n",
      "theta dot:  3.09057830757\n",
      "Angle:  226.4187144407455\n",
      "action:  [-0.48511346]\n",
      "state:  {1: array([ 0.93387976]), 2: array([ 2.4745163]), 3: array([-0.59471939, -0.80393336,  0.        ])}\n",
      "cos(angle):  -0.5947193937791175   sin(angle)  -0.8039333571030617\n",
      "theta dot:  2.47451630383\n",
      "Angle:  233.50766489097455\n",
      "action:  [-0.6687378]\n",
      "state:  {1: array([ 1.02244255]), 2: array([ 1.77125562]), 3: array([-0.52128308, -0.85338382,  0.        ])}\n",
      "cos(angle):  -0.5212830794004296   sin(angle)  -0.8533838240386359\n",
      "theta dot:  1.77125561591\n",
      "Angle:  238.58192658667497\n",
      "action:  [-0.73108579]\n",
      "state:  {1: array([ 1.07352029]), 2: array([ 1.02155488]), 3: array([-0.47703326, -0.87888524,  0.        ])}\n",
      "cos(angle):  -0.4770332606370213   sin(angle)  -0.878885241795544\n",
      "theta dot:  1.0215548798\n",
      "Angle:  241.50845890084508\n",
      "action:  [-0.66431249]\n",
      "state:  {1: array([ 1.08665749]), 2: array([ 0.26274408]), 3: array([-0.46544633, -0.8850761 ,  0.        ])}\n",
      "cos(angle):  -0.4654463341715589   sin(angle)  -0.8850761040759475\n",
      "theta dot:  0.262744075359\n",
      "Angle:  242.26116347120168\n",
      "action:  [-0.48491912]\n",
      "state:  {1: array([ 1.06296745]), 2: array([-0.47380087]), 3: array([-0.48628126, -0.87380234,  0.        ])}\n",
      "cos(angle):  -0.4862812620840226   sin(angle)  -0.8738023427217222\n",
      "theta dot:  -0.473800870768\n",
      "Angle:  240.90382713401362\n",
      "action:  [-0.21948701]\n",
      "state:  {1: array([ 1.00486367]), 2: array([-1.16207568]), 3: array([-0.5362033 , -0.84408887,  0.        ])}\n",
      "cos(angle):  -0.5362032978095521   sin(angle)  -0.844088871753538\n",
      "theta dot:  -1.16207567918\n",
      "Angle:  237.57473332427884\n",
      "action:  [ 0.08071394]\n",
      "state:  {1: array([ 0.9157119]), 2: array([-1.78303524]), 3: array([-0.60922619, -0.7929965 ,  0.        ])}\n",
      "cos(angle):  -0.6092261926802325   sin(angle)  -0.7929964981967753\n",
      "theta dot:  -1.78303524255\n",
      "Angle:  232.46672556297398\n",
      "action:  [ 0.33733989]\n",
      "state:  {1: array([ 0.79935282]), 2: array([-2.32718163]), 3: array([-0.69717082, -0.71690505,  0.        ])}\n",
      "cos(angle):  -0.6971708204143383   sin(angle)  -0.7169050475221934\n",
      "theta dot:  -2.32718163284\n",
      "Angle:  225.79985686689918\n",
      "action:  [ 0.48385486]\n",
      "state:  {1: array([ 0.65973871]), 2: array([-2.79228219]), 3: array([-0.7901524 , -0.61291042,  0.        ])}\n",
      "cos(angle):  -0.7901524042171257   sin(angle)  -0.6129104160559649\n",
      "theta dot:  -2.79228218949\n",
      "Angle:  217.80057633932887\n",
      "action:  [ 0.51542268]\n",
      "state:  {1: array([ 0.50100613]), 2: array([-3.1746516]), 3: array([-0.87709975, -0.48030826,  0.        ])}\n",
      "cos(angle):  -0.8770997520942827   sin(angle)  -0.48030826026224843\n",
      "theta dot:  -3.17465159964\n",
      "Angle:  208.70589070247877\n",
      "action:  [ 0.48767325]\n",
      "state:  {1: array([ 0.32791954]), 2: array([-3.46173181]), 3: array([-0.94671445, -0.32207413,  0.        ])}\n",
      "cos(angle):  -0.9467144534713628   sin(angle)  -0.32207412747443515\n",
      "theta dot:  -3.46173180667\n",
      "Angle:  198.78878277657296\n",
      "action:  [ 0.45498305]\n",
      "state:  {1: array([ 0.14616755]), 2: array([-3.63503995]), 3: array([-0.98933653, -0.14564762,  0.        ])}\n",
      "cos(angle):  -0.9893365300622321   sin(angle)  -0.14564762368271633\n",
      "theta dot:  -3.63503994543\n",
      "Angle:  188.37518476637055\n",
      "action:  [ 0.41848631]\n",
      "state:  {1: array([ 6.24527772]), 2: array([-3.68150272]), 3: array([-0.99928159,  0.03789851,  0.        ])}\n",
      "cos(angle):  -0.9992815933148018   sin(angle)  0.0378985126651576\n",
      "theta dot:  -3.68150271669\n",
      "Angle:  177.8276391979134\n",
      "action:  [ 0.34185955]\n",
      "state:  {1: array([ 6.06518772]), 2: array([-3.6017999]), 3: array([-0.97633248,  0.21627504,  0.        ])}\n",
      "cos(angle):  -0.9763324786389067   sin(angle)  0.2162750359029187\n",
      "theta dot:  -3.60179989899\n",
      "Angle:  167.50926668361706\n",
      "action:  [ 0.20736413]\n",
      "state:  {1: array([ 5.89476327]), 2: array([-3.408489]), 3: array([-0.92550783,  0.37872847,  0.        ])}\n",
      "cos(angle):  -0.9255078318006215   sin(angle)  0.37872846905891894\n",
      "theta dot:  -3.40848900218\n",
      "Angle:  157.74468780037284\n",
      "action:  [ 0.04514943]\n",
      "state:  {1: array([ 5.73887976]), 2: array([-3.11767024]), 3: array([-0.8554871 ,  0.51782412,  0.        ])}\n",
      "cos(angle):  -0.8554871012202787   sin(angle)  0.5178241203784587\n",
      "theta dot:  -3.11767023582\n",
      "Angle:  148.81324136467322\n",
      "action:  [-0.08635381]\n",
      "state:  {1: array([ 5.601767]), 2: array([-2.74225522]), 3: array([-0.77668011,  0.62989523,  0.        ])}\n",
      "cos(angle):  -0.7766801144825021   sin(angle)  0.6298952291988307\n",
      "theta dot:  -2.74225521706\n",
      "Angle:  140.9572772210679\n",
      "action:  [-0.1387441]\n",
      "state:  {1: array([ 5.48723473]), 2: array([-2.29064541]), 3: array([-0.69960587,  0.71452896,  0.        ])}\n",
      "cos(angle):  -0.6996058660057536   sin(angle)  0.7145289583007393\n",
      "theta dot:  -2.29064541009\n",
      "Angle:  134.3950768483969\n",
      "action:  [-0.10268521]\n",
      "state:  {1: array([ 5.39872715]), 2: array([-1.77015147]), 3: array([-0.63370875,  0.77357173,  0.        ])}\n",
      "cos(angle):  -0.6337087522884205   sin(angle)  0.7735717272968637\n",
      "theta dot:  -1.77015147303\n",
      "Angle:  129.3239782816534\n",
      "action:  [-0.01956823]\n",
      "state:  {1: array([ 5.33908176]), 2: array([-1.19290791]), 3: array([-0.58646921,  0.80997152,  0.        ])}\n",
      "cos(angle):  -0.5864692149684938   sin(angle)  0.8099715179524762\n",
      "theta dot:  -1.19290791176\n",
      "Angle:  125.90655683849457\n",
      "action:  [ 0.04168944]\n",
      "state:  {1: array([ 5.31012297]), 2: array([-0.57917586]), 3: array([-0.5627708 ,  0.82661298,  0.        ])}\n",
      "cos(angle):  -0.5627708030478349   sin(angle)  0.8266129827415578\n",
      "theta dot:  -0.579175856557\n",
      "Angle:  124.24734410962708\n",
      "action:  [ 0.04599546]\n",
      "state:  {1: array([ 5.31250713]), 2: array([ 0.0476832]), 3: array([-0.56473998,  0.8252689 ,  0.        ])}\n",
      "cos(angle):  -0.5647399793326525   sin(angle)  0.825268899052518\n",
      "theta dot:  0.0476831999612\n",
      "Angle:  124.38394609576511\n",
      "action:  [ 0.03392225]\n",
      "state:  {1: array([ 5.34609329]), 2: array([ 0.67172321]), 3: array([-0.59213389,  0.8058396 ,  0.        ])}\n",
      "cos(angle):  -0.5921338904220135   sin(angle)  0.8058395968266209\n",
      "theta dot:  0.67172321188\n",
      "Angle:  126.30828684790299\n",
      "action:  [ 0.08950421]\n",
      "state:  {1: array([ 5.41056971]), 2: array([ 1.28952854]), 3: array([-0.64282517,  0.76601292,  0.        ])}\n",
      "cos(angle):  -0.6428251700449009   sin(angle)  0.7660129246669042\n",
      "theta dot:  1.28952854096\n",
      "Angle:  130.00250535715747\n",
      "action:  [ 0.25588106]\n",
      "state:  {1: array([ 5.50569073]), 2: array([ 1.90242039]), 3: array([-0.71267332,  0.70149607,  0.        ])}\n",
      "cos(angle):  -0.7126733196510228   sin(angle)  0.7014960723037522\n",
      "theta dot:  1.90242039409\n",
      "Angle:  135.4525255846969\n",
      "action:  [ 0.47579444]\n",
      "state:  {1: array([ 5.63068631]), 2: array([ 2.49991161]), 3: array([-0.79456896,  0.60717392,  0.        ])}\n",
      "cos(angle):  -0.7945689577937288   sin(angle)  0.6071739217971961\n",
      "theta dot:  2.49991161499\n",
      "Angle:  142.61422807225847\n",
      "action:  [ 0.61723087]\n",
      "state:  {1: array([ 5.78308015]), 2: array([ 3.04787669]), 3: array([-0.87753214,  0.47951782,  0.        ])}\n",
      "cos(angle):  -0.8775321413201592   sin(angle)  0.4795178213060032\n",
      "theta dot:  3.04787668739\n",
      "Angle:  151.34573118739712\n",
      "action:  [ 0.57246788]\n",
      "state:  {1: array([ 5.95774941]), 2: array([ 3.49338524]), 3: array([-0.94751145,  0.31972183,  0.        ])}\n",
      "cos(angle):  -0.9475114507125152   sin(angle)  0.3197218334250334\n",
      "theta dot:  3.49338523604\n",
      "Angle:  161.35351929675505\n",
      "action:  [ 0.34596926]\n",
      "state:  {1: array([ 6.14700301]), 2: array([ 3.785072]), 3: array([-0.99074151,  0.13576176,  0.        ])}\n",
      "cos(angle):  -0.9907415129950244   sin(angle)  0.13576175613305067\n",
      "theta dot:  3.78507200009\n",
      "Angle:  172.19692647807506\n",
      "action:  [ 0.04862445]\n",
      "state:  {1: array([ 0.05852705]), 2: array([ 3.89418698]), 3: array([-0.99828778, -0.05849364,  0.        ])}\n",
      "cos(angle):  -0.998287780905264   sin(angle)  -0.058493644913301834\n",
      "theta dot:  3.89418698465\n",
      "Angle:  183.35376616789216\n",
      "action:  [-0.19429716]\n",
      "state:  {1: array([ 0.24958566]), 2: array([ 3.82117218]), 3: array([-0.96901485, -0.24700248,  0.        ])}\n",
      "cos(angle):  -0.9690148475758809   sin(angle)  -0.2470024801039299\n",
      "theta dot:  3.82117217695\n",
      "Angle:  194.30059249604818\n",
      "action:  [-0.328256]\n",
      "state:  {1: array([ 0.42891976]), 2: array([ 3.58668192]), 3: array([-0.90941554, -0.41588866,  0.        ])}\n",
      "cos(angle):  -0.9094155409361806   sin(angle)  -0.4158886556564799\n",
      "theta dot:  3.58668191762\n",
      "Angle:  204.57565528524782\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "3\n",
      "1\n",
      "Reset!\n",
      "min:  -0.851402037412  self.max:  0.875992110438\n",
      "state:  {1: array([ 2.33774615]), 2: array([-0.0207722]), 3: array([ 0.69394225, -0.72003066,  0.        ])}\n",
      "cos(angle):  0.6939422503682815   sin(angle)  -0.7200306612595087\n",
      "theta dot:  -0.0207722049379\n",
      "Angle:  313.94309575518537\n",
      "action:  [ 0.57668636]\n",
      "state:  {1: array([ 2.31403154]), 2: array([-0.47429225]), 3: array([ 0.67667348, -0.73628323,  0.        ])}\n",
      "cos(angle):  0.6766734813412406   sin(angle)  -0.7362832333073501\n",
      "theta dot:  -0.474292246443\n",
      "Angle:  312.5843517336715\n",
      "action:  [ 0.4624585]\n",
      "state:  {1: array([ 2.26617474]), 2: array([-0.9571359]), 3: array([ 0.64067604, -0.76781131,  0.        ])}\n",
      "cos(angle):  0.6406760386704952   sin(angle)  -0.7678113137180789\n",
      "theta dot:  -0.957135896791\n",
      "Angle:  309.8423657802988\n",
      "action:  [ 0.27681266]\n",
      "state:  {1: array([ 2.19160112]), 2: array([-1.49147248]), 3: array([ 0.58168998, -0.81341058,  0.        ])}\n",
      "cos(angle):  0.5816899763251487   sin(angle)  -0.8134105798690154\n",
      "theta dot:  -1.49147248259\n",
      "Angle:  305.5696218462159\n",
      "action:  [ 0.02890072]\n",
      "state:  {1: array([ 2.08674135]), 2: array([-2.09719531]), 3: array([ 0.49335708, -0.86982687,  0.        ])}\n",
      "cos(angle):  0.4933570799058524   sin(angle)  -0.8698268745599727\n",
      "theta dot:  -2.09719530979\n",
      "Angle:  299.56161389227134\n",
      "action:  [-0.23127284]\n",
      "state:  {1: array([ 1.94752854]), 2: array([-2.78425639]), 3: array([ 0.36788381, -0.92987177,  0.        ])}\n",
      "cos(angle):  0.36788381181694035   sin(angle)  -0.9298717658919632\n",
      "theta dot:  -2.78425639214\n",
      "Angle:  291.58532552671267\n",
      "action:  [-0.3621739]\n",
      "state:  {1: array([ 1.77072922]), 2: array([-3.5359863]), 3: array([ 0.19860356, -0.98007991,  0.        ])}\n",
      "cos(angle):  0.19860356122777054   sin(angle)  -0.9800799076951059\n",
      "theta dot:  -3.53598630218\n",
      "Angle:  281.4554946381111\n",
      "action:  [-0.42938493]\n",
      "state:  {1: array([ 1.55395652]), 2: array([-4.33545397]), 3: array([-0.01683901, -0.99985821,  0.        ])}\n",
      "cos(angle):  -0.01683900952838695   sin(angle)  -0.999858213827392\n",
      "theta dot:  -4.33545397264\n",
      "Angle:  269.03536293651683\n",
      "action:  [-0.45776372]\n",
      "state:  {1: array([ 1.29625591]), 2: array([-5.15401219]), 3: array([-0.27110459, -0.96254989,  0.        ])}\n",
      "cos(angle):  -0.2711045921138032   sin(angle)  -0.9625498948806801\n",
      "theta dot:  -5.1540121917\n",
      "Angle:  254.27024015663727\n",
      "action:  [-0.42024227]\n",
      "state:  {1: array([ 0.99930786]), 2: array([-5.93896095]), 3: array([-0.54088459, -0.84109682,  0.        ])}\n",
      "cos(angle):  -0.5408845886345663   sin(angle)  -0.8410968206916585\n",
      "theta dot:  -5.93896095266\n",
      "Angle:  237.25641007833843\n",
      "action:  [-0.46730111]\n",
      "state:  {1: array([ 0.66731393]), 2: array([-6.63987873]), 3: array([-0.78548685, -0.61887835,  0.        ])}\n",
      "cos(angle):  -0.7854868496857925   sin(angle)  -0.6188783474728206\n",
      "theta dot:  -6.63987873499\n",
      "Angle:  218.23460315990692\n",
      "action:  [-0.2767065]\n",
      "state:  {1: array([ 0.31003675]), 2: array([-7.14554347]), 3: array([-0.95232236, -0.30509364,  0.        ])}\n",
      "cos(angle):  -0.952322357155122   sin(angle)  -0.30509363818754415\n",
      "theta dot:  -7.14554347062\n",
      "Angle:  197.76417686897048\n",
      "action:  [ 0.13815414]\n",
      "state:  {1: array([ 6.22554003]), 2: array([-7.35364058]), 3: array([-0.99833897,  0.05761335,  0.        ])}\n",
      "cos(angle):  -0.9983389711731685   sin(angle)  0.05761335467493051\n",
      "theta dot:  -7.35364057816\n",
      "Angle:  176.6967558379274\n",
      "action:  [-0.14238302]\n",
      "state:  {1: array([ 5.85895063]), 2: array([-7.33178802]), 3: array([-0.91135403,  0.41162342,  0.        ])}\n",
      "cos(angle):  -0.9113540251858556   sin(angle)  0.4116234210750634\n",
      "theta dot:  -7.33178801548\n",
      "Angle:  155.6927794757671\n",
      "action:  [ 0.12118115]\n",
      "state:  {1: array([ 5.50870597]), 2: array([-7.00489328]), 3: array([-0.71478525,  0.69934401,  0.        ])}\n",
      "cos(angle):  -0.7147852516268403   sin(angle)  0.699344009809732\n",
      "theta dot:  -7.00489327671\n",
      "Angle:  135.6252853675436\n",
      "action:  [ 0.24071016]\n",
      "state:  {1: array([ 5.18649203]), 2: array([-6.44427875]), 3: array([-0.45654061,  0.88970257,  0.        ])}\n",
      "cos(angle):  -0.4565406121173268   sin(angle)  0.8897025736095948\n",
      "theta dot:  -6.44427874516\n",
      "Angle:  117.1638298332823\n",
      "action:  [-0.19332132]\n",
      "state:  {1: array([ 4.89619203]), 2: array([-5.80600001]), 3: array([-0.18276988,  0.98315572,  0.        ])}\n",
      "cos(angle):  -0.18276987579127574   sin(angle)  0.9831557213906867\n",
      "theta dot:  -5.80600001222\n",
      "Angle:  100.5309039006554\n",
      "action:  [ 0.26956974]\n",
      "state:  {1: array([ 4.64478214]), 2: array([-5.02819776]), 3: array([ 0.06755535,  0.99771553,  0.        ])}\n",
      "cos(angle):  0.06755534892446492   sin(angle)  0.9977155280097096\n",
      "theta dot:  -5.02819776061\n",
      "Angle:  86.12621207309394\n",
      "action:  [ 0.24972489]\n",
      "state:  {1: array([ 4.43265952]), 2: array([-4.24245238]), 3: array([ 0.27609563,  0.96113017,  0.        ])}\n",
      "cos(angle):  0.27609563242375085   sin(angle)  0.9611301689971703\n",
      "theta dot:  -4.24245238156\n",
      "Angle:  73.97250968131675\n",
      "action:  [ 0.21919239]\n",
      "state:  {1: array([ 4.25822323]), 2: array([-3.4887259]), 3: array([ 0.43871279,  0.89862734,  0.        ])}\n",
      "cos(angle):  0.4387127890634705   sin(angle)  0.8986273358362469\n",
      "theta dot:  -3.48872589606\n",
      "Angle:  63.97806956655165\n",
      "action:  [ 0.15768201]\n",
      "state:  {1: array([ 4.11866807]), 2: array([-2.79110309]), 3: array([ 0.55944902,  0.82886476,  0.        ])}\n",
      "cos(angle):  0.5594490175682109   sin(angle)  0.8288647638438756\n",
      "theta dot:  -2.79110309342\n",
      "Angle:  55.98216689251194\n",
      "action:  [ 0.00036791]\n",
      "state:  {1: array([ 4.01019811]), 2: array([-2.16939933]), 3: array([ 0.64589181,  0.76342895,  0.        ])}\n",
      "cos(angle):  0.6458918126823983   sin(angle)  0.7634289530204141\n",
      "theta dot:  -2.16939933462\n",
      "Angle:  49.76731012793232\n",
      "action:  [-0.16688207]\n",
      "state:  {1: array([ 3.92910511]), 2: array([-1.62185993]), 3: array([ 0.70561017,  0.70860023,  0.        ])}\n",
      "cos(angle):  0.7056101710144248   sin(angle)  0.7086002304268566\n",
      "theta dot:  -1.62185993014\n",
      "Angle:  45.12103454503591\n",
      "action:  [-0.2477504]\n",
      "state:  {1: array([ 3.87272649]), 2: array([-1.12757232]), 3: array([ 0.7444178 ,  0.66771412,  0.        ])}\n",
      "cos(angle):  0.7444177997593844   sin(angle)  0.6677141150233362\n",
      "theta dot:  -1.12757231665\n",
      "Angle:  41.89078535677396\n",
      "action:  [-0.25407165]\n",
      "state:  {1: array([ 3.83948162]), 2: array([-0.66489748]), 3: array([ 0.76620045,  0.64260165,  0.        ])}\n",
      "cos(angle):  0.7662004471105662   sin(angle)  0.6426016455375511\n",
      "theta dot:  -0.664897477347\n",
      "Angle:  39.985998847949304\n",
      "action:  [-0.19202948]\n",
      "state:  {1: array([ 3.82889409]), 2: array([-0.21175067]), 3: array([ 0.77296094,  0.63445361,  0.        ])}\n",
      "cos(angle):  0.7729609427572981   sin(angle)  0.634453608210836\n",
      "theta dot:  -0.211750665441\n",
      "Angle:  39.379379294547334\n",
      "action:  [-0.03424809]\n",
      "state:  {1: array([ 3.8418417]), 2: array([ 0.25895233]), 3: array([ 0.76468172,  0.64440815,  0.        ])}\n",
      "cos(angle):  0.7646817213739461   sin(angle)  0.6444081509389672\n",
      "theta dot:  0.258952327335\n",
      "Angle:  40.12122133236596\n",
      "action:  [ 0.29340687]\n",
      "state:  {1: array([ 3.88115518]), 2: array([ 0.78626947]), 3: array([ 0.73876347,  0.67396478,  0.        ])}\n",
      "cos(angle):  0.7387634733713762   sin(angle)  0.673964784252308\n",
      "theta dot:  0.786269470928\n",
      "Angle:  42.373712177269226\n",
      "action:  [ 0.08065494]\n",
      "state:  {1: array([ 3.94634724]), 2: array([ 1.3038413]), 3: array([ 0.69328811,  0.72066052,  0.        ])}\n",
      "cos(angle):  0.6932881145049756   sin(angle)  0.7206605236074306\n",
      "theta dot:  1.30384129967\n",
      "Angle:  46.108933624006\n",
      "action:  [ 0.15803537]\n",
      "state:  {1: array([ 4.03974934]), 2: array([ 1.868042]), 3: array([ 0.62305283,  0.78217976,  0.        ])}\n",
      "cos(angle):  0.6230528272213689   sin(angle)  0.782179758426066\n",
      "theta dot:  1.86804199814\n",
      "Angle:  51.46046723212385\n",
      "action:  [ 0.01876741]\n",
      "state:  {1: array([ 4.16262394]), 2: array([ 2.45749193]), 3: array([ 0.52248691,  0.85264731,  0.        ])}\n",
      "cos(angle):  0.5224869072799286   sin(angle)  0.8526473079304568\n",
      "theta dot:  2.45749192864\n",
      "Angle:  58.50064655404384\n",
      "action:  [-0.12550661]\n",
      "state:  {1: array([ 4.31653151]), 2: array([ 3.07815142]), 3: array([ 0.38559949,  0.92266626,  0.        ])}\n",
      "cos(angle):  0.3855994903512168   sin(angle)  0.9226662630880582\n",
      "theta dot:  3.07815141851\n",
      "Angle:  67.3188801823261\n",
      "action:  [-0.12325216]\n",
      "state:  {1: array([ 4.50411467]), 2: array([ 3.75166329]), 3: array([ 0.20677181,  0.9783892 ,  0.        ])}\n",
      "cos(angle):  0.20677180890654237   sin(angle)  0.9783891960981153\n",
      "theta dot:  3.75166329255\n",
      "Angle:  78.06657869040339\n",
      "action:  [ 0.11381919]\n",
      "state:  {1: array([ 4.72924108]), 2: array([ 4.50252807]), 3: array([-0.0168513 ,  0.99985801,  0.        ])}\n",
      "cos(angle):  -0.016851299028761577   sin(angle)  0.9998580067794843\n",
      "theta dot:  4.50252806765\n",
      "Angle:  90.96534129826404\n",
      "action:  [ 0.23555644]\n",
      "state:  {1: array([ 4.99362883]), 2: array([ 5.28775504]), 3: array([-0.277547  ,  0.96071206,  0.        ])}\n",
      "cos(angle):  -0.2775469991231365   sin(angle)  0.9607120605455839\n",
      "theta dot:  5.28775503944\n",
      "Angle:  106.11360821798519\n",
      "action:  [ 0.41983858]\n",
      "state:  {1: array([ 5.29719207]), 2: array([ 6.07126487]), 3: array([-0.55203521,  0.8338208 ,  0.        ])}\n",
      "cos(angle):  -0.552035207220653   sin(angle)  0.833820802084507\n",
      "theta dot:  6.07126487144\n",
      "Angle:  123.50646021792272\n",
      "action:  [ 0.13482573]\n",
      "state:  {1: array([ 5.63303479]), 2: array([ 6.71685433]), 3: array([-0.7959927 ,  0.60530622,  0.        ])}\n",
      "cos(angle):  -0.795992698105195   sin(angle)  0.6053062237935537\n",
      "theta dot:  6.71685433242\n",
      "Angle:  142.74878546359912\n",
      "action:  [-0.07137565]\n",
      "state:  {1: array([ 5.99104117]), 2: array([ 7.16012765]), 3: array([-0.95762855,  0.28800617,  0.        ])}\n",
      "cos(angle):  -0.9576285530441431   sin(angle)  0.2880061707578171\n",
      "theta dot:  7.16012765301\n",
      "Angle:  163.2609922617611\n",
      "action:  [-0.14154948]\n",
      "state:  {1: array([ 0.07560086]), 2: array([ 7.35489986]), 3: array([-0.99714362, -0.07552886,  0.        ])}\n",
      "cos(angle):  -0.9971436160214758   sin(angle)  -0.07552886221581444\n",
      "theta dot:  7.35489985879\n",
      "Angle:  184.33202085763958\n",
      "action:  [-0.0910239]\n",
      "state:  {1: array([ 0.43983084]), 2: array([ 7.28459963]), 3: array([-0.9048237 , -0.42578641,  0.        ])}\n",
      "cos(angle):  -0.9048237026379984   sin(angle)  -0.4257864102392926\n",
      "theta dot:  7.28459962668\n",
      "Angle:  205.2008127598849\n",
      "action:  [-0.08183882]\n",
      "state:  {1: array([ 0.78748004]), 2: array([ 6.952984]), 3: array([-0.70563314, -0.70857736,  0.        ])}\n",
      "cos(angle):  -0.7056331416974619   sin(angle)  -0.7085773559310018\n",
      "theta dot:  6.95298399584\n",
      "Angle:  225.11959808029044\n",
      "action:  [-0.21660447]\n",
      "state:  {1: array([ 1.10693305]), 2: array([ 6.38906031]), 3: array([-0.44740648, -0.89433072,  0.        ])}\n",
      "cos(angle):  -0.4474064804787646   sin(angle)  -0.8943307225101935\n",
      "theta dot:  6.38906030903\n",
      "Angle:  243.42286481738182\n",
      "action:  [-0.24481526]\n",
      "state:  {1: array([ 1.39101255]), 2: array([ 5.68158998]), 3: array([-0.17881684, -0.98388238,  0.        ])}\n",
      "cos(angle):  -0.17881683635691045   sin(angle)  -0.9838823806915672\n",
      "theta dot:  5.68158997793\n",
      "Angle:  259.6993830887574\n",
      "action:  [-0.0962836]\n",
      "state:  {1: array([ 1.63747434]), 2: array([ 4.92923565]), 3: array([ 0.06662861, -0.99777785,  0.        ])}\n",
      "cos(angle):  0.06662861194493488   sin(angle)  -0.9977778450488322\n",
      "theta dot:  4.92923565168\n",
      "Angle:  273.8205700206188\n",
      "action:  [-0.15651701]\n",
      "state:  {1: array([ 1.84534557]), 2: array([ 4.15742472]), 3: array([ 0.27111309, -0.9625475 ,  0.        ])}\n",
      "cos(angle):  0.27111309115612126   sin(angle)  -0.9625475010635957\n",
      "theta dot:  4.15742471687\n",
      "Angle:  285.7306866655916\n",
      "action:  [-0.52335989]\n",
      "state:  {1: array([ 2.01319608]), 2: array([ 3.35701011]), 3: array([ 0.42810941, -0.90372691,  0.        ])}\n",
      "cos(angle):  0.4281094144081601   sin(angle)  -0.9037269107950157\n",
      "theta dot:  3.35701010695\n",
      "Angle:  295.34778972218373\n",
      "action:  [-0.46158295]\n",
      "state:  {1: array([ 2.14369495]), 2: array([ 2.60997748]), 3: array([ 0.54207013, -0.84033325,  0.        ])}\n",
      "cos(angle):  0.5420701328100188   sin(angle)  -0.8403332500355609\n",
      "theta dot:  2.60997748126\n",
      "Angle:  302.82480695267094\n",
      "action:  [-0.54965127]\n",
      "state:  {1: array([ 2.23855894]), 2: array([ 1.89727985]), 3: array([ 0.61923072, -0.78520909,  0.        ])}\n",
      "cos(angle):  0.6192307247626522   sin(angle)  -0.7852090864922034\n",
      "theta dot:  1.89727985268\n",
      "Angle:  308.26010064826517\n",
      "action:  [-0.20968336]\n",
      "state:  {1: array([ 2.30240497]), 2: array([ 1.27692053]), 3: array([ 0.66806749, -0.74410068,  0.        ])}\n",
      "cos(angle):  0.66806749156307   sin(angle)  -0.7441006831851638\n",
      "theta dot:  1.27692053335\n",
      "Angle:  311.9181999607763\n",
      "action:  [-0.45391249]\n",
      "state:  {1: array([ 2.33494288]), 2: array([ 0.65075815]), 3: array([ 0.69192108, -0.72197314,  0.        ])}\n",
      "cos(angle):  0.691921083236339   sin(angle)  -0.7219731397864128\n",
      "theta dot:  0.650758147336\n",
      "Angle:  313.7824803675826\n",
      "action:  [-0.40004845]\n",
      "state:  {1: array([ 2.33740643]), 2: array([ 0.04927102]), 3: array([ 0.6936976 , -0.72026637,  0.        ])}\n",
      "cos(angle):  0.6936975995924602   sin(angle)  -0.720266367616633\n",
      "theta dot:  0.0492710246396\n",
      "Angle:  313.92363112571735\n",
      "action:  [-0.38865384]\n",
      "state:  {1: array([ 2.30994509]), 2: array([-0.54922683]), 3: array([ 0.67365905, -0.73904227,  0.        ])}\n",
      "cos(angle):  0.6736590539319598   sin(angle)  -0.7390422714943286\n",
      "theta dot:  -0.549226827276\n",
      "Angle:  312.35021584513413\n",
      "action:  [-0.45713334]\n",
      "state:  {1: array([ 2.25134116]), 2: array([-1.17207853]), 3: array([ 0.62921658, -0.77723002,  0.        ])}\n",
      "cos(angle):  0.6292165788112909   sin(angle)  -0.7772300154709767\n",
      "theta dot:  -1.17207853136\n",
      "Angle:  308.99246604176443\n",
      "action:  [-0.588873]\n",
      "state:  {1: array([ 2.15917456]), 2: array([-1.84333199]), 3: array([ 0.5550127 , -0.83184187,  0.        ])}\n",
      "cos(angle):  0.5550127009118733   sin(angle)  -0.8318418730903774\n",
      "theta dot:  -1.84333199261\n",
      "Angle:  303.7117212195307\n",
      "action:  [-0.73874086]\n",
      "state:  {1: array([ 2.03027333]), 2: array([-2.57802453]), 3: array([ 0.44347942, -0.89628456,  0.        ])}\n",
      "cos(angle):  0.4434794181012296   sin(angle)  -0.8962845562211784\n",
      "theta dot:  -2.57802452699\n",
      "Angle:  296.32624224609697\n",
      "action:  [-0.85195814]\n",
      "state:  {1: array([ 1.86137175]), 2: array([-3.37803167]), 3: array([ 0.28650358, -0.95807917,  0.        ])}\n",
      "cos(angle):  0.28650357507657115   sin(angle)  -0.9580791728601262\n",
      "theta dot:  -3.37803166549\n",
      "Angle:  286.6489170011758\n",
      "action:  [-0.87465021]\n",
      "state:  {1: array([ 1.64998232]), 2: array([-4.22778858]), 3: array([ 0.07910327, -0.99686643,  0.        ])}\n",
      "cos(angle):  0.07910326702111575   sin(angle)  -0.9968664269332106\n",
      "theta dot:  -4.22778857682\n",
      "Angle:  274.5372232173105\n",
      "action:  [-0.79585926]\n",
      "state:  {1: array([ 1.39524146]), 2: array([-5.09481729]), 3: array([-0.1746545 , -0.98462978,  0.        ])}\n",
      "cos(angle):  -0.17465450432423552   sin(angle)  -0.9846297802317658\n",
      "theta dot:  -5.0948172867\n",
      "Angle:  259.94168095213604\n",
      "action:  [-0.70351858]\n",
      "state:  {1: array([ 1.09830059]), 2: array([-5.93881741]), 3: array([-0.45510999, -0.89043523,  0.        ])}\n",
      "cos(angle):  -0.45510999412575914   sin(angle)  -0.8904352268676545\n",
      "theta dot:  -5.93881740814\n",
      "Angle:  242.92826209765292\n",
      "action:  [-0.54912203]\n",
      "state:  {1: array([ 0.76384998]), 2: array([-6.68901213]), 3: array([-0.72217831, -0.69170694,  0.        ])}\n",
      "cos(angle):  -0.722178310624926   sin(angle)  -0.6917069377004454\n",
      "theta dot:  -6.6890121323\n",
      "Angle:  223.76569869342308\n",
      "action:  [-0.2608234]\n",
      "state:  {1: array([ 0.40150419]), 2: array([-7.24691585]), 3: array([-0.92047419, -0.39080335,  0.        ])}\n",
      "cos(angle):  -0.9204741933816696   sin(angle)  -0.3908033512117888\n",
      "theta dot:  -7.24691584566\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  203.0048626191543\n",
      "action:  [-0.25154084]\n",
      "state:  {1: array([ 0.02261671]), 2: array([-7.57774949]), 3: array([-0.99974425, -0.02261479,  0.        ])}\n",
      "cos(angle):  -0.9997442530055567   sin(angle)  -0.022614786807782453\n",
      "theta dot:  -7.57774948553\n",
      "Angle:  181.29626019689556\n",
      "action:  [-0.29539633]\n",
      "state:  {1: array([ 5.92385102]), 2: array([-7.63902002]), 3: array([-0.93613113,  0.35165112,  0.        ])}\n",
      "cos(angle):  -0.9361311300485449   sin(angle)  0.3516511159573283\n",
      "theta dot:  -7.63902002456\n",
      "Angle:  159.4112891857044\n",
      "action:  [ 0.09571142]\n",
      "state:  {1: array([ 5.55580477]), 2: array([-7.36092498]), 3: array([-0.74691869,  0.66491539,  0.        ])}\n",
      "cos(angle):  -0.7469186854107911   sin(angle)  0.6649153911470358\n",
      "theta dot:  -7.36092497519\n",
      "Angle:  138.3238417777797\n",
      "action:  [-0.27574423]\n",
      "state:  {1: array([ 5.21062477]), 2: array([-6.90360007]), 3: array([-0.47787655,  0.878427  ,  0.        ])}\n",
      "cos(angle):  -0.47787655199167967   sin(angle)  0.8784270038293128\n",
      "theta dot:  -6.90360006573\n",
      "Angle:  118.54653066512111\n",
      "action:  [-0.30084509]\n",
      "state:  {1: array([ 4.89612944]), 2: array([-6.28990658]), 3: array([-0.18270834,  0.98316716,  0.        ])}\n",
      "cos(angle):  -0.18270834004302172   sin(angle)  0.9831671589708046\n",
      "theta dot:  -6.28990657594\n",
      "Angle:  100.5273177852178\n",
      "action:  [-0.1048465]\n",
      "state:  {1: array([ 4.61771653]), 2: array([-5.56825818]), 3: array([ 0.09453109,  0.99552191,  0.        ])}\n",
      "cos(angle):  0.09453108998271319   sin(angle)  0.9955219098677237\n",
      "theta dot:  -5.56825818174\n",
      "Angle:  84.57547043493827\n",
      "action:  [ 0.21778197]\n",
      "state:  {1: array([ 4.37826906]), 2: array([-4.78894945]), 3: array([ 0.32793789,  0.94469929,  0.        ])}\n",
      "cos(angle):  0.3279378880959444   sin(angle)  0.9446992863081732\n",
      "theta dot:  -4.78894945426\n",
      "Angle:  70.85617291510005\n",
      "action:  [ 0.55044723]\n",
      "state:  {1: array([ 4.17837616]), 2: array([-3.9978579]), 3: array([ 0.50899155,  0.86077152,  0.        ])}\n",
      "cos(angle):  0.5089915489416637   sin(angle)  0.8607715162027413\n",
      "theta dot:  -3.99785790491\n",
      "Angle:  59.40318044496814\n",
      "action:  [ 0.7697162]\n",
      "state:  {1: array([ 4.01653507]), 2: array([-3.23682184]), 3: array([ 0.64104105,  0.76750659,  0.        ])}\n",
      "cos(angle):  0.6410410541885615   sin(angle)  0.7675065907500845\n",
      "theta dot:  -3.23682183769\n",
      "Angle:  50.13039061200078\n",
      "action:  [ 0.81632697]\n",
      "state:  {1: array([ 3.88959793]), 2: array([-2.53874285]), 3: array([ 0.73304709,  0.68017789,  0.        ])}\n",
      "cos(angle):  0.7330470941677012   sin(angle)  0.6801778868298274\n",
      "theta dot:  -2.53874284928\n",
      "Angle:  42.85744509267636\n",
      "action:  [ 0.69202196]\n",
      "state:  {1: array([ 3.79335762]), 2: array([-1.92480614]), 3: array([ 0.79501442,  0.60659052,  0.        ])}\n",
      "cos(angle):  0.7950144245031874   sin(angle)  0.6065905248451099\n",
      "theta dot:  -1.92480613984\n",
      "Angle:  37.34329457747418\n",
      "action:  [ 0.47176619]\n",
      "state:  {1: array([ 3.72340271]), 2: array([-1.39909832]), 3: array([ 0.83546933,  0.54953708,  0.        ])}\n",
      "cos(angle):  0.8354693283086119   sin(angle)  0.549537079236294\n",
      "theta dot:  -1.39909831762\n",
      "Angle:  33.3351825140071\n",
      "action:  [ 0.25762237]\n",
      "state:  {1: array([ 3.6759876]), 2: array([-0.94830215]), 3: array([ 0.86057695,  0.50932044,  0.        ])}\n",
      "cos(angle):  0.8605769539014068   sin(angle)  0.5093204358886221\n",
      "theta dot:  -0.948302152467\n",
      "Angle:  30.61850331480906\n",
      "action:  [ 0.09220442]\n",
      "state:  {1: array([ 3.64836354]), 2: array([-0.55248116]), 3: array([ 0.87431633,  0.48535652,  0.        ])}\n",
      "cos(angle):  0.8743163345528611   sin(angle)  0.48535651549561964\n",
      "theta dot:  -0.552481161816\n",
      "Angle:  29.03576507431393\n",
      "action:  [ 0.0002632]\n",
      "state:  {1: array([ 3.63894233]), 2: array([-0.1884243]), 3: array([ 0.87885011,  0.47709798,  0.        ])}\n",
      "cos(angle):  0.878850113325002   sin(angle)  0.4770979755863896\n",
      "theta dot:  -0.18842429557\n",
      "Angle:  28.495970491893843\n",
      "action:  [ 0.01428935]\n",
      "state:  {1: array([ 3.64751945]), 2: array([ 0.17154259]), 3: array([ 0.87472571,  0.48461835,  0.        ])}\n",
      "cos(angle):  0.8747257053598123   sin(angle)  0.4846183450745327\n",
      "theta dot:  0.171542588661\n",
      "Angle:  28.98740265956144\n",
      "action:  [ 0.07396361]\n",
      "state:  {1: array([ 3.6748245]), 2: array([ 0.54610089]), 3: array([ 0.86116876,  0.50831916,  0.        ])}\n",
      "cos(angle):  0.8611687617724741   sin(angle)  0.5083191554006828\n",
      "theta dot:  0.546100888377\n",
      "Angle:  30.551862805790392\n",
      "action:  [-0.02253831]\n",
      "state:  {1: array([ 3.72102247]), 2: array([ 0.92395951]), 3: array([ 0.83677499,  0.54754691,  0.        ])}\n",
      "cos(angle):  0.8367749857104656   sin(angle)  0.5475469142358945\n",
      "theta dot:  0.92395950844\n",
      "Angle:  33.198805629816604\n",
      "action:  [-0.11630165]\n",
      "state:  {1: array([ 3.7868812]), 2: array([ 1.31717445]), 3: array([ 0.79892626,  0.60142899,  0.        ])}\n",
      "cos(angle):  0.798926261932608   sin(angle)  0.6014289883223038\n",
      "theta dot:  1.31717444635\n",
      "Angle:  36.97222363884731\n",
      "action:  [-0.16819203]\n",
      "state:  {1: array([ 3.87403207]), 2: array([ 1.74301738]), 3: array([ 0.74354542,  0.66868544,  0.        ])}\n",
      "cos(angle):  0.7435454167431397   sin(angle)  0.6686854366892334\n",
      "theta dot:  1.74301738291\n",
      "Angle:  41.96558894512009\n",
      "action:  [-0.27306783]\n",
      "state:  {1: array([ 3.98421063]), 2: array([ 2.20357129]), 3: array([ 0.66551108,  0.74638797,  0.        ])}\n",
      "cos(angle):  0.6655110824224296   sin(angle)  0.7463879682664547\n",
      "theta dot:  2.20357128569\n",
      "Angle:  48.27834090947516\n",
      "action:  [-0.26408494]\n",
      "state:  {1: array([ 4.12039811]), 2: array([ 2.72374952]), 3: array([ 0.55801422,  0.82983139,  0.        ])}\n",
      "cos(angle):  0.558014217512551   sin(angle)  0.8298313883276864\n",
      "theta dot:  2.72374952028\n",
      "Angle:  56.08129026093013\n",
      "action:  [-0.1322724]\n",
      "state:  {1: array([ 4.28671222]), 2: array([ 3.3262822]), 3: array([ 0.41293725,  0.91075948,  0.        ])}\n",
      "cos(angle):  0.4129372455257262   sin(angle)  0.9107594804654114\n",
      "theta dot:  3.32628220136\n",
      "Angle:  65.61036455819425\n",
      "action:  [ 0.02851023]\n",
      "state:  {1: array([ 4.48739363]), 2: array([ 4.01362835]), 3: array([ 0.22310183,  0.97479515,  0.        ])}\n",
      "cos(angle):  0.22310182615608254   sin(angle)  0.974795145230946\n",
      "theta dot:  4.01362834604\n",
      "Angle:  77.10853590858233\n",
      "action:  [ 0.08318856]\n",
      "state:  {1: array([ 4.72525378]), 2: array([ 4.75720299]), 3: array([-0.01286445,  0.99991725,  0.        ])}\n",
      "cos(angle):  -0.012864447880026527   sin(angle)  0.9999172495665539\n",
      "theta dot:  4.7572029895\n",
      "Angle:  90.7368867188435\n",
      "action:  [ 0.45181923]\n",
      "state:  {1: array([ 5.00399947]), 2: array([ 5.57491381]), 3: array([-0.2874951 ,  0.95778211,  0.        ])}\n",
      "cos(angle):  -0.2874950989096096   sin(angle)  0.9577821088864387\n",
      "theta dot:  5.57491381053\n",
      "Angle:  106.70780099653098\n",
      "action:  [-0.22121736]\n",
      "state:  {1: array([ 5.31700286]), 2: array([ 6.26006779]), 3: array([-0.56844445,  0.82272165,  0.        ])}\n",
      "cos(angle):  -0.5684444515852971   sin(angle)  0.8227216451886329\n",
      "theta dot:  6.26006778883\n",
      "Angle:  124.64153224796058\n",
      "action:  [-0.35733579]\n",
      "state:  {1: array([ 5.6581783]), 2: array([ 6.82350865]), 3: array([-0.81095902,  0.58510296,  0.        ])}\n",
      "cos(angle):  -0.8109590171655409   sin(angle)  0.5851029588695481\n",
      "theta dot:  6.82350865484\n",
      "Angle:  144.1893989062212\n",
      "action:  [-0.44766256]\n",
      "state:  {1: array([ 6.01793762]), 2: array([ 7.19518649]), 3: array([-0.9650276,  0.2621483,  0.       ])}\n",
      "cos(angle):  -0.9650275998026583   sin(angle)  0.26214830081295654\n",
      "theta dot:  7.19518649038\n",
      "Angle:  164.8020416403151\n",
      "action:  [-0.4563475]\n",
      "state:  {1: array([ 0.10091959]), 2: array([ 7.32334559]), 3: array([-0.99491194, -0.10074837,  0.        ])}\n",
      "cos(angle):  -0.9949119384945474   sin(angle)  -0.10074837289515857\n",
      "theta dot:  7.323345591\n",
      "Angle:  185.78267412907866\n",
      "action:  [-0.26957678]\n",
      "state:  {1: array([ 0.46128698]), 2: array([ 7.2073478]), 3: array([-0.8954804 , -0.44510094,  0.        ])}\n",
      "cos(angle):  -0.8954804021727963   sin(angle)  -0.4451009428483015\n",
      "theta dot:  7.20734779507\n",
      "Angle:  206.4301563533041\n",
      "action:  [-0.34780879]\n",
      "state:  {1: array([ 0.80235452]), 2: array([ 6.82135077]), 3: array([-0.69501575, -0.71899451,  0.        ])}\n",
      "cos(angle):  -0.6950157497990502   sin(angle)  -0.7189945114750627\n",
      "theta dot:  6.82135076877\n",
      "Angle:  225.97184113777934\n",
      "action:  [-0.25950795]\n",
      "state:  {1: array([ 1.11451346]), 2: array([ 6.24317869]), 3: array([-0.44061431, -0.89769652,  0.        ])}\n",
      "cos(angle):  -0.440614305099203   sin(angle)  -0.8976965156120116\n",
      "theta dot:  6.24317869292\n",
      "Angle:  243.85718880658527\n",
      "action:  [-0.16962027]\n",
      "state:  {1: array([ 1.39173662]), 2: array([ 5.54446327]), 3: array([-0.17810439, -0.9840116 ,  0.        ])}\n",
      "cos(angle):  -0.17810439403190045   sin(angle)  -0.9840115979126107\n",
      "theta dot:  5.54446326534\n",
      "Angle:  259.7408689020943\n",
      "action:  [-0.11273612]\n",
      "state:  {1: array([ 1.63121383]), 2: array([ 4.78954415]), 3: array([ 0.06038075, -0.99817542,  0.        ])}\n",
      "cos(angle):  0.060380749419397875   sin(angle)  -0.9981754180000386\n",
      "theta dot:  4.78954414838\n",
      "Angle:  273.4618700911072\n",
      "action:  [ 0.01866971]\n",
      "state:  {1: array([ 1.83339948]), 2: array([ 4.04371304]), 3: array([ 0.25959534, -0.96571748,  0.        ])}\n",
      "cos(angle):  0.259595337567996   sin(angle)  -0.9657174849369552\n",
      "theta dot:  4.0437130417\n",
      "Angle:  285.0462275444096\n",
      "action:  [ 0.09881214]\n",
      "state:  {1: array([ 2.00011182]), 2: array([ 3.33424675]), 3: array([ 0.41624851, -0.90925089,  0.        ])}\n",
      "cos(angle):  0.4162485077588333   sin(angle)  -0.9092508893526278\n",
      "theta dot:  3.33424674924\n",
      "Angle:  294.5981185372692\n",
      "action:  [ 0.1798353]\n",
      "state:  {1: array([ 2.13407601]), 2: array([ 2.67928388]), 3: array([ 0.53396206, -0.84550844,  0.        ])}\n",
      "cos(angle):  0.533962064206206   sin(angle)  -0.8455084351966262\n",
      "theta dot:  2.67928387729\n",
      "Angle:  302.273683502779\n",
      "action:  [ 0.23474845]\n",
      "state:  {1: array([ 2.23809425]), 2: array([ 2.08036482]), 3: array([ 0.61886578, -0.78549675,  0.        ])}\n",
      "cos(angle):  0.6188657767301912   sin(angle)  -0.7854967539029917\n",
      "theta dot:  2.08036481771\n",
      "Angle:  308.233475761299\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -0.874650211216  self.max:  0.875992110438\n",
      "state:  {1: array([ 4.92053048]), 2: array([ 0.46738417]), 3: array([-0.20664187,  0.97841665,  0.        ])}\n",
      "cos(angle):  -0.20664186961504766   sin(angle)  0.9784166483262627\n",
      "theta dot:  0.467384172636\n",
      "Angle:  101.92539109819\n",
      "action:  [-0.8634516]\n",
      "state:  {1: array([ 4.97411443]), 2: array([ 1.07167892]), 3: array([-0.25874762,  0.96594496,  0.        ])}\n",
      "cos(angle):  -0.25874762123321415   sin(angle)  0.9659449614269714\n",
      "theta dot:  1.07167891858\n",
      "Angle:  104.99551787027588\n",
      "action:  [-0.7077498]\n",
      "state:  {1: array([ 5.05861318]), 2: array([ 1.68997517]), 3: array([-0.33934849,  0.94066073,  0.        ])}\n",
      "cos(angle):  -0.33934849146509405   sin(angle)  0.9406607259476527\n",
      "theta dot:  1.68997516937\n",
      "Angle:  109.83692878330044\n",
      "action:  [-0.47352909]\n",
      "state:  {1: array([ 5.17483525]), 2: array([ 2.32444135]), 3: array([-0.44613876,  0.8949638 ,  0.        ])}\n",
      "cos(angle):  -0.44613876372918315   sin(angle)  0.894963800104896\n",
      "theta dot:  2.32444134987\n",
      "Angle:  116.49594716527832\n",
      "action:  [-0.27082795]\n",
      "state:  {1: array([ 5.32258725]), 2: array([ 2.95504001]), 3: array([-0.57302996,  0.81953442,  0.        ])}\n",
      "cos(angle):  -0.5730299616359523   sin(angle)  0.8195344184764292\n",
      "theta dot:  2.95504000763\n",
      "Angle:  124.96149340563363\n",
      "action:  [-0.19677392]\n",
      "state:  {1: array([ 5.49959599]), 2: array([ 3.54017473]), 3: array([-0.70838467,  0.70582658,  0.        ])}\n",
      "cos(angle):  -0.7083846700418761   sin(angle)  0.7058265787356426\n",
      "theta dot:  3.54017473297\n",
      "Angle:  135.10332323653222\n",
      "action:  [-0.22364392]\n",
      "state:  {1: array([ 5.70139589]), 2: array([ 4.03599808]), 3: array([-0.83548067,  0.54951984,  0.        ])}\n",
      "cos(angle):  -0.8354806691627179   sin(angle)  0.5495198371809857\n",
      "theta dot:  4.03599807971\n",
      "Angle:  146.66557900344918\n",
      "action:  [-0.29465631]\n",
      "state:  {1: array([ 5.92159287]), 2: array([ 4.40393951]), 3: array([-0.93533466,  0.35376414,  0.        ])}\n",
      "cos(angle):  -0.9353346619830955   sin(angle)  0.3537641447249404\n",
      "theta dot:  4.40393951139\n",
      "Angle:  159.28190686267703\n",
      "action:  [-0.40307536]\n",
      "state:  {1: array([ 6.15203293]), 2: array([ 4.60880132]), 3: array([-0.99141185,  0.13077671,  0.        ])}\n",
      "cos(angle):  -0.9914118484329717   sin(angle)  0.1307767058260699\n",
      "theta dot:  4.60880131527\n",
      "Angle:  172.48511918678446\n",
      "action:  [-0.5300803]\n",
      "state:  {1: array([ 0.10021622]), 2: array([ 4.6273718]), 3: array([-0.99498256, -0.10004855,  0.        ])}\n",
      "cos(angle):  -0.9949825563816397   sin(angle)  -0.10004855069543579\n",
      "theta dot:  4.62737179898\n",
      "Angle:  185.74237373813818\n",
      "action:  [-0.84680536]\n",
      "state:  {1: array([ 0.32148195]), 2: array([ 4.42531458]), 3: array([-0.94876821, -0.31597293,  0.        ])}\n",
      "cos(angle):  -0.9487682053571865   sin(angle)  -0.31597293001664467\n",
      "theta dot:  4.42531458245\n",
      "Angle:  198.4199365220733\n",
      "action:  [-0.60957374]\n",
      "state:  {1: array([ 0.52632689]), 2: array([ 4.09689882]), 3: array([-0.86465813, -0.50236075,  0.        ])}\n",
      "cos(angle):  -0.8646581271063511   sin(angle)  -0.5023607500879593\n",
      "theta dot:  4.09689882362\n",
      "Angle:  210.15665966079507\n",
      "action:  [-0.27495181]\n",
      "state:  {1: array([ 0.71027116]), 2: array([ 3.67888549]), 3: array([-0.7581851 , -0.65203939,  0.        ])}\n",
      "cos(angle):  -0.7581850959733089   sin(angle)  -0.6520393855005573\n",
      "theta dot:  3.67888549002\n",
      "Angle:  220.6958656100548\n",
      "action:  [-0.11052818]\n",
      "state:  {1: array([ 0.868935]), 2: array([ 3.17327672]), 3: array([-0.64564019, -0.76364176,  0.        ])}\n",
      "cos(angle):  -0.6456401935484266   sin(angle)  -0.7636417618718545\n",
      "theta dot:  3.17327672457\n",
      "Angle:  229.78661252917317\n",
      "action:  [-0.1079987]\n",
      "state:  {1: array([ 0.99815228]), 2: array([ 2.5843456]), 3: array([-0.54185619, -0.84047122,  0.        ])}\n",
      "cos(angle):  -0.541856187522354   sin(angle)  -0.8404712202352557\n",
      "theta dot:  2.5843455984\n",
      "Angle:  237.19019999593536\n",
      "action:  [ 0.0605186]\n",
      "state:  {1: array([ 1.09630578]), 2: array([ 1.96306997]), 3: array([-0.45688534, -0.8895256 ,  0.        ])}\n",
      "cos(angle):  -0.4568853382072744   sin(angle)  -0.8895255970073174\n",
      "theta dot:  1.96306997351\n",
      "Angle:  242.81396806365535\n",
      "action:  [ 0.0853597]\n",
      "state:  {1: array([ 1.16174226]), 2: array([ 1.30872973]), 3: array([-0.39774161, -0.91749747,  0.        ])}\n",
      "cos(angle):  -0.3977416121993793   sin(angle)  -0.9174974713453103\n",
      "theta dot:  1.30872973147\n",
      "Angle:  246.56319380316103\n",
      "action:  [ 0.20317815]\n",
      "state:  {1: array([ 1.19429643]), 2: array([ 0.65108335]), 3: array([-0.36766778, -0.9299572 ,  0.        ])}\n",
      "cos(angle):  -0.3676677817206115   sin(angle)  -0.9299572045447279\n",
      "theta dot:  0.651083349853\n",
      "Angle:  248.42840584437468\n",
      "action:  [ 0.16603437]\n",
      "state:  {1: array([ 1.19322246]), 2: array([-0.0214794]), 3: array([-0.36866632, -0.9295618 ,  0.        ])}\n",
      "cos(angle):  -0.36866631557122825   sin(angle)  -0.9295618041653473\n",
      "theta dot:  -0.0214793987039\n",
      "Angle:  248.36687204365683\n",
      "action:  [ 0.0708173]\n",
      "state:  {1: array([ 1.15782105]), 2: array([-0.70802816]), 3: array([-0.40133625, -0.91593079,  0.        ])}\n",
      "cos(angle):  -0.4013362456883928   sin(angle)  -0.9159307931807654\n",
      "theta dot:  -0.708028156834\n",
      "Angle:  246.33852552866395\n",
      "action:  [-0.0526916]\n",
      "state:  {1: array([ 1.08767705]), 2: array([-1.40287999]), 3: array([-0.4645437 , -0.88555019,  0.        ])}\n",
      "cos(angle):  -0.46454370465257594   sin(angle)  -0.8855501942112939\n",
      "theta dot:  -1.40287999199\n",
      "Angle:  242.31957979150087\n",
      "action:  [-0.16517167]\n",
      "state:  {1: array([ 0.98308613]), 2: array([-2.09181839]), 3: array([-0.55445687, -0.83221246,  0.        ])}\n",
      "cos(angle):  -0.5544568725030239   sin(angle)  -0.8322124587712956\n",
      "theta dot:  -2.0918183883\n",
      "Angle:  236.32697554695034\n",
      "action:  [-0.23791938]\n",
      "state:  {1: array([ 0.84550285]), 2: array([-2.75166564]), 3: array([-0.66335508, -0.74830478,  0.        ])}\n",
      "cos(angle):  -0.6633550802123991   sin(angle)  -0.7483047758476499\n",
      "theta dot:  -2.75166563994\n",
      "Angle:  228.44405259066582\n",
      "action:  [-0.29328394]\n",
      "state:  {1: array([ 0.67765851]), 2: array([-3.35688681]), 3: array([-0.7790429 , -0.62697062,  0.        ])}\n",
      "cos(angle):  -0.7790428979239266   sin(angle)  -0.626970623868687\n",
      "theta dot:  -3.35688681318\n",
      "Angle:  218.82730274389726\n",
      "action:  [-0.37070759]\n",
      "state:  {1: array([ 0.48352246]), 2: array([-3.88272092]), 3: array([-0.88536282, -0.46490071,  0.        ])}\n",
      "cos(angle):  -0.8853628224163238   sin(angle)  -0.46490071271509265\n",
      "theta dot:  -3.88272091889\n",
      "Angle:  207.70415267067142\n",
      "action:  [-0.44181403]\n",
      "state:  {1: array([ 0.26863904]), 2: array([-4.29766856]), 3: array([-0.96413302, -0.26541953,  0.        ])}\n",
      "cos(angle):  -0.9641330153168517   sin(angle)  -0.26541953352388276\n",
      "theta dot:  -4.29766855772\n",
      "Angle:  195.39226795604296\n",
      "action:  [-0.46617614]\n",
      "state:  {1: array([ 0.04030606]), 2: array([-4.56665963]), 3: array([-0.99918782, -0.04029514,  0.        ])}\n",
      "cos(angle):  -0.9991878209066257   sin(angle)  -0.04029514303075328\n",
      "theta dot:  -4.56665962898\n",
      "Angle:  182.30978238791727\n",
      "action:  [-0.24186049]\n",
      "state:  {1: array([ 6.09183336]), 2: array([-4.63316006]), 3: array([-0.98174801,  0.19018634,  0.        ])}\n",
      "cos(angle):  -0.9817480104513467   sin(angle)  0.19018634013730443\n",
      "theta dot:  -4.63316006045\n",
      "Angle:  169.03594572770544\n",
      "action:  [-0.53840141]\n",
      "state:  {1: array([ 5.86326933]), 2: array([-4.57128052]), 3: array([-0.9131232 ,  0.40768373,  0.        ])}\n",
      "cos(angle):  -0.9131231998383583   sin(angle)  0.40768372780497086\n",
      "theta dot:  -4.57128051699\n",
      "Angle:  155.94022232149575\n",
      "action:  [-0.28375767]\n",
      "state:  {1: array([ 5.64786527]), 2: array([-4.30808137]), 3: array([-0.80488181,  0.59343514,  0.        ])}\n",
      "cos(angle):  -0.8048818135707725   sin(angle)  0.5934351406708438\n",
      "theta dot:  -4.30808137179\n",
      "Angle:  143.5985071616651\n",
      "action:  [-0.24606853]\n",
      "state:  {1: array([ 5.4528695]), 2: array([-3.8999153]), 3: array([-0.67464268,  0.73814446,  0.        ])}\n",
      "cos(angle):  -0.6746426827573195   sin(angle)  0.7381444645880687\n",
      "theta dot:  -3.89991529524\n",
      "Angle:  132.42609894383224\n",
      "action:  [-0.22604926]\n",
      "state:  {1: array([ 5.28385878]), 2: array([-3.38021434]), 3: array([-0.54086889,  0.84110691,  0.        ])}\n",
      "cos(angle):  -0.5408688942399191   sin(angle)  0.8411069130875618\n",
      "theta dot:  -3.38021433588\n",
      "Angle:  122.7425208234846\n",
      "action:  [-0.09659009]\n",
      "state:  {1: array([ 5.14566515]), 2: array([-2.76387267]), 3: array([-0.41984649,  0.90759513,  0.        ])}\n",
      "cos(angle):  -0.41984648626933213   sin(angle)  0.9075951343893903\n",
      "theta dot:  -2.76387266528\n",
      "Angle:  114.82462739736388\n",
      "action:  [ 0.13454824]\n",
      "state:  {1: array([ 5.04251545]), 2: array([-2.06299408]), 3: array([-0.32416267,  0.94600136,  0.        ])}\n",
      "cos(angle):  -0.32416266811378214   sin(angle)  0.9460013554965733\n",
      "theta dot:  -2.06299407841\n",
      "Angle:  108.91459852489982\n",
      "action:  [ 0.1470486]\n",
      "state:  {1: array([ 4.97594366]), 2: array([-1.33143577]), 3: array([-0.26051413,  0.96547004,  0.        ])}\n",
      "cos(angle):  -0.2605141252253136   sin(angle)  0.9654700360747037\n",
      "theta dot:  -1.33143577139\n",
      "Angle:  105.10032492465717\n",
      "action:  [ 0.51823976]\n",
      "state:  {1: array([ 4.94946379]), 2: array([-0.52959728]), 3: array([-0.23486027,  0.97202914,  0.        ])}\n",
      "cos(angle):  -0.23486026878649666   sin(angle)  0.9720291426420993\n",
      "theta dot:  -0.529597280838\n",
      "Angle:  103.58314402080418\n",
      "action:  [-0.13923054]\n",
      "state:  {1: array([ 4.95839079]), 2: array([ 0.17854]), 3: array([-0.2435281 ,  0.96989384,  0.        ])}\n",
      "cos(angle):  -0.24352809936859188   sin(angle)  0.9698938420352617\n",
      "theta dot:  0.178539995414\n",
      "Angle:  104.09462223531972\n",
      "action:  [-0.24851158]\n",
      "state:  {1: array([ 5.00182498]), 2: array([ 0.86868364]), 3: array([-0.28541173,  0.958405  ,  0.        ])}\n",
      "cos(angle):  -0.28541172556323013   sin(angle)  0.9584050015056367\n",
      "theta dot:  0.868683639458\n",
      "Angle:  106.58321172956443\n",
      "action:  [-0.21880344]\n",
      "state:  {1: array([ 5.07955832]), 2: array([ 1.55466687]), 3: array([-0.35897488,  0.93334722,  0.        ])}\n",
      "cos(angle):  -0.3589748840690742   sin(angle)  0.9333472197460037\n",
      "theta dot:  1.55466687446\n",
      "Angle:  111.0369938374518\n",
      "action:  [-0.08411992]\n",
      "state:  {1: array([ 5.19166128]), 2: array([ 2.2420593]), 3: array([-0.46113359,  0.88733072,  0.        ])}\n",
      "cos(angle):  -0.4611335907305579   sin(angle)  0.887330722729661\n",
      "theta dot:  2.24205930161\n",
      "Angle:  117.46000558767724\n",
      "action:  [ 0.03202544]\n",
      "state:  {1: array([ 5.33727934]), 2: array([ 2.91236116]), 3: array([-0.58500836,  0.81102726,  0.        ])}\n",
      "cos(angle):  -0.5850083570970063   sin(angle)  0.8110272634915929\n",
      "theta dot:  2.91236115993\n",
      "Angle:  125.8032862215465\n",
      "action:  [ 0.02685406]\n",
      "state:  {1: array([ 5.51351233]), 2: array([ 3.52465972]), 3: array([-0.71813828,  0.69590043,  0.        ])}\n",
      "cos(angle):  -0.7181382821173803   sin(angle)  0.6959004294850649\n",
      "theta dot:  3.52465971635\n",
      "Angle:  135.90066890779292\n",
      "action:  [-0.43771553]\n",
      "state:  {1: array([ 5.71255871]), 2: array([ 3.98092771]), 3: array([-0.84156268,  0.54015947,  0.        ])}\n",
      "cos(angle):  -0.8415626799937539   sin(angle)  0.540159472416925\n",
      "theta dot:  3.980927709\n",
      "Angle:  147.30516005276496\n",
      "action:  [-0.50420317]\n",
      "state:  {1: array([ 5.92807956]), 2: array([ 4.31041684]), 3: array([-0.93760973,  0.34768952,  0.        ])}\n",
      "cos(angle):  -0.9376097253446002   sin(angle)  0.34768952089360344\n",
      "theta dot:  4.31041683769\n",
      "Angle:  159.6535658139179\n",
      "action:  [-0.5282879]\n",
      "state:  {1: array([ 6.1526766]), 2: array([ 4.49194079]), 3: array([-0.99149582,  0.13013855,  0.        ])}\n",
      "cos(angle):  -0.9914958189826596   sin(angle)  0.13013854517361545\n",
      "theta dot:  4.49194079339\n",
      "Angle:  172.5219981861178\n",
      "action:  [-0.72588939]\n",
      "state:  {1: array([ 0.09352435]), 2: array([ 4.48066129]), 3: array([-0.99562978, -0.09338807,  0.        ])}\n",
      "cos(angle):  -0.9956297845661922   sin(angle)  -0.09338807249685359\n",
      "theta dot:  4.48066129339\n",
      "Angle:  185.3589590812679\n",
      "action:  [-0.21859716]\n",
      "state:  {1: array([ 0.31241589]), 2: array([ 4.37783067]), 3: array([-0.9515938 , -0.30735847,  0.        ])}\n",
      "cos(angle):  -0.9515938045557021   sin(angle)  -0.30735847333562216\n",
      "theta dot:  4.37783066551\n",
      "Angle:  197.90049078155877\n",
      "action:  [-0.04427301]\n",
      "state:  {1: array([ 0.51944943]), 2: array([ 4.14067086]), 3: array([-0.86809262, -0.49640227,  0.        ])}\n",
      "cos(angle):  -0.8680926159401694   sin(angle)  -0.49640226646355423\n",
      "theta dot:  4.14067085889\n",
      "Angle:  209.76261127112897\n",
      "action:  [ 0.06326498]\n",
      "state:  {1: array([ 0.70834237]), 2: array([ 3.77785891]), 3: array([-0.75944133, -0.6505758 ,  0.        ])}\n",
      "cos(angle):  -0.7594413300319048   sin(angle)  -0.6505757959218674\n",
      "theta dot:  3.77785890621\n",
      "Angle:  220.58535450894433\n",
      "action:  [ 0.28079987]\n",
      "state:  {1: array([ 0.87494473]), 2: array([ 3.33204704]), 3: array([-0.64103928, -0.76750807,  0.        ])}\n",
      "cos(angle):  -0.6410392821864273   sin(angle)  -0.7675080707679302\n",
      "theta dot:  3.33204703942\n",
      "Angle:  230.13094381209956\n",
      "action:  [ 0.44196373]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "state:  {1: array([ 1.01608025]), 2: array([ 2.82271055]), 3: array([-0.52670197, -0.85005002,  0.        ])}\n",
      "cos(angle):  -0.5267019695246116   sin(angle)  -0.8500500192923326\n",
      "theta dot:  2.82271054511\n",
      "Angle:  238.2173949535568\n",
      "action:  [ 0.6258726]\n",
      "state:  {1: array([ 1.13003295]), 2: array([ 2.27905392]), 3: array([-0.42663001, -0.90442625,  0.        ])}\n",
      "cos(angle):  -0.42663000774366105   sin(angle)  -0.9044262471272291\n",
      "theta dot:  2.27905392031\n",
      "Angle:  244.74638823176238\n",
      "action:  [ 0.73432111]\n",
      "state:  {1: array([ 1.21557707]), 2: array([ 1.7108824]), 3: array([-0.34779594, -0.93757025,  0.        ])}\n",
      "cos(angle):  -0.3477959426130076   sin(angle)  -0.9375702545953181\n",
      "theta dot:  1.71088240155\n",
      "Angle:  249.64769381296628\n",
      "action:  [ 0.70705202]\n",
      "state:  {1: array([ 1.2712652]), 2: array([ 1.11376251]), 3: array([-0.29507225, -0.95547494,  0.        ])}\n",
      "cos(angle):  -0.2950722470038355   sin(angle)  -0.955474944228318\n",
      "theta dot:  1.11376251387\n",
      "Angle:  252.83838092299385\n",
      "action:  [ 0.57229985]\n",
      "state:  {1: array([ 1.29541526]), 2: array([ 0.48300128]), 3: array([-0.27191367, -0.96232165,  0.        ])}\n",
      "cos(angle):  -0.27191366601938877   sin(angle)  -0.962321650089977\n",
      "theta dot:  0.48300128357\n",
      "Angle:  254.22207443971567\n",
      "action:  [ 0.40666528]\n",
      "state:  {1: array([ 1.28652825]), 2: array([-0.17774016]), 3: array([-0.28045498, -0.95986718,  0.        ])}\n",
      "cos(angle):  -0.28045497612550957   sin(angle)  -0.9598671816279791\n",
      "theta dot:  -0.177740162669\n",
      "Angle:  253.71288757187023\n",
      "action:  [ 0.27708878]\n",
      "state:  {1: array([ 1.24372439]), 2: array([-0.85607723]), 3: array([-0.32127157, -0.94698711,  0.        ])}\n",
      "cos(angle):  -0.3212715717056635   sin(angle)  -0.9469871050937139\n",
      "theta dot:  -0.856077231706\n",
      "Angle:  251.26041269112307\n",
      "action:  [ 0.16699215]\n",
      "state:  {1: array([ 1.16666095]), 2: array([-1.54126874]), 3: array([-0.39322393, -0.91944273,  0.        ])}\n",
      "cos(angle):  -0.39322393327407756   sin(angle)  -0.919442732474657\n",
      "theta dot:  -1.5412687374\n",
      "Angle:  246.84501332883465\n",
      "action:  [-0.04631414]\n",
      "state:  {1: array([ 1.05477106]), 2: array([-2.23779791]), 3: array([-0.49342687, -0.86978728,  0.        ])}\n",
      "cos(angle):  -0.49342687439213245   sin(angle)  -0.8697872841261883\n",
      "theta dot:  -2.23779790726\n",
      "Angle:  240.43420954562708\n",
      "action:  [-0.16507673]\n",
      "state:  {1: array([ 0.90902606]), 2: array([-2.91489988]), 3: array([-0.61451438, -0.78890562,  0.        ])}\n",
      "cos(angle):  -0.6145143847494801   sin(angle)  -0.7889056159870887\n",
      "theta dot:  -2.91489987946\n",
      "Angle:  232.08365603303167\n",
      "action:  [-0.03926029]\n",
      "state:  {1: array([ 0.73340266]), 2: array([-3.51246813]), 3: array([-0.74290096, -0.66940134,  0.        ])}\n",
      "cos(angle):  -0.7429009643848985   sin(angle)  -0.6694013423320778\n",
      "theta dot:  -3.51246813498\n",
      "Angle:  222.02119957300297\n",
      "action:  [-0.08329392]\n",
      "state:  {1: array([ 0.532052]), 2: array([-4.02701323]), 3: array([-0.8617679 , -0.50730275,  0.        ])}\n",
      "cos(angle):  -0.8617679027048647   sin(angle)  -0.5073027516854791\n",
      "theta dot:  -4.02701323031\n",
      "Angle:  210.48468344338804\n",
      "action:  [ 0.04187162]\n",
      "state:  {1: array([ 0.31199152]), 2: array([-4.40120955]), 3: array([-0.95172415, -0.30695462,  0.        ])}\n",
      "cos(angle):  -0.9517241520495809   sin(angle)  -0.3069546194562092\n",
      "theta dot:  -4.40120955054\n",
      "Angle:  197.87617632764048\n",
      "action:  [ 0.27708187]\n",
      "state:  {1: array([ 0.08249836]), 2: array([-4.58986323]), 3: array([-0.99659894, -0.08240481,  0.        ])}\n",
      "cos(angle):  -0.9965989402435032   sin(angle)  -0.08240480753891945\n",
      "theta dot:  -4.58986323439\n",
      "Angle:  184.7272174819716\n",
      "action:  [ 0.38572261]\n",
      "state:  {1: array([ 6.13599324]), 2: array([-4.59380845]), 3: array([-0.98918679,  0.14666114,  0.        ])}\n",
      "cos(angle):  -0.9891867917588623   sin(angle)  0.14666114349005085\n",
      "theta dot:  -4.59380844928\n",
      "Angle:  171.56611461999535\n",
      "action:  [ 0.55102623]\n",
      "state:  {1: array([ 5.91593531]), 2: array([-4.40115866]), 3: array([-0.93331826,  0.35905017,  0.        ])}\n",
      "cos(angle):  -0.9333182616796263   sin(angle)  0.35905016698968495\n",
      "theta dot:  -4.40115865765\n",
      "Angle:  158.95775330128583\n",
      "action:  [ 0.62863435]\n",
      "state:  {1: array([ 5.71405651]), 2: array([-4.03757588]), 3: array([-0.84237079,  0.53889837,  0.        ])}\n",
      "cos(angle):  -0.8423707866693625   sin(angle)  0.5388983742469813\n",
      "theta dot:  -4.03757587946\n",
      "Angle:  147.39097748159702\n",
      "action:  [ 0.66886341]\n",
      "state:  {1: array([ 5.53740288]), 2: array([-3.53307259]), 3: array([-0.73455722,  0.67854675,  0.        ])}\n",
      "cos(angle):  -0.734557216951016   sin(angle)  0.6785467522766415\n",
      "theta dot:  -3.53307258683\n",
      "Angle:  137.26949375310426\n",
      "action:  [ 0.6594924]\n",
      "state:  {1: array([ 5.39114095]), 2: array([-2.92523866]), 3: array([-0.6278221 ,  0.77835686,  0.        ])}\n",
      "cos(angle):  -0.6278221012949703   sin(angle)  0.7783568648926841\n",
      "theta dot:  -2.92523866291\n",
      "Angle:  128.88932187693035\n",
      "action:  [ 0.52825694]\n",
      "state:  {1: array([ 5.27802933]), 2: array([-2.26223247]), 3: array([-0.53595654,  0.84424558,  0.        ])}\n",
      "cos(angle):  -0.5359565362683221   sin(angle)  0.8442455751919952\n",
      "theta dot:  -2.26223247375\n",
      "Angle:  122.40851838071137\n",
      "action:  [ 0.32847307]\n",
      "state:  {1: array([ 5.19904046]), 2: array([-1.57977733]), 3: array([-0.46766875,  0.88390381,  0.        ])}\n",
      "cos(angle):  -0.46766874669288094   sin(angle)  0.8839038088879977\n",
      "theta dot:  -1.57977733129\n",
      "Angle:  117.88280028114985\n",
      "action:  [ 0.15515438]\n",
      "state:  {1: array([ 5.15436165]), 2: array([-0.89357632]), 3: array([-0.42772341,  0.90390967,  0.        ])}\n",
      "cos(angle):  -0.4277234069795788   sin(angle)  0.9039096675673857\n",
      "theta dot:  -0.893576318162\n",
      "Angle:  115.3228986821361\n",
      "action:  [ 0.10212451]\n",
      "state:  {1: array([ 5.14434538]), 2: array([-0.20032539]), 3: array([-0.4186483 ,  0.90814845,  0.        ])}\n",
      "cos(angle):  -0.4186482998586285   sin(angle)  0.9081484465798969\n",
      "theta dot:  -0.200325390843\n",
      "Angle:  114.74901005290792\n",
      "action:  [ 0.19600246]\n",
      "state:  {1: array([ 5.16985469]), 2: array([ 0.51018631]), 3: array([-0.44167583,  0.89717471,  0.        ])}\n",
      "cos(angle):  -0.4416758277527541   sin(angle)  0.8971747116247312\n",
      "theta dot:  0.510186313401\n",
      "Angle:  116.21058276127621\n",
      "action:  [ 0.32803985]\n",
      "state:  {1: array([ 5.23146836]), 2: array([ 1.23227332]), 3: array([-0.49608099,  0.86827625,  0.        ])}\n",
      "cos(angle):  -0.4960809932638834   sin(angle)  0.8682762510412909\n",
      "theta dot:  1.23227332391\n",
      "Angle:  119.74077753947381\n",
      "action:  [ 0.37413594]\n",
      "state:  {1: array([ 5.3284484]), 2: array([ 1.9396009]), 3: array([-0.57782351,  0.81616175,  0.        ])}\n",
      "cos(angle):  -0.5778235063774578   sin(angle)  0.8161617459043642\n",
      "theta dot:  1.93960090291\n",
      "Angle:  125.29731182970545\n",
      "action:  [ 0.30902627]\n",
      "state:  {1: array([ 5.45835221]), 2: array([ 2.59807615]), 3: array([-0.67867955,  0.73443452,  0.        ])}\n",
      "cos(angle):  -0.6786795545109555   sin(angle)  0.7344345187209074\n",
      "theta dot:  2.59807615211\n",
      "Angle:  132.74023434339318\n",
      "action:  [ 0.17191127]\n",
      "state:  {1: array([ 5.61708665]), 2: array([ 3.17468873]), 3: array([-0.78623837,  0.61792331,  0.        ])}\n",
      "cos(angle):  -0.7862383719653576   sin(angle)  0.617923314375873\n",
      "theta dot:  3.17468873093\n",
      "Angle:  141.8350263533245\n",
      "action:  [-0.01576474]\n",
      "state:  {1: array([ 5.79887497]), 2: array([ 3.63576651]), 3: array([-0.88499627,  0.46559812,  0.        ])}\n",
      "cos(angle):  -0.8849962662674419   sin(angle)  0.4655981192967677\n",
      "theta dot:  3.63576650557\n",
      "Angle:  152.25070580015176\n",
      "action:  [-0.06298882]\n",
      "state:  {1: array([ 5.99765081]), 2: array([ 3.97551677]), 3: array([-0.95951124,  0.28167035,  0.        ])}\n",
      "cos(angle):  -0.9595112379367147   sin(angle)  0.2816703468119308\n",
      "theta dot:  3.97551677241\n",
      "Angle:  163.63969578986064\n",
      "action:  [ 0.03495685]\n",
      "state:  {1: array([ 6.20725146]), 2: array([ 4.19201306]), 3: array([-0.99711841,  0.07586089,  0.        ])}\n",
      "cos(angle):  -0.9971184107021241   sin(angle)  0.07586089268437392\n",
      "theta dot:  4.19201305963\n",
      "Angle:  175.6489005061518\n",
      "action:  [ 0.05025891]\n",
      "state:  {1: array([ 0.13688853]), 2: array([ 4.25644757]), 3: array([-0.99064539, -0.13646142,  0.        ])}\n",
      "cos(angle):  -0.9906453858070204   sin(angle)  -0.1364614215786264\n",
      "theta dot:  4.25644756543\n",
      "Angle:  187.84353788728234\n",
      "action:  [ 0.04093406]\n",
      "state:  {1: array([ 0.34490062]), 2: array([ 4.16024161]), 3: array([-0.94110906, -0.33810315,  0.        ])}\n",
      "cos(angle):  -0.9411090595592871   sin(angle)  -0.3381031470061082\n",
      "theta dot:  4.16024160753\n",
      "Angle:  199.76172431068133\n",
      "action:  [-0.04858647]\n",
      "state:  {1: array([ 0.53986943]), 2: array([ 3.89937628]), 3: array([-0.85777581, -0.514024  ,  0.        ])}\n",
      "cos(angle):  -0.8577758052615477   sin(angle)  -0.514023995459262\n",
      "theta dot:  3.89937627709\n",
      "Angle:  210.93258835888292\n",
      "action:  [-0.17616865]\n",
      "state:  {1: array([ 0.71424108]), 2: array([ 3.48743298]), 3: array([-0.75559059, -0.65504417,  0.        ])}\n",
      "cos(angle):  -0.7555905860389802   sin(angle)  -0.6550441712505122\n",
      "theta dot:  3.48743298307\n",
      "Angle:  220.92332455943955\n",
      "action:  [-0.2872872]\n",
      "state:  {1: array([ 0.86189392]), 2: array([ 2.95305678]), 3: array([-0.65100101, -0.75907687,  0.        ])}\n",
      "cos(angle):  -0.6510010079914382   sin(angle)  -0.7590768654056923\n",
      "theta dot:  2.95305677524\n",
      "Angle:  229.38318927077825\n",
      "action:  [-0.20068362]\n",
      "state:  {1: array([ 0.97957625]), 2: array([ 2.35364658]), 3: array([-0.55737442, -0.83026126,  0.        ])}\n",
      "cos(angle):  -0.5573744231789057   sin(angle)  -0.830261255500931\n",
      "theta dot:  2.35364658365\n",
      "Angle:  236.12587428886997\n",
      "action:  [-0.30123751]\n",
      "state:  {1: array([ 1.0638645]), 2: array([ 1.68576502]), 3: array([-0.48549722, -0.87423821,  0.        ])}\n",
      "cos(angle):  -0.4854972246668612   sin(angle)  -0.8742382082938124\n",
      "theta dot:  1.6857650154\n",
      "Angle:  240.95522402740744\n",
      "action:  [-0.21540163]\n",
      "state:  {1: array([ 1.1137533]), 2: array([ 0.99777611]), 3: array([-0.44129656, -0.89736132,  0.        ])}\n",
      "cos(angle):  -0.4412965643566108   sin(angle)  -0.8973613220364758\n",
      "theta dot:  0.997776113938\n",
      "Angle:  243.81363535457893\n",
      "action:  [-0.11872248]\n",
      "state:  {1: array([ 1.12910064]), 2: array([ 0.30694675]), 3: array([-0.42747303, -0.9040281 ,  0.        ])}\n",
      "cos(angle):  -0.42747302725446845   sin(angle)  -0.9040281029757319\n",
      "theta dot:  0.306946750664\n",
      "Angle:  244.69297096572467\n",
      "action:  [-0.04099651]\n",
      "state:  {1: array([ 1.11023945]), 2: array([-0.3772238]), 3: array([-0.44444703, -0.89580513,  0.        ])}\n",
      "cos(angle):  -0.4444470289051643   sin(angle)  -0.895805134221373\n",
      "theta dot:  -0.377223802657\n",
      "Angle:  243.61230690158257\n",
      "action:  [ 0.03361864]\n",
      "state:  {1: array([ 1.05803771]), 2: array([-1.04403486]), 3: array([-0.49058296, -0.87139449,  0.        ])}\n",
      "cos(angle):  -0.49058295641065935   sin(angle)  -0.8713944932574322\n",
      "theta dot:  -1.04403485668\n",
      "Angle:  240.6213743480664\n",
      "action:  [-0.06586194]\n",
      "state:  {1: array([ 0.97266471]), 2: array([-1.70746002]), 3: array([-0.56309945, -0.82638914,  0.        ])}\n",
      "cos(angle):  -0.5630994485570167   sin(angle)  -0.8263891401965443\n",
      "theta dot:  -1.70746001704\n",
      "Angle:  235.72987315332736\n",
      "action:  [-0.08891064]\n",
      "state:  {1: array([ 0.85563528]), 2: array([-2.34058847]), 3: array([-0.65573901, -0.75498765,  0.        ])}\n",
      "cos(angle):  -0.6557390112935484   sin(angle)  -0.7549876482882085\n",
      "theta dot:  -2.34058846765\n",
      "Angle:  229.0245967945111\n",
      "action:  [-0.09071408]\n",
      "state:  {1: array([ 0.70961347]), 2: array([-2.92043632]), 3: array([-0.75861377, -0.65154059,  0.        ])}\n",
      "cos(angle):  -0.758613774492628   sin(angle)  -0.6515405905621292\n",
      "theta dot:  -2.9204363164\n",
      "Angle:  220.6581825955021\n",
      "action:  [-0.22037517]\n",
      "state:  {1: array([ 0.53750607]), 2: array([-3.44214803]), 3: array([-0.85898823, -0.51199533,  0.        ])}\n",
      "cos(angle):  -0.85898823421903   sin(angle)  -0.5119953256361555\n",
      "theta dot:  -3.44214803489\n",
      "Angle:  210.79717791189194\n",
      "action:  [-0.04805557]\n",
      "state:  {1: array([ 0.34583842]), 2: array([-3.83335286]), 3: array([-0.94079157, -0.33898558,  0.        ])}\n",
      "cos(angle):  -0.9407915702794369   sin(angle)  -0.33898557681876573\n",
      "theta dot:  -3.83335286466\n",
      "Angle:  199.81545656545057\n",
      "action:  [-0.25164254]\n",
      "state:  {1: array([ 0.1395715]), 2: array([-4.12533843]), 3: array([-0.9902757, -0.1391188,  0.       ])}\n",
      "cos(angle):  -0.9902756994262206   sin(angle)  -0.1391187950131454\n",
      "theta dot:  -4.12533842901\n",
      "Angle:  187.99726014926486\n",
      "action:  [ 0.05227833]\n",
      "state:  {1: array([ 6.21166502]), 2: array([-4.22183578]), 3: array([-0.99744351,  0.07145933,  0.        ])}\n",
      "cos(angle):  -0.9974435142097132   sin(angle)  0.07145933081814813\n",
      "theta dot:  -4.22183577614\n",
      "Angle:  175.90177800866695\n",
      "action:  [ 0.11417787]\n",
      "state:  {1: array([ 6.00410929]), 2: array([-4.1511146]), 3: array([-0.96131038,  0.27546753,  0.        ])}\n",
      "cos(angle):  -0.9613103756882698   sin(angle)  0.2754675327403896\n",
      "theta dot:  -4.15111459727\n",
      "Angle:  164.00973848249475\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -0.874650211216  self.max:  0.875992110438\n",
      "state:  {1: array([ 2.6852563]), 2: array([-0.8808824]), 3: array([ 0.89767295, -0.44066232,  0.        ])}\n",
      "cos(angle):  0.8976729486335229   sin(angle)  -0.4406623166230539\n",
      "theta dot:  -0.880882403884\n",
      "Angle:  333.85391396334717\n",
      "action:  [-0.53309913]\n",
      "state:  {1: array([ 2.6206891]), 2: array([-1.29134401]), 3: array([ 0.86736987, -0.49766406,  0.        ])}\n",
      "cos(angle):  0.8673698667114563   sin(angle)  -0.4976640576944958\n",
      "theta dot:  -1.29134401067\n",
      "Angle:  330.1544945286473\n",
      "action:  [-0.51855172]\n",
      "state:  {1: array([ 2.53357036]), 2: array([-1.74237481]), 3: array([ 0.82077938, -0.57124532,  0.        ])}\n",
      "cos(angle):  0.820779376371159   sin(angle)  -0.5712453197390516\n",
      "theta dot:  -1.74237481201\n",
      "Angle:  325.16297004810264\n",
      "action:  [-0.48857582]\n",
      "state:  {1: array([ 2.4213656]), 2: array([-2.24409517]), 3: array([ 0.75165599, -0.65955536,  0.        ])}\n",
      "cos(angle):  0.7516559933069933   sin(angle)  -0.6595553560738305\n",
      "theta dot:  -2.24409517416\n",
      "Angle:  318.73412596628935\n",
      "action:  [-0.45549398]\n",
      "state:  {1: array([ 2.28101131]), 2: array([-2.80708579]), 3: array([ 0.65199679, -0.75822173,  0.        ])}\n",
      "cos(angle):  0.6519967905582457   sin(angle)  -0.7582217255537771\n",
      "theta dot:  -2.80708578804\n",
      "Angle:  310.6924363519553\n",
      "action:  [-0.4185619]\n",
      "state:  {1: array([ 2.10908449]), 2: array([-3.43853637]), 3: array([ 0.51266698, -0.85858754,  0.        ])}\n",
      "cos(angle):  0.512666982647097   sin(angle)  -0.8585875406174499\n",
      "theta dot:  -3.43853636757\n",
      "Angle:  300.84177830888274\n",
      "action:  [-0.28599543]\n",
      "state:  {1: array([ 1.90281567]), 2: array([-4.12537634]), 3: array([ 0.32595275, -0.94538606,  0.        ])}\n",
      "cos(angle):  0.32595275451026773   sin(angle)  -0.9453860596746544\n",
      "theta dot:  -4.12537633726\n",
      "Angle:  289.02347329381206\n",
      "action:  [-0.16660014]\n",
      "state:  {1: array([ 1.65984538]), 2: array([-4.8594059]), 3: array([ 0.08893141, -0.99603775,  0.        ])}\n",
      "cos(angle):  0.08893140960468887   sin(angle)  -0.9960377524901971\n",
      "theta dot:  -4.85940590374\n",
      "Angle:  275.102333386244\n",
      "action:  [-0.16737572]\n",
      "state:  {1: array([ 1.37826835]), 2: array([-5.63154058]), 3: array([-0.19134077, -0.98152367,  0.        ])}\n",
      "cos(angle):  -0.1913407726412104   sin(angle)  -0.9815236669205\n",
      "theta dot:  -5.63154057624\n",
      "Angle:  258.9691957537894\n",
      "action:  [-0.22977559]\n",
      "state:  {1: array([ 1.05816087]), 2: array([-6.40214967]), 3: array([-0.49047563, -0.87145491,  0.        ])}\n",
      "cos(angle):  -0.49047563204514344   sin(angle)  -0.8714549066761383\n",
      "theta dot:  -6.40214966559\n",
      "Angle:  240.62843086000123\n",
      "action:  [-0.37420338]\n",
      "state:  {1: array([ 0.7025673]), 2: array([-7.11187135]), 3: array([-0.76318577, -0.64617914,  0.        ])}\n",
      "cos(angle):  -0.7631857690018355   sin(angle)  -0.6461791407907539\n",
      "theta dot:  -7.1118713533\n",
      "Angle:  220.25446785397202\n",
      "action:  [-0.21595339]\n",
      "state:  {1: array([ 0.32112236]), 2: array([-7.62889872]), 3: array([-0.94888176, -0.31563175,  0.        ])}\n",
      "cos(angle):  -0.948881762299928   sin(angle)  -0.3156317493133142\n",
      "theta dot:  -7.62889871688\n",
      "Angle:  198.39933402028154\n",
      "action:  [ 0.21504511]\n",
      "state:  {1: array([ 6.21263938]), 2: array([-7.83336576]), 3: array([-0.99751267,  0.07048742,  0.        ])}\n",
      "cos(angle):  -0.9975126680692631   sin(angle)  0.07048742470356101\n",
      "theta dot:  -7.83336576285\n",
      "Angle:  175.95760478252782\n",
      "action:  [-0.0900977]\n",
      "state:  {1: array([ 5.82293864]), 2: array([-7.79401485]), 3: array([-0.89594296,  0.44416912,  0.        ])}\n",
      "cos(angle):  -0.8959429628127106   sin(angle)  0.44416912025306526\n",
      "theta dot:  -7.79401484908\n",
      "Angle:  153.62944917975165\n",
      "action:  [ 0.12231899]\n",
      "state:  {1: array([ 5.45081163]), 2: array([-7.44254016]), 3: array([-0.67312225,  0.73953123,  0.        ])}\n",
      "cos(angle):  -0.6731222510560619   sin(angle)  0.7395312266113041\n",
      "theta dot:  -7.44254015998\n",
      "Angle:  132.3081920369441\n",
      "action:  [ 0.16922886]\n",
      "state:  {1: array([ 5.10768626]), 2: array([-6.86250741]), 3: array([-0.38508256,  0.92288213,  0.        ])}\n",
      "cos(angle):  -0.38508256111476596   sin(angle)  0.9228821274275998\n",
      "theta dot:  -6.86250741172\n",
      "Angle:  112.64860243117494\n",
      "action:  [-0.26776093]\n",
      "state:  {1: array([ 4.79716076]), 2: array([-6.21050996]), 3: array([-0.08467029,  0.99640902,  0.        ])}\n",
      "cos(angle):  -0.08467028774445372   sin(angle)  0.9964090236309944\n",
      "theta dot:  -6.21050995513\n",
      "Angle:  94.85684358339984\n",
      "action:  [ 0.17417461]\n",
      "state:  {1: array([ 4.52530691]), 2: array([-5.437077]), 3: array([ 0.18599267,  0.98255113,  0.        ])}\n",
      "cos(angle):  0.18599267205674314   sin(angle)  0.9825511314640032\n",
      "theta dot:  -5.43707699575\n",
      "Angle:  79.28080176974646\n",
      "action:  [ 0.18941699]\n",
      "state:  {1: array([ 4.29171936]), 2: array([-4.6717511]), 3: array([ 0.40837179,  0.91281569,  0.        ])}\n",
      "cos(angle):  0.4083717855736269   sin(angle)  0.9128156904585984\n",
      "theta dot:  -4.67175109836\n",
      "Angle:  65.89725202272359\n",
      "action:  [ 0.22221051]\n",
      "state:  {1: array([ 4.09402897]), 2: array([-3.95380775]), 3: array([ 0.57969963,  0.81483025,  0.        ])}\n",
      "cos(angle):  0.5796996265509581   sin(angle)  0.8148302540877331\n",
      "theta dot:  -3.95380775328\n",
      "Angle:  54.570453646261626\n",
      "action:  [ 0.23210956]\n",
      "state:  {1: array([ 3.92863554]), 2: array([-3.30786863]), 3: array([ 0.70594283,  0.70826882,  0.        ])}\n",
      "cos(angle):  0.7059428306547277   sin(angle)  0.7082688189149586\n",
      "theta dot:  -3.30786862857\n",
      "Angle:  45.09413022597852\n",
      "action:  [ 0.10488383]\n",
      "state:  {1: array([ 3.79058882]), 2: array([-2.76093444]), 3: array([ 0.79669091,  0.60438696,  0.        ])}\n",
      "cos(angle):  0.7966909051317638   sin(angle)  0.6043869635261261\n",
      "theta dot:  -2.76093444042\n",
      "Angle:  37.18465417434294\n",
      "action:  [-0.09389972]\n",
      "state:  {1: array([ 3.67450236]), 2: array([-2.32172918]), 3: array([ 0.86133247,  0.50804171,  0.        ])}\n",
      "cos(angle):  0.8613324670839673   sin(angle)  0.5080417120149154\n",
      "theta dot:  -2.32172917628\n",
      "Angle:  30.533405579192163\n",
      "action:  [-0.26392306]\n",
      "state:  {1: array([ 3.57548804]), 2: array([-1.98028635]), 3: array([ 0.90733498,  0.4204084 ,  0.        ])}\n",
      "cos(angle):  0.9073349840091163   sin(angle)  0.4204084047603909\n",
      "theta dot:  -1.98028635155\n",
      "Angle:  24.86031633679147\n",
      "action:  [-0.41060704]\n",
      "state:  {1: array([ 3.48915949]), 2: array([-1.7265711]), 3: array([ 0.94020426,  0.34061114,  0.        ])}\n",
      "cos(angle):  0.9402042591010686   sin(angle)  0.3406111436348062\n",
      "theta dot:  -1.72657110365\n",
      "Angle:  19.914066039843135\n",
      "action:  [-0.52268526]\n",
      "state:  {1: array([ 3.41168371]), 2: array([-1.54951553]), 3: array([ 0.9637466 ,  0.26681919,  0.        ])}\n",
      "cos(angle):  0.9637466049064375   sin(angle)  0.2668191925842575\n",
      "theta dot:  -1.54951553483\n",
      "Angle:  15.47504139843403\n",
      "action:  [-0.53049723]\n",
      "state:  {1: array([ 3.34023492]), 2: array([-1.42897572]), 3: array([ 0.98033541,  0.19733848,  0.        ])}\n",
      "cos(angle):  0.9803354137940365   sin(angle)  0.1973384819574611\n",
      "theta dot:  -1.42897572432\n",
      "Angle:  11.381337069793346\n",
      "action:  [-0.29877232]\n",
      "state:  {1: array([ 3.27394554]), 2: array([-1.32578771]), 3: array([ 0.99125414,  0.13196681,  0.        ])}\n",
      "cos(angle):  0.991254135207233   sin(angle)  0.1319668118678346\n",
      "theta dot:  -1.3257877108\n",
      "Angle:  7.583243933440144\n",
      "action:  [-0.47972162]\n",
      "state:  {1: array([ 3.209007]), 2: array([-1.29877085]), 3: array([ 0.99772851,  0.06736329,  0.        ])}\n",
      "cos(angle):  0.9977285137202714   sin(angle)  0.0673632905189338\n",
      "theta dot:  -1.29877084563\n",
      "Angle:  3.8625482335914967\n",
      "action:  [-0.26158441]\n",
      "state:  {1: array([ 3.14463269]), 2: array([-1.28748604]), 3: array([ 0.99999538,  0.00304004,  0.        ])}\n",
      "cos(angle):  0.9999953790824008   sin(angle)  0.003040035171764216\n",
      "theta dot:  -1.28748603965\n",
      "Angle:  0.1741810458951818\n",
      "action:  [-0.17558875]\n",
      "state:  {1: array([ 3.07905548]), 2: array([-1.31154433]), 3: array([ 0.99804519, -0.06249642,  0.        ])}\n",
      "cos(angle):  0.998045187997209   sin(angle)  -0.06249642162248866\n",
      "theta dot:  -1.31154432528\n",
      "Angle:  356.416892107917\n",
      "action:  [-0.09340134]\n",
      "state:  {1: array([ 3.01043414]), 2: array([-1.37242684]), 3: array([ 0.99141104, -0.1307828 ,  0.        ])}\n",
      "cos(angle):  0.9914110447712963   sin(angle)  -0.13078279820177754\n",
      "theta dot:  -1.37242684254\n",
      "Angle:  352.4851880135521\n",
      "action:  [-0.02214612]\n",
      "state:  {1: array([ 2.93674234]), 2: array([-1.47383586]), 3: array([ 0.97909145, -0.2034206 ,  0.        ])}\n",
      "cos(angle):  0.9790914452157681   sin(angle)  -0.2034206034434532\n",
      "theta dot:  -1.47383585904\n",
      "Angle:  348.26296916604076\n",
      "action:  [-0.02616646]\n",
      "state:  {1: array([ 2.85522603]), 2: array([-1.63032628]), 3: array([ 0.95927652, -0.28246869,  0.        ])}\n",
      "cos(angle):  0.9592765196692841   sin(angle)  -0.2824686864259216\n",
      "theta dot:  -1.63032628037\n",
      "Angle:  343.59243933304646\n",
      "action:  [-0.10418871]\n",
      "state:  {1: array([ 2.76233572]), 2: array([-1.8578061]), 3: array([ 0.92894  , -0.3702303,  0.       ])}\n",
      "cos(angle):  0.9289399988361333   sin(angle)  -0.3702303047595167\n",
      "theta dot:  -1.85780610097\n",
      "Angle:  338.27022934172834\n",
      "action:  [ 0.24542395]\n",
      "state:  {1: array([ 2.65740246]), 2: array([-2.09866524]), 3: array([ 0.8850522 , -0.46549179,  0.        ])}\n",
      "cos(angle):  0.8850521980397128   sin(angle)  -0.46549179020158143\n",
      "theta dot:  -2.09866523777\n",
      "Angle:  332.25801036417255\n",
      "action:  [-0.42200508]\n",
      "state:  {1: array([ 2.53184822]), 2: array([-2.51108484]), 3: array([ 0.8197944 , -0.57265797,  0.        ])}\n",
      "cos(angle):  0.8197943961873914   sin(angle)  -0.5726579677082564\n",
      "theta dot:  -2.51108484281\n",
      "Angle:  325.0642990115986\n",
      "action:  [-0.50120415]\n",
      "state:  {1: array([ 2.38106027]), 2: array([-3.01575894]), 3: array([ 0.72446914, -0.68930724,  0.        ])}\n",
      "cos(angle):  0.7244691389199088   sin(angle)  -0.6893072368346685\n",
      "theta dot:  -3.01575894081\n",
      "Angle:  316.4248062476398\n",
      "action:  [-0.58697716]\n",
      "state:  {1: array([ 2.20002097]), 2: array([-3.62078594]), 3: array([ 0.58851808, -0.80848406,  0.        ])}\n",
      "cos(angle):  0.5885180751944126   sin(angle)  -0.8084840599352987\n",
      "theta dot:  -3.62078594286\n",
      "Angle:  306.05204285137455\n",
      "action:  [-0.64115484]\n",
      "state:  {1: array([ 1.98385486]), 2: array([-4.32332221]), 3: array([ 0.40141251, -0.91589737,  0.        ])}\n",
      "cos(angle):  0.4014125074439287   sin(angle)  -0.9158973735455178\n",
      "theta dot:  -4.3233222143\n",
      "Angle:  293.66666599605816\n",
      "action:  [-0.54880803]\n",
      "state:  {1: array([ 1.72922654]), 2: array([-5.09256645]), 3: array([ 0.15776828, -0.98747616,  0.        ])}\n",
      "cos(angle):  0.15776827625409362   sin(angle)  -0.9874761622478854\n",
      "theta dot:  -5.09256644864\n",
      "Angle:  279.0775718918594\n",
      "action:  [-0.33503545]\n",
      "state:  {1: array([ 1.4350551]), 2: array([-5.88342889]), 3: array([-0.13532476, -0.9908013 ,  0.        ])}\n",
      "cos(angle):  -0.13532475930798465   sin(angle)  -0.9908012966878051\n",
      "theta dot:  -5.88342888848\n",
      "Angle:  262.22282908680523\n",
      "action:  [ 0.02317958]\n",
      "state:  {1: array([ 1.10390245]), 2: array([-6.62305292]), 3: array([-0.45011478, -0.89297071,  0.        ])}\n",
      "cos(angle):  -0.45011478323430515   sin(angle)  -0.8929707060782759\n",
      "theta dot:  -6.62305292427\n",
      "Angle:  243.24922445272608\n",
      "action:  [ 0.11170861]\n",
      "state:  {1: array([ 0.74010122]), 2: array([-7.27602466]), 3: array([-0.7384003 , -0.67436265,  0.        ])}\n",
      "cos(angle):  -0.7384003049220706   sin(angle)  -0.6743626544308285\n",
      "theta dot:  -7.27602466222\n",
      "Angle:  222.40499795669314\n",
      "action:  [-0.09937637]\n",
      "state:  {1: array([ 0.35026606]), 2: array([-7.79670311]), 3: array([-0.93928145, -0.34314773,  0.        ])}\n",
      "cos(angle):  -0.9392814473567884   sin(angle)  -0.3431477271545546\n",
      "theta dot:  -7.79670310884\n",
      "Angle:  200.06914107500225\n",
      "action:  [-0.86306327]\n",
      "state:  {1: array([ 6.2242752]), 2: array([-8.]), 3: array([-0.9982653 ,  0.05887604,  0.        ])}\n",
      "cos(angle):  -0.998265301395033   sin(angle)  0.058876039529540285\n",
      "theta dot:  -8.0\n",
      "Angle:  176.62428646059914\n",
      "action:  [-0.19820964]\n",
      "state:  {1: array([ 5.82499648]), 2: array([-7.98557442]), 3: array([-0.89685509,  0.44232447,  0.        ])}\n",
      "cos(angle):  -0.8968550937814028   sin(angle)  0.44232447451882095\n",
      "theta dot:  -7.98557441563\n",
      "Angle:  153.74735440664938\n",
      "action:  [-0.22042481]\n",
      "state:  {1: array([ 5.44065174]), 2: array([-7.68689478]), 3: array([-0.66557408,  0.74633179,  0.        ])}\n",
      "cos(angle):  -0.6655740823510755   sin(angle)  0.7463317900923984\n",
      "theta dot:  -7.68689478149\n",
      "Angle:  131.7260744749439\n",
      "action:  [ 0.5043888]\n",
      "state:  {1: array([ 5.08807736]), 2: array([-7.05148762]), 3: array([-0.36691298,  0.93025527,  0.        ])}\n",
      "cos(angle):  -0.36691298395678323   sin(angle)  0.9302552672271893\n",
      "theta dot:  -7.05148761852\n",
      "Angle:  111.5250977221192\n",
      "action:  [-0.21985174]\n",
      "state:  {1: array([ 4.76873866]), 2: array([-6.38677393]), 3: array([-0.05631987,  0.99841278,  0.        ])}\n",
      "cos(angle):  -0.05631986597397786   sin(angle)  0.9984127767094495\n",
      "theta dot:  -6.38677392952\n",
      "Angle:  93.22838096452267\n",
      "action:  [-0.11180433]\n",
      "state:  {1: array([ 4.48600191]), 2: array([-5.654735]), 3: array([ 0.22445825,  0.97448371,  0.        ])}\n",
      "cos(angle):  0.22445825232894803   sin(angle)  0.9744837058470677\n",
      "theta dot:  -5.65473499671\n",
      "Angle:  77.02879636736452\n",
      "action:  [ 0.0231342]\n",
      "state:  {1: array([ 4.23998181]), 2: array([-4.92040209]), 3: array([ 0.45503113,  0.89047553,  0.        ])}\n",
      "cos(angle):  0.45503112931963946   sin(angle)  0.890475531022663\n",
      "theta dot:  -4.92040208744\n",
      "Angle:  62.93291567377133\n",
      "action:  [ 0.13321642]\n",
      "state:  {1: array([ 4.02835366]), 2: array([-4.23256298]), 3: array([ 0.63192565,  0.77502901,  0.        ])}\n",
      "cos(angle):  0.6319256512403337   sin(angle)  0.7750290132017512\n",
      "theta dot:  -4.2325629764\n",
      "Angle:  50.80754427461528\n",
      "action:  [ 0.16389052]\n",
      "state:  {1: array([ 3.84701828]), 2: array([-3.62670764]), 3: array([ 0.76133566,  0.64835793,  0.        ])}\n",
      "cos(angle):  0.7613356639576787   sin(angle)  0.6483579310736628\n",
      "theta dot:  -3.62670763909\n",
      "Angle:  40.41781650793973\n",
      "action:  [ 0.10787212]\n",
      "state:  {1: array([ 3.69080536]), 2: array([-3.12425837]), 3: array([ 0.85293577,  0.52201588,  0.        ])}\n",
      "cos(angle):  0.8529357665951496   sin(angle)  0.5220158791289251\n",
      "theta dot:  -3.12425837271\n",
      "Angle:  31.467496494445868\n",
      "action:  [ 0.00283982]\n",
      "state:  {1: array([ 3.55418933]), 2: array([-2.73232049]), 3: array([ 0.91608267,  0.40098945,  0.        ])}\n",
      "cos(angle):  0.9160826707770808   sin(angle)  0.4009894516093042\n",
      "theta dot:  -2.73232049082\n",
      "Angle:  23.639993178506757\n",
      "action:  [-0.07672307]\n",
      "state:  {1: array([ 3.43203499]), 2: array([-2.44308686]), 3: array([ 0.95811729,  0.28637606,  0.        ])}\n",
      "cos(angle):  0.958117294294178   sin(angle)  0.28637606459759085\n",
      "theta dot:  -2.44308686297\n",
      "Angle:  16.641081233402417\n",
      "action:  [-0.01547707]\n",
      "state:  {1: array([ 3.32050367]), 2: array([-2.23062638]), 3: array([ 0.98403807,  0.17795808,  0.        ])}\n",
      "cos(angle):  0.9840380691925588   sin(angle)  0.17795808040035999\n",
      "theta dot:  -2.23062637577\n",
      "Angle:  10.250822326491232\n",
      "action:  [ 0.32748665]\n",
      "state:  {1: array([ 3.21810193]), 2: array([-2.04803482]), 3: array([ 0.99707459,  0.07643466,  0.        ])}\n",
      "cos(angle):  0.9970745926313441   sin(angle)  0.07643465659659379\n",
      "theta dot:  -2.04803481825\n",
      "Angle:  4.383648477411214\n",
      "action:  [ 0.64465022]\n",
      "state:  {1: array([ 3.12340137]), 2: array([-1.89401129]), 3: array([ 0.99983454, -0.01819028,  0.        ])}\n",
      "cos(angle):  0.999834543109592   sin(angle)  -0.018190283253247667\n",
      "theta dot:  -1.89401129296\n",
      "Angle:  358.95771849376143\n",
      "action:  [ 0.79101204]\n",
      "state:  {1: array([ 3.03395126]), 2: array([-1.7890022]), 3: array([ 0.99421226, -0.10743365,  0.        ])}\n",
      "cos(angle):  0.9942122565057243   sin(angle)  -0.10743364935529184\n",
      "theta dot:  -1.78900219939\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  353.83261670025286\n",
      "action:  [ 0.77009347]\n",
      "state:  {1: array([ 2.94624809]), 2: array([-1.75406342]), 3: array([ 0.98098085, -0.19410456,  0.        ])}\n",
      "cos(angle):  0.9809808457000434   sin(angle)  -0.19410456040399368\n",
      "theta dot:  -1.75406341559\n",
      "Angle:  348.80760691533817\n",
      "action:  [ 0.50636875]\n",
      "state:  {1: array([ 2.85506376]), 2: array([-1.82368652]), 3: array([ 0.95923067, -0.28262434,  0.        ])}\n",
      "cos(angle):  0.9592306714311006   sin(angle)  -0.2826243425217295\n",
      "theta dot:  -1.82368652266\n",
      "Angle:  343.5831420872379\n",
      "action:  [ 0.69411958]\n",
      "state:  {1: array([ 2.75848692]), 2: array([-1.93153684]), 3: array([ 0.92750818, -0.37380286,  0.        ])}\n",
      "cos(angle):  0.9275081777698175   sin(angle)  -0.373802862709895\n",
      "theta dot:  -1.93153684297\n",
      "Angle:  338.04970957299724\n",
      "action:  [ 0.14522827]\n",
      "state:  {1: array([ 2.64898168]), 2: array([-2.19010475]), 3: array([ 0.88110106, -0.47292803,  0.        ])}\n",
      "cos(angle):  0.8811010609732282   sin(angle)  -0.47292802872303064\n",
      "theta dot:  -2.19010474983\n",
      "Angle:  331.7755363018978\n",
      "action:  [-0.02292175]\n",
      "state:  {1: array([ 2.52156973]), 2: array([-2.54823903]), 3: array([ 0.81386514, -0.58105382,  0.        ])}\n",
      "cos(angle):  0.8138651363443947   sin(angle)  -0.5810538183706565\n",
      "theta dot:  -2.54823903409\n",
      "Angle:  324.47538628064535\n",
      "action:  [-0.09085504]\n",
      "state:  {1: array([ 2.37168685]), 2: array([-2.99765765]), 3: array([ 0.71797624, -0.69606761,  0.        ])}\n",
      "cos(angle):  0.7179762371629328   sin(angle)  -0.6960676137196415\n",
      "theta dot:  -2.99765765384\n",
      "Angle:  315.88774976277904\n",
      "action:  [-0.00913496]\n",
      "state:  {1: array([ 2.19563292]), 2: array([-3.52107861]), 3: array([ 0.58496474, -0.81105872,  0.        ])}\n",
      "cos(angle):  0.5849647447486305   sin(angle)  -0.811058720069743\n",
      "theta dot:  -3.52107860855\n",
      "Angle:  305.8006261706786\n",
      "action:  [ 0.08717976]\n",
      "state:  {1: array([ 1.98981813]), 2: array([-4.11629569]), 3: array([ 0.40686708, -0.91348737,  0.        ])}\n",
      "cos(angle):  0.40686707874304184   sin(angle)  -0.9134873727836107\n",
      "theta dot:  -4.11629568526\n",
      "Angle:  294.00833524653035\n",
      "action:  [ 0.06168539]\n",
      "state:  {1: array([ 1.75021021]), 2: array([-4.79215841]), 3: array([ 0.1784529 , -0.98394846,  0.        ])}\n",
      "cos(angle):  0.17845289651843788   sin(angle)  -0.9839484558269197\n",
      "theta dot:  -4.7921584057\n",
      "Angle:  280.27984477948564\n",
      "action:  [-0.02465685]\n",
      "state:  {1: array([ 1.4735193]), 2: array([-5.53381827]), 3: array([-0.09712368, -0.99527232,  0.        ])}\n",
      "cos(angle):  -0.09712368278834867   sin(angle)  -0.995272319640021\n",
      "theta dot:  -5.53381827478\n",
      "Angle:  264.4266602642574\n",
      "action:  [-0.09846678]\n",
      "state:  {1: array([ 1.15876717]), 2: array([-6.29504253]), 3: array([-0.40046949, -0.91631009,  0.        ])}\n",
      "cos(angle):  -0.40046948723454057   sin(angle)  -0.9163100947791114\n",
      "theta dot:  -6.2950425308\n",
      "Angle:  246.39273399192302\n",
      "action:  [-0.14040253]\n",
      "state:  {1: array([ 0.8086004]), 2: array([-7.00333548]), 3: array([-0.69051147, -0.72332144,  0.        ])}\n",
      "cos(angle):  -0.6905114719597514   sin(angle)  -0.7233214410564486\n",
      "theta dot:  -7.00333548172\n",
      "Angle:  226.32970262716884\n",
      "action:  [-0.14069305]\n",
      "state:  {1: array([ 0.43025387]), 2: array([-7.56693052]), 3: array([-0.90885989, -0.41710155,  0.        ])}\n",
      "cos(angle):  -0.9088598890735182   sin(angle)  -0.41710154882626854\n",
      "theta dot:  -7.56693052015\n",
      "Angle:  204.65209418517327\n",
      "action:  [-0.12693433]\n",
      "state:  {1: array([ 0.03531403]), 2: array([-7.89879683]), 3: array([-0.99937652, -0.03530669,  0.        ])}\n",
      "cos(angle):  -0.9993765244679321   sin(angle)  -0.035306689768322824\n",
      "theta dot:  -7.89879683101\n",
      "Angle:  182.02376101764773\n",
      "action:  [-0.09648752]\n",
      "state:  {1: array([ 5.92151184]), 2: array([-7.93974998]), 3: array([-0.93530599,  0.35383993,  0.        ])}\n",
      "cos(angle):  -0.9353059932454448   sin(angle)  0.3538399341498242\n",
      "theta dot:  -7.93974997624\n",
      "Angle:  159.27726417087126\n",
      "action:  [ 0.05474506]\n",
      "state:  {1: array([ 5.53820392]), 2: array([-7.66615827]), 3: array([-0.73510052,  0.67795813,  0.        ])}\n",
      "cos(angle):  -0.7351005240879589   sin(angle)  0.6779581251711704\n",
      "theta dot:  -7.66615826715\n",
      "Angle:  137.31538983794064\n",
      "action:  [ 0.23669736]\n",
      "state:  {1: array([ 5.18209467]), 2: array([-7.12218507]), 3: array([-0.45262387,  0.89170154,  0.        ])}\n",
      "cos(angle):  -0.4526238686514168   sin(angle)  0.8917015383675331\n",
      "theta dot:  -7.12218506911\n",
      "Angle:  116.91188028165391\n",
      "action:  [ 0.16648685]\n",
      "state:  {1: array([ 4.86067288]), 2: array([-6.42843589]), 3: array([-0.14774108,  0.98902607,  0.        ])}\n",
      "cos(angle):  -0.1477410792221046   sin(angle)  0.9890260732206648\n",
      "theta dot:  -6.42843588844\n",
      "Angle:  98.49581108254313\n",
      "action:  [ 0.17588991]\n",
      "state:  {1: array([ 4.57765873]), 2: array([-5.66028285]), 3: array([ 0.13432301,  0.9909376 ,  0.        ])}\n",
      "cos(angle):  0.13432300689145435   sin(angle)  0.9909376013754037\n",
      "theta dot:  -5.66028284689\n",
      "Angle:  82.28033310252215\n",
      "action:  [ 0.34024044]\n",
      "state:  {1: array([ 4.33435656]), 2: array([-4.86604358]), 3: array([ 0.36909254,  0.92939265,  0.        ])}\n",
      "cos(angle):  0.36909253562133565   sin(angle)  0.9293926512236973\n",
      "theta dot:  -4.86604357955\n",
      "Angle:  68.3401776989208\n",
      "action:  [ 0.35451483]\n",
      "state:  {1: array([ 4.12856546]), 2: array([-4.11582187]), 3: array([ 0.55121815,  0.83436116,  0.        ])}\n",
      "cos(angle):  0.5512181537230971   sin(angle)  0.8343611610124839\n",
      "theta dot:  -4.1158218668\n",
      "Angle:  56.549244161483955\n",
      "action:  [ 0.42542419]\n",
      "state:  {1: array([ 3.95725359]), 2: array([-3.42623737]), 3: array([ 0.68538726,  0.72817876,  0.        ])}\n",
      "cos(angle):  0.6853872609402177   sin(angle)  0.7281787572710878\n",
      "theta dot:  -3.42623736783\n",
      "Angle:  46.73382007487049\n",
      "action:  [ 0.55398119]\n",
      "state:  {1: array([ 3.81740329]), 2: array([-2.79700612]), 3: array([ 0.78020013,  0.62552998,  0.        ])}\n",
      "cos(angle):  0.7802001319134486   sin(angle)  0.6255299786279132\n",
      "theta dot:  -2.79700612209\n",
      "Angle:  38.72100650890064\n",
      "action:  [ 0.66520403]\n",
      "state:  {1: array([ 3.70599939]), 2: array([-2.22807803]), 3: array([ 0.8449061 ,  0.53491465,  0.        ])}\n",
      "cos(angle):  0.8449060968677221   sin(angle)  0.5349146543849321\n",
      "theta dot:  -2.22807803302\n",
      "Angle:  32.338048049132226\n",
      "action:  [ 0.68637884]\n",
      "state:  {1: array([ 3.61980262]), 2: array([-1.72393522]), 3: array([ 0.8878201 ,  0.46019069,  0.        ])}\n",
      "cos(angle):  0.8878200993267633   sin(angle)  0.46019069007468655\n",
      "theta dot:  -1.72393521593\n",
      "Angle:  27.39934899661972\n",
      "action:  [ 0.87746204]\n",
      "state:  {1: array([ 3.55744398]), 2: array([-1.24717289]), 3: array([ 0.91477274,  0.40396885,  0.        ])}\n",
      "cos(angle):  0.9147727426179477   sin(angle)  0.40396884702070535\n",
      "theta dot:  -1.24717289173\n",
      "Angle:  23.82647020061586\n",
      "action:  [ 0.49351665]\n",
      "state:  {1: array([ 3.51393554]), 2: array([-0.87016876]), 3: array([ 0.93147756,  0.36379878,  0.        ])}\n",
      "cos(angle):  0.9314775628653923   sin(angle)  0.36379877663118815\n",
      "theta dot:  -0.870168758693\n",
      "Angle:  21.333626163107795\n",
      "action:  [ 0.43621123]\n",
      "state:  {1: array([ 3.48734114]), 2: array([-0.53188799]), 3: array([ 0.94082205,  0.33890097,  0.        ])}\n",
      "cos(angle):  0.9408220525054697   sin(angle)  0.3389009671266744\n",
      "theta dot:  -0.531887991497\n",
      "Angle:  19.809882871960156\n",
      "action:  [ 0.39697459]\n",
      "state:  {1: array([ 3.47643284]), 2: array([-0.21816608]), 3: array([ 0.94446284,  0.32861823,  0.        ])}\n",
      "cos(angle):  0.9444628397664618   sin(angle)  0.32861823488703523\n",
      "theta dot:  -0.218166078109\n",
      "Angle:  19.18488455804902\n",
      "action:  [ 0.41238698]\n",
      "state:  {1: array([ 3.48094062]), 2: array([ 0.09015564]), 3: array([ 0.94297191,  0.33287231,  0.        ])}\n",
      "cos(angle):  0.9429719095651227   sin(angle)  0.33287231451580057\n",
      "theta dot:  0.0901556448859\n",
      "Angle:  19.443160851648827\n",
      "action:  [ 0.62498085]\n",
      "state:  {1: array([ 3.50261847]), 2: array([ 0.43355701]), 3: array([ 0.93553496,  0.35323411,  0.        ])}\n",
      "cos(angle):  0.9355349621348805   sin(angle)  0.3532341073895436\n",
      "theta dot:  0.433557008299\n",
      "Angle:  20.685207284896364\n",
      "action:  [ 0.34449445]\n",
      "state:  {1: array([ 3.54012631]), 2: array([ 0.75015676]), 3: array([ 0.92163102,  0.38806733,  0.        ])}\n",
      "cos(angle):  0.9216310249824873   sin(angle)  0.3880673315157176\n",
      "theta dot:  0.750156755983\n",
      "Angle:  22.834243064067834\n",
      "action:  [ 0.2871661]\n",
      "state:  {1: array([ 3.59434042]), 2: array([ 1.08428217]), 3: array([ 0.89924852,  0.43743811,  0.        ])}\n",
      "cos(angle):  0.8992485219004512   sin(angle)  0.43743810517586795\n",
      "theta dot:  1.08428216967\n",
      "Angle:  25.940475406500468\n",
      "action:  [ 0.23119575]\n",
      "state:  {1: array([ 3.66669242]), 2: array([ 1.44704011]), 3: array([ 0.86527393,  0.50129934,  0.        ])}\n",
      "cos(angle):  0.8652739314884986   sin(angle)  0.5012993352144376\n",
      "theta dot:  1.44704011038\n",
      "Angle:  30.08593026816021\n",
      "action:  [ 0.09853774]\n",
      "state:  {1: array([ 3.75858219]), 2: array([ 1.83779527]), 3: array([ 0.81562395,  0.57858238,  0.        ])}\n",
      "cos(angle):  0.8156239529907944   sin(angle)  0.5785823772875133\n",
      "theta dot:  1.83779527308\n",
      "Angle:  35.35081359439806\n",
      "action:  [ 0.29525365]\n",
      "state:  {1: array([ 3.87438319]), 2: array([ 2.3160201]), 3: array([ 0.74331058,  0.66894647,  0.        ])}\n",
      "cos(angle):  0.7433105780195086   sin(angle)  0.6689464736466619\n",
      "theta dot:  2.31602010392\n",
      "Angle:  41.98570694024852\n",
      "action:  [ 0.20419235]\n",
      "state:  {1: array([ 4.01680113]), 2: array([ 2.84835881]), 3: array([ 0.64083683,  0.76767712,  0.        ])}\n",
      "cos(angle):  0.6408368274733028   sin(angle)  0.7676771199885746\n",
      "theta dot:  2.84835881227\n",
      "Angle:  50.14563478288994\n",
      "action:  [ 0.51090417]\n",
      "state:  {1: array([ 4.19183875]), 2: array([ 3.50075228]), 3: array([ 0.49735757,  0.86754565,  0.        ])}\n",
      "cos(angle):  0.4973575660669549   sin(angle)  0.8675456480646737\n",
      "theta dot:  3.5007522779\n",
      "Angle:  60.174527863209164\n",
      "action:  [ 0.57960478]\n",
      "state:  {1: array([ 4.40375636]), 2: array([ 4.23835223]), 3: array([ 0.30375615,  0.95274981,  0.        ])}\n",
      "cos(angle):  0.30375615192045563   sin(angle)  0.9527498098506644\n",
      "theta dot:  4.2383522316\n",
      "Angle:  72.31648421806563\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -0.874650211216  self.max:  0.877462044328\n",
      "state:  {1: array([ 5.18498927]), 2: array([ 0.0259705]), 3: array([-0.45520309,  0.89038764,  0.        ])}\n",
      "cos(angle):  -0.4552030895890802   sin(angle)  0.8903876387442471\n",
      "theta dot:  0.0259705035128\n",
      "Angle:  117.07772835177934\n",
      "action:  [ 0.53002661]\n",
      "state:  {1: array([ 5.22365253]), 2: array([ 0.77326522]), 3: array([-0.48927962,  0.87212697,  0.        ])}\n",
      "cos(angle):  -0.4892796160267484   sin(angle)  0.8721269731757628\n",
      "theta dot:  0.773265223823\n",
      "Angle:  119.29296486005848\n",
      "action:  [ 0.53589489]\n",
      "state:  {1: array([ 5.29903977]), 2: array([ 1.50774469]), 3: array([-0.55357491,  0.83279939,  0.        ])}\n",
      "cos(angle):  -0.5535749108466478   sin(angle)  0.8327993864557814\n",
      "theta dot:  1.50774468663\n",
      "Angle:  123.61232511585968\n",
      "action:  [ 0.52546889]\n",
      "state:  {1: array([ 5.409598]), 2: array([ 2.21116456]), 3: array([-0.64208052,  0.76663721,  0.        ])}\n",
      "cos(angle):  -0.6420805182496494   sin(angle)  0.766637207604915\n",
      "theta dot:  2.21116455946\n",
      "Angle:  129.94683015634138\n",
      "action:  [ 0.4540367]\n",
      "state:  {1: array([ 5.55231039]), 2: array([ 2.85424797]), 3: array([-0.74459066,  0.66752134,  0.        ])}\n",
      "cos(angle):  -0.7445906644889646   sin(angle)  0.6675213422474837\n",
      "theta dot:  2.85424797085\n",
      "Angle:  138.12362915611396\n",
      "action:  [ 0.30455784]\n",
      "state:  {1: array([ 5.72233903]), 2: array([ 3.40057265]), 3: array([-0.84680528,  0.53190302,  0.        ])}\n",
      "cos(angle):  -0.8468052754054473   sin(angle)  0.5319030226888213\n",
      "theta dot:  3.40057265406\n",
      "Angle:  147.86552942556273\n",
      "action:  [ 0.1623979]\n",
      "state:  {1: array([ 5.91353201]), 2: array([ 3.82385961]), 3: array([-0.93245266,  0.36129217,  0.        ])}\n",
      "cos(angle):  -0.9324526617576843   sin(angle)  0.36129217204502195\n",
      "theta dot:  3.8238596068\n",
      "Angle:  158.8200546551269\n",
      "action:  [ 0.01963729]\n",
      "state:  {1: array([ 6.11842072]), 2: array([ 4.09777433]), 3: array([-0.986457  ,  0.16402011,  0.        ])}\n",
      "cos(angle):  -0.9864569957699212   sin(angle)  0.1640201069886914\n",
      "theta dot:  4.09777432951\n",
      "Angle:  170.55928592758937\n",
      "action:  [-0.1383214]\n",
      "state:  {1: array([ 0.04523748]), 2: array([ 4.2000412]), 3: array([-0.99897696, -0.04522205,  0.        ])}\n",
      "cos(angle):  -0.9989769598302634   sin(angle)  -0.04522204913849309\n",
      "theta dot:  4.20004119953\n",
      "Angle:  182.59233135139786\n",
      "action:  [-0.13272434]\n",
      "state:  {1: array([ 0.25254828]), 2: array([ 4.14621601]), 3: array([-0.96827882, -0.24987221,  0.        ])}\n",
      "cos(angle):  -0.968278822560906   sin(angle)  -0.2498722109000226\n",
      "theta dot:  4.14621601198\n",
      "Angle:  194.47033749726677\n",
      "action:  [-0.53319352]\n",
      "state:  {1: array([ 0.44648992]), 2: array([ 3.87883283]), 3: array([-0.90196832, -0.43180222,  0.        ])}\n",
      "cos(angle):  -0.9019683165667973   sin(angle)  -0.4318022185094208\n",
      "theta dot:  3.87883282592\n",
      "Angle:  205.5823490306627\n",
      "action:  [-0.2663855]\n",
      "state:  {1: array([ 0.62224109]), 2: array([ 3.51502334]), 3: array([-0.81257426, -0.58285767,  0.        ])}\n",
      "cos(angle):  -0.8125742644323181   sin(angle)  -0.582857671119011\n",
      "theta dot:  3.51502333691\n",
      "Angle:  215.65212558790182\n",
      "action:  [ 0.13948947]\n",
      "state:  {1: array([ 0.77718126]), 2: array([ 3.0988035]), 3: array([-0.71289307, -0.70127275,  0.        ])}\n",
      "cos(angle):  -0.7128930724815121   sin(angle)  -0.7012727480858426\n",
      "theta dot:  3.09880350473\n",
      "Angle:  224.52952294675876\n",
      "action:  [ 0.41281644]\n",
      "state:  {1: array([ 0.90891983]), 2: array([ 2.63477141]), 3: array([-0.61459819, -0.78884033,  0.        ])}\n",
      "cos(angle):  -0.6145981885784646   sin(angle)  -0.7888403302291727\n",
      "theta dot:  2.634771409\n",
      "Angle:  232.077569382026\n",
      "action:  [ 0.65353024]\n",
      "state:  {1: array([ 1.01597837]), 2: array([ 2.1411707]), 3: array([-0.52678858, -0.84999635,  0.        ])}\n",
      "cos(angle):  -0.526788576193555   sin(angle)  -0.8499963505756758\n",
      "theta dot:  2.14117069778\n",
      "Angle:  238.21155724808239\n",
      "action:  [ 0.91503074]\n",
      "state:  {1: array([ 1.09802477]), 2: array([ 1.64092805]), 3: array([-0.45535558, -0.89030966,  0.        ])}\n",
      "cos(angle):  -0.45535557637201646   sin(angle)  -0.8903096647048759\n",
      "theta dot:  1.64092804585\n",
      "Angle:  242.91245883093274\n",
      "action:  [ 0.80101153]\n",
      "state:  {1: array([ 1.15269214]), 2: array([ 1.09334753]), 3: array([-0.40602867, -0.91386034,  0.        ])}\n",
      "cos(angle):  -0.4060286709347555   sin(angle)  -0.9138603385523173\n",
      "theta dot:  1.09334752692\n",
      "Angle:  246.04466144815348\n",
      "action:  [ 1.01048159]\n",
      "state:  {1: array([ 1.18066837]), 2: array([ 0.55952451]), 3: array([-0.38030676, -0.9248604 ,  0.        ])}\n",
      "cos(angle):  -0.38030676012800557   sin(angle)  -0.9248604047103215\n",
      "theta dot:  0.559524511452\n",
      "Angle:  247.64757735185424\n",
      "action:  [ 0.88676554]\n",
      "state:  {1: array([ 1.18061307]), 2: array([-0.00110596]), 3: array([-0.3803579 , -0.92483937,  0.        ])}\n",
      "cos(angle):  -0.38035790251900553   sin(angle)  -0.9248393730758562\n",
      "theta dot:  -0.00110596090461\n",
      "Angle:  247.64440901465622\n",
      "action:  [ 0.69125014]\n",
      "state:  {1: array([ 1.15106067]), 2: array([-0.59104797]), 3: array([-0.40751907, -0.9131967 ,  0.        ])}\n",
      "cos(angle):  -0.4075190665234001   sin(angle)  -0.913196698647064\n",
      "theta dot:  -0.591047969941\n",
      "Angle:  245.95118526578113\n",
      "action:  [ 0.46583685]\n",
      "state:  {1: array([ 1.09075718]), 2: array([-1.20606997]), 3: array([-0.4618139 , -0.88697684,  0.        ])}\n",
      "cos(angle):  -0.46181390224335406   sin(angle)  -0.8869768428176498\n",
      "theta dot:  -1.2060699665\n",
      "Angle:  242.49605740147467\n",
      "action:  [ 0.2675757]\n",
      "state:  {1: array([ 0.99919886]), 2: array([-1.83116624]), 3: array([-0.54097627, -0.84103786,  0.        ])}\n",
      "cos(angle):  -0.540976265720621   sin(angle)  -0.8410378587952935\n",
      "theta dot:  -1.83116624333\n",
      "Angle:  237.25016480216078\n",
      "action:  [ 0.12683827]\n",
      "state:  {1: array([ 0.87705292]), 2: array([-2.4429189]), 3: array([-0.6394198, -0.7688578,  0.       ])}\n",
      "cos(angle):  -0.6394198043541907   sin(angle)  -0.7688577981653361\n",
      "theta dot:  -2.44291889726\n",
      "Angle:  230.25173404225342\n",
      "action:  [ 0.03738069]\n",
      "state:  {1: array([ 0.72635516]), 2: array([-3.01395514]), 3: array([-0.74760008, -0.66414917,  0.        ])}\n",
      "cos(angle):  -0.7476000794562074   sin(angle)  -0.6641491708924075\n",
      "theta dot:  -3.01395514211\n",
      "Angle:  221.61740876882646\n",
      "action:  [ 0.00037015]\n",
      "state:  {1: array([ 0.55075459]), 2: array([-3.5120115]), 3: array([-0.85212987, -0.52333038,  0.        ])}\n",
      "cos(angle):  -0.8521298666955788   sin(angle)  -0.523330383491514\n",
      "theta dot:  -3.51201149754\n",
      "Angle:  211.55626047564786\n",
      "action:  [ 0.05019543]\n",
      "state:  {1: array([ 0.35590559]), 2: array([-3.89697997]), 3: array([-0.93733133, -0.34843934,  0.        ])}\n",
      "cos(angle):  -0.9373313307540099   sin(angle)  -0.34843934391356685\n",
      "theta dot:  -3.89697997035\n",
      "Angle:  200.3922613245222\n",
      "action:  [ 0.16086103]\n",
      "state:  {1: array([ 0.14919657]), 2: array([-4.13418032]), 3: array([-0.98889082, -0.14864368,  0.        ])}\n",
      "cos(angle):  -0.9888908216204054   sin(angle)  -0.14864367768230016\n",
      "theta dot:  -4.13418032373\n",
      "Angle:  188.548734805052\n",
      "action:  [ 0.42141899]\n",
      "state:  {1: array([ 6.22325937]), 2: array([-4.18245023]), 3: array([-0.99820498,  0.05989008,  0.        ])}\n",
      "cos(angle):  -0.9982049781359109   sin(angle)  0.059890079518111414\n",
      "theta dot:  -4.18245023311\n",
      "Angle:  176.5660836700655\n",
      "action:  [ 0.09190677]\n",
      "state:  {1: array([ 6.01707203]), 2: array([-4.12374666]), 3: array([-0.96480033,  0.26298352,  0.        ])}\n",
      "cos(angle):  -0.9648003264096245   sin(angle)  0.26298351689769806\n",
      "theta dot:  -4.12374665816\n",
      "Angle:  164.75244733079637\n",
      "action:  [ 0.22531472]\n",
      "state:  {1: array([ 5.82243644]), 2: array([-3.89271181]), 3: array([-0.89571979,  0.444619  ,  0.        ])}\n",
      "cos(angle):  -0.8957197897866049   sin(angle)  0.4446190034002597\n",
      "theta dot:  -3.89271181248\n",
      "Angle:  153.60067552270834\n",
      "action:  [ 0.11477479]\n",
      "state:  {1: array([ 5.64533488]), 2: array([-3.54203134]), 3: array([-0.80337762,  0.5954699 ,  0.        ])}\n",
      "cos(angle):  -0.8033776168984885   sin(angle)  0.5954699024018808\n",
      "theta dot:  -3.54203134076\n",
      "Angle:  143.45352691473823\n",
      "action:  [-0.00991141]\n",
      "state:  {1: array([ 5.4904891]), 2: array([-3.09691563]), 3: array([-0.7019275,  0.7122484,  0.       ])}\n",
      "cos(angle):  -0.7019274967976143   sin(angle)  0.7122484041676997\n",
      "theta dot:  -3.09691562533\n",
      "Angle:  134.5815379193441\n",
      "action:  [ 0.00273704]\n",
      "state:  {1: array([ 5.36237316]), 2: array([-2.56231877]), 3: array([-0.60517381,  0.79609337,  0.        ])}\n",
      "cos(angle):  -0.6051738090000246   sin(angle)  0.7960933744859342\n",
      "theta dot:  -2.56231876617\n",
      "Angle:  127.24105253116613\n",
      "action:  [ 0.18992942]\n",
      "state:  {1: array([ 5.26553519]), 2: array([-1.93675932]), 3: array([-0.52536686,  0.85087582,  0.        ])}\n",
      "cos(angle):  -0.5253668592726133   sin(angle)  0.8508758212442226\n",
      "theta dot:  -1.93675932176\n",
      "Angle:  121.69265875224272\n",
      "action:  [ 0.25660348]\n",
      "state:  {1: array([ 5.20252959]), 2: array([-1.26011193]), 3: array([-0.47074995,  0.88226667,  0.        ])}\n",
      "cos(angle):  -0.47074995181299273   sin(angle)  0.8822666733295921\n",
      "theta dot:  -1.26011193386\n",
      "Angle:  118.08271241765908\n",
      "action:  [ 0.67066589]\n",
      "state:  {1: array([ 5.17763899]), 2: array([-0.49781205]), 3: array([-0.44864625,  0.89370943,  0.        ])}\n",
      "cos(angle):  -0.4486462532623504   sin(angle)  0.8937094267342461\n",
      "theta dot:  -0.497812045041\n",
      "Angle:  116.65658929397455\n",
      "action:  [ 0.14078871]\n",
      "state:  {1: array([ 5.18731841]), 2: array([ 0.19358833]), 3: array([-0.45727569,  0.88932499,  0.        ])}\n",
      "cos(angle):  -0.45727568711083144   sin(angle)  0.8893249945758395\n",
      "theta dot:  0.19358833167\n",
      "Angle:  117.21117771548813\n",
      "action:  [ 0.06185375]\n",
      "state:  {1: array([ 5.23081142]), 2: array([ 0.86986014]), 3: array([-0.49551048,  0.86860196,  0.        ])}\n",
      "cos(angle):  -0.49551047921714975   sin(angle)  0.8686019600403804\n",
      "theta dot:  0.869860140785\n",
      "Angle:  119.7031376298844\n",
      "action:  [ 0.09990692]\n",
      "state:  {1: array([ 5.3076263]), 2: array([ 1.53629765]), 3: array([-0.56070527,  0.82801546,  0.        ])}\n",
      "cos(angle):  -0.5607052730766824   sin(angle)  0.8280154568267449\n",
      "theta dot:  1.5362976495\n",
      "Angle:  124.10429590767876\n",
      "action:  [ 0.23457347]\n",
      "state:  {1: array([ 5.41725106]), 2: array([ 2.19249526]), 3: array([-0.64792878,  0.76170092,  0.        ])}\n",
      "cos(angle):  -0.6479287826195463   sin(angle)  0.7617009207380234\n",
      "theta dot:  2.19249526309\n",
      "Angle:  130.38531747879927\n",
      "action:  [ 0.32070462]\n",
      "state:  {1: array([ 5.55784489]), 2: array([ 2.81187665]), 3: array([-0.74827364,  0.6633902 ,  0.        ])}\n",
      "cos(angle):  -0.7482736383416269   sin(angle)  0.6633902035476438\n",
      "theta dot:  2.8118766471\n",
      "Angle:  138.4407318612365\n",
      "action:  [ 0.27288617]\n",
      "state:  {1: array([ 5.7253625]), 2: array([ 3.35035222]), 3: array([-0.8484096,  0.5293403,  0.       ])}\n",
      "cos(angle):  -0.8484095994147598   sin(angle)  0.5293402985045508\n",
      "theta dot:  3.35035222489\n",
      "Angle:  148.03876153528682\n",
      "action:  [-0.22339275]\n",
      "state:  {1: array([ 5.91105493]), 2: array([ 3.71384854]), 3: array([-0.93155485,  0.36360082,  0.        ])}\n",
      "cos(angle):  -0.9315548538686929   sin(angle)  0.36360081715210485\n",
      "theta dot:  3.71384853629\n",
      "Angle:  158.6781289998197\n",
      "action:  [-0.31474616]\n",
      "state:  {1: array([ 6.10802179]), 2: array([ 3.93933723]), 3: array([-0.98469806,  0.17426915,  0.        ])}\n",
      "cos(angle):  -0.9846980565932307   sin(angle)  0.17426915200779128\n",
      "theta dot:  3.93933722553\n",
      "Angle:  169.96347246485948\n",
      "action:  [-0.39018678]\n",
      "state:  {1: array([ 0.02541204]), 2: array([ 4.01151107]), 3: array([-0.99967713, -0.0254093 ,  0.        ])}\n",
      "cos(angle):  -0.9996771315001441   sin(angle)  -0.025409304509242468\n",
      "theta dot:  4.01151107307\n",
      "Angle:  181.4564201237974\n",
      "action:  [-0.35088958]\n",
      "state:  {1: array([ 0.22240307]), 2: array([ 3.93982066]), 3: array([-0.97537021, -0.22057414,  0.        ])}\n",
      "cos(angle):  -0.9753702104696161   sin(angle)  -0.22057414292807934\n",
      "theta dot:  3.93982065812\n",
      "Angle:  192.7431485179395\n",
      "action:  [-0.5223839]\n",
      "state:  {1: array([ 0.4072047]), 2: array([ 3.69603247]), 3: array([-0.91823147, -0.39604414,  0.        ])}\n",
      "cos(angle):  -0.9182314726771459   sin(angle)  -0.3960441422179603\n",
      "theta dot:  3.6960324659\n",
      "Angle:  203.33147681978033\n",
      "action:  [-0.50753888]\n",
      "state:  {1: array([ 0.57334812]), 2: array([ 3.32286853]), 3: array([-0.84008951, -0.54244781,  0.        ])}\n",
      "cos(angle):  -0.840089505745122   sin(angle)  -0.5424478060946663\n",
      "theta dot:  3.32286852783\n",
      "Angle:  212.85077168560844\n",
      "action:  [-0.33387001]\n",
      "state:  {1: array([ 0.71664573]), 2: array([ 2.86595217]), 3: array([-0.75401325, -0.65685921,  0.        ])}\n",
      "cos(angle):  -0.754013249461415   sin(angle)  -0.6568592083823123\n",
      "theta dot:  2.86595217153\n",
      "Angle:  221.06110067204983\n",
      "action:  [-0.20960163]\n",
      "state:  {1: array([ 0.83373911]), 2: array([ 2.34186752]), 3: array([-0.67211184, -0.74044964,  0.        ])}\n",
      "cos(angle):  -0.6721118447115733   sin(angle)  -0.740449639204724\n",
      "theta dot:  2.34186752011\n",
      "Angle:  227.77004123768626\n",
      "action:  [-0.09962282]\n",
      "state:  {1: array([ 0.92231845]), 2: array([ 1.77158687]), 3: array([-0.60397397, -0.79700404,  0.        ])}\n",
      "cos(angle):  -0.6039739674611664   sin(angle)  -0.7970040442991604\n",
      "theta dot:  1.77158686819\n",
      "Angle:  232.84525189905239\n",
      "action:  [-0.05722089]\n",
      "state:  {1: array([ 0.98058099]), 2: array([ 1.1652507]), 3: array([-0.55653995, -0.83082085,  0.        ])}\n",
      "cos(angle):  -0.5565399461361606   sin(angle)  -0.8308208521424817\n",
      "theta dot:  1.16525070184\n",
      "Angle:  236.1834414574074\n",
      "action:  [-0.11209596]\n",
      "state:  {1: array([ 1.00684702]), 2: array([ 0.52532067]), 3: array([-0.53452812, -0.84515069,  0.        ])}\n",
      "cos(angle):  -0.5345281183137639   sin(angle)  -0.845150691138537\n",
      "theta dot:  0.525320668866\n",
      "Angle:  237.6883707990772\n",
      "action:  [-0.23478339]\n",
      "state:  {1: array([ 0.99965903]), 2: array([-0.14375986]), 3: array([-0.54058919, -0.84128671,  0.        ])}\n",
      "cos(angle):  -0.5405891944540937   sin(angle)  -0.8412867066817793\n",
      "theta dot:  -0.143759858541\n",
      "Angle:  237.27653010425047\n",
      "action:  [-0.34951324]\n",
      "state:  {1: array([ 0.95830143]), 2: array([-0.82715187]), 3: array([-0.57491061, -0.81821622,  0.        ])}\n",
      "cos(angle):  -0.5749106106618906   sin(angle)  -0.8182162243248248\n",
      "theta dot:  -0.827151873894\n",
      "Angle:  234.90692007590567\n",
      "action:  [-0.39467265]\n",
      "state:  {1: array([ 0.88330069]), 2: array([-1.50001494]), 3: array([-0.63460371, -0.77283771,  0.        ])}\n",
      "cos(angle):  -0.6346037118306788   sin(angle)  -0.7728377118973457\n",
      "theta dot:  -1.50001493949\n",
      "Angle:  230.60970386270847\n",
      "action:  [-0.37005189]\n",
      "state:  {1: array([ 0.77654313]), 2: array([-2.13515101]), 3: array([-0.71334043, -0.70081769,  0.        ])}\n",
      "cos(angle):  -0.7133404277334279   sin(angle)  -0.7008176896747755\n",
      "theta dot:  -2.13515100664\n",
      "Angle:  224.49296110113153\n",
      "action:  [-0.36026086]\n",
      "state:  {1: array([ 0.64080296]), 2: array([-2.7148034]), 3: array([-0.80161597, -0.5978393 ,  0.        ])}\n",
      "cos(angle):  -0.8016159725983026   sin(angle)  -0.5978393032205873\n",
      "theta dot:  -2.71480340276\n",
      "Angle:  216.71564042860427\n",
      "action:  [-0.48934545]\n",
      "state:  {1: array([ 0.47897373]), 2: array([-3.2365847]), 3: array([-0.88746837, -0.46086864,  0.        ])}\n",
      "cos(angle):  -0.8874683658490169   sin(angle)  -0.4608686359661238\n",
      "theta dot:  -3.23658469696\n",
      "Angle:  207.44352995221448\n",
      "action:  [-0.55048636]\n",
      "state:  {1: array([ 0.29573327]), 2: array([-3.66480913]), 3: array([-0.95658869, -0.29144137,  0.        ])}\n",
      "cos(angle):  -0.9565886933445668   sin(angle)  -0.2914413693443922\n",
      "theta dot:  -3.66480912813\n",
      "Angle:  196.94464971500952\n",
      "action:  [-0.38692421]\n",
      "state:  {1: array([ 0.09866183]), 2: array([-3.94142879]), 3: array([-0.99513687, -0.09850185,  0.        ])}\n",
      "cos(angle):  -0.995136868057248   sin(angle)  -0.0985018468517793\n",
      "theta dot:  -3.94142878692\n",
      "Angle:  185.6533143819645\n",
      "action:  [-0.42678743]\n",
      "state:  {1: array([ 6.17788098]), 2: array([-4.07932329]), 3: array([-0.99446062,  0.10510982,  0.        ])}\n",
      "cos(angle):  -0.9944606206913977   sin(angle)  0.10510981825728707\n",
      "theta dot:  -4.07932328607\n",
      "Angle:  173.96609949710785\n",
      "action:  [-0.2245261]\n",
      "state:  {1: array([ 5.97617248]), 2: array([-4.03416984]), 3: array([-0.95324058,  0.30221249,  0.        ])}\n",
      "cos(angle):  -0.9532405839550021   sin(angle)  0.3022124899820103\n",
      "theta dot:  -4.03416983791\n",
      "Angle:  162.4090812448959\n",
      "action:  [ 0.13536183]\n",
      "state:  {1: array([ 5.78681218]), 2: array([-3.7872062]), 3: array([-0.8793156 ,  0.47623952,  0.        ])}\n",
      "cos(angle):  -0.8793155992511688   sin(angle)  0.476239516329292\n",
      "theta dot:  -3.78720619652\n",
      "Angle:  151.5595600554758\n",
      "action:  [-0.08469764]\n",
      "state:  {1: array([ 5.61467561]), 2: array([-3.44273121]), 3: array([-0.78474626,  0.61981716,  0.        ])}\n",
      "cos(angle):  -0.7847462552424447   sin(angle)  0.6198171624624151\n",
      "theta dot:  -3.44273120596\n",
      "Angle:  141.69688471372814\n",
      "action:  [-0.17745622]\n",
      "state:  {1: array([ 5.46445128]), 2: array([-3.00448677]), 3: array([-0.68314627,  0.73028157,  0.        ])}\n",
      "cos(angle):  -0.6831462684726831   sin(angle)  0.7302815730058159\n",
      "theta dot:  -3.00448676769\n",
      "Angle:  133.0896842715423\n",
      "action:  [-0.13417536]\n",
      "state:  {1: array([ 5.34060618]), 2: array([-2.47690189]), 3: array([-0.58770327,  0.80907655,  0.        ])}\n",
      "cos(angle):  -0.5877032721831742   sin(angle)  0.8090765500650664\n",
      "theta dot:  -2.47690189129\n",
      "Angle:  125.9938996326232\n",
      "action:  [-0.04975309]\n",
      "state:  {1: array([ 5.24672831]), 2: array([-1.87755744]), 3: array([-0.50927257,  0.86060528,  0.        ])}\n",
      "cos(angle):  -0.5092725739654674   sin(angle)  0.8606052785142486\n",
      "theta dot:  -1.87755744222\n",
      "Angle:  120.61510634895762\n",
      "action:  [ 0.05762053]\n",
      "state:  {1: array([ 5.18555529]), 2: array([-1.2234604]), 3: array([-0.45570699,  0.89012984,  0.        ])}\n",
      "cos(angle):  -0.4557069912658823   sin(angle)  0.8901298434000497\n",
      "theta dot:  -1.22346040438\n",
      "Angle:  117.11015866642128\n",
      "action:  [ 0.1377162]\n",
      "state:  {1: array([ 5.15879501]), 2: array([-0.53520559]), 3: array([-0.43172655,  0.90200454,  0.        ])}\n",
      "cos(angle):  -0.431726551647108   sin(angle)  0.902004536908156\n",
      "theta dot:  -0.535205591887\n",
      "Angle:  115.57691117247424\n",
      "action:  [ 0.19728916]\n",
      "state:  {1: array([ 5.16733957]), 2: array([ 0.17089119]), 3: array([-0.43941793,  0.89828274,  0.        ])}\n",
      "cos(angle):  -0.4394179291128835   sin(angle)  0.8982827414428849\n",
      "theta dot:  0.170891185266\n",
      "Angle:  116.06647721124261\n",
      "action:  [ 0.22743359]\n",
      "state:  {1: array([ 5.21127548]), 2: array([ 0.87871828]), 3: array([-0.47844806,  0.87811586,  0.        ])}\n",
      "cos(angle):  -0.47844805585568273   sin(angle)  0.8781158567341314\n",
      "theta dot:  0.878718279778\n",
      "Angle:  118.58381376522783\n",
      "action:  [ 0.18398755]\n",
      "state:  {1: array([ 5.28952065]), 2: array([ 1.56490331]), 3: array([-0.54562243,  0.83803112,  0.        ])}\n",
      "cos(angle):  -0.5456224331578285   sin(angle)  0.8380311213999937\n",
      "theta dot:  1.56490330536\n",
      "Angle:  123.06692101893145\n",
      "action:  [ 0.04129495]\n",
      "state:  {1: array([ 5.39950169]), 2: array([ 2.19962089]), 3: array([-0.63430772,  0.77308066,  0.        ])}\n",
      "cos(angle):  -0.6343077231058684   sin(angle)  0.7730806635844988\n",
      "theta dot:  2.19962088864\n",
      "Angle:  129.368355955818\n",
      "action:  [-0.16827476]\n",
      "state:  {1: array([ 5.5372112]), 2: array([ 2.75419017]), 3: array([-0.73442714,  0.67868754,  0.        ])}\n",
      "cos(angle):  -0.7344271375302738   sin(angle)  0.6786875419949068\n",
      "theta dot:  2.75419017231\n",
      "Angle:  137.2585111476833\n",
      "action:  [-0.35337779]\n",
      "state:  {1: array([ 5.69772116]), 2: array([ 3.21019916]), 3: array([-0.83345569,  0.55258629,  0.        ])}\n",
      "cos(angle):  -0.8334556940237734   sin(angle)  0.552586288374359\n",
      "theta dot:  3.21019916004\n",
      "Angle:  146.45503280554834\n",
      "action:  [-0.4483717]\n",
      "state:  {1: array([ 5.87559032]), 2: array([ 3.55738312]), 3: array([-0.91807683,  0.39640249,  0.        ])}\n",
      "cos(angle):  -0.9180768283305083   sin(angle)  0.39640249404184436\n",
      "theta dot:  3.55738312171\n",
      "Angle:  156.6461609235064\n",
      "action:  [-0.47091921]\n",
      "state:  {1: array([ 6.06479267]), 2: array([ 3.78404711]), 3: array([-0.97624696,  0.21666072,  0.        ])}\n",
      "cos(angle):  -0.9762469629258435   sin(angle)  0.2166607195087905\n",
      "theta dot:  3.78404711023\n",
      "Angle:  167.4866320185056\n",
      "action:  [-0.47183042]\n",
      "state:  {1: array([ 6.25858108]), 2: array([ 3.87576809]), 3: array([-0.99969733,  0.02460175,  0.        ])}\n",
      "cos(angle):  -0.9996973311591975   sin(angle)  0.024601749392630826\n",
      "theta dot:  3.87576808679\n",
      "Angle:  178.58986374153756\n",
      "action:  [-0.40883602]\n",
      "state:  {1: array([ 0.16704047]), 2: array([ 3.832894]), 3: array([-0.98608115, -0.16626474,  0.        ])}\n",
      "cos(angle):  -0.9860811504729782   sin(angle)  -0.16626474275049352\n",
      "theta dot:  3.8328939951\n",
      "Angle:  189.57111236122162\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "action:  [-0.23107525]\n",
      "state:  {1: array([ 0.35071718]), 2: array([ 3.67353415]), 3: array([-0.93912655, -0.34357141,  0.        ])}\n",
      "cos(angle):  -0.9391265533657741   sin(angle)  -0.3435714143570764\n",
      "theta dot:  3.67353415099\n",
      "Angle:  200.09498788927337\n",
      "action:  [ 0.00378909]\n",
      "state:  {1: array([ 0.52153837]), 2: array([ 3.41642395]), 3: array([-0.86705377, -0.49821458,  0.        ])}\n",
      "cos(angle):  -0.8670537660003934   sin(angle)  -0.49821457913687656\n",
      "theta dot:  3.41642395316\n",
      "Angle:  209.88229867945722\n",
      "action:  [ 0.24251213]\n",
      "state:  {1: array([ 0.67549537]), 2: array([ 3.07913984]), 3: array([-0.7803973 , -0.62528398,  0.        ])}\n",
      "cos(angle):  -0.780397303037123   sin(angle)  -0.6252839750164599\n",
      "theta dot:  3.07913983779\n",
      "Angle:  218.70336391375736\n",
      "action:  [ 0.40484319]\n",
      "state:  {1: array([ 0.80904053]), 2: array([ 2.67090334]), 3: array([-0.69019305, -0.72362529,  0.        ])}\n",
      "cos(angle):  -0.6901930459451855   sin(angle)  -0.7236252893099487\n",
      "theta dot:  2.67090333522\n",
      "Angle:  226.35492045085368\n",
      "action:  [ 0.43016448]\n",
      "state:  {1: array([ 0.91867598]), 2: array([ 2.19270904]), 3: array([-0.60687301, -0.79479881,  0.        ])}\n",
      "cos(angle):  -0.6068730146826935   sin(angle)  -0.7947988072776275\n",
      "theta dot:  2.19270904083\n",
      "Angle:  232.6365544486498\n",
      "action:  [ 0.33272752]\n",
      "state:  {1: array([ 1.00100194]), 2: array([ 1.64651906]), 3: array([-0.53945893, -0.84201191,  0.        ])}\n",
      "cos(angle):  -0.5394589338667964   sin(angle)  -0.8420119112407493\n",
      "theta dot:  1.6465190627\n",
      "Angle:  237.3534730774646\n",
      "action:  [ 0.24699078]\n",
      "state:  {1: array([ 1.05360487]), 2: array([ 1.05205875]), 3: array([-0.49444087, -0.86921127,  0.        ])}\n",
      "cos(angle):  -0.4944408699204989   sin(angle)  -0.8692112666965726\n",
      "theta dot:  1.0520587466\n",
      "Angle:  240.36739232861277\n",
      "action:  [ 0.08356446]\n",
      "state:  {1: array([ 1.07423912]), 2: array([ 0.41268497]), 3: array([-0.47640137, -0.87922792,  0.        ])}\n",
      "cos(angle):  -0.4764013657069849   sin(angle)  -0.8792279219591014\n",
      "theta dot:  0.412684965335\n",
      "Angle:  241.54964490310203\n",
      "action:  [ 0.03145441]\n",
      "state:  {1: array([ 1.06213823]), 2: array([-0.24201781]), 3: array([-0.48700567, -0.87339881,  0.        ])}\n",
      "cos(angle):  -0.48700566740692264   sin(angle)  -0.873398809200893\n",
      "theta dot:  -0.242017814991\n",
      "Angle:  240.85631655610712\n",
      "action:  [ 0.0120885]\n",
      "state:  {1: array([ 1.01737555]), 2: array([-0.89525365]), 3: array([-0.52560046, -0.85073154,  0.        ])}\n",
      "cos(angle):  -0.5256004615896888   sin(angle)  -0.8507315409556097\n",
      "theta dot:  -0.89525364728\n",
      "Angle:  238.29160977436618\n",
      "action:  [ 0.01948384]\n",
      "state:  {1: array([ 0.94085656]), 2: array([-1.53037973]), 3: array([-0.58909608, -0.80806299,  0.        ])}\n",
      "cos(angle):  -0.5890960842071247   sin(angle)  -0.808062994804138\n",
      "theta dot:  -1.53037972652\n",
      "Angle:  233.9074050574561\n",
      "action:  [ 0.09225286]\n",
      "state:  {1: array([ 0.83472711]), 2: array([-2.12258904]), 3: array([-0.67137995, -0.74111333,  0.        ])}\n",
      "cos(angle):  -0.6713799493248354   sin(angle)  -0.741113327126548\n",
      "theta dot:  -2.1225890437\n",
      "Angle:  227.82664958466313\n",
      "action:  [ 0.00308592]\n",
      "state:  {1: array([ 0.70082905]), 2: array([-2.67796115]), 3: array([-0.76430783, -0.64485156,  0.        ])}\n",
      "cos(angle):  -0.7643078335823557   sin(angle)  -0.6448515608453206\n",
      "theta dot:  -2.6779611506\n",
      "Angle:  220.15487394315443\n",
      "action:  [-0.01778973]\n",
      "state:  {1: array([ 0.54261564]), 2: array([-3.16426828]), 3: array([-0.85636095, -0.51637769,  0.        ])}\n",
      "cos(angle):  -0.8563609544778295   sin(angle)  -0.5163776870138959\n",
      "theta dot:  -3.16426828048\n",
      "Angle:  211.0899342549834\n",
      "action:  [-0.07395661]\n",
      "state:  {1: array([ 0.36448339]), 2: array([-3.56264504]), 3: array([-0.93430804, -0.35646667,  0.        ])}\n",
      "cos(angle):  -0.9343080409440916   sin(angle)  -0.35646666692302886\n",
      "theta dot:  -3.56264503759\n",
      "Angle:  200.88373189366516\n",
      "action:  [-0.3321408]\n",
      "state:  {1: array([ 0.17049258]), 2: array([-3.87981616]), 3: array([-0.98550131, -0.16966781,  0.        ])}\n",
      "cos(angle):  -0.9855013115523693   sin(angle)  -0.16966780757869165\n",
      "theta dot:  -3.87981615733\n",
      "Angle:  189.76890332988495\n",
      "action:  [-0.24299364]\n",
      "state:  {1: array([ 6.25150208]), 2: array([-4.04351606]), 3: array([-0.99949813,  0.03167792,  0.        ])}\n",
      "cos(angle):  -0.9994981286668381   sin(angle)  0.031677922777366786\n",
      "theta dot:  -4.04351605925\n",
      "Angle:  178.1842683537759\n",
      "action:  [-0.50424216]\n",
      "state:  {1: array([ 6.04673239]), 2: array([-4.09539394]), 3: array([-0.97217501,  0.23425573,  0.        ])}\n",
      "cos(angle):  -0.9721750126790736   sin(angle)  0.23425572505798697\n",
      "theta dot:  -4.09539394043\n",
      "Angle:  166.45185637776228\n",
      "action:  [-0.25714671]\n",
      "state:  {1: array([ 5.84881868]), 2: array([-3.95827415]), 3: array([-0.90713677,  0.42083593,  0.        ])}\n",
      "cos(angle):  -0.9071367699737013   sin(angle)  0.4208359306923308\n",
      "theta dot:  -3.9582741524\n",
      "Angle:  155.11226274025023\n",
      "action:  [-0.1783725]\n",
      "state:  {1: array([ 5.66534853]), 2: array([-3.66940308]), 3: array([-0.81513346,  0.57927321,  0.        ])}\n",
      "cos(angle):  -0.8151334571846723   sin(angle)  0.5792732058175694\n",
      "theta dot:  -3.66940307997\n",
      "Angle:  144.60022183125227\n",
      "Reset!\n",
      "min:  -0.874650211216  self.max:  1.01048158965\n",
      "state:  {1: array([ 0.51581363]), 2: array([ 0.97400391]), 3: array([-0.86989169, -0.49324278,  0.        ])}\n",
      "cos(angle):  -0.8698916935998374   sin(angle)  -0.4932427814028368\n",
      "theta dot:  0.974003910395\n",
      "Angle:  209.55429576944002\n",
      "action:  [ 0.41600909]\n",
      "state:  {1: array([ 0.54913729]), 2: array([ 0.66647319]), 3: array([-0.85297513, -0.52195155,  0.        ])}\n",
      "cos(angle):  -0.8529751330591857   sin(angle)  -0.521951551758077\n",
      "theta dot:  0.666473187251\n",
      "Angle:  211.46359634406974\n",
      "action:  [ 0.43995575]\n",
      "state:  {1: array([ 0.56618743]), 2: array([ 0.34100289]), 3: array([-0.84395223, -0.53641833,  0.        ])}\n",
      "cos(angle):  -0.8439522348646872   sin(angle)  -0.536418330472496\n",
      "theta dot:  0.341002886028\n",
      "Angle:  212.44049536821495\n",
      "action:  [ 0.47791671]\n",
      "state:  {1: array([ 0.56670627]), 2: array([ 0.01037665]), 3: array([-0.84367381, -0.53685613,  0.        ])}\n",
      "cos(angle):  -0.8436738101479653   sin(angle)  -0.5368561279061783\n",
      "theta dot:  0.0103766453512\n",
      "Angle:  212.47022219790693\n",
      "action:  [ 0.50576707]\n",
      "state:  {1: array([ 0.55088625]), 2: array([-0.31640039]), 3: array([-0.85206096, -0.52344257,  0.        ])}\n",
      "cos(angle):  -0.8520609581225453   sin(angle)  -0.5234425695750109\n",
      "theta dot:  -0.316400390596\n",
      "Angle:  211.56380396663567\n",
      "action:  [ 0.4646683]\n",
      "state:  {1: array([ 0.51892214]), 2: array([-0.63928207]), 3: array([-0.86835424, -0.49594446,  0.        ])}\n",
      "cos(angle):  -0.8683542417809196   sin(angle)  -0.4959444634040028\n",
      "theta dot:  -0.639282073051\n",
      "Angle:  209.73240001404537\n",
      "action:  [ 0.2987897]\n",
      "state:  {1: array([ 0.47060104]), 2: array([-0.96642197]), 3: array([-0.89129592, -0.45342207,  0.        ])}\n",
      "cos(angle):  -0.8912959227886724   sin(angle)  -0.4534220749150717\n",
      "theta dot:  -0.96642196564\n",
      "Angle:  206.96381149521363\n",
      "action:  [ 0.0591042]\n",
      "state:  {1: array([ 0.4057199]), 2: array([-1.29762289]), 3: array([-0.91881851, -0.39468032,  0.        ])}\n",
      "cos(angle):  -0.9188185053156454   sin(angle)  -0.39468031910588514\n",
      "theta dot:  -1.29762289113\n",
      "Angle:  203.24640443505862\n",
      "action:  [-0.14394397]\n",
      "state:  {1: array([ 0.32495866]), 2: array([-1.61522473]), 3: array([-0.94766392, -0.31926961,  0.        ])}\n",
      "cos(angle):  -0.9476639249685003   sin(angle)  -0.3192696122610117\n",
      "theta dot:  -1.61522472594\n",
      "Angle:  198.61913726753326\n",
      "action:  [-0.23103278]\n",
      "state:  {1: array([ 0.23049207]), 2: array([-1.88933185]), 3: array([-0.9735541 , -0.22845661,  0.        ])}\n",
      "cos(angle):  -0.9735540962076731   sin(angle)  -0.2284566080402595\n",
      "theta dot:  -1.88933185189\n",
      "Angle:  193.2066128637213\n",
      "action:  [-0.50029359]\n",
      "state:  {1: array([ 0.12370615]), 2: array([-2.13571835]), 3: array([-0.99235815, -0.12339088,  0.        ])}\n",
      "cos(angle):  -0.9923581467526456   sin(angle)  -0.12339087718974531\n",
      "theta dot:  -2.13571834659\n",
      "Angle:  187.08824479670832\n",
      "action:  [-0.127369]\n",
      "state:  {1: array([ 0.01133781]), 2: array([-2.24736685]), 3: array([-0.99993573, -0.01133757,  0.        ])}\n",
      "cos(angle):  -0.9999357277197062   sin(angle)  -0.011337567184432368\n",
      "theta dot:  -2.24736685374\n",
      "Angle:  180.65002806520147\n",
      "action:  [ 0.17853131]\n",
      "state:  {1: array([ 6.1830686]), 2: array([-2.22909033]), 3: array([-0.99499251,  0.09994954,  0.        ])}\n",
      "cos(angle):  -0.9949925072900863   sin(angle)  0.09994953945160365\n",
      "theta dot:  -2.22909033238\n",
      "Angle:  174.26332775353285\n",
      "action:  [-0.06906647]\n",
      "state:  {1: array([ 6.07484419]), 2: array([-2.16448815]), 3: array([-0.97837538,  0.20683717,  0.        ])}\n",
      "cos(angle):  -0.9783753800647509   sin(angle)  0.20683717190861578\n",
      "theta dot:  -2.16448814816\n",
      "Angle:  168.06254046887454\n",
      "action:  [ 0.01906039]\n",
      "state:  {1: array([ 5.97451913]), 2: array([-2.00650121]), 3: array([-0.95273962,  0.30378812,  0.        ])}\n",
      "cos(angle):  -0.9527396176241676   sin(angle)  0.3037881186112367\n",
      "theta dot:  -2.00650121025\n",
      "Angle:  162.31435136387313\n",
      "action:  [ 0.02400967]\n",
      "state:  {1: array([ 5.8857662]), 2: array([-1.77505867]), 3: array([-0.92206297,  0.38703989,  0.        ])}\n",
      "cos(angle):  -0.9220629719939026   sin(angle)  0.38703988900082564\n",
      "theta dot:  -1.77505867013\n",
      "Angle:  157.22919474586706\n",
      "action:  [-0.50253826]\n",
      "state:  {1: array([ 5.80775822]), 2: array([-1.56015949]), 3: array([-0.88909732,  0.45771821,  0.        ])}\n",
      "cos(angle):  -0.8890973192325745   sin(angle)  0.45771820690622467\n",
      "theta dot:  -1.56015949283\n",
      "Angle:  152.75967748220944\n",
      "action:  [ 0.06792262]\n",
      "state:  {1: array([ 5.7474241]), 2: array([-1.20668245]), 3: array([-0.85988029,  0.51049573,  0.        ])}\n",
      "cos(angle):  -0.8598802866054155   sin(angle)  0.5104957323106516\n",
      "theta dot:  -1.20668244503\n",
      "Angle:  149.3027950002637\n",
      "action:  [ 0.15460615]\n",
      "state:  {1: array([ 5.70739312]), 2: array([-0.80061972]), 3: array([-0.83876122,  0.54449942,  0.        ])}\n",
      "cos(angle):  -0.8387612181033763   sin(angle)  0.5444994205742926\n",
      "theta dot:  -0.800619723373\n",
      "Angle:  147.00919380649094\n",
      "action:  [ 0.17910366]\n",
      "state:  {1: array([ 5.68912414]), 2: array([-0.36537961]), 3: array([-0.82867436,  0.55973102,  0.        ])}\n",
      "cos(angle):  -0.8286743554067212   sin(angle)  0.5597310181607369\n",
      "theta dot:  -0.365379609026\n",
      "Angle:  145.9624607783427\n",
      "action:  [ 0.16174553]\n",
      "state:  {1: array([ 5.69305816]), 2: array([ 0.07868048]), 3: array([-0.83086993,  0.55646667,  0.        ])}\n",
      "cos(angle):  -0.8308699325965783   sin(angle)  0.5564666702570402\n",
      "theta dot:  0.0786804839672\n",
      "Angle:  146.18786323432198\n",
      "action:  [ 0.24258189]\n",
      "state:  {1: array([ 5.71967905]), 2: array([ 0.53241777]), 3: array([-0.84538743,  0.53415362,  0.        ])}\n",
      "cos(angle):  -0.84538743055558   sin(angle)  0.534153622339711\n",
      "theta dot:  0.532417770049\n",
      "Angle:  147.71312422567803\n",
      "action:  [ 0.43921667]\n",
      "state:  {1: array([ 5.76962482]), 2: array([ 0.99891549]), 3: array([-0.87100083,  0.49128154,  0.        ])}\n",
      "cos(angle):  -0.8710008319231235   sin(angle)  0.49128153923104684\n",
      "theta dot:  0.998915486751\n",
      "Angle:  150.57479960788947\n",
      "action:  [ 0.60124961]\n",
      "state:  {1: array([ 5.84250303]), 2: array([ 1.45756408]), 3: array([-0.90446084,  0.42655666,  0.        ])}\n",
      "cos(angle):  -0.9044608425649299   sin(angle)  0.42655666008953275\n",
      "theta dot:  1.45756408324\n",
      "Angle:  154.75040336049756\n",
      "action:  [ 0.62554599]\n",
      "state:  {1: array([ 5.9360687]), 2: array([ 1.87131348]), 3: array([-0.94035752,  0.3401878 ,  0.        ])}\n",
      "cos(angle):  -0.9403575158576564   sin(angle)  0.34018780455803743\n",
      "theta dot:  1.87131347666\n",
      "Angle:  160.11130904230868\n",
      "action:  [ 0.54300546]\n",
      "state:  {1: array([ 6.04646396]), 2: array([ 2.20790515]), 3: array([-0.9721121 ,  0.23451668,  0.        ])}\n",
      "cos(angle):  -0.9721120967021136   sin(angle)  0.2345166762629482\n",
      "theta dot:  2.20790514979\n",
      "Angle:  166.43647658372015\n",
      "action:  [ 0.3758689]\n",
      "state:  {1: array([ 6.16847261]), 2: array([ 2.44017299]), 3: array([-0.99342771,  0.11446128,  0.        ])}\n",
      "cos(angle):  -0.9934277101150822   sin(angle)  0.1144612806826141\n",
      "theta dot:  2.44017299221\n",
      "Angle:  173.42704092352471\n",
      "action:  [ 0.27985809]\n",
      "state:  {1: array([ 0.01368718]), 2: array([ 2.56799767]), 3: array([-0.99990633, -0.01368676,  0.        ])}\n",
      "cos(angle):  -0.9999063319596316   sin(angle)  -0.013686756629490576\n",
      "theta dot:  2.56799766641\n",
      "Angle:  180.7846369591062\n",
      "action:  [ 0.03425185]\n",
      "state:  {1: array([ 0.1418307]), 2: array([ 2.56287038]), 3: array([-0.98995888, -0.14135567,  0.        ])}\n",
      "cos(angle):  -0.9899588750474024   sin(angle)  -0.14135567096824023\n",
      "theta dot:  2.56287037588\n",
      "Angle:  188.12670258902412\n",
      "action:  [-0.03247372]\n",
      "state:  {1: array([ 0.26442983]), 2: array([ 2.45198256]), 3: array([-0.96524168, -0.26135896,  0.        ])}\n",
      "cos(angle):  -0.9652416765413128   sin(angle)  -0.2613589597999576\n",
      "theta dot:  2.45198256486\n",
      "Angle:  195.1510987832339\n",
      "action:  [ -4.21072676e-05]\n",
      "state:  {1: array([ 0.37722768]), 2: array([ 2.25595703]), 3: array([-0.92968937, -0.36834449,  0.        ])}\n",
      "cos(angle):  -0.929689374843171   sin(angle)  -0.3683444940591808\n",
      "theta dot:  2.25595702892\n",
      "Angle:  201.6139244963319\n",
      "action:  [ 0.0070797]\n",
      "state:  {1: array([ 0.47626571]), 2: array([ 1.98076061]), 3: array([-0.88871315, -0.45846367,  0.        ])}\n",
      "cos(angle):  -0.8887131501498224   sin(angle)  -0.45846367004461674\n",
      "theta dot:  1.98076061389\n",
      "Angle:  207.28837239708426\n",
      "action:  [-0.03567168]\n",
      "state:  {1: array([ 0.55784382]), 2: array([ 1.63156211]), 3: array([-0.84839847, -0.52935813,  0.        ])}\n",
      "cos(angle):  -0.8483984744049098   sin(angle)  -0.5293581288951948\n",
      "theta dot:  1.63156210982\n",
      "Angle:  211.9624426123805\n",
      "action:  [-0.15034054]\n",
      "state:  {1: array([ 0.61844344]), 2: array([ 1.21199243]), 3: array([-0.81478189, -0.57976761,  0.        ])}\n",
      "cos(angle):  -0.8147818862657106   sin(angle)  -0.5797676067298783\n",
      "theta dot:  1.21199243238\n",
      "Angle:  215.4345370519828\n",
      "action:  [-0.42932813]\n",
      "state:  {1: array([ 0.65408182]), 2: array([ 0.71276751]), 3: array([-0.79360691, -0.60843082,  0.        ])}\n",
      "cos(angle):  -0.7936069141892866   sin(angle)  -0.6084308224859735\n",
      "theta dot:  0.712767508355\n",
      "Angle:  217.47646077721606\n",
      "action:  [-0.54540622]\n",
      "state:  {1: array([ 0.66281349]), 2: array([ 0.17463346]), 3: array([-0.78826411, -0.61533706,  0.        ])}\n",
      "cos(angle):  -0.7882641098303539   sin(angle)  -0.6153370565416646\n",
      "theta dot:  0.174633458024\n",
      "Angle:  217.97674761265534\n",
      "action:  [-0.80215208]\n",
      "state:  {1: array([ 0.64245388]), 2: array([-0.40719215]), 3: array([-0.8006279 , -0.59916189,  0.        ])}\n",
      "cos(angle):  -0.8006278977109946   sin(angle)  -0.5991618891475602\n",
      "theta dot:  -0.40719214592\n",
      "Angle:  216.81023076987373\n",
      "action:  [-0.75220802]\n",
      "state:  {1: array([ 0.59398414]), 2: array([-0.96939477]), 3: array([-0.82871747, -0.55966719,  0.        ])}\n",
      "cos(angle):  -0.8287174681508516   sin(angle)  -0.5596671850141318\n",
      "theta dot:  -0.969394765068\n",
      "Angle:  214.03312582792958\n",
      "action:  [-0.52185464]\n",
      "state:  {1: array([ 0.52061298]), 2: array([-1.46742335]), 3: array([-0.86751444, -0.497412  ,  0.        ])}\n",
      "cos(angle):  -0.8675144412687827   sin(angle)  -0.49741199642762113\n",
      "theta dot:  -1.46742334918\n",
      "Angle:  209.82927742500965\n",
      "action:  [-0.19502657]\n",
      "state:  {1: array([ 0.42712616]), 2: array([-1.86973633]), 3: array([-0.91016001, -0.41425686,  0.        ])}\n",
      "cos(angle):  -0.9101600148798298   sin(angle)  -0.41425686151704005\n",
      "theta dot:  -1.86973633184\n",
      "Angle:  204.47288991974304\n",
      "action:  [ 0.09970596]\n",
      "state:  {1: array([ 0.3188525]), 2: array([-2.16547308]), 3: array([-0.94959576, -0.31347711,  0.        ])}\n",
      "cos(angle):  -0.9495957566844451   sin(angle)  -0.31347711062675093\n",
      "theta dot:  -2.16547308463\n",
      "Angle:  198.26928100654433\n",
      "action:  [ 0.26664062]\n",
      "state:  {1: array([ 0.20082326]), 2: array([-2.36058482]), 3: array([-0.97990269, -0.19947612,  0.        ])}\n",
      "cos(angle):  -0.9799026885137979   sin(angle)  -0.19947611647370378\n",
      "theta dot:  -2.36058482404\n",
      "Angle:  191.5067194403359\n",
      "action:  [ 0.065987]\n",
      "state:  {1: array([ 0.07580857]), 2: array([-2.50029386]), 3: array([-0.99712791, -0.07573598,  0.        ])}\n",
      "cos(angle):  -0.9971279061949023   sin(angle)  -0.07573598013738243\n",
      "theta dot:  -2.5002938614\n",
      "Angle:  184.34392190004266\n",
      "action:  [ 0.01217655]\n",
      "state:  {1: array([ 6.23123041]), 2: array([-2.55526936]), 3: array([-0.99865065,  0.05193153,  0.        ])}\n",
      "cos(angle):  -0.9986506478710286   sin(angle)  0.051931527098428916\n",
      "theta dot:  -2.55526936341\n",
      "Angle:  177.02278968131782\n",
      "action:  [-0.18467033]\n",
      "state:  {1: array([ 6.10402935]), 2: array([-2.54402127]), 3: array([-0.98399445,  0.17819911,  0.        ])}\n",
      "cos(angle):  -0.983994450259984   sin(angle)  0.17819910734218594\n",
      "theta dot:  -2.54402126701\n",
      "Angle:  169.7347226444432\n",
      "action:  [-0.97993464]\n",
      "state:  {1: array([ 5.97616124]), 2: array([-2.55736213]), 3: array([-0.95323719,  0.30222321,  0.        ])}\n",
      "cos(angle):  -0.9532371853874334   sin(angle)  0.3022232095565194\n",
      "theta dot:  -2.557362133\n",
      "Angle:  162.40843693112402\n",
      "action:  [-0.20614496]\n",
      "state:  {1: array([ 5.85808042]), 2: array([-2.36161647]), 3: array([-0.91099548,  0.41241634,  0.        ])}\n",
      "cos(angle):  -0.9109954791340028   sin(angle)  0.4124163393918925\n",
      "theta dot:  -2.36161646966\n",
      "Angle:  155.64291992485073\n",
      "action:  [-0.28373006]\n",
      "state:  {1: array([ 5.75333723]), 2: array([-2.09486372]), 3: array([-0.86288386,  0.50540226,  0.        ])}\n",
      "cos(angle):  -0.8628838625139507   sin(angle)  0.5054022554490685\n",
      "theta dot:  -2.09486372397\n",
      "Angle:  149.64159145664064\n",
      "action:  [ 0.2539969]\n",
      "state:  {1: array([ 5.6694516]), 2: array([-1.6777125]), 3: array([-0.81750339,  0.57592378,  0.        ])}\n",
      "cos(angle):  -0.8175033931518919   sin(angle)  0.5759237815762979\n",
      "theta dot:  -1.67771249698\n",
      "Angle:  144.8353104301488\n",
      "action:  [-0.56441256]\n",
      "state:  {1: array([ 5.60293003]), 2: array([-1.33043155]), 3: array([-0.77741218,  0.6289915 ,  0.        ])}\n",
      "cos(angle):  -0.7774121754656049   sin(angle)  0.628991501880459\n",
      "theta dot:  -1.33043154512\n",
      "Angle:  141.02391371954144\n",
      "action:  [-0.66566295]\n",
      "state:  {1: array([ 5.55500316]), 2: array([-0.95853736]), 3: array([-0.74638544,  0.66551392,  0.        ])}\n",
      "cos(angle):  -0.7463854410614239   sin(angle)  0.6655139167376921\n",
      "theta dot:  -0.958537360638\n",
      "Angle:  138.27791287737801\n",
      "action:  [-0.69760125]\n",
      "state:  {1: array([ 5.52680105]), 2: array([-0.56404211]), 3: array([-0.72732223,  0.68629612,  0.        ])}\n",
      "cos(angle):  -0.7273222328431895   sin(angle)  0.686296123850337\n",
      "theta dot:  -0.56404211011\n",
      "Angle:  136.66205503710927\n",
      "action:  [-0.64958152]\n",
      "state:  {1: array([ 5.51946319]), 2: array([-0.14675724]), 3: array([-0.72226675,  0.69161459,  0.        ])}\n",
      "cos(angle):  -0.7222667506671598   sin(angle)  0.6916145898408324\n",
      "theta dot:  -0.146757244624\n",
      "Angle:  136.24162748375664\n",
      "action:  [-0.55442407]\n",
      "state:  {1: array([ 5.5339027]), 2: array([ 0.28879009]), 3: array([-0.73217768,  0.68111368,  0.        ])}\n",
      "cos(angle):  -0.7321776807883156   sin(angle)  0.681113679025347\n",
      "theta dot:  0.288790087405\n",
      "Angle:  137.06894820779746\n",
      "action:  [-0.40715877]\n",
      "state:  {1: array([ 5.57083027]), 2: array([ 0.73855153]), 3: array([-0.75682468,  0.65361793,  0.        ])}\n",
      "cos(angle):  -0.7568246834935561   sin(angle)  0.6536179300286052\n",
      "theta dot:  0.738551530781\n",
      "Angle:  139.18473754349566\n",
      "action:  [-0.19927409]\n",
      "state:  {1: array([ 5.63077397]), 2: array([ 1.19887387]), 3: array([-0.79462217,  0.60710427,  0.        ])}\n",
      "cos(angle):  -0.7946221746882501   sin(angle)  0.6071042739873572\n",
      "theta dot:  1.19887386521\n",
      "Angle:  142.619250144356\n",
      "action:  [-0.00399274]\n",
      "state:  {1: array([ 5.71345412]), 2: array([ 1.65360316]), 3: array([-0.84204601,  0.53940571,  0.        ])}\n",
      "cos(angle):  -0.842046006968289   sin(angle)  0.5394057120097637\n",
      "theta dot:  1.65360315986\n",
      "Angle:  147.3564631691636\n",
      "action:  [ 0.01980997]\n",
      "state:  {1: array([ 5.81651057]), 2: array([ 2.06112894]), 3: array([-0.89306932,  0.44991909,  0.        ])}\n",
      "cos(angle):  -0.8930693226286118   sin(angle)  0.4499190871475363\n",
      "theta dot:  2.06112893924\n",
      "Angle:  153.26114882398656\n",
      "action:  [-0.28856319]\n",
      "state:  {1: array([ 5.93427476]), 2: array([ 2.35528378]), 3: array([-0.93974573,  0.3418742 ,  0.        ])}\n",
      "cos(angle):  -0.9397457262325851   sin(angle)  0.3418742020503903\n",
      "theta dot:  2.35528377578\n",
      "Angle:  160.00852404107036\n",
      "action:  [-0.53348348]\n",
      "state:  {1: array([ 6.06085811]), 2: array([ 2.53166691]), 3: array([-0.97538694,  0.22050014,  0.        ])}\n",
      "cos(angle):  -0.9753869426933667   sin(angle)  0.22050014064232953\n",
      "theta dot:  2.53166690516\n",
      "Angle:  167.26119852107956\n",
      "action:  [-0.42884306]\n",
      "state:  {1: array([ 6.19249388]), 2: array([ 2.63271555]), 3: array([-0.99589035,  0.09056715,  0.        ])}\n",
      "cos(angle):  -0.9958903507192208   sin(angle)  0.09056715378296584\n",
      "theta dot:  2.6327155513\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  174.80335537164137\n",
      "action:  [-0.41013834]\n",
      "state:  {1: array([ 0.04126458]), 2: array([ 2.63912017]), 3: array([-0.99914874, -0.04125287,  0.        ])}\n",
      "cos(angle):  -0.9991487378650347   sin(angle)  -0.041252874114517564\n",
      "theta dot:  2.6391201655\n",
      "Angle:  182.3647018820806\n",
      "action:  [-0.17355957]\n",
      "state:  {1: array([ 0.17037191]), 2: array([ 2.58214657]), 3: array([-0.98552178, -0.16954889,  0.        ])}\n",
      "cos(angle):  -0.9855217777148926   sin(angle)  -0.16954888867131457\n",
      "theta dot:  2.58214657495\n",
      "Angle:  189.7619896254308\n",
      "action:  [ 0.03975426]\n",
      "state:  {1: array([ 0.29341931]), 2: array([ 2.46094805]), 3: array([-0.95726051, -0.28922708,  0.        ])}\n",
      "cos(angle):  -0.9572605149619727   sin(angle)  -0.2892270846493095\n",
      "theta dot:  2.46094804766\n",
      "Angle:  196.81206997588015\n",
      "action:  [ 0.07625692]\n",
      "state:  {1: array([ 0.40619263]), 2: array([ 2.25546627]), 3: array([-0.91863183, -0.39511463,  0.        ])}\n",
      "cos(angle):  -0.9186318256225092   sin(angle)  -0.3951146276126154\n",
      "theta dot:  2.25546627211\n",
      "Angle:  203.2734897775795\n",
      "action:  [ 0.1585183]\n",
      "state:  {1: array([ 0.50533803]), 2: array([ 1.98290805]), 3: array([-0.87501088, -0.48410325,  0.        ])}\n",
      "cos(angle):  -0.8750108825859673   sin(angle)  -0.4841032486527297\n",
      "theta dot:  1.98290804694\n",
      "Angle:  208.95408960646222\n",
      "action:  [ 0.05628283]\n",
      "state:  {1: array([ 0.58675168]), 2: array([ 1.62827304]), 3: array([-0.83274353, -0.55365893,  0.        ])}\n",
      "cos(angle):  -0.832743529238102   sin(angle)  -0.5536589333805338\n",
      "theta dot:  1.6282730355\n",
      "Angle:  213.6187373399495\n",
      "action:  [-0.02681078]\n",
      "state:  {1: array([ 0.64720204]), 2: array([ 1.20900722]), 3: array([-0.79777397, -0.60295663,  0.        ])}\n",
      "cos(angle):  -0.7977739654713852   sin(angle)  -0.6029566319529631\n",
      "theta dot:  1.20900721783\n",
      "Angle:  217.08227978981614\n",
      "action:  [-0.06767043]\n",
      "state:  {1: array([ 0.684534]), 2: array([ 0.74663918]), 3: array([-0.77471379, -0.63231207,  0.        ])}\n",
      "cos(angle):  -0.7747137870489599   sin(angle)  -0.6323120654836968\n",
      "theta dot:  0.746639179257\n",
      "Angle:  219.22123847752664\n",
      "action:  [-0.18961078]\n",
      "state:  {1: array([ 0.69673218]), 2: array([ 0.24396351]), 3: array([-0.76694329, -0.64171488,  0.        ])}\n",
      "cos(angle):  -0.7669432884210238   sin(angle)  -0.6417148839990751\n",
      "theta dot:  0.243963513632\n",
      "Angle:  219.92014082750305\n",
      "action:  [-0.32884196]\n",
      "state:  {1: array([ 0.68239973]), 2: array([-0.28664894]), 3: array([-0.77606155, -0.63065718,  0.        ])}\n",
      "cos(angle):  -0.7760615471813823   sin(angle)  -0.6306571770672551\n",
      "theta dot:  -0.286648943345\n",
      "Angle:  219.09895401501737\n",
      "action:  [-0.40375713]\n",
      "state:  {1: array([ 0.64138946]), 2: array([-0.8202054]), 3: array([-0.8012652 , -0.59830935,  0.        ])}\n",
      "cos(angle):  -0.8012652039901955   sin(angle)  -0.5983093454681704\n",
      "theta dot:  -0.820205395884\n",
      "Angle:  216.74924413376155\n",
      "action:  [-0.37123716]\n",
      "state:  {1: array([ 0.57515831]), 2: array([-1.32462298]), 3: array([-0.8391062 , -0.54396764,  0.        ])}\n",
      "cos(angle):  -0.8391061961915938   sin(angle)  -0.5439676382955834\n",
      "theta dot:  -1.32462297895\n",
      "Angle:  212.95448770056885\n",
      "action:  [-0.24461313]\n",
      "state:  {1: array([ 0.48669378]), 2: array([-1.76929068]), 3: array([-0.88388403, -0.46770613,  0.        ])}\n",
      "cos(angle):  -0.8838840268388445   sin(angle)  -0.4677061332708274\n",
      "theta dot:  -1.76929067787\n",
      "Angle:  207.8858551245478\n",
      "action:  [-0.11745591]\n",
      "state:  {1: array([ 0.37980935]), 2: array([-2.13768866]), 3: array([-0.92873534, -0.37074341,  0.        ])}\n",
      "cos(angle):  -0.9287353364769659   sin(angle)  -0.3707434082745328\n",
      "theta dot:  -2.13768866387\n",
      "Angle:  201.76184252751642\n",
      "action:  [-0.09147816]\n",
      "state:  {1: array([ 0.25833595]), 2: array([-2.42946794]), 3: array([-0.96681644, -0.25547207,  0.        ])}\n",
      "cos(angle):  -0.9668164353497005   sin(angle)  -0.25547207349864764\n",
      "theta dot:  -2.42946794388\n",
      "Angle:  194.80194582046167\n",
      "action:  [-0.17189391]\n",
      "state:  {1: array([ 0.12599314]), 2: array([-2.64685609]), 3: array([-0.99207336, -0.12566007,  0.        ])}\n",
      "cos(angle):  -0.9920733580017693   sin(angle)  -0.12566006662059806\n",
      "theta dot:  -2.64685608511\n",
      "Angle:  187.21927941926185\n",
      "action:  [-0.32574671]\n",
      "state:  {1: array([ 6.26968029]), 2: array([-2.78996314]), 3: array([-0.99990881,  0.0135046 ,  0.        ])}\n",
      "cos(angle):  -0.9999088086935896   sin(angle)  0.013504602806693635\n",
      "theta dot:  -2.78996314086\n",
      "Angle:  179.2258006264707\n",
      "action:  [-0.53534524]\n",
      "state:  {1: array([ 6.12667347]), 2: array([-2.86013647]), 3: array([-0.987777  ,  0.15587363,  0.        ])}\n",
      "cos(angle):  -0.9877770041823636   sin(angle)  0.15587363474466995\n",
      "theta dot:  -2.86013647483\n",
      "Angle:  171.0321323448602\n",
      "action:  [-0.62262167]\n",
      "state:  {1: array([ 5.98484225]), 2: array([-2.8366245]), 3: array([-0.95582484,  0.29393687,  0.        ])}\n",
      "cos(angle):  -0.9558248361399377   sin(angle)  0.29393686842255967\n",
      "theta dot:  -2.83662449946\n",
      "Angle:  162.90582075359225\n",
      "action:  [-0.47369602]\n",
      "state:  {1: array([ 5.85048093]), 2: array([-2.68722625]), 3: array([-0.90783505,  0.41932746,  0.        ])}\n",
      "cos(angle):  -0.9078350523344108   sin(angle)  0.4193274588587988\n",
      "theta dot:  -2.6872262504\n",
      "Angle:  155.20750261836224\n",
      "action:  [-0.33569644]\n",
      "state:  {1: array([ 5.72932668]), 2: array([-2.42308512]), 3: array([-0.85050132,  0.52597291,  0.        ])}\n",
      "cos(angle):  -0.8505013234489422   sin(angle)  0.5259729069178352\n",
      "theta dot:  -2.42308512298\n",
      "Angle:  148.26589130349112\n",
      "action:  [-0.16032087]\n",
      "state:  {1: array([ 5.626694]), 2: array([-2.05265357]), 3: array([-0.7921386 ,  0.61034125,  0.        ])}\n",
      "cos(angle):  -0.7921386016657459   sin(angle)  0.6103412453300504\n",
      "theta dot:  -2.05265357376\n",
      "Angle:  142.38548572550178\n",
      "action:  [ 0.00411203]\n",
      "state:  {1: array([ 5.54697996]), 2: array([-1.59428084]), 3: array([-0.74102192,  0.67148083,  0.        ])}\n",
      "cos(angle):  -0.7410219217015843   sin(angle)  0.6714808348401994\n",
      "theta dot:  -1.59428083527\n",
      "Angle:  137.81821824478948\n",
      "action:  [ 0.10650773]\n",
      "state:  {1: array([ 5.49324525]), 2: array([-1.07469405]), 3: array([-0.7038879 ,  0.71031108,  0.        ])}\n",
      "cos(angle):  -0.703887897492419   sin(angle)  0.7103110781648432\n",
      "theta dot:  -1.07469404932\n",
      "Angle:  134.73945377959348\n",
      "action:  [ 0.10990851]\n",
      "state:  {1: array([ 5.46697153]), 2: array([-0.52547446]), 3: array([-0.68498459,  0.72855755,  0.        ])}\n",
      "cos(angle):  -0.6849845919459725   sin(angle)  0.728557553523817\n",
      "theta dot:  -0.525474464911\n",
      "Angle:  133.23408384573872\n",
      "action:  [ 0.43049897]\n",
      "state:  {1: array([ 5.47124746]), 2: array([ 0.08551855]), 3: array([-0.68809358,  0.72562196,  0.        ])}\n",
      "cos(angle):  -0.6880935796237021   sin(angle)  0.7256219578269665\n",
      "theta dot:  0.0855185461359\n",
      "Angle:  133.47907586102485\n",
      "action:  [-0.00800535]\n",
      "state:  {1: array([ 5.50267417]), 2: array([ 0.62853421]), 3: array([-0.71055397,  0.7036427 ,  0.        ])}\n",
      "cos(angle):  -0.7105539716700843   sin(angle)  0.7036427028996102\n",
      "theta dot:  0.628534212632\n",
      "Angle:  135.27968953357532\n",
      "action:  [ 0.03986443]\n",
      "state:  {1: array([ 5.56078646]), 2: array([ 1.1622459]), 3: array([-0.75022181,  0.66118624,  0.        ])}\n",
      "cos(angle):  -0.7502218067521202   sin(angle)  0.6611862375107216\n",
      "theta dot:  1.16224590382\n",
      "Angle:  138.60927099984008\n",
      "action:  [ 0.07146564]\n",
      "state:  {1: array([ 5.64422923]), 2: array([ 1.66885543]), 3: array([-0.80271875,  0.59635779,  0.        ])}\n",
      "cos(angle):  -0.8027187495483261   sin(angle)  0.5963577861683133\n",
      "theta dot:  1.66885542777\n",
      "Angle:  143.3901784514273\n",
      "action:  [ 0.03541454]\n",
      "state:  {1: array([ 5.75030103]), 2: array([ 2.12143595]), 3: array([-0.86134539,  0.50801981,  0.        ])}\n",
      "cos(angle):  -0.8613453865310396   sin(angle)  0.5080198077847888\n",
      "theta dot:  2.12143594836\n",
      "Angle:  149.46763055711682\n",
      "action:  [ 0.05354136]\n",
      "state:  {1: array([ 5.87582513]), 2: array([ 2.51048201]), 3: array([-0.91816989,  0.3961869 ,  0.        ])}\n",
      "cos(angle):  -0.9181698850976756   sin(angle)  0.39618690298862874\n",
      "theta dot:  2.5104820083\n",
      "Angle:  156.65961492009365\n",
      "action:  [-0.38521494]\n",
      "state:  {1: array([ 6.01331713]), 2: array([ 2.74983994]), 3: array([-0.96380605,  0.26660439,  0.        ])}\n",
      "cos(angle):  -0.9638060492488216   sin(angle)  0.2666043874946134\n",
      "theta dot:  2.74983994426\n",
      "Angle:  164.53730765574505\n",
      "action:  [-0.54010787]\n",
      "state:  {1: array([ 6.15675598]), 2: array([ 2.86877705]), 3: array([-0.99201845,  0.12609278,  0.        ])}\n",
      "cos(angle):  -0.9920184531107408   sin(angle)  0.12609277809523056\n",
      "theta dot:  2.86877705385\n",
      "Angle:  172.75572931497538\n",
      "action:  [-0.56328065]\n",
      "state:  {1: array([ 0.0175134]), 2: array([ 2.87885454]), 3: array([-0.99984664, -0.01751251,  0.        ])}\n",
      "cos(angle):  -0.9998466442876613   sin(angle)  -0.017512507150687636\n",
      "theta dot:  2.87885454065\n",
      "Angle:  181.00386261440883\n",
      "action:  [-0.6002823]\n",
      "state:  {1: array([ 0.15629729]), 2: array([ 2.77567782]), 3: array([-0.98781042, -0.15566171,  0.        ])}\n",
      "cos(angle):  -0.9878104231774637   sin(angle)  -0.15566170968468812\n",
      "theta dot:  2.77567781603\n",
      "Angle:  188.95557522717522\n",
      "action:  [-0.31442665]\n",
      "state:  {1: array([ 0.28688567]), 2: array([ 2.61176754]), 3: array([-0.95912978, -0.28296656,  0.        ])}\n",
      "cos(angle):  -0.9591297766396419   sin(angle)  -0.28296655555593575\n",
      "theta dot:  2.61176753572\n",
      "Angle:  196.4377205739796\n",
      "action:  [-0.33043748]\n",
      "state:  {1: array([ 0.40438452]), 2: array([ 2.349977]), 3: array([-0.91934473, -0.393453  ,  0.        ])}\n",
      "cos(angle):  -0.9193447338010834   sin(angle)  -0.39345299647126214\n",
      "theta dot:  2.34997699633\n",
      "Angle:  203.16989302336327\n",
      "action:  [-0.13375611]\n",
      "state:  {1: array([ 0.50612571]), 2: array([ 2.03482383]), 3: array([-0.87462929, -0.48479233,  0.        ])}\n",
      "cos(angle):  -0.8746292924427568   sin(angle)  -0.4847923274981595\n",
      "theta dot:  2.03482383257\n",
      "Angle:  208.9992202748051\n",
      "action:  [-0.10855635]\n",
      "state:  {1: array([ 0.58887302]), 2: array([ 1.65494613]), 3: array([-0.83156716, -0.55542421,  0.        ])}\n",
      "cos(angle):  -0.8315671600544349   sin(angle)  -0.5554242147395105\n",
      "theta dot:  1.65494613409\n",
      "Angle:  213.7402806283846\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "3\n",
      "1\n",
      "Reset!\n",
      "min:  -0.979934643317  self.max:  1.01048158965\n",
      "state:  {1: array([ 0.95287294]), 2: array([-0.53372797]), 3: array([-0.57934379, -0.81508329,  0.        ])}\n",
      "cos(angle):  -0.5793437946772219   sin(angle)  -0.8150832887312787\n",
      "theta dot:  -0.533727968935\n",
      "Angle:  234.5958913709677\n",
      "action:  [-0.1369208]\n",
      "state:  {1: array([ 0.89459402]), 2: array([-1.16557855]), 3: array([-0.62583552, -0.77995507,  0.        ])}\n",
      "cos(angle):  -0.625835516793039   sin(angle)  -0.7799550666034486\n",
      "theta dot:  -1.16557855499\n",
      "Angle:  231.2567625846952\n",
      "action:  [-0.13164853]\n",
      "state:  {1: array([ 0.80607941]), 2: array([-1.77029213]), 3: array([-0.69233276, -0.72157837,  0.        ])}\n",
      "cos(angle):  -0.6923327597096063   sin(angle)  -0.7215783740058184\n",
      "theta dot:  -1.77029213474\n",
      "Angle:  226.18526105278224\n",
      "action:  [-0.17991508]\n",
      "state:  {1: array([ 0.68915625]), 2: array([-2.33846318]), 3: array([-0.77178282, -0.63588622,  0.        ])}\n",
      "cos(angle):  -0.7717828180503625   sin(angle)  -0.6358862176224934\n",
      "theta dot:  -2.33846317775\n",
      "Angle:  219.48607318681726\n",
      "action:  [-0.19093695]\n",
      "state:  {1: array([ 0.54695533]), 2: array([-2.84401838]), 3: array([-0.85411198, -0.52008916,  0.        ])}\n",
      "cos(angle):  -0.8541119773686342   sin(angle)  -0.5200891559294827\n",
      "theta dot:  -2.84401838416\n",
      "Angle:  211.33857972570726\n",
      "action:  [-0.03388986]\n",
      "state:  {1: array([ 0.3849969]), 2: array([-3.23916873]), 3: array([-0.9267996 , -0.37555626,  0.        ])}\n",
      "cos(angle):  -0.9267995986942309   sin(angle)  -0.37555625924781577\n",
      "theta dot:  -3.23916873016\n",
      "Angle:  202.05906655680772\n",
      "action:  [ 0.08010834]\n",
      "state:  {1: array([ 0.20955591]), 2: array([-3.50881967]), 3: array([-0.97812339, -0.20802555,  0.        ])}\n",
      "cos(angle):  -0.9781233929123924   sin(angle)  -0.20802554707907825\n",
      "theta dot:  -3.50881967341\n",
      "Angle:  192.00706214482665\n",
      "action:  [ 0.11716007]\n",
      "state:  {1: array([ 0.02719267]), 2: array([-3.64726482]), 3: array([-0.9996303 , -0.02718932,  0.        ])}\n",
      "cos(angle):  -0.9996303021191122   sin(angle)  -0.02718931932308114\n",
      "theta dot:  -3.64726482255\n",
      "Angle:  181.55844252330579\n",
      "action:  [ 0.48096636]\n",
      "state:  {1: array([ 6.13060238]), 2: array([-3.59551186]), 3: array([-0.98838179,  0.15199155,  0.        ])}\n",
      "cos(angle):  -0.988381792966543   sin(angle)  0.1519915502001401\n",
      "theta dot:  -3.59551185837\n",
      "Angle:  171.25724204169614\n",
      "action:  [ 0.22625522]\n",
      "state:  {1: array([ 5.95822339]), 2: array([-3.44757991]), 3: array([-0.94766289,  0.3192727 ,  0.        ])}\n",
      "cos(angle):  -0.9476628855948204   sin(angle)  0.31927269733896496\n",
      "theta dot:  -3.44757991336\n",
      "Angle:  161.38067620892696\n",
      "action:  [-0.06750237]\n",
      "state:  {1: array([ 5.79731085]), 2: array([-3.21825075]), 3: array([-0.88426693,  0.46698179,  0.        ])}\n",
      "cos(angle):  -0.8842669324855129   sin(angle)  0.4669817899154756\n",
      "theta dot:  -3.21825074548\n",
      "Angle:  152.1610885117865\n",
      "action:  [ 0.1113322]\n",
      "state:  {1: array([ 5.65474512]), 2: array([-2.85131457]), 3: array([-0.80894548,  0.58788367,  0.        ])}\n",
      "cos(angle):  -0.8089454823534533   sin(angle)  0.5878836675567188\n",
      "theta dot:  -2.85131457241\n",
      "Angle:  143.99269305989253\n",
      "action:  [ 27.84196613]\n",
      "state:  {1: array([ 5.54922503]), 2: array([-2.11040182]), 3: array([-0.74252758,  0.66981549,  0.        ])}\n",
      "cos(angle):  -0.7425275784446717   sin(angle)  0.6698154934376273\n",
      "theta dot:  -2.11040182174\n",
      "Angle:  137.9468513245787\n",
      "action:  [-0.3652668]\n",
      "state:  {1: array([ 5.46608352]), 2: array([-1.66283022]), 3: array([-0.68433736,  0.72916554,  0.        ])}\n",
      "cos(angle):  -0.6843373560560857   sin(angle)  0.7291655388909752\n",
      "theta dot:  -1.66283022105\n",
      "Angle:  133.18320477840706\n",
      "action:  [-0.25364803]\n",
      "state:  {1: array([ 5.40838336]), 2: array([-1.15400327]), 3: array([-0.64114886,  0.76741654,  0.        ])}\n",
      "cos(angle):  -0.6411488577767296   sin(angle)  0.7674165375932389\n",
      "theta dot:  -1.15400327111\n",
      "Angle:  129.87723666026494\n",
      "action:  [-0.21118536]\n",
      "state:  {1: array([ 5.37787742]), 2: array([-0.61011867]), 3: array([-0.61744342,  0.78661529,  0.        ])}\n",
      "cos(angle):  -0.6174434234675   sin(angle)  0.7866152927681571\n",
      "theta dot:  -0.610118671639\n",
      "Angle:  128.12937950316382\n",
      "action:  [-0.52680307]\n",
      "state:  {1: array([ 5.37291854]), 2: array([-0.09917766]), 3: array([-0.61353511,  0.78966744,  0.        ])}\n",
      "cos(angle):  -0.6135351145536992   sin(angle)  0.7896674383622381\n",
      "theta dot:  -0.099177662171\n",
      "Angle:  127.84525709434695\n",
      "action:  [ 0.05296931]\n",
      "state:  {1: array([ 5.39796946]), 2: array([ 0.50101831]), 3: array([-0.63312244,  0.77405167,  0.        ])}\n",
      "cos(angle):  -0.6331224365766891   sin(angle)  0.7740516651381846\n",
      "theta dot:  0.501018312963\n",
      "Angle:  129.28056547754926\n",
      "action:  [ 0.20830575]\n",
      "state:  {1: array([ 5.4536096]), 2: array([ 1.11280292]), 3: array([-0.6751888 ,  0.73764496,  0.        ])}\n",
      "cos(angle):  -0.6751888002155472   sin(angle)  0.7376449580004529\n",
      "theta dot:  1.11280292418\n",
      "Angle:  132.46850357202334\n",
      "action:  [ 0.24694625]\n",
      "state:  {1: array([ 5.53876353]), 2: array([ 1.70307858]), 3: array([-0.7354798 ,  0.67754665,  0.        ])}\n",
      "cos(angle):  -0.7354797993570467   sin(angle)  0.6775466513368053\n",
      "theta dot:  1.70307858077\n",
      "Angle:  137.34745290578928\n",
      "action:  [ 0.16840314]\n",
      "state:  {1: array([ 5.65058848]), 2: array([ 2.23649904]), 3: array([-0.80649488,  0.59124107,  0.        ])}\n",
      "cos(angle):  -0.8064948805842306   sin(angle)  0.5912410740057119\n",
      "theta dot:  2.2364990397\n",
      "Angle:  143.75453571623518\n",
      "action:  [ 0.10185146]\n",
      "state:  {1: array([ 5.78534886]), 2: array([ 2.69520756]), 3: array([-0.87861777,  0.47752572,  0.        ])}\n",
      "cos(angle):  -0.8786177704748492   sin(angle)  0.47752572015107747\n",
      "theta dot:  2.69520756444\n",
      "Angle:  151.47571857846344\n",
      "action:  [ 0.13054691]\n",
      "state:  {1: array([ 5.93899556]), 2: array([ 3.07293389]), 3: array([-0.94134917,  0.33743406,  0.        ])}\n",
      "cos(angle):  -0.9413491672481182   sin(angle)  0.3374340607011604\n",
      "theta dot:  3.07293389033\n",
      "Angle:  160.27900512447707\n",
      "action:  [ 0.18437797]\n",
      "state:  {1: array([ 6.10667886]), 2: array([ 3.35366613]), 3: array([-0.98446314,  0.17559138,  0.        ])}\n",
      "cos(angle):  -0.9844631374409537   sin(angle)  0.17559137512962814\n",
      "theta dot:  3.35366613088\n",
      "Angle:  169.8865284176727\n",
      "action:  [ 0.1657334]\n",
      "state:  {1: array([ 6.28218985]), 2: array([ 3.51021967]), 3: array([ -9.99999505e-01,   9.95460531e-04,   0.00000000e+00])}\n",
      "cos(angle):  -0.9999995045290433   sin(angle)  0.0009954605305617487\n",
      "theta dot:  3.51021967157\n",
      "Angle:  179.9425435195659\n",
      "action:  [ 0.20657644]\n",
      "state:  {1: array([ 0.17610218]), 2: array([ 3.54195273]), 3: array([-0.98453404, -0.17519337,  0.        ])}\n",
      "cos(angle):  -0.9845340430273196   sin(angle)  -0.17519337350562073\n",
      "theta dot:  3.54195273241\n",
      "Angle:  190.0903087663664\n",
      "action:  [ 0.0968592]\n",
      "state:  {1: array([ 0.34735651]), 2: array([ 3.42508658]), 3: array([-0.94027588, -0.34041339,  0.        ])}\n",
      "cos(angle):  -0.9402758782265732   sin(angle)  -0.34041338520282427\n",
      "theta dot:  3.42508658192\n",
      "Angle:  199.90243610189748\n",
      "action:  [ 0.08436709]\n",
      "state:  {1: array([ 0.50647809]), 2: array([ 3.18243161]), 3: array([-0.87445841, -0.48510049,  0.        ])}\n",
      "cos(angle):  -0.8744584099985018   sin(angle)  -0.4851004939008949\n",
      "theta dot:  3.18243160598\n",
      "Angle:  209.01940976303754\n",
      "action:  [ 0.13941955]\n",
      "state:  {1: array([ 0.64845404]), 2: array([ 2.83951917]), 3: array([-0.79701844, -0.60395497,  0.        ])}\n",
      "cos(angle):  -0.7970184385925625   sin(angle)  -0.603954972281439\n",
      "theta dot:  2.83951916785\n",
      "Angle:  217.1540139490233\n",
      "action:  [ 0.01447551]\n",
      "state:  {1: array([ 0.76789026]), 2: array([ 2.38872427]), 3: array([-0.71937774, -0.69461908,  0.        ])}\n",
      "cos(angle):  -0.7193777372591252   sin(angle)  -0.6946190834809688\n",
      "theta dot:  2.38872426556\n",
      "Angle:  223.9971888885445\n",
      "action:  [-0.2459395]\n",
      "state:  {1: array([ 0.85943371]), 2: array([ 1.83086903]), 3: array([-0.65286652, -0.75747297,  0.        ])}\n",
      "cos(angle):  -0.6528665234124232   sin(angle)  -0.7574729715358667\n",
      "theta dot:  1.83086902797\n",
      "Angle:  229.2422300305635\n",
      "action:  [-0.51669096]\n",
      "state:  {1: array([ 0.91869674]), 2: array([ 1.18526065]), 3: array([-0.60685652, -0.7948114 ,  0.        ])}\n",
      "cos(angle):  -0.6068565167384774   sin(angle)  -0.7948114041029117\n",
      "theta dot:  1.1852606546\n",
      "Angle:  232.63774374695325\n",
      "action:  [-0.56821652]\n",
      "state:  {1: array([ 0.94389272]), 2: array([ 0.50391962]), 3: array([-0.58663996, -0.80984786,  0.        ])}\n",
      "cos(angle):  -0.5866399649125144   sin(angle)  -0.809847857049362\n",
      "theta dot:  0.503919624193\n",
      "Angle:  234.08136375514476\n",
      "action:  [-0.4817702]\n",
      "state:  {1: array([ 0.93510613]), 2: array([-0.1757318]), 3: array([-0.59373303, -0.8046621 ,  0.        ])}\n",
      "cos(angle):  -0.5937330290205735   sin(angle)  -0.8046620969388671\n",
      "theta dot:  -0.175731799138\n",
      "Angle:  233.57793041154991\n",
      "action:  [-0.3960539]\n",
      "state:  {1: array([ 0.89317431]), 2: array([-0.83863646]), 3: array([-0.62694219, -0.77906578,  0.        ])}\n",
      "cos(angle):  -0.62694219338439   sin(angle)  -0.7790657778097883\n",
      "theta dot:  -0.838636456158\n",
      "Angle:  231.17541955548242\n",
      "action:  [-0.17046615]\n",
      "state:  {1: array([ 0.82074902]), 2: array([-1.44850571]), 3: array([-0.68167337, -0.73165662,  0.        ])}\n",
      "cos(angle):  -0.6816733702370384   sin(angle)  -0.7316566245922178\n",
      "theta dot:  -1.4485057126\n",
      "Angle:  227.02576606253854\n",
      "action:  [-0.23003902]\n",
      "state:  {1: array([ 0.71916132]), 2: array([-2.03175403]), 3: array([-0.75235848, -0.65875392,  0.        ])}\n",
      "cos(angle):  -0.7523584759496154   sin(angle)  -0.6587539173824867\n",
      "theta dot:  -2.0317540344\n",
      "Angle:  221.20523311449895\n",
      "action:  [-0.14306563]\n",
      "state:  {1: array([ 0.59179736]), 2: array([-2.54727932]), 3: array([-0.82993936, -0.55785362,  0.        ])}\n",
      "cos(angle):  -0.8299393584178333   sin(angle)  -0.5578536200016948\n",
      "theta dot:  -2.54727931619\n",
      "Angle:  213.9078324760713\n",
      "action:  [ 0.09643182]\n",
      "state:  {1: array([ 0.44423712]), 2: array([-2.95120476]), 3: array([-0.90293879, -0.42976917,  0.        ])}\n",
      "cos(angle):  -0.9029387910721521   sin(angle)  -0.42976917011014226\n",
      "theta dot:  -2.95120475852\n",
      "Angle:  205.4532733894013\n",
      "action:  [-0.07459356]\n",
      "state:  {1: array([ 0.28000108]), 2: array([-3.28472067]), 3: array([-0.96105514, -0.27635669,  0.        ])}\n",
      "cos(angle):  -0.9610551384688568   sin(angle)  -0.2763566912933474\n",
      "theta dot:  -3.28472066992\n",
      "Angle:  196.0432638308144\n",
      "action:  [ 0.06333686]\n",
      "state:  {1: array([ 0.1058767]), 2: array([-3.48248766]), 3: array([-0.9944003, -0.105679 ,  0.       ])}\n",
      "cos(angle):  -0.9944002959174244   sin(angle)  -0.10567900207391606\n",
      "theta dot:  -3.48248765936\n",
      "Angle:  186.06669490599683\n",
      "action:  [ 0.10842232]\n",
      "state:  {1: array([ 6.21178783]), 2: array([-3.54548356]), 3: array([-0.99745228,  0.07133683,  0.        ])}\n",
      "cos(angle):  -0.9974522827447868   sin(angle)  0.07133683233234975\n",
      "theta dot:  -3.5454835636\n",
      "Angle:  175.9088145965652\n",
      "action:  [-0.26950202]\n",
      "state:  {1: array([ 6.03516752]), 2: array([-3.53240624]), 3: array([-0.96940092,  0.24548288,  0.        ])}\n",
      "cos(angle):  -0.9694009249049654   sin(angle)  0.2454828849296783\n",
      "theta dot:  -3.53240624274\n",
      "Angle:  165.78923979881324\n",
      "action:  [-0.38234476]\n",
      "state:  {1: array([ 5.86488523]), 2: array([-3.40564579]), 3: array([-0.91378078,  0.40620768,  0.        ])}\n",
      "cos(angle):  -0.9137807816721297   sin(angle)  0.40620768462286827\n",
      "theta dot:  -3.40564579295\n",
      "Angle:  156.03280609098124\n",
      "action:  [-0.58625352]\n",
      "state:  {1: array([ 5.70543883]), 2: array([-3.18892806]), 3: array([-0.83769551,  0.54613756,  0.        ])}\n",
      "cos(angle):  -0.8376955074111482   sin(angle)  0.5461375622159484\n",
      "theta dot:  -3.18892805698\n",
      "Angle:  146.89722151215256\n",
      "action:  [-0.56848708]\n",
      "state:  {1: array([ 5.56220893]), 2: array([-2.86459795]), 3: array([-0.75116156,  0.6601184 ,  0.        ])}\n",
      "cos(angle):  -0.7511615621037735   sin(angle)  0.660118404241102\n",
      "theta dot:  -2.86459794704\n",
      "Angle:  138.69077208404684\n",
      "action:  [ 0.14426469]\n",
      "state:  {1: array([ 5.44481546]), 2: array([-2.34786944]), 3: array([-0.66867582,  0.74355407,  0.        ])}\n",
      "cos(angle):  -0.6686758183338041   sin(angle)  0.7435540666122522\n",
      "theta dot:  -2.34786943964\n",
      "Angle:  131.9646373257138\n",
      "action:  [ 0.27515601]\n",
      "state:  {1: array([ 5.35736893]), 2: array([-1.74893049]), 3: array([-0.60118242,  0.79911182,  0.        ])}\n",
      "cos(angle):  -0.6011824182359089   sin(angle)  0.799111819462098\n",
      "theta dot:  -1.74893048839\n",
      "Angle:  126.95433225966474\n",
      "action:  [ 0.8332448]\n",
      "state:  {1: array([ 5.30613844]), 2: array([-1.0246099]), 3: array([-0.55947268,  0.82884879,  0.        ])}\n",
      "cos(angle):  -0.559472681218512   sin(angle)  0.8288487913788433\n",
      "theta dot:  -1.02460990401\n",
      "Angle:  124.01904796628787\n",
      "action:  [ 0.31528047]\n",
      "state:  {1: array([ 5.28835438]), 2: array([-0.35568124]), 3: array([-0.54464469,  0.8386669 ,  0.        ])}\n",
      "cos(angle):  -0.544644689205348   sin(angle)  0.8386668960441981\n",
      "theta dot:  -0.35568124043\n",
      "Angle:  123.00009865260353\n",
      "action:  [ 0.41772161]\n",
      "state:  {1: array([ 5.30515323]), 2: array([ 0.33597717]), 3: array([-0.55865583,  0.82939958,  0.        ])}\n",
      "cos(angle):  -0.5586558252391238   sin(angle)  0.8293995833893296\n",
      "theta dot:  0.33597717376\n",
      "Angle:  123.96260010531438\n",
      "action:  [ 0.59173444]\n",
      "state:  {1: array([ 5.35749259]), 2: array([ 1.04678703]), 3: array([-0.60128123,  0.79903748,  0.        ])}\n",
      "cos(angle):  -0.6012812260545047   sin(angle)  0.7990374754630671\n",
      "theta dot:  1.0467870267\n",
      "Angle:  126.96141702672719\n",
      "action:  [ 0.7831419]\n",
      "state:  {1: array([ 5.44566941]), 2: array([ 1.76353642]), 3: array([-0.66931053,  0.74298278,  0.        ])}\n",
      "cos(angle):  -0.6693105318968607   sin(angle)  0.7429827803468539\n",
      "theta dot:  1.76353641768\n",
      "Angle:  132.01356490014564\n",
      "action:  [ 0.85920703]\n",
      "state:  {1: array([ 5.56815213]), 2: array([ 2.44965456]), 3: array([-0.75507149,  0.65564247,  0.        ])}\n",
      "cos(angle):  -0.7550714919835417   sin(angle)  0.6556424650628941\n",
      "theta dot:  2.44965455683\n",
      "Angle:  139.0312918582173\n",
      "action:  [ 0.76072851]\n",
      "state:  {1: array([ 5.72092692]), 2: array([ 3.05549568]), 3: array([-0.84605333,  0.53309827,  0.        ])}\n",
      "cos(angle):  -0.8460533263675669   sin(angle)  0.5330982732502285\n",
      "theta dot:  3.05549568185\n",
      "Angle:  147.7846217336438\n",
      "action:  [ 0.56783806]\n",
      "state:  {1: array([ 5.89795167]), 2: array([ 3.5404951]), 3: array([-0.92671066,  0.37577566,  0.        ])}\n",
      "cos(angle):  -0.9267106639436475   sin(angle)  0.3757756582498713\n",
      "theta dot:  3.54049509585\n",
      "Angle:  157.92736933443462\n",
      "action:  [ 0.39330312]\n",
      "state:  {1: array([ 6.09201779]), 2: array([ 3.88132231]), 3: array([-0.98178307,  0.19000527,  0.        ])}\n",
      "cos(angle):  -0.9817830695850163   sin(angle)  0.19000527433790654\n",
      "theta dot:  3.88132230816\n",
      "Angle:  169.04651269243152\n",
      "action:  [ 0.20243328]\n",
      "state:  {1: array([ 0.01154204]), 2: array([ 4.05419126]), 3: array([-0.99993339, -0.01154179,  0.        ])}\n",
      "cos(angle):  -0.9999333913486986   sin(angle)  -0.011541787811704765\n",
      "theta dot:  4.05419125621\n",
      "Angle:  180.661729783597\n",
      "action:  [-0.09789086]\n",
      "state:  {1: array([ 0.21308461]), 2: array([ 4.03085129]), 3: array([-0.97738325, -0.21147575,  0.        ])}\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "cos(angle):  -0.977383245709086   sin(angle)  -0.21147574567115826\n",
      "theta dot:  4.03085128562\n",
      "Angle:  192.2092411060395\n",
      "action:  [-0.34446885]\n",
      "state:  {1: array([ 0.40411332]), 2: array([ 3.82057415]), 3: array([-0.91945141, -0.39320365,  0.        ])}\n",
      "cos(angle):  -0.9194514060297758   sin(angle)  -0.3932036520047447\n",
      "theta dot:  3.8205741486\n",
      "Angle:  203.15435421319106\n",
      "action:  [-0.24746392]\n",
      "state:  {1: array([ 0.57854091]), 2: array([ 3.48855182]), 3: array([-0.83726138, -0.54680288,  0.        ])}\n",
      "cos(angle):  -0.8372613772094626   sin(angle)  -0.5468028769431578\n",
      "theta dot:  3.48855182117\n",
      "Angle:  213.1482956413\n",
      "action:  [-0.33856203]\n",
      "state:  {1: array([ 0.72992417]), 2: array([ 3.02766536]), 3: array([-0.74522497, -0.66681313,  0.        ])}\n",
      "cos(angle):  -0.7452249657107004   sin(angle)  -0.6668131301057931\n",
      "theta dot:  3.02766535948\n",
      "Angle:  221.82189770244227\n",
      "action:  [-0.08151688]\n",
      "state:  {1: array([ 0.85569057]), 2: array([ 2.51532798]), 3: array([-0.65569727, -0.7550239 ,  0.        ])}\n",
      "cos(angle):  -0.6556972664028239   sin(angle)  -0.7550239034837667\n",
      "theta dot:  2.51532797953\n",
      "Angle:  229.02776471790648\n",
      "action:  [-0.28560148]\n",
      "state:  {1: array([ 0.95100157]), 2: array([ 1.90621983]), 3: array([-0.58086811, -0.81399769,  0.        ])}\n",
      "cos(angle):  -0.5808681091076944   sin(angle)  -0.813997690304863\n",
      "theta dot:  1.9062198299\n",
      "Angle:  234.48866950180124\n",
      "action:  [ 0.21338436]\n",
      "state:  {1: array([ 1.01738803]), 2: array([ 1.32772922]), 3: array([-0.52558985, -0.8507381 ,  0.        ])}\n",
      "cos(angle):  -0.5255898472996093   sin(angle)  -0.850738098603544\n",
      "theta dot:  1.32772921676\n",
      "Angle:  238.29232463002722\n",
      "action:  [ 0.08086445]\n",
      "state:  {1: array([ 1.05247829]), 2: array([ 0.70180531]), 3: array([-0.49541979, -0.86865369,  0.        ])}\n",
      "cos(angle):  -0.4954197943359319   sin(angle)  -0.8686536866784962\n",
      "theta dot:  0.70180531028\n",
      "Angle:  240.30284404450464\n",
      "action:  [-0.00060549]\n",
      "state:  {1: array([ 1.0549895]), 2: array([ 0.05022422]), 3: array([-0.49323686, -0.86989505,  0.        ])}\n",
      "cos(angle):  -0.49323686173735853   sin(angle)  -0.8698950501201176\n",
      "theta dot:  0.0502242221965\n",
      "Angle:  240.44672550610642\n",
      "action:  [-0.01297687]\n",
      "state:  {1: array([ 1.02478232]), 2: array([-0.6041436]), 3: array([-0.51928493, -0.85460117,  0.        ])}\n",
      "cos(angle):  -0.5192849261706024   sin(angle)  -0.8546011733270625\n",
      "theta dot:  -0.604143595511\n",
      "Angle:  238.71598564119768\n",
      "action:  [ 0.06539758]\n",
      "state:  {1: array([ 0.96301808]), 2: array([-1.23528484]), 3: array([-0.57104499, -0.82091876,  0.        ])}\n",
      "cos(angle):  -0.5710449912423246   sin(angle)  -0.8209187645419329\n",
      "theta dot:  -1.23528483877\n",
      "Angle:  235.1771635286023\n",
      "action:  [ 0.07983921]\n",
      "state:  {1: array([ 0.87106818]), 2: array([-1.83899803]), 3: array([-0.64400974, -0.76501729,  0.        ])}\n",
      "cos(angle):  -0.6440097389922171   sin(angle)  -0.7650172913622125\n",
      "theta dot:  -1.83899803018\n",
      "Angle:  229.9088345651264\n",
      "action:  [ 0.06360114]\n",
      "state:  {1: array([ 0.75090714]), 2: array([-2.40322083]), 3: array([-0.73107023, -0.68230222,  0.        ])}\n",
      "cos(angle):  -0.731070227363774   sin(angle)  -0.6823022223782359\n",
      "theta dot:  -2.40322082714\n",
      "Angle:  223.02413013291215\n",
      "action:  [ 0.03663323]\n",
      "state:  {1: array([ 0.60543451]), 2: array([-2.90945251]), 3: array([-0.82225489, -0.56911941,  0.        ])}\n",
      "cos(angle):  -0.8222548856572383   sin(angle)  -0.5691194101529151\n",
      "theta dot:  -2.90945250978\n",
      "Angle:  214.68918214844462\n",
      "action:  [ 0.0206333]\n",
      "state:  {1: array([ 0.43877466]), 2: array([-3.33319707]), 3: array([-0.9052729 , -0.42483052,  0.        ])}\n",
      "cos(angle):  -0.9052729049181363   sin(angle)  -0.4248305163486718\n",
      "theta dot:  -3.33319707201\n",
      "Angle:  205.1402982523141\n",
      "action:  [ 0.04950842]\n",
      "state:  {1: array([ 0.25655497]), 2: array([-3.6443937]), 3: array([-0.96726989, -0.25374979,  0.        ])}\n",
      "cos(angle):  -0.967269890854245   sin(angle)  -0.25374979457492597\n",
      "theta dot:  -3.6443936968\n",
      "Angle:  194.6999037809736\n",
      "action:  [ 0.07902884]\n",
      "state:  {1: array([ 0.06541239]), 2: array([-3.82285172]), 3: array([-0.99786137, -0.06536575,  0.        ])}\n",
      "cos(angle):  -0.9978613724433804   sin(angle)  -0.06536575085940188\n",
      "theta dot:  -3.82285171647\n",
      "Angle:  183.74826593775106\n",
      "action:  [ 0.11846139]\n",
      "state:  {1: array([ 6.15589235]), 2: array([-3.85410682]), 3: array([-0.99190919,  0.12694947,  0.        ])}\n",
      "cos(angle):  -0.9919091859162878   sin(angle)  0.12694946591020792\n",
      "theta dot:  -3.85410682136\n",
      "Angle:  172.7062471893562\n",
      "action:  [-0.0180528]\n",
      "state:  {1: array([ 5.96781222]), 2: array([-3.76160264]), 3: array([-0.95068072,  0.31017118,  0.        ])}\n",
      "cos(angle):  -0.9506807249486257   sin(angle)  0.31017117727338156\n",
      "theta dot:  -3.76160264182\n",
      "Angle:  161.93007460965123\n",
      "action:  [-0.19521481]\n",
      "state:  {1: array([ 5.7898994]), 2: array([-3.55825648]), 3: array([-0.88078166,  0.47352261,  0.        ])}\n",
      "cos(angle):  -0.8807816643050931   sin(angle)  0.47352260751093\n",
      "theta dot:  -3.55825648061\n",
      "Angle:  151.73644450860468\n",
      "action:  [-0.4824205]\n",
      "state:  {1: array([ 5.62612552]), 2: array([-3.2754776]), 3: array([-0.79179151,  0.61079146,  0.        ])}\n",
      "cos(angle):  -0.79179150729609   sin(angle)  0.6107914611172833\n",
      "theta dot:  -3.27547759945\n",
      "Angle:  142.35291433448253\n",
      "action:  [-0.27337421]\n",
      "state:  {1: array([ 5.48320601]), 2: array([-2.85839013]), 3: array([-0.69672156,  0.71734167,  0.        ])}\n",
      "cos(angle):  -0.6967215617072994   sin(angle)  0.7173416657717171\n",
      "theta dot:  -2.85839013494\n",
      "Angle:  134.16424893644728\n",
      "action:  [-0.21022461]\n",
      "state:  {1: array([ 5.36561013]), 2: array([-2.35191758]), 3: array([-0.60774757,  0.79413027,  0.        ])}\n",
      "cos(angle):  -0.6077475689100225   sin(angle)  0.7941302742522524\n",
      "theta dot:  -2.35191757766\n",
      "Angle:  127.42651714407145\n",
      "action:  [ 0.27722582]\n",
      "state:  {1: array([ 5.27987333]), 2: array([-1.714736]), 3: array([-0.53751242,  0.84325583,  0.        ])}\n",
      "cos(angle):  -0.537512417328088   sin(angle)  0.8432558337883678\n",
      "theta dot:  -1.71473599927\n",
      "Angle:  122.51417184441078\n",
      "action:  [-0.09900558]\n",
      "state:  {1: array([ 5.22501608]), 2: array([-1.09714496]), 3: array([-0.49046835,  0.871459  ,  0.        ])}\n",
      "cos(angle):  -0.4904683505858911   sin(angle)  0.8714590048152324\n",
      "theta dot:  -1.0971449607\n",
      "Angle:  119.37109040618918\n",
      "action:  [ 0.10582412]\n",
      "state:  {1: array([ 5.20363223]), 2: array([-0.42767709]), 3: array([-0.47172248,  0.88174707,  0.        ])}\n",
      "cos(angle):  -0.47172248444602255   sin(angle)  0.8817470712557383\n",
      "theta dot:  -0.427677089753\n",
      "Angle:  118.14588865938026\n",
      "action:  [ 0.35871379]\n",
      "state:  {1: array([ 5.21800424]), 2: array([ 0.28744028]), 3: array([-0.48434581,  0.87487664,  0.        ])}\n",
      "cos(angle):  -0.48434581211413164   sin(angle)  0.8748766394683894\n",
      "theta dot:  0.287440282315\n",
      "Angle:  118.969342485722\n",
      "action:  [ 0.6039148]\n",
      "state:  {1: array([ 5.26971349]), 2: array([ 1.03418498]), 3: array([-0.52891748,  0.84867326,  0.        ])}\n",
      "cos(angle):  -0.5289174795724104   sin(angle)  0.8486732585646662\n",
      "theta dot:  1.0341849826\n",
      "Angle:  121.93205729455156\n",
      "action:  [ 1.09980104]\n",
      "state:  {1: array([ 5.3614965]), 2: array([ 1.83566008]), 3: array([-0.60447567,  0.7966236 ,  0.        ])}\n",
      "cos(angle):  -0.6044756738034652   sin(angle)  0.7966235998135172\n",
      "theta dot:  1.83566008264\n",
      "Angle:  127.19082376505332\n",
      "action:  [ 0.78046633]\n",
      "state:  {1: array([ 5.48900638]), 2: array([ 2.55019773]), 3: array([-0.70087067,  0.71328838,  0.        ])}\n",
      "cos(angle):  -0.7008706669884198   sin(angle)  0.7132883765737441\n",
      "theta dot:  2.5501977318\n",
      "Angle:  134.49658502881346\n",
      "action:  [ 0.76248878]\n",
      "state:  {1: array([ 5.64898325]), 2: array([ 3.19953733]), 3: array([-0.80554476,  0.59253492,  0.        ])}\n",
      "cos(angle):  -0.805544762351944   sin(angle)  0.5925349237364409\n",
      "theta dot:  3.19953733067\n",
      "Angle:  143.6625628668719\n",
      "action:  [ 0.68651002]\n",
      "state:  {1: array([ 5.836329]), 2: array([ 3.74691503]), 3: array([-0.90181005,  0.43213266,  0.        ])}\n",
      "cos(angle):  -0.9018100495835127   sin(angle)  0.43213265841658194\n",
      "theta dot:  3.74691502624\n",
      "Angle:  154.39665862578397\n",
      "action:  [ 0.50214662]\n",
      "state:  {1: array([ 6.04364583]), 2: array([ 4.14633651]), 3: array([-0.97144734,  0.23725528,  0.        ])}\n",
      "cos(angle):  -0.971447338812332   sin(angle)  0.23725528005597316\n",
      "theta dot:  4.14633651352\n",
      "Angle:  166.27500998232767\n",
      "action:  [ 0.71060985]\n",
      "state:  {1: array([ 6.2651893]), 2: array([ 4.43086945]), 3: array([-0.99983808,  0.01799504,  0.        ])}\n",
      "cos(angle):  -0.9998380762398237   sin(angle)  0.01799503545560803\n",
      "theta dot:  4.43086945144\n",
      "Angle:  178.96848625649363\n",
      "action:  [ 0.59511933]\n",
      "state:  {1: array([ 0.20868567]), 2: array([ 4.53363363]), 3: array([-0.97830405, -0.20717427,  0.        ])}\n",
      "cos(angle):  -0.978304054043571   sin(angle)  -0.20717426925637683\n",
      "theta dot:  4.53363362716\n",
      "Angle:  191.9572013545626\n",
      "action:  [ 0.63952711]\n",
      "state:  {1: array([ 0.43239477]), 2: array([ 4.47418199]), 3: array([-0.90796483, -0.41904637,  0.        ])}\n",
      "cos(angle):  -0.9079648327863322   sin(angle)  -0.4190463726406515\n",
      "theta dot:  4.47418199118\n",
      "Angle:  204.7747586249368\n",
      "action:  [ 0.62885889]\n",
      "state:  {1: array([ 0.64510608]), 2: array([ 4.25422605]), 3: array([-0.79903599, -0.6012832 ,  0.        ])}\n",
      "cos(angle):  -0.7990359894908252   sin(angle)  -0.6012832007452211\n",
      "theta dot:  4.25422604521\n",
      "Angle:  216.9621899996687\n",
      "action:  [ 0.42990595]\n",
      "state:  {1: array([ 0.83849355]), 2: array([ 3.86774954]), 3: array([-0.66858383, -0.74363678,  0.        ])}\n",
      "cos(angle):  -0.6685838332519743   sin(angle)  -0.7436367782150746\n",
      "theta dot:  3.86774953757\n",
      "Angle:  228.04245032501277\n",
      "action:  [ 0.42992676]\n",
      "state:  {1: array([ 1.0072191]), 2: array([ 3.37451097]), 3: array([-0.53421362, -0.84534952,  0.        ])}\n",
      "cos(angle):  -0.5342136150488775   sin(angle)  -0.8453495214977115\n",
      "theta dot:  3.37451096837\n",
      "Angle:  237.70968953922366\n",
      "action:  [ 0.20031684]\n",
      "state:  {1: array([ 1.14574642]), 2: array([ 2.77054635]), 3: array([-0.41236625, -0.91101815,  0.        ])}\n",
      "cos(angle):  -0.41236624882889944   sin(angle)  -0.9110181539501736\n",
      "theta dot:  2.77054635374\n",
      "Angle:  245.64670162976594\n",
      "action:  [ 0.14337897]\n",
      "state:  {1: array([ 1.2511859]), 2: array([ 2.10878958]), 3: array([-0.31419674, -0.94935789,  0.        ])}\n",
      "cos(angle):  -0.31419674143453485   sin(angle)  -0.9493578922998007\n",
      "theta dot:  2.10878958391\n",
      "Angle:  251.68792465472046\n",
      "action:  [ 0.1027109]\n",
      "state:  {1: array([ 1.32179479]), 2: array([ 1.4121778]), 3: array([-0.24643641, -0.96915896,  0.        ])}\n",
      "cos(angle):  -0.2464364140623536   sin(angle)  -0.9691589621027544\n",
      "theta dot:  1.41217780043\n",
      "Angle:  255.73350658873483\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -0.979934643317  self.max:  27.8419661301\n",
      "state:  {1: array([ 2.88546817]), 2: array([ 0.66321741]), 3: array([ 0.96737904, -0.25333337,  0.        ])}\n",
      "cos(angle):  0.9673790379735809   sin(angle)  -0.2533333710534581\n",
      "theta dot:  0.663217409133\n",
      "Angle:  345.3251823391623\n",
      "action:  [ 0.27116458]\n",
      "state:  {1: array([ 2.91116277]), 2: array([ 0.51389207]), 3: array([ 0.9735683 , -0.22839606,  0.        ])}\n",
      "cos(angle):  0.9735683018795769   sin(angle)  -0.2283960629593186\n",
      "theta dot:  0.513892068542\n",
      "Angle:  346.797371229179\n",
      "action:  [ 0.18260347]\n",
      "state:  {1: array([ 2.92966205]), 2: array([ 0.36998554]), 3: array([ 0.97762664, -0.2103477 ,  0.        ])}\n",
      "cos(angle):  0.9776266388498099   sin(angle)  -0.21034770027557537\n",
      "theta dot:  0.369985542499\n",
      "Angle:  347.85729925390956\n",
      "action:  [ 0.08150103]\n",
      "state:  {1: array([ 2.94088455]), 2: array([ 0.22444992]), 3: array([ 0.97992565, -0.19936327,  0.        ])}\n",
      "cos(angle):  0.9799256528843175   sin(angle)  -0.19936327349650956\n",
      "theta dot:  0.224449922275\n",
      "Angle:  348.50029941321503\n",
      "action:  [-0.05263213]\n",
      "state:  {1: array([ 2.94423618]), 2: array([ 0.06703265]), 3: array([ 0.98058834, -0.19607781,  0.        ])}\n",
      "cos(angle):  0.9805883400801778   sin(angle)  -0.19607780929723173\n",
      "theta dot:  0.0670326482847\n",
      "Angle:  348.69233335597096\n",
      "action:  [-0.36430718]\n",
      "state:  {1: array([ 2.93750259]), 2: array([-0.13467179]), 3: array([ 0.97924581, -0.20267619,  0.        ])}\n",
      "cos(angle):  0.9792458121723848   sin(angle)  -0.2026761933302638\n",
      "theta dot:  -0.13467178524\n",
      "Angle:  348.3065280124649\n",
      "action:  [-0.75646965]\n",
      "state:  {1: array([ 2.91749512]), 2: array([-0.40014938]), 3: array([ 0.97499506, -0.22222655,  0.        ])}\n",
      "cos(angle):  0.9749950561512905   sin(angle)  -0.22222655215014706\n",
      "theta dot:  -0.400149377018\n",
      "Angle:  347.16018716921167\n",
      "action:  [-1.00422916]\n",
      "state:  {1: array([ 2.88162244]), 2: array([-0.71745367]), 3: array([ 0.96639763, -0.25705177,  0.        ])}\n",
      "cos(angle):  0.966397634452578   sin(angle)  -0.25705176934707435\n",
      "theta dot:  -0.717453665461\n",
      "Angle:  345.1048386241552\n",
      "action:  [-1.64828773]\n",
      "state:  {1: array([ 2.82374815]), 2: array([-1.15748565]), 3: array([ 0.94991126, -0.31251975,  0.        ])}\n",
      "cos(angle):  0.9499112608942007   sin(angle)  -0.3125197536578923\n",
      "theta dot:  -1.15748565154\n",
      "Angle:  341.78889424427115\n",
      "action:  [-1.1294365]\n",
      "state:  {1: array([ 2.74568361]), 2: array([-1.56129094]), 3: array([ 0.92264637, -0.38564708,  0.        ])}\n",
      "cos(angle):  0.9226463746301217   sin(angle)  -0.3856470762003689\n",
      "theta dot:  -1.56129094119\n",
      "Angle:  337.3161356274115\n",
      "action:  [-0.74025601]\n",
      "state:  {1: array([ 2.64760537]), 2: array([-1.96156465]), 3: array([ 0.88044933, -0.47414024,  0.        ])}\n",
      "cos(angle):  0.8804493331725364   sin(angle)  -0.47414024477577954\n",
      "theta dot:  -1.96156465047\n",
      "Angle:  331.6966799824504\n",
      "action:  [-0.33805072]\n",
      "state:  {1: array([ 2.5292115]), 2: array([-2.36787744]), 3: array([ 0.81828161, -0.57481754,  0.        ])}\n",
      "cos(angle):  0.818281611653072   sin(angle)  -0.574817539772797\n",
      "theta dot:  -2.36787744254\n",
      "Angle:  324.9132266520376\n",
      "action:  [ 7.07181538]\n",
      "state:  {1: array([ 2.40426197]), 2: array([-2.4989906]), 3: array([ 0.74026582, -0.6723143 ,  0.        ])}\n",
      "cos(angle):  0.740265815436753   sin(angle)  -0.67231430335503\n",
      "theta dot:  -2.49899059737\n",
      "Angle:  317.75416267944684\n",
      "action:  [-0.08570918]\n",
      "state:  {1: array([ 2.25345784]), 2: array([-3.0160827]), 3: array([ 0.63086031, -0.77589643,  0.        ])}\n",
      "cos(angle):  0.630860312841208   sin(angle)  -0.7758964271614435\n",
      "theta dot:  -3.01608270207\n",
      "Angle:  309.11374240996923\n",
      "action:  [ 0.45103459]\n",
      "state:  {1: array([ 2.07694035]), 2: array([-3.53034983]), 3: array([ 0.48480834, -0.87462042,  0.        ])}\n",
      "cos(angle):  0.48480833996814504   sin(angle)  -0.874620416807961\n",
      "theta dot:  -3.53034983445\n",
      "Angle:  299.00005877422313\n",
      "action:  [ 0.50964204]\n",
      "state:  {1: array([ 1.8714469]), 2: array([-4.10986884]), 3: array([ 0.29614166, -0.95514403,  0.        ])}\n",
      "cos(angle):  0.2961416642304948   sin(angle)  -0.9551440282526991\n",
      "theta dot:  -4.10986884105\n",
      "Angle:  287.2261793594455\n",
      "action:  [ 0.20063176]\n",
      "state:  {1: array([ 1.6316403]), 2: array([-4.7961321]), 3: array([ 0.06080644, -0.99814958,  0.        ])}\n",
      "cos(angle):  0.060806438598743066   sin(angle)  -0.9981495764788648\n",
      "theta dot:  -4.79613209891\n",
      "Angle:  273.4863051265086\n",
      "action:  [ 0.17899668]\n",
      "state:  {1: array([ 1.35574556]), 2: array([-5.51789478]), 3: array([-0.21339703, -0.97696556,  0.        ])}\n",
      "cos(angle):  -0.21339702589121778   sin(angle)  -0.9769655620034838\n",
      "theta dot:  -5.51789477935\n",
      "Angle:  257.67873795878245\n",
      "action:  [ 0.04226501]\n",
      "state:  {1: array([ 1.0435316]), 2: array([-6.2442792]), 3: array([-0.50317144, -0.86418661,  0.        ])}\n",
      "cos(angle):  -0.5031714398535697   sin(angle)  -0.8641866130157799\n",
      "theta dot:  -6.24427919952\n",
      "Angle:  239.79023757819758\n",
      "action:  [ 0.05043866]\n",
      "state:  {1: array([ 0.69928893]), 2: array([-6.88485336]), 3: array([-0.76530008, -0.64367367,  0.        ])}\n",
      "cos(angle):  -0.7653000764211152   sin(angle)  -0.6436736696726341\n",
      "theta dot:  -6.88485336092\n",
      "Angle:  220.06663169319884\n",
      "action:  [ 0.18532889]\n",
      "state:  {1: array([ 0.33229847]), 2: array([-7.33980928]), 3: array([-0.94529504, -0.32621662,  0.        ])}\n",
      "cos(angle):  -0.9452950426732679   sin(angle)  -0.3262166186713127\n",
      "theta dot:  -7.3398092796\n",
      "Angle:  199.0396761557603\n",
      "action:  [ 0.37297353]\n",
      "state:  {1: array([ 6.23905749]), 2: array([-7.52852571]), 3: array([-0.99902653,  0.0441135 ,  0.        ])}\n",
      "cos(angle):  -0.9990265258410297   sin(angle)  0.044113497548962724\n",
      "theta dot:  -7.5285257134\n",
      "Angle:  177.47124728884432\n",
      "action:  [ 0.20267978]\n",
      "state:  {1: array([ 5.86580556]), 2: array([-7.46503862]), 3: array([-0.91415424,  0.40536653,  0.        ])}\n",
      "cos(angle):  -0.9141542394391201   sin(angle)  0.40536653354153934\n",
      "theta dot:  -7.46503862272\n",
      "Angle:  156.0855369487365\n",
      "action:  [ 0.25159698]\n",
      "state:  {1: array([ 5.50964185]), 2: array([-7.12327418]), 3: array([-0.71543944,  0.69867475,  0.        ])}\n",
      "cos(angle):  -0.7154394420852652   sin(angle)  0.6986747488701195\n",
      "theta dot:  -7.12327417533\n",
      "Angle:  135.67890734027765\n",
      "action:  [ 0.18329095]\n",
      "state:  {1: array([ 5.18105313]), 2: array([-6.57177447]), 3: array([-0.45169488,  0.89217248,  0.        ])}\n",
      "cos(angle):  -0.45169487634052824   sin(angle)  0.8921724825882689\n",
      "theta dot:  -6.57177447172\n",
      "Angle:  116.85220430822915\n",
      "action:  [ 0.05731173]\n",
      "state:  {1: array([ 4.88635071]), 2: array([-5.89404835]), 3: array([-0.17308563,  0.98490678,  0.        ])}\n",
      "cos(angle):  -0.1730856301753936   sin(angle)  0.9849067796633277\n",
      "theta dot:  -5.89404834983\n",
      "Angle:  99.96703905851751\n",
      "action:  [ 0.07293653]\n",
      "state:  {1: array([ 4.62912932]), 2: array([-5.14442779]), 3: array([ 0.0831635 ,  0.99653592,  0.        ])}\n",
      "cos(angle):  0.08316349936163164   sin(angle)  0.9965359162488464\n",
      "theta dot:  -5.14442778603\n",
      "Angle:  85.229373514109\n",
      "action:  [ 0.59325407]\n",
      "state:  {1: array([ 4.41372743]), 2: array([-4.30803774]), 3: array([ 0.29424127,  0.95573117,  0.        ])}\n",
      "cos(angle):  0.29424127027233654   sin(angle)  0.95573117290822\n",
      "theta dot:  -4.30803773811\n",
      "Angle:  72.88778335526445\n",
      "action:  [ 0.35780303]\n",
      "state:  {1: array([ 4.23684899]), 2: array([-3.5375689]), 3: array([ 0.45781859,  0.88904563,  0.        ])}\n",
      "cos(angle):  0.45781859249438656   sin(angle)  0.8890456323307926\n",
      "theta dot:  -3.53756890452\n",
      "Angle:  62.75341865552116\n",
      "action:  [ 0.61389198]\n",
      "state:  {1: array([ 4.09791394]), 2: array([-2.77870088]), 3: array([ 0.57652967,  0.81707622,  0.        ])}\n",
      "cos(angle):  0.5765296672869448   sin(angle)  0.8170762159908981\n",
      "theta dot:  -2.778700884\n",
      "Angle:  54.79304561121762\n",
      "action:  [ 0.57230494]\n",
      "state:  {1: array([ 3.99391154]), 2: array([-2.08004798]), 3: array([ 0.65823924,  0.75280881,  0.        ])}\n",
      "cos(angle):  0.6582392361149135   sin(angle)  0.7528088124078085\n",
      "theta dot:  -2.08004798095\n",
      "Angle:  48.83416102101239\n",
      "action:  [ 0.38812826]\n",
      "state:  {1: array([ 3.92105044]), 2: array([-1.45722213]), 3: array([ 0.71129476,  0.70289385,  0.        ])}\n",
      "cos(angle):  0.7112947630236388   sin(angle)  0.7028938469606527\n",
      "theta dot:  -1.45722213219\n",
      "Angle:  44.65953688371263\n",
      "action:  [ 0.20798155]\n",
      "state:  {1: array([ 3.87610771]), 2: array([-0.89885451]), 3: array([ 0.74215586,  0.67022733,  0.        ])}\n",
      "cos(angle):  0.7421558620344626   sin(angle)  0.6702273319164803\n",
      "theta dot:  -0.89885451476\n",
      "Angle:  42.084514400634475\n",
      "action:  [ 0.12727039]\n",
      "state:  {1: array([ 3.85725304]), 2: array([-0.37709346]), 3: array([ 0.75466012,  0.65611593,  0.        ])}\n",
      "cos(angle):  0.7546601165950114   sin(angle)  0.6561159260533186\n",
      "theta dot:  -0.3770934574\n",
      "Angle:  41.004223747272874\n",
      "action:  [ 0.01240394]\n",
      "state:  {1: array([ 3.86309574]), 2: array([ 0.11685408]), 3: array([ 0.75081377,  0.66051396,  0.        ])}\n",
      "cos(angle):  0.7508137663930217   sin(angle)  0.660513957607805\n",
      "theta dot:  0.116854077452\n",
      "Angle:  41.33898523729914\n",
      "action:  [ 0.39882015]\n",
      "state:  {1: array([ 3.89669887]), 2: array([ 0.67206257]), 3: array([ 0.72819875,  0.68536602,  0.        ])}\n",
      "cos(angle):  0.7281987494759788   sin(angle)  0.685366019920466\n",
      "theta dot:  0.672062568358\n",
      "Angle:  43.26429817186232\n",
      "action:  [-0.11616829]\n",
      "state:  {1: array([ 3.95513196]), 2: array([ 1.16866184]), 3: array([ 0.68693064,  0.72672298,  0.        ])}\n",
      "cos(angle):  0.6869306433034718   sin(angle)  0.7267229811218842\n",
      "theta dot:  1.1686618402\n",
      "Angle:  46.6122598989499\n",
      "action:  [-0.08838326]\n",
      "state:  {1: array([ 4.04015429]), 2: array([ 1.70044659]), 3: array([ 0.62273603,  0.782432  ,  0.        ])}\n",
      "cos(angle):  0.6227360318919392   sin(angle)  0.7824319999741075\n",
      "theta dot:  1.7004465874\n",
      "Angle:  51.48366914474576\n",
      "action:  [-0.15703006]\n",
      "state:  {1: array([ 4.1533401]), 2: array([ 2.26371608]), 3: array([ 0.53038012,  0.84775995,  0.        ])}\n",
      "cos(angle):  0.5303801199460516   sin(angle)  0.8477599473707236\n",
      "theta dot:  2.26371607843\n",
      "Angle:  57.968722845352886\n",
      "action:  [-0.00793482]\n",
      "state:  {1: array([ 4.29825739]), 2: array([ 2.89834582]), 3: array([ 0.40239509,  0.91546611,  0.        ])}\n",
      "cos(angle):  0.40239508525809176   sin(angle)  0.9154661082531308\n",
      "theta dot:  2.89834581654\n",
      "Angle:  66.27185257187836\n",
      "action:  [-0.0106788]\n",
      "state:  {1: array([ 4.47742457]), 2: array([ 3.58334358]), 3: array([ 0.23280838,  0.97252263,  0.        ])}\n",
      "cos(angle):  0.23280837782932295   sin(angle)  0.9725226265812427\n",
      "theta dot:  3.58334357741\n",
      "Angle:  76.53735174322304\n",
      "action:  [-0.01697297]\n",
      "state:  {1: array([ 4.69293405]), 2: array([ 4.3101896]), 3: array([ 0.01945371,  0.99981076,  0.        ])}\n",
      "cos(angle):  0.019453707021298536   sin(angle)  0.9998107587354367\n",
      "theta dot:  4.31018960215\n",
      "Angle:  88.88510652402562\n",
      "action:  [-0.40457827]\n",
      "state:  {1: array([ 4.94290209]), 2: array([ 4.99936093]), 3: array([-0.22847709,  0.97354929,  0.        ])}\n",
      "cos(angle):  -0.2284770936924907   sin(angle)  0.9735492887665385\n",
      "theta dot:  4.99936093098\n",
      "Angle:  103.20718711316914\n",
      "action:  [-0.3064355]\n",
      "state:  {1: array([ 5.22707997]), 2: array([ 5.68355757]), 3: array([-0.4922659 ,  0.87044488,  0.        ])}\n",
      "cos(angle):  -0.49226589717929653   sin(angle)  0.870444878481264\n",
      "theta dot:  5.68355757284\n",
      "Angle:  119.48934211556298\n",
      "action:  [-0.44357208]\n",
      "state:  {1: array([ 5.54057274]), 2: array([ 6.26985542]), 3: array([-0.73670442,  0.67621491,  0.        ])}\n",
      "cos(angle):  -0.736704419596917   sin(angle)  0.6762149052973985\n",
      "theta dot:  6.2698554193\n",
      "Angle:  137.45111279728314\n",
      "action:  [-0.20466765]\n",
      "state:  {1: array([ 5.87788856]), 2: array([ 6.74631645]), 3: array([-0.91898543,  0.39429148,  0.        ])}\n",
      "cos(angle):  -0.918985434638246   sin(angle)  0.39429147964767663\n",
      "theta dot:  6.74631645051\n",
      "Angle:  156.77784059668738\n",
      "action:  [-0.13448233]\n",
      "state:  {1: array([ 6.2289817]), 2: array([ 7.02186271]), 3: array([-0.99853134,  0.05417707,  0.        ])}\n",
      "cos(angle):  -0.9985313441353217   sin(angle)  0.05417706875891026\n",
      "theta dot:  7.02186271072\n",
      "Angle:  176.89394843870423\n",
      "action:  [-0.16866733]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "state:  {1: array([ 0.29765616]), 2: array([ 7.03719541]), 3: array([-0.95602652, -0.29328024,  0.        ])}\n",
      "cos(angle):  -0.9560265153573606   sin(angle)  -0.2932802447040413\n",
      "theta dot:  7.03719541214\n",
      "Angle:  197.05482296662717\n",
      "action:  [-0.2765174]\n",
      "state:  {1: array([ 0.63644404]), 2: array([ 6.77575762]), 3: array([-0.80421428, -0.59433945,  0.        ])}\n",
      "cos(angle):  -0.8042142825546469   sin(angle)  -0.5943394549709068\n",
      "theta dot:  6.77575761788\n",
      "Angle:  216.46589330050085\n",
      "action:  [-0.21852316]\n",
      "state:  {1: array([ 0.95130527]), 2: array([ 6.29722455]), 3: array([-0.58062087, -0.81417407,  0.        ])}\n",
      "cos(angle):  -0.5806208655292208   sin(angle)  -0.8141740664698788\n",
      "theta dot:  6.29722455259\n",
      "Angle:  234.50607059018665\n",
      "action:  [-0.29229807]\n",
      "state:  {1: array([ 1.23344274]), 2: array([ 5.64274929]), 3: array([-0.33099101, -0.94363391,  0.        ])}\n",
      "cos(angle):  -0.3309910117871855   sin(angle)  -0.9436339068283288\n",
      "theta dot:  5.64274929233\n",
      "Angle:  250.671318753865\n",
      "action:  [-0.42147095]\n",
      "state:  {1: array([ 1.4770329]), 2: array([ 4.87180322]), 3: array([-0.0936261 , -0.99560743,  0.        ])}\n",
      "cos(angle):  -0.09362610113972808   sin(angle)  -0.9956074292538065\n",
      "theta dot:  4.87180322009\n",
      "Angle:  264.6279742736064\n",
      "action:  [-0.58282618]\n",
      "state:  {1: array([ 1.67891658]), 2: array([ 4.03767372]), 3: array([ 0.10790973, -0.9941607 ,  0.        ])}\n",
      "cos(angle):  0.10790972612432677   sin(angle)  -0.9941606967727967\n",
      "theta dot:  4.03767372071\n",
      "Angle:  276.1950303871699\n",
      "action:  [-0.73913623]\n",
      "state:  {1: array([ 1.83797572]), 2: array([ 3.18118276]), 3: array([ 0.26401196, -0.96451941,  0.        ])}\n",
      "cos(angle):  0.2640119625809878   sin(angle)  -0.964519405514547\n",
      "theta dot:  3.18118276377\n",
      "Angle:  285.3084263872756\n",
      "action:  [-0.84060379]\n",
      "state:  {1: array([ 1.95456085]), 2: array([ 2.33170264]), 3: array([ 0.37441382, -0.92726172,  0.        ])}\n",
      "cos(angle):  0.3744138163508166   sin(angle)  -0.9272617182465892\n",
      "theta dot:  2.33170264133\n",
      "Angle:  291.98824678832483\n",
      "action:  [-0.8624566]\n",
      "state:  {1: array([ 2.02990525]), 2: array([ 1.50688786]), 3: array([ 0.44314948, -0.89644773,  0.        ])}\n",
      "cos(angle):  0.4431494768279572   sin(angle)  -0.8964477347771636\n",
      "theta dot:  1.50688786232\n",
      "Angle:  296.30515242903846\n",
      "action:  [-0.80429659]\n",
      "state:  {1: array([ 2.06560063]), 2: array([ 0.71390757]), 3: array([ 0.47485943, -0.88006166,  0.        ])}\n",
      "cos(angle):  0.4748594315762355   sin(angle)  -0.8800616570690343\n",
      "theta dot:  0.713907573113\n",
      "Angle:  298.3503421915849\n",
      "action:  [-0.71807932]\n",
      "state:  {1: array([ 2.0629081]), 2: array([-0.05385057]), 3: array([ 0.47248812, -0.88133704,  0.        ])}\n",
      "cos(angle):  0.4724881221649884   sin(angle)  -0.8813370379219309\n",
      "theta dot:  -0.0538505674015\n",
      "Angle:  298.19607204051107\n",
      "action:  [-0.7091321]\n",
      "state:  {1: array([ 2.02184694]), 2: array([-0.82122316]), 3: array([ 0.43591132, -0.89998963,  0.        ])}\n",
      "cos(angle):  0.43591131507578906   sin(angle)  -0.8999896251562549\n",
      "theta dot:  -0.821223161465\n",
      "Angle:  295.84344648245576\n",
      "action:  [-0.62165657]\n",
      "state:  {1: array([ 1.94237375]), 2: array([-1.58946387]), 3: array([ 0.36308565, -0.93175577,  0.        ])}\n",
      "cos(angle):  0.363085652737135   sin(angle)  -0.9317557666987892\n",
      "theta dot:  -1.58946386596\n",
      "Angle:  291.289978570055\n",
      "action:  [-0.34090686]\n",
      "state:  {1: array([ 1.82540291]), 2: array([-2.33941672]), 3: array([ 0.25186469, -0.96776246,  0.        ])}\n",
      "cos(angle):  0.25186469432962627   sin(angle)  -0.9677624583286148\n",
      "theta dot:  -2.33941672054\n",
      "Angle:  284.5880590115801\n",
      "action:  [-0.1465345]\n",
      "state:  {1: array([ 1.67104197]), 2: array([-3.08721874]), 3: array([ 0.10007783, -0.99497961,  0.        ])}\n",
      "cos(angle):  0.10007783286283647   sin(angle)  -0.9949796115345672\n",
      "theta dot:  -3.08721873942\n",
      "Angle:  275.7438494830786\n",
      "action:  [ 0.13753764]\n",
      "state:  {1: array([ 1.48040083]), 2: array([-3.8128228]), 3: array([-0.09027244, -0.99591711,  0.        ])}\n",
      "cos(angle):  -0.09027243513285133   sin(angle)  -0.9959171087270191\n",
      "theta dot:  -3.81282280164\n",
      "Angle:  264.8209422973135\n",
      "action:  [ 0.08378225]\n",
      "state:  {1: array([ 1.25304117]), 2: array([-4.5471933]), 3: array([-0.31243489, -0.94993918,  0.        ])}\n",
      "cos(angle):  -0.31243488666350683   sin(angle)  -0.9499391778401192\n",
      "theta dot:  -4.5471932954\n",
      "Angle:  251.79422353662767\n",
      "action:  [ 0.55854933]\n",
      "state:  {1: array([ 0.9942479]), 2: array([-5.17586528]), 3: array([-0.54513356, -0.83834921,  0.        ])}\n",
      "cos(angle):  -0.5451335623548899   sin(angle)  -0.8383492107673671\n",
      "theta dot:  -5.17586527936\n",
      "Angle:  236.96649641851292\n",
      "action:  [ 0.3824883]\n",
      "state:  {1: array([ 0.70688521]), 2: array([-5.74725394]), 3: array([-0.76038852, -0.64946847,  0.        ])}\n",
      "cos(angle):  -0.7603885209855613   sin(angle)  -0.6494684727940152\n",
      "theta dot:  -5.74725394203\n",
      "Angle:  220.50186518656662\n",
      "action:  [ 0.07640298]\n",
      "state:  {1: array([ 0.39574046]), 2: array([-6.22289485]), 3: array([-0.92271137, -0.38549153,  0.        ])}\n",
      "cos(angle):  -0.9227113744105527   sin(angle)  -0.38549152978163953\n",
      "theta dot:  -6.2228948489\n",
      "Angle:  202.67462631461322\n",
      "action:  [-0.07809302]\n",
      "state:  {1: array([ 0.06955409]), 2: array([-6.52372745]), 3: array([-0.99758209, -0.06949802,  0.        ])}\n",
      "cos(angle):  -0.9975820891267606   sin(angle)  -0.06949802481716857\n",
      "theta dot:  -6.52372744953\n",
      "Angle:  183.98556754016272\n",
      "action:  [-0.11820566]\n",
      "state:  {1: array([ 6.02306031]), 2: array([-6.59358182]), 3: array([-0.96635784,  0.25720135,  0.        ])}\n",
      "cos(angle):  -0.966357835912834   sin(angle)  0.2572013471385099\n",
      "theta dot:  -6.59358181693\n",
      "Angle:  165.0955493774195\n",
      "action:  [-0.2720421]\n",
      "state:  {1: array([ 5.70098595]), 2: array([-6.44148712]), 3: array([-0.83525533,  0.54986229,  0.        ])}\n",
      "cos(angle):  -0.8352553289060916   sin(angle)  0.5498622877902947\n",
      "theta dot:  -6.44148712154\n",
      "Angle:  146.64209123701482\n",
      "action:  [-0.38911937]\n",
      "state:  {1: array([ 5.39661304]), 2: array([-6.08745831]), 3: array([-0.63207192,  0.77490973,  0.        ])}\n",
      "cos(angle):  -0.6320719158458886   sin(angle)  0.774909732290741\n",
      "theta dot:  -6.08745831117\n",
      "Angle:  129.20284855795887\n",
      "action:  [-0.44603661]\n",
      "state:  {1: array([ 5.11795396]), 2: array([-5.5731815]), 3: array([-0.39453797,  0.91887964,  0.        ])}\n",
      "cos(angle):  -0.3945379733297606   sin(angle)  0.9188796371673741\n",
      "theta dot:  -5.57318150359\n",
      "Angle:  113.23689696248789\n",
      "action:  [-0.4328187]\n",
      "state:  {1: array([ 4.87050673]), 2: array([-4.94894458]), 3: array([-0.15745972,  0.98752541,  0.        ])}\n",
      "cos(angle):  -0.15745971938815287   sin(angle)  0.9875254106959497\n",
      "theta dot:  -4.94894458081\n",
      "Angle:  99.05924823976625\n",
      "action:  [-0.37975371]\n",
      "state:  {1: array([ 4.65724355]), 2: array([-4.26526358]), 3: array([ 0.05511748,  0.99847988,  0.        ])}\n",
      "cos(angle):  0.05511748103868284   sin(angle)  0.9984798762538735\n",
      "theta dot:  -4.26526357957\n",
      "Angle:  86.84019673220035\n",
      "action:  [-0.34564076]\n",
      "state:  {1: array([ 4.47883106]), 2: array([-3.56824979]), 3: array([ 0.2314403 ,  0.97284911,  0.        ])}\n",
      "cos(angle):  0.23144029635200203   sin(angle)  0.9728491091759798\n",
      "theta dot:  -3.56824978671\n",
      "Angle:  76.61793798493636\n",
      "action:  [-0.19903845]\n",
      "state:  {1: array([ 4.33540763]), 2: array([-2.86846872]), 3: array([ 0.36811547,  0.92978008,  0.        ])}\n",
      "cos(angle):  0.36811547201299444   sin(angle)  0.9297800811292154\n",
      "theta dot:  -2.86846872216\n",
      "Angle:  68.40039962885413\n",
      "action:  [-0.31035278]\n",
      "state:  {1: array([ 4.2245233]), 2: array([-2.21768658]), 3: array([ 0.46874164,  0.88333531,  0.        ])}\n",
      "cos(angle):  0.46874163776178124   sin(angle)  0.8833353140390138\n",
      "theta dot:  -2.21768657827\n",
      "Angle:  62.04721042448034\n",
      "action:  [-0.27937784]\n",
      "state:  {1: array([ 4.14466871]), 2: array([-1.59709177]), 3: array([ 0.53771134,  0.843129  ,  0.        ])}\n",
      "cos(angle):  0.5377113406126983   sin(angle)  0.8431290020966511\n",
      "theta dot:  -1.59709176883\n",
      "Angle:  57.47189023111474\n",
      "action:  [-0.31850851]\n",
      "state:  {1: array([ 4.09404265]), 2: array([-1.01252129]), 3: array([ 0.57968848,  0.81483818,  0.        ])}\n",
      "cos(angle):  0.579688483552457   sin(angle)  0.8148381815039432\n",
      "theta dot:  -1.01252129377\n",
      "Angle:  54.57123717409871\n",
      "action:  [-0.18119347]\n",
      "state:  {1: array([ 4.07261406]), 2: array([-0.42857168]), 3: array([ 0.59701489,  0.80223016,  0.        ])}\n",
      "cos(angle):  0.5970148888063708   sin(angle)  0.8022301555934661\n",
      "theta dot:  -0.428571677533\n",
      "Angle:  53.34347262807324\n",
      "action:  [ 0.1253352]\n",
      "state:  {1: array([ 4.08220912]), 2: array([ 0.19190122]), 3: array([ 0.58929008,  0.80792153,  0.        ])}\n",
      "cos(angle):  0.5892900777524964   sin(angle)  0.8079215334811027\n",
      "theta dot:  0.191901218765\n",
      "Angle:  53.89322783844011\n",
      "action:  [ 0.49565376]\n",
      "state:  {1: array([ 4.12581865]), 2: array([ 0.87219043]), 3: array([ 0.55350791,  0.83284392,  0.        ])}\n",
      "cos(angle):  0.5535079084125795   sin(angle)  0.832843920146345\n",
      "theta dot:  0.872190433612\n",
      "Angle:  56.391863534425624\n",
      "action:  [ 0.29767504]\n",
      "state:  {1: array([ 4.20289238]), 2: array([ 1.54147463]), 3: array([ 0.48773785,  0.87299014,  0.        ])}\n",
      "cos(angle):  0.48773784874211457   sin(angle)  0.8729901436467735\n",
      "theta dot:  1.54147462965\n",
      "Angle:  60.80785273320771\n",
      "action:  [ 0.3799184]\n",
      "state:  {1: array([ 4.31555263]), 2: array([ 2.253205]), 3: array([ 0.38650249,  0.92228836,  0.        ])}\n",
      "cos(angle):  0.38650248735036424   sin(angle)  0.9222883644891013\n",
      "theta dot:  2.25320499763\n",
      "Angle:  67.26279447585506\n",
      "action:  [ 0.46308639]\n",
      "state:  {1: array([ 4.46627184]), 2: array([ 3.01438423]), 3: array([ 0.24363995,  0.96986575,  0.        ])}\n",
      "cos(angle):  0.243639954715392   sin(angle)  0.9698657497129599\n",
      "theta dot:  3.01438423017\n",
      "Angle:  75.8983489931536\n",
      "action:  [ 0.52921667]\n",
      "state:  {1: array([ 4.65733014]), 2: array([ 3.82116604]), 3: array([ 0.05503103,  0.99848464,  0.        ])}\n",
      "cos(angle):  0.05503102619537174   sin(angle)  0.9984846449274442\n",
      "theta dot:  3.82116604262\n",
      "Angle:  86.84515774778288\n",
      "action:  [ 0.94624927]\n",
      "state:  {1: array([ 4.89292849]), 2: array([ 4.71196692]), 3: array([-0.17956034,  0.98374696,  0.        ])}\n",
      "cos(angle):  -0.17956033649669506   sin(angle)  0.9837469621590674\n",
      "theta dot:  4.71196691612\n",
      "Angle:  100.34391705675898\n",
      "action:  [ 0.47366938]\n",
      "state:  {1: array([ 5.16896986]), 2: array([ 5.52082755]), 3: array([-0.44088181,  0.89756517,  0.        ])}\n",
      "cos(angle):  -0.44088180950524786   sin(angle)  0.8975651675769165\n",
      "theta dot:  5.52082754532\n",
      "Angle:  116.15988596046043\n",
      "action:  [ 0.44129729]\n",
      "state:  {1: array([ 5.48197966]), 2: array([ 6.26019601]), 3: array([-0.69584133,  0.71819555,  0.        ])}\n",
      "cos(angle):  -0.6958413276564754   sin(angle)  0.7181955490848391\n",
      "theta dot:  6.26019601382\n",
      "Angle:  134.09398454856853\n",
      "action:  [ 0.39823838]\n",
      "state:  {1: array([ 5.82490859]), 2: array([ 6.85857843]), 3: array([-0.89681621,  0.4424033 ,  0.        ])}\n",
      "cos(angle):  -0.8968162128231261   sin(angle)  0.4424033006404738\n",
      "theta dot:  6.85857843296\n",
      "Angle:  153.742318485625\n",
      "action:  [ 0.29309481]\n",
      "state:  {1: array([ 6.18662584]), 2: array([ 7.23434513]), 3: array([-0.99534176,  0.09640949,  0.        ])}\n",
      "cos(angle):  -0.995341755885805   sin(angle)  0.09640948599677625\n",
      "theta dot:  7.23434512981\n",
      "Angle:  174.46714219593844\n",
      "action:  [ 0.45990176]\n",
      "state:  {1: array([ 0.27222241]), 2: array([ 7.37563751]), 3: array([-0.96317573, -0.26887267,  0.        ])}\n",
      "cos(angle):  -0.9631757300735829   sin(angle)  -0.26887267060305786\n",
      "theta dot:  7.37563750897\n",
      "Angle:  195.59757964450264\n",
      "action:  [ 0.32164926]\n",
      "state:  {1: array([ 0.63333393]), 2: array([ 7.2222304]), 3: array([-0.80605885, -0.59183539,  0.        ])}\n",
      "cos(angle):  -0.8060588538789658   sin(angle)  -0.5918353859675239\n",
      "theta dot:  7.22223039512\n",
      "Angle:  216.28769727759232\n",
      "action:  [ 0.22761526]\n",
      "state:  {1: array([ 0.97395874]), 2: array([ 6.81249615]), 3: array([-0.5620296 , -0.82711712,  0.        ])}\n",
      "cos(angle):  -0.5620296042466825   sin(angle)  -0.8271171162237652\n",
      "theta dot:  6.8124961453\n",
      "Angle:  235.80401549368628\n",
      "action:  [ 0.114026]\n",
      "state:  {1: array([ 1.28442185]), 2: array([ 6.20926221]), 3: array([-0.28247622, -0.9592743 ,  0.        ])}\n",
      "cos(angle):  -0.28247622016455287   sin(angle)  -0.9592743012514966\n",
      "theta dot:  6.20926220752\n",
      "Angle:  253.59219981621897\n",
      "action:  [-0.0082238]\n",
      "state:  {1: array([ 1.55885049]), 2: array([ 5.48857291]), 3: array([-0.01194555, -0.99992865,  0.        ])}\n",
      "cos(angle):  -0.011945549405992137   sin(angle)  -0.999928649379239\n",
      "theta dot:  5.48857291116\n",
      "Angle:  269.3157662156349\n",
      "action:  [ 0.0180555]\n",
      "state:  {1: array([ 1.79591723]), 2: array([ 4.74133475]), 3: array([ 0.22322422, -0.97476713,  0.        ])}\n",
      "cos(angle):  0.2232242169769641   sin(angle)  -0.9747671254997377\n",
      "theta dot:  4.7413347498\n",
      "Angle:  282.89865797405025\n",
      "action:  [ 0.03496188]\n",
      "state:  {1: array([ 1.99669242]), 2: array([ 4.01550369]), 3: array([ 0.41313699, -0.91066889,  0.        ])}\n",
      "cos(angle):  0.4131369873009189   sin(angle)  -0.9106688913781563\n",
      "theta dot:  4.01550368792\n",
      "Angle:  294.4022017706059\n",
      "action:  [-0.00491922]\n",
      "state:  {1: array([ 2.16328062]), 2: array([ 3.33176414]), 3: array([ 0.55842361, -0.82955595,  0.        ])}\n",
      "cos(angle):  0.5584236058429317   sin(angle)  -0.8295559513603515\n",
      "theta dot:  3.33176413587\n",
      "Angle:  303.94698061667043\n",
      "action:  [-0.00774793]\n",
      "state:  {1: array([ 2.29870237]), 2: array([ 2.70843498]), 3: array([ 0.66530781, -0.74656916,  0.        ])}\n",
      "cos(angle):  0.6653078119564972   sin(angle)  -0.7465691631387263\n",
      "theta dot:  2.70843498269\n",
      "Angle:  311.7060571522695\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -1.64828772711  self.max:  27.8419661301\n",
      "state:  {1: array([ 3.2193686]), 2: array([ 0.79620604]), 3: array([ 0.99697698,  0.07769756,  0.        ])}\n",
      "cos(angle):  0.9969769753799796   sin(angle)  0.07769755827686887\n",
      "theta dot:  0.796206043416\n",
      "Angle:  4.45622308731555\n",
      "action:  [ 0.61650952]\n",
      "state:  {1: array([ 3.26671638]), 2: array([ 0.94695564]), 3: array([ 0.99218223,  0.1247975 ,  0.        ])}\n",
      "cos(angle):  0.9921822337554951   sin(angle)  0.12479749604842244\n",
      "theta dot:  0.94695564084\n",
      "Angle:  7.169044823869115\n",
      "action:  [ 0.60734212]\n",
      "state:  {1: array([ 3.32329914]), 2: array([ 1.13165508]), 3: array([ 0.98353675,  0.18070822,  0.        ])}\n",
      "cos(angle):  0.9835367494902102   sin(angle)  0.18070822450080012\n",
      "theta dot:  1.13165508072\n",
      "Angle:  10.410990242270941\n",
      "action:  [ 0.62788473]\n",
      "state:  {1: array([ 3.39136758]), 2: array([ 1.36136896]), 3: array([ 0.96896808,  0.24718588,  0.        ])}\n",
      "cos(angle):  0.9689680801351397   sin(angle)  0.2471858808249806\n",
      "theta dot:  1.36136895912\n",
      "Angle:  14.31101590818696\n",
      "action:  [ 0.60341858]\n",
      "state:  {1: array([ 3.47323114]), 2: array([ 1.63727116]), 3: array([ 0.94551013,  0.32559267,  0.        ])}\n",
      "cos(angle):  0.9455101329987283   sin(angle)  0.3255926725169458\n",
      "theta dot:  1.63727115635\n",
      "Angle:  19.001441298803627\n",
      "action:  [ 0.32660485]\n",
      "state:  {1: array([ 3.56975396]), 2: array([ 1.93045639]), 3: array([ 0.90973071,  0.41519879,  0.        ])}\n",
      "cos(angle):  0.9097307096495814   sin(angle)  0.41519879084658856\n",
      "theta dot:  1.93045638844\n",
      "Angle:  24.531778546055477\n",
      "action:  [-0.12792774]\n",
      "state:  {1: array([ 3.68088728]), 2: array([ 2.22266632]), 3: array([ 0.85807113,  0.51353086,  0.        ])}\n",
      "cos(angle):  0.858071127011172   sin(angle)  0.5135308569012937\n",
      "theta dot:  2.22266632071\n",
      "Angle:  30.899233628293928\n",
      "action:  [-0.39818666]\n",
      "state:  {1: array([ 3.8082916]), 2: array([ 2.54808646]), 3: array([ 0.7858673 ,  0.61839517,  0.        ])}\n",
      "cos(angle):  0.7858672988936579   sin(angle)  0.618395171819433\n",
      "theta dot:  2.54808646421\n",
      "Angle:  38.19894657007558\n",
      "action:  [-1.14457346]\n",
      "state:  {1: array([ 3.95030144]), 2: array([ 2.84019682]), 3: array([ 0.69043307,  0.72339628,  0.        ])}\n",
      "cos(angle):  0.6904330657072328   sin(angle)  0.7233962826681597\n",
      "theta dot:  2.84019682443\n",
      "Angle:  46.335492094607616\n",
      "action:  [-0.42223326]\n",
      "state:  {1: array([ 4.11627189]), 2: array([ 3.31940905]), 3: array([ 0.56143352,  0.82752185,  0.        ])}\n",
      "cos(angle):  0.5614335174812698   sin(angle)  0.8275218459041481\n",
      "theta dot:  3.31940904678\n",
      "Angle:  55.844876300446344\n",
      "action:  [-0.33480748]\n",
      "state:  {1: array([ 4.31076336]), 2: array([ 3.88982931]), 3: array([ 0.39091512,  0.92042673,  0.        ])}\n",
      "cos(angle):  0.3909151225828389   sin(angle)  0.9204267308895608\n",
      "theta dot:  3.88982930862\n",
      "Angle:  66.98839036258107\n",
      "action:  [ 0.0780153]\n",
      "state:  {1: array([ 4.54035594]), 2: array([ 4.59185165]), 3: array([ 0.17118573,  0.98523878,  0.        ])}\n",
      "cos(angle):  0.17118572910042215   sin(angle)  0.9852387762123235\n",
      "theta dot:  4.59185165141\n",
      "Angle:  80.1430455900803\n",
      "action:  [ 22.84208708]\n",
      "state:  {1: array([ 4.82189498]), 2: array([ 5.63078073]), 3: array([-0.10928727,  0.99401021,  0.        ])}\n",
      "cos(angle):  -0.10928727207220594   sin(angle)  0.9940102072730519\n",
      "theta dot:  5.63078073357\n",
      "Angle:  96.27400643873847\n",
      "action:  [-0.50909285]\n",
      "state:  {1: array([ 5.1368912]), 2: array([ 6.29992446]), 3: array([-0.41186724,  0.91124386,  0.        ])}\n",
      "cos(angle):  -0.41186723563042915   sin(angle)  0.9112438642943768\n",
      "theta dot:  6.29992446146\n",
      "Angle:  114.32191837950398\n",
      "action:  [-0.31404757]\n",
      "state:  {1: array([ 5.48370371]), 2: array([ 6.93625022]), 3: array([-0.6970785 ,  0.71699482,  0.        ])}\n",
      "cos(angle):  -0.6970784977024918   sin(angle)  0.7169948173040285\n",
      "theta dot:  6.93625022417\n",
      "Angle:  134.192765087402\n",
      "action:  [-0.38467621]\n",
      "state:  {1: array([ 5.85451846]), 2: array([ 7.41629491]), 3: array([-0.90952069,  0.41565864,  0.        ])}\n",
      "cos(angle):  -0.909520693768547   sin(angle)  0.4156586431277243\n",
      "theta dot:  7.41629490541\n",
      "Angle:  155.43883529006052\n",
      "action:  [-0.89490591]\n",
      "state:  {1: array([ 6.23420861]), 2: array([ 7.593803]), 3: array([-0.99880088,  0.04895712,  0.        ])}\n",
      "cos(angle):  -0.9988008812369069   sin(angle)  0.04895712042571668\n",
      "theta dot:  7.59380300087\n",
      "Angle:  177.19342753854403\n",
      "action:  [-0.5687336]\n",
      "state:  {1: array([ 0.32828384]), 2: array([ 7.5452108]), 3: array([-0.94659706, -0.32241899,  0.        ])}\n",
      "cos(angle):  -0.9465970592201971   sin(angle)  -0.32241899366457105\n",
      "theta dot:  7.54521080133\n",
      "Angle:  198.80965554963186\n",
      "action:  [-0.44450467]\n",
      "state:  {1: array([ 0.69011988]), 2: array([ 7.23672086]), 3: array([-0.7711697 , -0.63662964,  0.        ])}\n",
      "cos(angle):  -0.771169698481463   sin(angle)  -0.6366296381288021\n",
      "theta dot:  7.23672085534\n",
      "Angle:  219.54128519633466\n",
      "action:  [-0.58934129]\n",
      "state:  {1: array([ 1.02366226]), 2: array([ 6.67084743]), 3: array([-0.52024181, -0.854019  ,  0.        ])}\n",
      "cos(angle):  -0.5202418106408843   sin(angle)  -0.8540190035714044\n",
      "theta dot:  6.67084743254\n",
      "Angle:  238.65181069063482\n",
      "action:  [-0.74616898]\n",
      "state:  {1: array([ 1.31958265]), 2: array([ 5.91840783]), 3: array([-0.24857973, -0.96861144,  0.        ])}\n",
      "cos(angle):  -0.24857972539393514   sin(angle)  -0.9686114391865687\n",
      "theta dot:  5.91840783269\n",
      "Angle:  255.60676055509066\n",
      "action:  [-0.76942735]\n",
      "state:  {1: array([ 1.57340941]), 2: array([ 5.07653515]), 3: array([ 0.00261308, -0.99999659,  0.        ])}\n",
      "cos(angle):  0.0026130755908475873   sin(angle)  -0.9999965859121502\n",
      "theta dot:  5.07653515096\n",
      "Angle:  270.1499284818399\n",
      "action:  [-0.58287606]\n",
      "state:  {1: array([ 1.78536472]), 2: array([ 4.2391063]), 3: array([ 0.21292574, -0.97706839,  0.        ])}\n",
      "cos(angle):  0.21292573924451264   sin(angle)  -0.9770683853073836\n",
      "theta dot:  4.23910630207\n",
      "Angle:  282.2940450844142\n",
      "action:  [-0.27059743]\n",
      "state:  {1: array([ 1.95865049]), 2: array([ 3.4657154]), 3: array([ 0.37820284, -0.92572275,  0.        ])}\n",
      "cos(angle):  0.378202837781746   sin(angle)  -0.9257227519586165\n",
      "theta dot:  3.46571539805\n",
      "Angle:  292.2225651323031\n",
      "action:  [-0.08675829]\n",
      "state:  {1: array([ 2.09657097]), 2: array([ 2.75840959]), 3: array([ 0.50188317, -0.86493542,  0.        ])}\n",
      "cos(angle):  0.5018831714727269   sin(angle)  -0.8649354208219695\n",
      "theta dot:  2.75840959122\n",
      "Angle:  300.1248080406763\n",
      "action:  [ 0.10561821]\n",
      "state:  {1: array([ 2.20284851]), 2: array([ 2.12555076]), 3: array([ 0.59080173, -0.80681678,  0.        ])}\n",
      "cos(angle):  0.5908017348987136   sin(angle)  -0.8068167760034928\n",
      "theta dot:  2.12555075727\n",
      "Angle:  306.2140481779933\n",
      "action:  [ 0.24161929]\n",
      "state:  {1: array([ 2.28068256]), 2: array([ 1.55668107]), 3: array([ 0.65174749, -0.75843603,  0.        ])}\n",
      "cos(angle):  0.6517474913612318   sin(angle)  -0.7584360272985066\n",
      "theta dot:  1.55668106946\n",
      "Angle:  310.67360051602884\n",
      "action:  [ 0.16317788]\n",
      "state:  {1: array([ 2.3312991]), 2: array([ 1.01233073]), 3: array([ 0.68928578, -0.72448955,  0.        ])}\n",
      "cos(angle):  0.6892857845555288   sin(angle)  -0.724489549413702\n",
      "theta dot:  1.01233073083\n",
      "Angle:  313.5737076517007\n",
      "action:  [ 0.14741164]\n",
      "state:  {1: array([ 2.35585286]), 2: array([ 0.49107532]), 3: array([ 0.70686517, -0.70734831,  0.        ])}\n",
      "cos(angle):  0.7068651734277498   sin(angle)  -0.7073483064197985\n",
      "theta dot:  0.491075315333\n",
      "Angle:  314.98053151151726\n",
      "action:  [ 0.26796419]\n",
      "state:  {1: array([ 2.3558908]), 2: array([ 0.00075871]), 3: array([ 0.70689201, -0.70732149,  0.        ])}\n",
      "cos(angle):  0.7068920066929409   sin(angle)  -0.7073214904649987\n",
      "theta dot:  0.000758714584761\n",
      "Angle:  314.98270506361257\n",
      "action:  [ 0.17879495]\n",
      "state:  {1: array([ 2.33074514]), 2: array([-0.50291316]), 3: array([ 0.68888434, -0.72487127,  0.        ])}\n",
      "cos(angle):  0.6888843430685643   sin(angle)  -0.7248712726236244\n",
      "theta dot:  -0.50291316137\n",
      "Angle:  313.541968352279\n",
      "action:  [ 0.04317529]\n",
      "state:  {1: array([ 2.27874063]), 2: array([-1.04009032]), 3: array([ 0.65027343, -0.75970025,  0.        ])}\n",
      "cos(angle):  0.6502734290572467   sin(angle)  -0.7597002484283719\n",
      "theta dot:  -1.04009032194\n",
      "Angle:  310.56233603197893\n",
      "action:  [ 0.01038355]\n",
      "state:  {1: array([ 2.19832523]), 2: array([-1.60830798]), 3: array([ 0.58714624, -0.80948088,  0.        ])}\n",
      "cos(angle):  0.5871462440898794   sin(angle)  -0.8094808756549766\n",
      "theta dot:  -1.60830797549\n",
      "Angle:  305.9548838485683\n",
      "action:  [-0.0819641]\n",
      "state:  {1: array([ 2.08693956]), 2: array([-2.22771325]), 3: array([ 0.49352948, -0.86972907,  0.        ])}\n",
      "cos(angle):  0.493529477985957   sin(angle)  -0.8697290695147017\n",
      "theta dot:  -2.22771324682\n",
      "Angle:  299.5729704218376\n",
      "action:  [-0.21263326]\n",
      "state:  {1: array([ 1.94134431]), 2: array([-2.91190504]), 3: array([ 0.36212628, -0.93212905,  0.        ])}\n",
      "cos(angle):  0.3621262793354607   sin(angle)  -0.9321290456876966\n",
      "theta dot:  -2.91190503826\n",
      "Angle:  291.2309964772405\n",
      "action:  [ 0.02612951]\n",
      "state:  {1: array([ 1.76099019]), 2: array([-3.6070824]), 3: array([ 0.18904927, -0.9819676 ,  0.        ])}\n",
      "cos(angle):  0.1890492681561878   sin(angle)  -0.9819676034419923\n",
      "theta dot:  -3.60708239647\n",
      "Angle:  280.8974907577963\n",
      "action:  [-0.36236391]\n",
      "state:  {1: array([ 1.54109456]), 2: array([-4.39791269]), 3: array([-0.0296974 , -0.99955893,  0.        ])}\n",
      "cos(angle):  -0.029697401837813255   sin(angle)  -0.9995589348928273\n",
      "theta dot:  -4.39791268517\n",
      "Angle:  268.2984284435217\n",
      "action:  [-0.12926681]\n",
      "state:  {1: array([ 1.28274596]), 2: array([-5.16697191]), 3: array([-0.28408346, -0.95879956,  0.        ])}\n",
      "cos(angle):  -0.2840834562135466   sin(angle)  -0.9587995566883445\n",
      "theta dot:  -5.16697190761\n",
      "Angle:  253.49617889917926\n",
      "action:  [ 0.31055074]\n",
      "state:  {1: array([ 0.99077151]), 2: array([-5.83948896]), 3: array([-0.54804469, -0.83644905,  0.        ])}\n",
      "cos(angle):  -0.5480446912492112   sin(angle)  -0.8364490518815577\n",
      "theta dot:  -5.8394889645\n",
      "Angle:  236.76731440959492\n",
      "action:  [ 0.04313127]\n",
      "state:  {1: array([ 0.66775371]), 2: array([-6.46035606]), 3: array([-0.7852146 , -0.61922373,  0.        ])}\n",
      "cos(angle):  -0.785214601037424   sin(angle)  -0.6192237320368456\n",
      "theta dot:  -6.46035606341\n",
      "Angle:  218.25980085896575\n",
      "action:  [ 0.19841788]\n",
      "state:  {1: array([ 0.32300315]), 2: array([-6.89501118]), 3: array([-0.94828645, -0.31741584,  0.        ])}\n",
      "cos(angle):  -0.9482864476919768   sin(angle)  -0.3174158362837805\n",
      "theta dot:  -6.89501118005\n",
      "Angle:  198.50709503375967\n",
      "action:  [ 0.30754978]\n",
      "state:  {1: array([ 6.25184143]), 2: array([-7.08694059]), 3: array([-0.99950882,  0.03133875,  0.        ])}\n",
      "cos(angle):  -0.9995088208931522   sin(angle)  0.03133874529684792\n",
      "theta dot:  -7.0869405906\n",
      "Angle:  178.20371140024827\n",
      "action:  [-0.03996018]\n",
      "state:  {1: array([ 5.8983699]), 2: array([-7.06943056]), 3: array([-0.92686774,  0.37538805,  0.        ])}\n",
      "cos(angle):  -0.9268677431423568   sin(angle)  0.3753880481877308\n",
      "theta dot:  -7.06943055852\n",
      "Angle:  157.95133203093133\n",
      "action:  [-0.19241376]\n",
      "state:  {1: array([ 5.55753232]), 2: array([-6.81675159]), 3: array([-0.74806625,  0.66362406,  0.        ])}\n",
      "cos(angle):  -0.7480662452388855   sin(angle)  0.6636240597915326\n",
      "theta dot:  -6.81675158631\n",
      "Angle:  138.4228229028601\n",
      "action:  [-0.60232912]\n",
      "state:  {1: array([ 5.23706318]), 2: array([-6.40938291]), 3: array([-0.50093105,  0.86548719,  0.        ])}\n",
      "cos(angle):  -0.5009310527343815   sin(angle)  0.8654871925143804\n",
      "theta dot:  -6.40938291018\n",
      "Angle:  120.06133633818308\n",
      "action:  [-0.43184458]\n",
      "state:  {1: array([ 4.94581097]), 2: array([-5.8250442]), 3: array([-0.23130806,  0.97288056,  0.        ])}\n",
      "cos(angle):  -0.23130805563648915   sin(angle)  0.9728805596771203\n",
      "theta dot:  -5.82504420211\n",
      "Angle:  103.3738529478627\n",
      "action:  [ 0.16427357]\n",
      "state:  {1: array([ 4.69227383]), 2: array([-5.07074275]), 3: array([ 0.02011379,  0.9997977 ,  0.        ])}\n",
      "cos(angle):  0.02011379418341434   sin(angle)  0.9997976971785578\n",
      "theta dot:  -5.07074274615\n",
      "Angle:  88.84727899976758\n",
      "action:  [ 0.28433806]\n",
      "state:  {1: array([ 4.47836164]), 2: array([-4.27824376]), 3: array([ 0.23189695,  0.97274036,  0.        ])}\n",
      "cos(angle):  0.23189694860100277   sin(angle)  0.9727403585898674\n",
      "theta dot:  -4.27824376395\n",
      "Angle:  76.59104209004931\n",
      "action:  [ 0.86477775]\n",
      "state:  {1: array([ 4.30741305]), 2: array([-3.41897183]), 3: array([ 0.39399664,  0.91911188,  0.        ])}\n",
      "cos(angle):  0.3939966378917367   sin(angle)  0.9191118807468478\n",
      "theta dot:  -3.41897183235\n",
      "Angle:  66.79643218070726\n",
      "action:  [ 0.19652503]\n",
      "state:  {1: array([ 4.17240509]), 2: array([-2.70015917]), 3: array([ 0.51412217,  0.85771696,  0.        ])}\n",
      "cos(angle):  0.5141221729044491   sin(angle)  0.8577169645798127\n",
      "theta dot:  -2.70015916744\n",
      "Angle:  59.06106405395673\n",
      "action:  [ 0.03637871]\n",
      "state:  {1: array([ 4.06983436]), 2: array([-2.05141464]), 3: array([ 0.59924254,  0.80056753,  0.        ])}\n",
      "cos(angle):  0.5992425409080414   sin(angle)  0.8005675344192233\n",
      "theta dot:  -2.05141463774\n",
      "Angle:  53.18420775790855\n",
      "action:  [-0.19445945]\n",
      "state:  {1: array([ 3.99582646]), 2: array([-1.4801579]), 3: array([ 0.65679646,  0.75406791,  0.        ])}\n",
      "cos(angle):  0.6567964611177577   sin(angle)  0.7540679071961556\n",
      "theta dot:  -1.48015790441\n",
      "Angle:  48.9438776268662\n",
      "action:  [-0.34444589]\n",
      "state:  {1: array([ 3.94751277]), 2: array([-0.96627386]), 3: array([ 0.69244769,  0.72146808,  0.        ])}\n",
      "cos(angle):  0.6924476924913086   sin(angle)  0.721468081874356\n",
      "theta dot:  -0.966273857654\n",
      "Angle:  46.175713405167414\n",
      "action:  [-0.36076448]\n",
      "state:  {1: array([ 3.9235484]), 2: array([-0.47928747]), 3: array([ 0.70953674,  0.70466844,  0.        ])}\n",
      "cos(angle):  0.7095367444059941   sin(angle)  0.704668438584944\n",
      "theta dot:  -0.479287468506\n",
      "Angle:  44.802659160021676\n",
      "action:  [-0.28284997]\n",
      "state:  {1: array([ 3.92388772]), 2: array([ 0.00678636]), 3: array([ 0.7092976 ,  0.70490916,  0.        ])}\n",
      "cos(angle):  0.7092975967108204   sin(angle)  0.7049091567714625\n",
      "theta dot:  0.00678636475685\n",
      "Angle:  44.8221006174993\n",
      "action:  [-0.16677505]\n",
      "state:  {1: array([ 3.94941032]), 2: array([ 0.51045198]), 3: array([ 0.69107743,  0.72278073,  0.        ])}\n",
      "cos(angle):  0.6910774295534294   sin(angle)  0.7227807318695102\n",
      "theta dot:  0.510451975196\n",
      "Angle:  46.284434389070384\n",
      "action:  [-0.03968896]\n",
      "state:  {1: array([ 4.00173952]), 2: array([ 1.04658418]), 3: array([ 0.65232616,  0.75793838,  0.        ])}\n",
      "cos(angle):  0.6523261563777932   sin(angle)  0.757938378567397\n",
      "theta dot:  1.04658418014\n",
      "Angle:  49.28267019926102\n",
      "action:  [ 0.160855]\n",
      "state:  {1: array([ 4.08369783]), 2: array([ 1.63916621]), 3: array([ 0.58808666,  0.80879792,  0.        ])}\n",
      "cos(angle):  0.588086663190094   sin(angle)  0.8087979207304757\n",
      "theta dot:  1.63916621456\n",
      "Angle:  53.97852451904873\n",
      "action:  [ 0.48278451]\n",
      "state:  {1: array([ 4.19960695]), 2: array([ 2.31818233]), 3: array([ 0.49060336,  0.87138301,  0.        ])}\n",
      "cos(angle):  0.49060335506157654   sin(angle)  0.8713830087867933\n",
      "theta dot:  2.31818233163\n",
      "Angle:  60.61961217649383\n",
      "action:  [ 0.70287733]\n",
      "state:  {1: array([ 4.35346451]), 2: array([ 3.07715119]), 3: array([ 0.35126744,  0.93627516,  0.        ])}\n",
      "cos(angle):  0.35126744420403533   sin(angle)  0.9362751639568172\n",
      "theta dot:  3.07715118748\n",
      "Angle:  69.43498036063562\n",
      "action:  [ 0.72158975]\n",
      "state:  {1: array([ 4.54784431]), 2: array([ 3.88759602]), 3: array([ 0.16380317,  0.98649304,  0.        ])}\n",
      "cos(angle):  0.16380316653999216   sin(angle)  0.9864930423634378\n",
      "theta dot:  3.88759602316\n",
      "Angle:  80.57209654616958\n",
      "action:  [ 0.54340746]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "state:  {1: array([ 4.78329316]), 2: array([ 4.70897692]), 3: array([-0.07084478,  0.99748735,  0.        ])}\n",
      "cos(angle):  -0.07084478204519275   sin(angle)  0.9974873517278147\n",
      "theta dot:  4.70897692322\n",
      "Angle:  94.06229017648889\n",
      "action:  [ 0.31115454]\n",
      "state:  {1: array([ 5.05848144]), 2: array([ 5.50376562]), 3: array([-0.33922456,  0.94070543,  0.        ])}\n",
      "cos(angle):  -0.3392245612701175   sin(angle)  0.9407054252161493\n",
      "theta dot:  5.50376561841\n",
      "Angle:  109.82938037438777\n",
      "action:  [ 0.88455678]\n",
      "state:  {1: array([ 5.37558035]), 2: array([ 6.3419782]), 3: array([-0.61563488,  0.78803153,  0.        ])}\n",
      "cos(angle):  -0.6156348820511918   sin(angle)  0.7880315298272114\n",
      "theta dot:  6.34197820361\n",
      "Angle:  127.99776713032412\n",
      "action:  [-0.33750374]\n",
      "state:  {1: array([ 5.71969916]), 2: array([ 6.88237629]), 3: array([-0.84539818,  0.53413662,  0.        ])}\n",
      "cos(angle):  -0.8453981750517923   sin(angle)  0.5341366169989651\n",
      "theta dot:  6.88237628968\n",
      "Angle:  147.71427674551524\n",
      "action:  [ 0.34437642]\n",
      "state:  {1: array([ 6.08643092]), 2: array([ 7.33463521]), 3: array([-0.98070622,  0.19548737,  0.        ])}\n",
      "cos(angle):  -0.9807062193014451   sin(angle)  0.19548736896143937\n",
      "theta dot:  7.33463521484\n",
      "Angle:  168.72640971394048\n",
      "action:  [ 0.57246566]\n",
      "state:  {1: array([ 0.18160165]), 2: array([ 7.56712059]), 3: array([-0.98355569, -0.18060511,  0.        ])}\n",
      "cos(angle):  -0.9835556888515412   sin(angle)  -0.18060511323816453\n",
      "theta dot:  7.56712059059\n",
      "Angle:  190.40540450099323\n",
      "action:  [ 0.52324511]\n",
      "state:  {1: array([ 0.55710932]), 2: array([ 7.51015352]), 3: array([-0.84878706, -0.52873484,  0.        ])}\n",
      "cos(angle):  -0.8487870568215888   sin(angle)  -0.5287348410802384\n",
      "theta dot:  7.51015352158\n",
      "Angle:  211.92035920376432\n",
      "action:  [ 0.4061484]\n",
      "state:  {1: array([ 0.91583556]), 2: array([ 7.17452465]), 3: array([-0.60912813, -0.79307182,  0.        ])}\n",
      "cos(angle):  -0.6091281326836577   sin(angle)  -0.7930718239688763\n",
      "theta dot:  7.17452465047\n",
      "Angle:  232.47381026508776\n",
      "action:  [ 0.13243442]\n",
      "state:  {1: array([ 1.24581485]), 2: array([ 6.59958595]), 3: array([-0.31929123, -0.94765664,  0.        ])}\n",
      "cos(angle):  -0.31929122971309454   sin(angle)  -0.947656641737027\n",
      "theta dot:  6.59958594571\n",
      "Angle:  251.38018711489593\n",
      "action:  [-0.20896032]\n",
      "state:  {1: array([ 1.53868982]), 2: array([ 5.85749942]), 3: array([-0.03210099, -0.99948463,  0.        ])}\n",
      "cos(angle):  -0.03210098749028184   sin(angle)  -0.9994846304982127\n",
      "theta dot:  5.85749941685\n",
      "Angle:  268.1606476291692\n",
      "action:  [-0.50355081]\n",
      "state:  {1: array([ 1.79030749]), 2: array([ 5.03235332]), 3: array([ 0.21775254, -0.97600401,  0.        ])}\n",
      "cos(angle):  0.2177525420967612   sin(angle)  -0.976004011472493\n",
      "theta dot:  5.03235332199\n",
      "Angle:  282.5772442353358\n",
      "action:  [-0.68506776]\n",
      "state:  {1: array([ 2.000187]), 2: array([ 4.19759015]), 3: array([ 0.41631687, -0.90921959,  0.        ])}\n",
      "cos(angle):  0.41631686528144185   sin(angle)  -0.9092195926629791\n",
      "theta dot:  4.19759014977\n",
      "Angle:  294.60242610059964\n",
      "action:  [-0.68462864]\n",
      "state:  {1: array([ 2.17083606]), 2: array([ 3.41298116]), 3: array([ 0.56467526, -0.82531318,  0.        ])}\n",
      "cos(angle):  0.5646752621368668   sin(angle)  -0.8253131819683124\n",
      "theta dot:  3.41298115941\n",
      "Angle:  304.3798740362689\n",
      "action:  [-0.48992303]\n",
      "state:  {1: array([ 2.30686145]), 2: array([ 2.72050782]), 3: array([ 0.67137691, -0.74111608,  0.        ])}\n",
      "cos(angle):  0.671376913322925   sin(angle)  -0.7411160774514216\n",
      "theta dot:  2.7205078189\n",
      "Angle:  312.17353661906617\n",
      "action:  [-0.28027366]\n",
      "state:  {1: array([ 2.41299293]), 2: array([ 2.12262971]), 3: array([ 0.74610747, -0.66582553,  0.        ])}\n",
      "cos(angle):  0.7461074743867706   sin(angle)  -0.6658255301985607\n",
      "theta dot:  2.12262971171\n",
      "Angle:  318.25440859685483\n",
      "action:  [ 0.04793799]\n",
      "state:  {1: array([ 2.49451549]), 2: array([ 1.63045126]), 3: array([ 0.79784926, -0.602857  ,  0.        ])}\n",
      "cos(angle):  0.7978492592951018   sin(angle)  -0.6028569975062557\n",
      "theta dot:  1.63045126226\n",
      "Angle:  322.92529647576066\n",
      "action:  [ 0.08358405]\n",
      "state:  {1: array([ 2.5540578]), 2: array([ 1.19084612]), 3: array([ 0.83230966, -0.55431094,  0.        ])}\n",
      "cos(angle):  0.8323096647619291   sin(angle)  -0.5543109433737397\n",
      "theta dot:  1.19084612175\n",
      "Angle:  326.3368113394353\n",
      "action:  [-0.65031521]\n",
      "state:  {1: array([ 2.58793608]), 2: array([ 0.67756563]), 3: array([ 0.85060758, -0.52580104,  0.        ])}\n",
      "cos(angle):  0.8506075839600914   sin(angle)  -0.5258010442264033\n",
      "theta dot:  0.677565632947\n",
      "Angle:  328.27788935589956\n",
      "action:  [ 0.66067049]\n",
      "state:  {1: array([ 2.60705185]), 2: array([ 0.38231542]), 3: array([ 0.86050266, -0.50944595,  0.        ])}\n",
      "cos(angle):  0.8605026575974526   sin(angle)  -0.5094459502908245\n",
      "theta dot:  0.382315423178\n",
      "Angle:  329.37313980426944\n",
      "action:  [-0.05618623]\n",
      "state:  {1: array([ 2.606642]), 2: array([-0.00819697]), 3: array([ 0.86029379, -0.50979858,  0.        ])}\n",
      "cos(angle):  0.8602937895514706   sin(angle)  -0.5097985834220904\n",
      "theta dot:  -0.00819697476512\n",
      "Angle:  329.349657256241\n",
      "action:  [-0.28280052]\n",
      "state:  {1: array([ 2.58499371]), 2: array([-0.43296599]), 3: array([ 0.8490568 , -0.52830157,  0.        ])}\n",
      "cos(angle):  0.8490567991563817   sin(angle)  -0.5283015727842572\n",
      "theta dot:  -0.432965989813\n",
      "Angle:  328.10930396227707\n",
      "action:  [ 0.12534315]\n",
      "state:  {1: array([ 2.54447417]), 2: array([-0.8103907]), 3: array([ 0.82695921, -0.56226191,  0.        ])}\n",
      "cos(angle):  0.826959213090704   sin(angle)  -0.5622619139550566\n",
      "theta dot:  -0.810390697041\n",
      "Angle:  325.7877110563157\n",
      "action:  [ 0.16960361]\n",
      "state:  {1: array([ 2.48414184]), 2: array([-1.20664659]), 3: array([ 0.79155261, -0.61110102,  0.        ])}\n",
      "cos(angle):  0.7915526127349474   sin(angle)  -0.6111010237861482\n",
      "theta dot:  -1.20664659137\n",
      "Angle:  322.33093128730945\n",
      "action:  [ 0.11173067]\n",
      "state:  {1: array([ 2.4017312]), 2: array([-1.64821276]), 3: array([ 0.73856197, -0.67418559,  0.        ])}\n",
      "cos(angle):  0.7385619738893331   sin(angle)  -0.6741855907127444\n",
      "theta dot:  -1.64821275818\n",
      "Angle:  317.6091605897066\n",
      "action:  [-0.05921773]\n",
      "state:  {1: array([ 2.29359447]), 2: array([-2.16273461]), 3: array([ 0.66148575, -0.74995773,  0.        ])}\n",
      "cos(angle):  0.6614857498614745   sin(angle)  -0.7499577339625233\n",
      "theta dot:  -2.16273461093\n",
      "Angle:  311.4133968074527\n",
      "action:  [-0.17077478]\n",
      "state:  {1: array([ 2.15605352]), 2: array([-2.75081913]), 3: array([ 0.55241379, -0.83357004,  0.        ])}\n",
      "cos(angle):  0.5524137859041863   sin(angle)  -0.8335700385348574\n",
      "theta dot:  -2.75081912854\n",
      "Angle:  303.53289892201724\n",
      "action:  [-0.43087646]\n",
      "state:  {1: array([ 1.98402211]), 2: array([-3.44062813]), 3: array([ 0.40156568, -0.91583023,  0.        ])}\n",
      "cos(angle):  0.40156568168840984   sin(angle)  -0.9158302262374411\n",
      "theta dot:  -3.44062812686\n",
      "Angle:  293.6762484440126\n",
      "action:  [-0.47995631]\n",
      "state:  {1: array([ 1.7740474]), 2: array([-4.19949424]), 3: array([ 0.20185454, -0.97941551,  0.        ])}\n",
      "cos(angle):  0.20185454110558995   sin(angle)  -0.9794155115348397\n",
      "theta dot:  -4.19949424336\n",
      "Angle:  281.64561176517867\n",
      "action:  [-0.4435713]\n",
      "state:  {1: array([ 1.52401782]), 2: array([-5.00059157]), 3: array([-0.04676145, -0.99890609,  0.        ])}\n",
      "cos(angle):  -0.04676144930151173   sin(angle)  -0.998906085104712\n",
      "theta dot:  -5.00059157169\n",
      "Angle:  267.3200056583562\n",
      "action:  [-0.34416245]\n",
      "state:  {1: array([ 1.23394804]), 2: array([-5.8013955]), 3: array([-0.33051414, -0.94380104,  0.        ])}\n",
      "cos(angle):  -0.33051414478176067   sin(angle)  -0.9438010384075561\n",
      "theta dot:  -5.80139550318\n",
      "Angle:  250.70027064160644\n",
      "action:  [-0.14619149]\n",
      "state:  {1: array([ 0.90738929]), 2: array([-6.531175]), 3: array([-0.61580482, -0.78789874,  0.        ])}\n",
      "cos(angle):  -0.6158048180236202   sin(angle)  -0.7878987410187276\n",
      "theta dot:  -6.53117500482\n",
      "Angle:  231.98987624275955\n",
      "action:  [-0.17768419]\n",
      "state:  {1: array([ 0.54995171]), 2: array([-7.14875169]), 3: array([-0.85254976, -0.52264606,  0.        ])}\n",
      "cos(angle):  -0.8525497620798292   sin(angle)  -0.5226460591811888\n",
      "theta dot:  -7.14875168941\n",
      "Angle:  211.51025910349026\n",
      "action:  [-0.15705563]\n",
      "state:  {1: array([ 0.17173698]), 2: array([-7.56429458]), 3: array([-0.98528941, -0.17089403,  0.        ])}\n",
      "cos(angle):  -0.9852894138556925   sin(angle)  -0.17089403425487348\n",
      "theta dot:  -7.56429457798\n",
      "Angle:  189.84020206191875\n",
      "action:  [-0.17531186]\n",
      "state:  {1: array([ 6.06898419]), 2: array([-7.71876188]), 3: array([-0.97714652,  0.21256687,  0.        ])}\n",
      "cos(angle):  -0.9771465227230693   sin(angle)  0.21256686743284825\n",
      "theta dot:  -7.7187618827\n",
      "Angle:  167.726787988831\n",
      "action:  [-0.45289557]\n",
      "state:  {1: array([ 5.68762064]), 2: array([-7.62727107]), 3: array([-0.82783187,  0.56097629,  0.        ])}\n",
      "cos(angle):  -0.8278318658835392   sin(angle)  0.5609762934632604\n",
      "theta dot:  -7.62727106748\n",
      "Angle:  145.87631701629164\n",
      "action:  [-0.29079563]\n",
      "state:  {1: array([ 5.32511273]), 2: array([-7.25015819]), 3: array([-0.57509785,  0.81808463,  0.        ])}\n",
      "cos(angle):  -0.5750978486006177   sin(angle)  0.8180846316457369\n",
      "theta dot:  -7.25015819134\n",
      "Angle:  125.10619232757597\n",
      "action:  [-0.63450271]\n",
      "state:  {1: array([ 4.98852422]), 2: array([-6.73177012]), 3: array([-0.27263935,  0.96211631,  0.        ])}\n",
      "cos(angle):  -0.2726393489211266   sin(angle)  0.9621163055576307\n",
      "theta dot:  -6.73177012443\n",
      "Angle:  105.82113658531432\n",
      "action:  [-0.52540444]\n",
      "state:  {1: array([ 4.68407455]), 2: array([-6.08899356]), 3: array([ 0.02831065,  0.99959917,  0.        ])}\n",
      "cos(angle):  0.028310651141744574   sin(angle)  0.9995991731848973\n",
      "theta dot:  -6.0889935609\n",
      "Angle:  88.37749575003119\n",
      "action:  [-0.41573067]\n",
      "state:  {1: array([ 4.41399186]), 2: array([-5.40165378]), 3: array([ 0.29398854,  0.95580894,  0.        ])}\n",
      "cos(angle):  0.2939885411520298   sin(angle)  0.9558089441260221\n",
      "theta dot:  -5.4016537812\n",
      "Angle:  72.90293373362061\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -1.64828772711  self.max:  27.8419661301\n",
      "state:  {1: array([ 3.29175997]), 2: array([ 0.97481442]), 3: array([ 0.98874606,  0.14960357,  0.        ])}\n",
      "cos(angle):  0.9887460604990389   sin(angle)  0.14960356896688945\n",
      "theta dot:  0.974814421321\n",
      "Angle:  8.60393338691631\n",
      "action:  [-0.05444707]\n",
      "state:  {1: array([ 3.34570247]), 2: array([ 1.07885004]), 3: array([ 0.97924181,  0.20269554,  0.        ])}\n",
      "cos(angle):  0.9792418081418075   sin(angle)  0.20269553815307206\n",
      "theta dot:  1.07885003802\n",
      "Angle:  11.694603854899412\n",
      "action:  [-0.0753988]\n",
      "state:  {1: array([ 3.40668057]), 2: array([ 1.21956187]), 3: array([ 0.96506947,  0.26199411,  0.        ])}\n",
      "cos(angle):  0.9650694719595501   sin(angle)  0.26199411117755167\n",
      "theta dot:  1.21956187155\n",
      "Angle:  15.188383089663716\n",
      "action:  [ 0.02997271]\n",
      "state:  {1: array([ 3.47770823]), 2: array([ 1.42055336]), 3: array([ 0.94404295,  0.32982253,  0.        ])}\n",
      "cos(angle):  0.9440429534130389   sin(angle)  0.32982253123640126\n",
      "theta dot:  1.42055336206\n",
      "Angle:  19.257959184182937\n",
      "action:  [ 0.23317628]\n",
      "state:  {1: array([ 3.56285307]), 2: array([ 1.7028967]), 3: array([ 0.91257427,  0.408911  ,  0.        ])}\n",
      "cos(angle):  0.912574267338776   sin(angle)  0.4089110008193668\n",
      "theta dot:  1.70289670184\n",
      "Angle:  24.136387474409617\n",
      "action:  [ 0.18347892]\n",
      "state:  {1: array([ 3.66470816]), 2: array([ 2.03710179]), 3: array([ 0.86626694,  0.49958142,  0.        ])}\n",
      "cos(angle):  0.8662669376430732   sin(angle)  0.4995814175352121\n",
      "theta dot:  2.03710179097\n",
      "Angle:  29.97224058071538\n",
      "action:  [-0.23568638]\n",
      "state:  {1: array([ 3.7835299]), 2: array([ 2.3764349]), 3: array([ 0.80093734,  0.59874818,  0.        ])}\n",
      "cos(angle):  0.8009373363795622   sin(angle)  0.5987481801168267\n",
      "theta dot:  2.37643489639\n",
      "Angle:  36.7802091532601\n",
      "action:  [-0.70754675]\n",
      "state:  {1: array([ 3.9194981]), 2: array([ 2.71936402]), 3: array([ 0.71238503,  0.70178883,  0.        ])}\n",
      "cos(angle):  0.7123850304619137   sin(angle)  0.7017888346032434\n",
      "theta dot:  2.71936401844\n",
      "Angle:  44.57059499676126\n",
      "action:  [-1.6934366]\n",
      "state:  {1: array([ 4.06908261]), 2: array([ 2.99169015]), 3: array([ 0.5998442 ,  0.80011683,  0.        ])}\n",
      "cos(angle):  0.5998441958025365   sin(angle)  0.8001168294455556\n",
      "theta dot:  2.99169015369\n",
      "Angle:  53.14113592598244\n",
      "action:  [-1.29931529]\n",
      "state:  {1: array([ 4.23892664]), 2: array([ 3.39688048]), 3: array([ 0.45597048,  0.8899949 ,  0.        ])}\n",
      "cos(angle):  0.4559704804400683   sin(angle)  0.8899948994051894\n",
      "theta dot:  3.39688048267\n",
      "Angle:  62.8724589282823\n",
      "action:  [-1.26927453]\n",
      "state:  {1: array([ 4.43262591]), 2: array([ 3.87398548]), 3: array([ 0.27612794,  0.96112089,  0.        ])}\n",
      "cos(angle):  0.2761279377126686   sin(angle)  0.961120888345763\n",
      "theta dot:  3.87398547749\n",
      "Angle:  73.97058386379638\n",
      "action:  [-0.72335354]\n",
      "state:  {1: array([ 4.65694207]), 2: array([ 4.48632311]), 3: array([ 0.05541851,  0.99846321,  0.        ])}\n",
      "cos(angle):  0.05541850788328061   sin(angle)  0.9984632136358308\n",
      "theta dot:  4.48632311221\n",
      "Angle:  86.8229228025225\n",
      "action:  [ 23.11510594]\n",
      "state:  {1: array([ 4.93370059]), 2: array([ 5.53517052]), 3: array([-0.21950943,  0.97561038,  0.        ])}\n",
      "cos(angle):  -0.21950943378050064   sin(angle)  0.9756103773952817\n",
      "theta dot:  5.53517052244\n",
      "Angle:  102.67998121286871\n",
      "action:  [-0.82415759]\n",
      "state:  {1: array([ 5.24086333]), 2: array([ 6.14325467]), 3: array([-0.50421641,  0.86357734,  0.        ])}\n",
      "cos(angle):  -0.5042164077473819   sin(angle)  0.8635773353662809\n",
      "theta dot:  6.14325466744\n",
      "Angle:  120.2790683044617\n",
      "action:  [-0.66394978]\n",
      "state:  {1: array([ 5.57543059]), 2: array([ 6.6913452]), 3: array([-0.75982351,  0.6501294 ,  0.        ])}\n",
      "cos(angle):  -0.7598235116325408   sin(angle)  0.6501293957131873\n",
      "theta dot:  6.69134520129\n",
      "Angle:  139.44831544336944\n",
      "action:  [-0.78233216]\n",
      "state:  {1: array([ 5.92851021]), 2: array([ 7.06159242]), 3: array([-0.93775937,  0.34728571,  0.        ])}\n",
      "cos(angle):  -0.9377593713784013   sin(angle)  0.3472857057121494\n",
      "theta dot:  7.06159242426\n",
      "Angle:  159.6782402645947\n",
      "action:  [-1.26780409]\n",
      "state:  {1: array([ 0.0019192]), 2: array([ 7.13188609]), 3: array([-0.99999816, -0.0019192 ,  0.        ])}\n",
      "cos(angle):  -0.9999981583280021   sin(angle)  -0.001919203116941864\n",
      "theta dot:  7.13188608934\n",
      "Angle:  180.11038296631\n",
      "action:  [-0.80168669]\n",
      "state:  {1: array([ 0.35242889]), 2: array([ 7.01019368]), 3: array([-0.93853708, -0.34517843,  0.        ])}\n",
      "cos(angle):  -0.938537082220013   sin(angle)  -0.3451784253077598\n",
      "theta dot:  7.01019368364\n",
      "Angle:  200.19306158635987\n",
      "action:  [-0.78195699]\n",
      "state:  {1: array([ 0.6841297]), 2: array([ 6.63401632]), 3: array([-0.77496937, -0.6319988 ,  0.        ])}\n",
      "cos(angle):  -0.7749693663784132   sin(angle)  -0.631998798396833\n",
      "theta dot:  6.63401631542\n",
      "Angle:  219.19807394911155\n",
      "action:  [-0.84940392]\n",
      "state:  {1: array([ 0.98576004]), 2: array([ 6.03260663]), 3: array([-0.55222964, -0.83369205,  0.        ])}\n",
      "cos(angle):  -0.5522296383277204   sin(angle)  -0.8336920453935224\n",
      "theta dot:  6.03260662815\n",
      "Angle:  236.48017849882586\n",
      "action:  [-0.96286087]\n",
      "state:  {1: array([ 1.24890546]), 2: array([ 5.26290846]), 3: array([-0.31636088, -0.94863892,  0.        ])}\n",
      "cos(angle):  -0.3163608760255039   sin(angle)  -0.9486389176711947\n",
      "theta dot:  5.26290846383\n",
      "Angle:  251.5572653891006\n",
      "action:  [-0.96710332]\n",
      "state:  {1: array([ 1.46922365]), 2: array([ 4.40636378]), 3: array([-0.10139811, -0.99484593,  0.        ])}\n",
      "cos(angle):  -0.10139811477979836   sin(angle)  -0.9948459289352813\n",
      "theta dot:  4.40636377729\n",
      "Angle:  264.1805382423055\n",
      "action:  [-0.74396421]\n",
      "state:  {1: array([ 1.64665538]), 2: array([ 3.5486347]), 3: array([ 0.07578632, -0.99712408,  0.        ])}\n",
      "cos(angle):  0.07578632044126121   sin(angle)  -0.9971240813629839\n",
      "theta dot:  3.54863469934\n",
      "Angle:  274.3466040349233\n",
      "action:  [-0.3497364]\n",
      "state:  {1: array([ 1.78407194]), 2: array([ 2.74833118]), 3: array([ 0.21166243, -0.97734283,  0.        ])}\n",
      "cos(angle):  0.21166242832300736   sin(angle)  -0.977342834647294\n",
      "theta dot:  2.74833117866\n",
      "Angle:  282.21997448562314\n",
      "action:  [-0.05822938]\n",
      "state:  {1: array([ 1.88440142]), 2: array([ 2.00658965]), 3: array([ 0.3084899 , -0.95122762,  0.        ])}\n",
      "cos(angle):  0.3084899015966665   sin(angle)  -0.9512276176672327\n",
      "theta dot:  2.0065896452\n",
      "Angle:  287.9684169375005\n",
      "action:  [ 0.19466547]\n",
      "state:  {1: array([ 1.95051986]), 2: array([ 1.32236875]), 3: array([ 0.37066371, -0.92876715,  0.        ])}\n",
      "cos(angle):  0.37066371171736173   sin(angle)  -0.9287671467143466\n",
      "theta dot:  1.32236875296\n",
      "Angle:  291.75671550403246\n",
      "action:  [ 0.31097111]\n",
      "state:  {1: array([ 1.98414181]), 2: array([ 0.67243906]), 3: array([ 0.40167531, -0.91578215,  0.        ])}\n",
      "cos(angle):  0.4016753080435794   sin(angle)  -0.9157821503546002\n",
      "theta dot:  0.672439060008\n",
      "Angle:  293.68310700520095\n",
      "action:  [ 0.1297964]\n",
      "state:  {1: array([ 1.98439541]), 2: array([ 0.00507191]), 3: array([ 0.40190753, -0.91568026,  0.        ])}\n",
      "cos(angle):  0.4019075332120777   sin(angle)  -0.915680257921608\n",
      "theta dot:  0.00507190682603\n",
      "Angle:  293.69763691398447\n",
      "action:  [-0.02111054]\n",
      "state:  {1: array([ 1.95015267]), 2: array([-0.68485487]), 3: array([ 0.37032265, -0.92890319,  0.        ])}\n",
      "cos(angle):  0.37032264802495446   sin(angle)  -0.9289031899825653\n",
      "theta dot:  -0.684854868103\n",
      "Angle:  291.7356768258364\n",
      "action:  [ 0.03336221]\n",
      "state:  {1: array([ 1.88132627]), 2: array([-1.37652793]), 3: array([ 0.30556328, -0.95217177,  0.        ])}\n",
      "cos(angle):  0.3055632762898492   sin(angle)  -0.9521717724145226\n",
      "theta dot:  -1.37652792909\n",
      "Angle:  287.792224011399\n",
      "action:  [-0.01466779]\n",
      "state:  {1: array([ 1.77668342]), 2: array([-2.09285693]), 3: array([ 0.2044356 , -0.97888002,  0.        ])}\n",
      "cos(angle):  0.20443560164422936   sin(angle)  -0.9788800155179194\n",
      "theta dot:  -2.09285692641\n",
      "Angle:  281.79664458125825\n",
      "action:  [-0.01558742]\n",
      "state:  {1: array([ 1.63521567]), 2: array([-2.82935505]), 3: array([ 0.0643748 , -0.99792579,  0.        ])}\n",
      "cos(angle):  0.0643747984323184   sin(angle)  -0.9979257914929338\n",
      "theta dot:  -2.8293550515\n",
      "Angle:  273.69115837567455\n",
      "action:  [ 0.14111616]\n",
      "state:  {1: array([ 1.45738407]), 2: array([-3.55663197]), 3: array([-0.11316929, -0.99357572,  0.        ])}\n",
      "cos(angle):  -0.11316928604592813   sin(angle)  -0.9935757206654432\n",
      "theta dot:  -3.55663197065\n",
      "Angle:  263.50218214195894\n",
      "action:  [ 0.21026275]\n",
      "state:  {1: array([ 1.24387036]), 2: array([-4.27027435]), 3: array([-0.32113334, -0.94703399,  0.        ])}\n",
      "cos(angle):  -0.3211333409831816   sin(angle)  -0.9470339895214848\n",
      "theta dot:  -4.27027434938\n",
      "Angle:  251.26877586984926\n",
      "action:  [ 0.18938332]\n",
      "state:  {1: array([ 0.99626324]), 2: array([-4.95214234]), 3: array([-0.5434429 , -0.83944613,  0.        ])}\n",
      "cos(angle):  -0.5434429031637062   sin(angle)  -0.8394461334719474\n",
      "theta dot:  -4.95214234292\n",
      "Angle:  237.08196625490046\n",
      "action:  [ 0.373724]\n",
      "state:  {1: array([ 0.71997982]), 2: array([-5.52566834]), 3: array([-0.75181903, -0.6593695 ,  0.        ])}\n",
      "cos(angle):  -0.7518190348363099   sin(angle)  -0.659369501006681\n",
      "theta dot:  -5.52566834297\n",
      "Angle:  221.25212951988263\n",
      "action:  [ 0.03652893]\n",
      "state:  {1: array([ 0.41924401]), 2: array([-6.01471613]), 3: array([-0.91339694, -0.40707005,  0.        ])}\n",
      "cos(angle):  -0.9133969404277835   sin(angle)  -0.40707005443432465\n",
      "theta dot:  -6.01471612996\n",
      "Angle:  204.0212773523176\n",
      "action:  [-0.02760691]\n",
      "state:  {1: array([ 0.10303603]), 2: array([-6.32415971]), 3: array([-0.99469648, -0.10285381,  0.        ])}\n",
      "cos(angle):  -0.9946964828962283   sin(angle)  -0.10285381331712266\n",
      "theta dot:  -6.32415970794\n",
      "Angle:  185.90393670695806\n",
      "action:  [ 0.12531153]\n",
      "state:  {1: array([ 6.06709617]), 2: array([-6.38250334]), 3: array([-0.97674345,  0.21441136,  0.        ])}\n",
      "cos(angle):  -0.976743450007573   sin(angle)  0.21441136366177924\n",
      "theta dot:  -6.38250333818\n",
      "Angle:  167.61861242910356\n",
      "action:  [-0.38659805]\n",
      "state:  {1: array([ 5.75311194]), 2: array([-6.27968452]), 3: array([-0.86276998,  0.50559664,  0.        ])}\n",
      "cos(angle):  -0.8627699801621245   sin(angle)  0.505596638963361\n",
      "theta dot:  -6.27968452339\n",
      "Angle:  149.62868350417716\n",
      "action:  [-0.52899897]\n",
      "state:  {1: array([ 5.4541201]), 2: array([-5.97983689]), 3: array([-0.67556528,  0.73730018,  0.        ])}\n",
      "cos(angle):  -0.6755652769866762   sin(angle)  0.7373001807472417\n",
      "theta dot:  -5.97983688923\n",
      "Angle:  132.49775276725654\n",
      "action:  [-0.64640426]\n",
      "state:  {1: array([ 5.17792898]), 2: array([-5.52382239]), 3: array([-0.4489054 ,  0.89357929,  0.        ])}\n",
      "cos(angle):  -0.44890539837597293   sin(angle)  0.8935792876454272\n",
      "theta dot:  -5.52382239275\n",
      "Angle:  116.67320427771796\n",
      "action:  [-1.08214494]\n",
      "state:  {1: array([ 4.927131]), 2: array([-5.01595967]), 3: array([-0.21309538,  0.9770314 ,  0.        ])}\n",
      "cos(angle):  -0.21309537610855986   sin(angle)  0.9770314020957317\n",
      "theta dot:  -5.01595966737\n",
      "Angle:  102.30357192275585\n",
      "action:  [-0.82397498]\n",
      "state:  {1: array([ 4.70679188]), 2: array([-4.40678236]), 3: array([ 0.00559707,  0.99998434,  0.        ])}\n",
      "cos(angle):  0.005597073781682919   sin(angle)  0.9999843362598648\n",
      "theta dot:  -4.40678236338\n",
      "Angle:  89.67909991153522\n",
      "action:  [-1.07433878]\n",
      "state:  {1: array([ 4.51589463]), 2: array([-3.81794493]), 3: array([ 0.19523235,  0.98075702,  0.        ])}\n",
      "cos(angle):  0.19523234606910086   sin(angle)  0.9807570193724615\n",
      "theta dot:  -3.81794492797\n",
      "Angle:  78.74151894905367\n",
      "action:  [-0.27456244]\n",
      "state:  {1: array([ 4.35971655]), 2: array([-3.12356153]), 3: array([ 0.34540698,  0.93845299,  0.        ])}\n",
      "cos(angle):  0.34540698397874775   sin(angle)  0.9384529905214779\n",
      "theta dot:  -3.12356153007\n",
      "Angle:  69.79319523799626\n",
      "action:  [-0.00656911]\n",
      "state:  {1: array([ 4.2386812]), 2: array([-2.42070715]), 3: array([ 0.45618891,  0.88988296,  0.        ])}\n",
      "cos(angle):  0.45618890704116827   sin(angle)  0.8898829592101336\n",
      "theta dot:  -2.42070715427\n",
      "Angle:  62.85839628573888\n",
      "action:  [ 0.01604933]\n",
      "state:  {1: array([ 4.15113682]), 2: array([-1.75088754]), 3: array([ 0.53224668,  0.84658932,  0.        ])}\n",
      "cos(angle):  0.5322466805198849   sin(angle)  0.8465893166556991\n",
      "theta dot:  -1.75088753563\n",
      "Angle:  57.842484705427864\n",
      "action:  [ 0.50470724]\n",
      "state:  {1: array([ 4.09912485]), 2: array([-1.04023946]), 3: array([ 0.57553984,  0.81777374,  0.        ])}\n",
      "cos(angle):  0.5755398441116836   sin(angle)  0.8177737387810268\n",
      "theta dot:  -1.04023946277\n",
      "Angle:  54.862425129127246\n",
      "action:  [-0.17156513]\n",
      "state:  {1: array([ 4.07649265]), 2: array([-0.45264393]), 3: array([ 0.59389889,  0.80453969,  0.        ])}\n",
      "cos(angle):  0.5938988858328973   sin(angle)  0.8045396903860264\n",
      "theta dot:  -0.452643927725\n",
      "Angle:  53.5656988273949\n",
      "action:  [-0.25372192]\n",
      "state:  {1: array([ 4.08212778]), 2: array([ 0.11270255]), 3: array([ 0.5893558 ,  0.80787359,  0.        ])}\n",
      "cos(angle):  0.5893557965106979   sin(angle)  0.807873594765444\n",
      "theta dot:  0.112702551933\n",
      "Angle:  53.88856710069342\n",
      "action:  [-0.3703093]\n",
      "state:  {1: array([ 4.11528085]), 2: array([ 0.66306135]), 3: array([ 0.56225336,  0.82696503,  0.        ])}\n",
      "cos(angle):  0.5622533562387565   sin(angle)  0.8269650315450189\n",
      "theta dot:  0.663061353559\n",
      "Angle:  55.78809351463269\n",
      "action:  [-0.45359539]\n",
      "state:  {1: array([ 4.17604314]), 2: array([ 1.21524582]), 3: array([ 0.51099836,  0.85958168,  0.        ])}\n",
      "cos(angle):  0.5109983644943373   sin(angle)  0.8595816840092118\n",
      "theta dot:  1.21524581821\n",
      "Angle:  59.26950819628814\n",
      "action:  [-0.51326969]\n",
      "state:  {1: array([ 4.26519022]), 2: array([ 1.78294163]), 3: array([ 0.43244146,  0.90166201,  0.        ])}\n",
      "cos(angle):  0.4324414645428023   sin(angle)  0.9016620097043438\n",
      "theta dot:  1.7829416272\n",
      "Angle:  64.37724777000898\n",
      "action:  [-0.56502255]\n",
      "state:  {1: array([ 4.38391196]), 2: array([ 2.37443475]), 3: array([ 0.32260185,  0.94653476,  0.        ])}\n",
      "cos(angle):  0.3226018541195759   sin(angle)  0.9465347556844449\n",
      "theta dot:  2.37443475172\n",
      "Angle:  71.17948636356414\n",
      "action:  [-0.58026767]\n",
      "state:  {1: array([ 4.53377674]), 2: array([ 2.99729567]), 3: array([ 0.17766406,  0.9840912 ,  0.        ])}\n",
      "cos(angle):  0.17766406388949776   sin(angle)  0.9840911951655031\n",
      "theta dot:  2.99729566863\n",
      "Angle:  79.76608587262069\n",
      "action:  [-0.5169499]\n",
      "state:  {1: array([ 4.71666782]), 2: array([ 3.65782158]), 3: array([-0.00427882,  0.99999085,  0.        ])}\n",
      "cos(angle):  -0.004278824557634716   sin(angle)  0.9999908457883027\n",
      "theta dot:  3.65782157958\n",
      "Angle:  90.24494830456462\n",
      "action:  [-0.3046826]\n",
      "state:  {1: array([ 4.93477343]), 2: array([ 4.36211232]), 3: array([-0.22055598,  0.97537432,  0.        ])}\n",
      "cos(angle):  -0.2205559828647092   sin(angle)  0.9753743170816946\n",
      "theta dot:  4.36211232356\n",
      "Angle:  102.74145037741721\n",
      "action:  [-0.02401714]\n",
      "state:  {1: array([ 5.18927546]), 2: array([ 5.09004049]), 3: array([-0.45901526,  0.88842838,  0.        ])}\n",
      "cos(angle):  -0.4590152638476817   sin(angle)  0.8884283806558878\n",
      "theta dot:  5.09004049083\n",
      "Angle:  117.32330816245036\n",
      "action:  [ 0.21266311]\n",
      "state:  {1: array([ 5.47868852]), 2: array([ 5.78826124]), 3: array([-0.69347388,  0.72048177,  0.        ])}\n",
      "cos(angle):  -0.6934738790936026   sin(angle)  0.7204817686901394\n",
      "theta dot:  5.7882612429\n",
      "Angle:  133.90541638313255\n",
      "action:  [ 0.43694266]\n",
      "state:  {1: array([ 5.79839672]), 2: array([ 6.39416397]), 3: array([-0.88477349,  0.46602132,  0.        ])}\n",
      "cos(angle):  -0.8847734914172382   sin(angle)  0.46602131805889563\n",
      "theta dot:  6.39416396796\n",
      "Angle:  152.22330399186956\n",
      "action:  [ 0.31448356]\n",
      "state:  {1: array([ 6.13793934]), 2: array([ 6.79085249]), 3: array([-0.98947034,  0.14473581,  0.        ])}\n",
      "cos(angle):  -0.9894703360877045   sin(angle)  0.14473580760297416\n",
      "theta dot:  6.79085248979\n",
      "Angle:  171.6776178472553\n",
      "action:  [ 0.32971056]\n",
      "state:  {1: array([ 0.20219708]), 2: array([ 6.94886093]), 3: array([-0.97962772, -0.20082214,  0.        ])}\n",
      "cos(angle):  -0.9796277196992497   sin(angle)  -0.20082213721810716\n",
      "theta dot:  6.94886092948\n",
      "Angle:  191.58543331483935\n",
      "action:  [ 0.09829529]\n",
      "state:  {1: array([ 0.54284651]), 2: array([ 6.81298862]), 3: array([-0.85624171, -0.51657539,  0.        ])}\n",
      "cos(angle):  -0.8562417131483135   sin(angle)  -0.5165753852680566\n",
      "theta dot:  6.81298862077\n",
      "Angle:  211.10316236592388\n",
      "action:  [ 0.08328554]\n",
      "state:  {1: array([ 0.86474901]), 2: array([ 6.43804991]), 3: array([-0.64883112, -0.76093244,  0.        ])}\n",
      "cos(angle):  -0.6488311227774761   sin(angle)  -0.7609324372868591\n",
      "theta dot:  6.43804991246\n",
      "Angle:  229.54677365067417\n",
      "action:  [ 0.00819024]\n",
      "state:  {1: array([ 1.15817797]), 2: array([ 5.86857912]), 3: array([-0.40100931, -0.91607398,  0.        ])}\n",
      "cos(angle):  -0.40100931229076536   sin(angle)  -0.9160739770652191\n",
      "theta dot:  5.86857912067\n",
      "Angle:  246.3589751040887\n",
      "action:  [-0.1255751]\n",
      "state:  {1: array([ 1.41631233]), 2: array([ 5.16268737]), 3: array([-0.15387026, -0.98809106,  0.        ])}\n",
      "cos(angle):  -0.15387025731757123   sin(angle)  -0.9880910605369448\n",
      "theta dot:  5.16268737279\n",
      "Angle:  261.14895038900465\n",
      "action:  [-0.28578432]\n",
      "state:  {1: array([ 1.63524991]), 2: array([ 4.37875143]), 3: array([ 0.06440896, -0.99792359,  0.        ])}\n",
      "cos(angle):  0.06440896209212793   sin(angle)  -0.9979235870557499\n",
      "theta dot:  4.3787514287\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  273.69311987533814\n",
      "action:  [-0.41157187]\n",
      "state:  {1: array([ 1.81367855]), 2: array([ 3.56857296]), 3: array([ 0.24050125, -0.97064883,  0.        ])}\n",
      "cos(angle):  0.24050125165881303   sin(angle)  -0.9706488283362549\n",
      "theta dot:  3.56857295746\n",
      "Angle:  283.9163044364414\n",
      "action:  [-0.55040948]\n",
      "state:  {1: array([ 1.9515798]), 2: array([ 2.75802491]), 3: array([ 0.37164794, -0.92837374,  0.        ])}\n",
      "cos(angle):  0.37164793881356223   sin(angle)  -0.9283737445531464\n",
      "theta dot:  2.75802491457\n",
      "Angle:  291.8174453299832\n",
      "action:  [-0.60874157]\n",
      "state:  {1: array([ 2.05010147]), 2: array([ 1.97043337]), 3: array([ 0.46116273, -0.88731558,  0.        ])}\n",
      "cos(angle):  0.4611627278318376   sin(angle)  -0.8873155799706768\n",
      "theta dot:  1.97043337042\n",
      "Angle:  297.46230792668644\n",
      "action:  [-0.54122394]\n",
      "state:  {1: array([ 2.11128962]), 2: array([ 1.22376309]), 3: array([ 0.51455903, -0.85745496,  0.        ])}\n",
      "cos(angle):  0.5145590332312507   sin(angle)  -0.8574549558548954\n",
      "theta dot:  1.22376309457\n",
      "Angle:  300.9681227506912\n",
      "action:  [-0.37497051]\n",
      "state:  {1: array([ 2.13751094]), 2: array([ 0.5244263]), 3: array([ 0.53686317, -0.84366933,  0.        ])}\n",
      "cos(angle):  0.5368631691043633   sin(angle)  -0.8436693295712603\n",
      "theta dot:  0.524426300931\n",
      "Angle:  302.4704899229523\n",
      "action:  [-0.14642887]\n",
      "state:  {1: array([ 2.13099644]), 2: array([-0.13029003]), 3: array([ 0.53135573, -0.8471488 ,  0.        ])}\n",
      "cos(angle):  0.5313557311281503   sin(angle)  -0.8471487986164349\n",
      "theta dot:  -0.130290027224\n",
      "Angle:  302.0972373621501\n",
      "action:  [ 0.07459285]\n",
      "state:  {1: array([ 2.0932733]), 2: array([-0.7544627]), 3: array([ 0.49902818, -0.86658576,  0.        ])}\n",
      "cos(angle):  0.4990281774403025   sin(angle)  -0.8665857592417556\n",
      "theta dot:  -0.754462698919\n",
      "Angle:  299.93586599397185\n",
      "action:  [ 0.14293855]\n",
      "state:  {1: array([ 2.02412524]), 2: array([-1.38296124]), 3: array([ 0.43796063, -0.89899415,  0.        ])}\n",
      "cos(angle):  0.43796062841333155   sin(angle)  -0.8989941534625238\n",
      "theta dot:  -1.38296123571\n",
      "Angle:  295.97398315674496\n",
      "action:  [ 0.30189923]\n",
      "state:  {1: array([ 1.92352914]), 2: array([-2.01192197]), 3: array([ 0.34546366, -0.93843213,  0.        ])}\n",
      "cos(angle):  0.34546365524179795   sin(angle)  -0.9384321301548536\n",
      "theta dot:  -2.01192196567\n",
      "Angle:  290.2102647676938\n",
      "action:  [ 0.18041412]\n",
      "state:  {1: array([ 1.78909494]), 2: array([-2.68868395]), 3: array([ 0.21656893, -0.97626733,  0.        ])}\n",
      "cos(angle):  0.21656893296416302   sin(angle)  -0.9762673287961469\n",
      "theta dot:  -2.68868394533\n",
      "Angle:  282.50777065387115\n",
      "action:  [-1.38864328]\n",
      "state:  {1: array([ 1.6076359]), 2: array([-3.62918093]), 3: array([ 0.03683124, -0.9993215 ,  0.        ])}\n",
      "cos(angle):  0.036831238346790966   sin(angle)  -0.9993214997596328\n",
      "theta dot:  -3.62918093328\n",
      "Angle:  272.11095743781544\n",
      "action:  [ 1.48684706]\n",
      "state:  {1: array([ 1.39985365]), 2: array([-4.155645]), 3: array([-0.17011136, -0.98542484,  0.        ])}\n",
      "cos(angle):  -0.17011136414429956   sin(angle)  -0.9854248443128302\n",
      "theta dot:  -4.15564499876\n",
      "Angle:  260.20593929773577\n",
      "action:  [-0.43985551]\n",
      "state:  {1: array([ 1.15181905]), 2: array([-4.96069196]), 3: array([-0.4068264 , -0.91350549,  0.        ])}\n",
      "cos(angle):  -0.40682640312283813   sin(angle)  -0.9135054886108425\n",
      "theta dot:  -4.96069195806\n",
      "Angle:  245.9946368968632\n",
      "action:  [-0.75499607]\n",
      "state:  {1: array([ 0.86386553]), 2: array([-5.75907048]), 3: array([-0.64950314, -0.76035891,  0.        ])}\n",
      "cos(angle):  -0.649503141381304   sin(angle)  -0.7603589082372993\n",
      "theta dot:  -5.75907048446\n",
      "Angle:  229.49615384359527\n",
      "action:  [-0.59005783]\n",
      "state:  {1: array([ 0.54297311]), 2: array([-6.41784834]), 3: array([-0.85617631, -0.51668378,  0.        ])}\n",
      "cos(angle):  -0.8561763108084568   sin(angle)  -0.5166837764149566\n",
      "theta dot:  -6.41784833993\n",
      "Angle:  211.11041566580315\n",
      "action:  [-0.42954343]\n",
      "state:  {1: array([ 0.19948347]), 2: array([-6.86979269]), 3: array([-0.98016906, -0.19816307,  0.        ])}\n",
      "cos(angle):  -0.9801690649671765   sin(angle)  -0.19816307446487333\n",
      "theta dot:  -6.86979268649\n",
      "Angle:  191.42995533400358\n",
      "action:  [-0.19389574]\n",
      "state:  {1: array([ 6.13029381]), 2: array([-7.04749935]), 3: array([-0.98833485,  0.15229653,  0.        ])}\n",
      "cos(angle):  -0.988334845705742   sin(angle)  0.15229652906027494\n",
      "theta dot:  -7.04749935305\n",
      "Angle:  171.23956225878317\n",
      "action:  [-0.01025828]\n",
      "state:  {1: array([ 5.78355303]), 2: array([-6.9348157]), 3: array([-0.8777588,  0.4791028,  0.       ])}\n",
      "cos(angle):  -0.87775879762564   sin(angle)  0.47910280023267515\n",
      "theta dot:  -6.93481569759\n",
      "Angle:  151.37282515721665\n",
      "action:  [ 0.41504301]\n",
      "state:  {1: array([ 5.45789142]), 2: array([-6.51323215]), 3: array([-0.67834106,  0.73474717,  0.        ])}\n",
      "cos(angle):  -0.6783410631720734   sin(angle)  0.7347471687693536\n",
      "theta dot:  -6.51323214545\n",
      "Angle:  132.71383314388208\n",
      "action:  [ 0.18065457]\n",
      "state:  {1: array([ 5.16113774]), 2: array([-5.93507358]), 3: array([-0.43383852,  0.90099064,  0.        ])}\n",
      "cos(angle):  -0.43383852014593904   sin(angle)  0.9009906428135542\n",
      "theta dot:  -5.93507358325\n",
      "Angle:  115.71113953259203\n",
      "action:  [ 0.21153535]\n",
      "state:  {1: array([ 4.89975773]), 2: array([-5.2276003]), 3: array([-0.18627434,  0.98249777,  0.        ])}\n",
      "cos(angle):  -0.18627434247442784   sin(angle)  0.9824977706517809\n",
      "theta dot:  -5.22760029924\n",
      "Angle:  100.73520284645087\n",
      "action:  [ 0.17481307]\n",
      "state:  {1: array([ 4.67653248]), 2: array([-4.46450501]), 3: array([ 0.03584882,  0.99935722,  0.        ])}\n",
      "cos(angle):  0.035848820770632464   sin(angle)  0.9993572244444702\n",
      "theta dot:  -4.46450501093\n",
      "Angle:  87.94536801758875\n",
      "action:  [-0.05449674]\n",
      "state:  {1: array([ 4.4903744]), 2: array([-3.7231616]), 3: array([ 0.22019521,  0.97545583,  0.        ])}\n",
      "cos(angle):  0.22019520610842877   sin(angle)  0.975455827399102\n",
      "theta dot:  -3.72316160304\n",
      "Angle:  77.27932064447003\n",
      "action:  [ 0.40870039]\n",
      "state:  {1: array([ 4.34386116]), 2: array([-2.93026467]), 3: array([ 0.36024248,  0.9328587 ,  0.        ])}\n",
      "cos(angle):  0.3602424845677292   sin(angle)  0.932858699006698\n",
      "theta dot:  -2.93026467326\n",
      "Angle:  68.88475034293572\n",
      "action:  [-0.02516188]\n",
      "state:  {1: array([ 4.23214142]), 2: array([-2.23439493]), 3: array([ 0.46199875,  0.88688058,  0.        ])}\n",
      "cos(angle):  0.4619987494053102   sin(angle)  0.8868805756965982\n",
      "theta dot:  -2.23439493061\n",
      "Angle:  62.483695346929494\n",
      "action:  [-0.11563487]\n",
      "state:  {1: array([ 4.15281243]), 2: array([-1.58657973]), 3: array([ 0.53082738,  0.84747997,  0.        ])}\n",
      "cos(angle):  0.5308273805010909   sin(angle)  0.8474799656100137\n",
      "theta dot:  -1.5865797286\n",
      "Angle:  57.93848986012085\n",
      "action:  [-0.14757737]\n",
      "state:  {1: array([ 4.10415711]), 2: array([-0.97310636]), 3: array([ 0.57141732,  0.82065964,  0.        ])}\n",
      "cos(angle):  0.5714173198740736   sin(angle)  0.8206596411106926\n",
      "theta dot:  -0.973106360595\n",
      "Angle:  55.15075200509199\n",
      "action:  [-0.25732287]\n",
      "state:  {1: array([ 4.08434661]), 2: array([-0.39621006]), 3: array([ 0.58756181,  0.80917929,  0.        ])}\n",
      "cos(angle):  0.5875618123398049   sin(angle)  0.8091792858693084\n",
      "theta dot:  -0.396210059651\n",
      "Angle:  54.01569644841504\n",
      "action:  [ 0.01531112]\n",
      "state:  {1: array([ 4.09499516]), 2: array([ 0.21297107]), 3: array([ 0.57891207,  0.81538997,  0.        ])}\n",
      "cos(angle):  0.5789120741547075   sin(angle)  0.8153899744281226\n",
      "theta dot:  0.212971072697\n",
      "Angle:  54.62581220289462\n",
      "action:  [-0.22812755]\n",
      "state:  {1: array([ 4.13450988]), 2: array([ 0.79029442]), 3: array([ 0.54624865,  0.83762307,  0.        ])}\n",
      "cos(angle):  0.5462486492186012   sin(angle)  0.8376230734804608\n",
      "theta dot:  0.790294420845\n",
      "Angle:  56.88983365298532\n",
      "action:  [-0.23510711]\n",
      "state:  {1: array([ 4.20367217]), 2: array([ 1.38324566]), 3: array([ 0.48705695,  0.87337021,  0.        ])}\n",
      "cos(angle):  0.48705695154536827   sin(angle)  0.8733702112800349\n",
      "theta dot:  1.38324566008\n",
      "Angle:  60.852531304088984\n",
      "action:  [-0.35215185]\n",
      "state:  {1: array([ 4.30294469]), 2: array([ 1.98545054]), 3: array([ 0.39809961,  0.91734219,  0.        ])}\n",
      "cos(angle):  0.39809961020407725   sin(angle)  0.9173421937071039\n",
      "theta dot:  1.98545054176\n",
      "Angle:  66.54041482709192\n",
      "Reset!\n",
      "min:  -1.69343660465  self.max:  27.8419661301\n",
      "state:  {1: array([ 4.40590247]), 2: array([-0.77673094]), 3: array([ 0.30171075,  0.95339951,  0.        ])}\n",
      "cos(angle):  0.3017107461861775   sin(angle)  0.9533995099829766\n",
      "theta dot:  -0.776730943035\n",
      "Angle:  72.43944708956599\n",
      "action:  [-0.29396252]\n",
      "state:  {1: array([ 4.40061369]), 2: array([-0.10577569]), 3: array([ 0.30674883,  0.9517905 ,  0.        ])}\n",
      "cos(angle):  0.3067488275882474   sin(angle)  0.951790500463855\n",
      "theta dot:  -0.105775689165\n",
      "Angle:  72.13642276995756\n",
      "action:  [-0.31545062]\n",
      "state:  {1: array([ 4.42865117]), 2: array([ 0.56074959]), 3: array([ 0.27994596,  0.96001576,  0.        ])}\n",
      "cos(angle):  0.2799459571394786   sin(angle)  0.9600157608504463\n",
      "theta dot:  0.56074959385\n",
      "Angle:  73.74284826799988\n",
      "action:  [-0.28515774]\n",
      "state:  {1: array([ 4.49055055]), 2: array([ 1.23798775]), 3: array([ 0.22002337,  0.9754946 ,  0.        ])}\n",
      "cos(angle):  0.2200233698455914   sin(angle)  0.9754946010725996\n",
      "theta dot:  1.23798775345\n",
      "Angle:  77.28941364266797\n",
      "action:  [-0.17419911]\n",
      "state:  {1: array([ 4.58772449]), 2: array([ 1.94347884]), 3: array([ 0.12434183,  0.99223944,  0.        ])}\n",
      "cos(angle):  0.12434182960826329   sin(angle)  0.992239441571272\n",
      "theta dot:  1.94347883827\n",
      "Angle:  82.85705737339867\n",
      "action:  [-0.21402127]\n",
      "state:  {1: array([ 4.72050226]), 2: array([ 2.65555523]), 3: array([-0.00811319,  0.99996709,  0.        ])}\n",
      "cos(angle):  -0.008113186995488955   sin(angle)  0.9999670875567737\n",
      "theta dot:  2.65555522886\n",
      "Angle:  90.46464492741552\n",
      "action:  [-0.45372863]\n",
      "state:  {1: array([ 4.88737582]), 2: array([ 3.33747125]), 3: array([-0.17409518,  0.98472883,  0.        ])}\n",
      "cos(angle):  -0.17409517709634914   sin(angle)  0.984728830344573\n",
      "theta dot:  3.33747124982\n",
      "Angle:  100.02577341239567\n",
      "action:  [-0.52533551]\n",
      "state:  {1: array([ 5.0872367]), 2: array([ 3.99721755]), 3: array([-0.36613082,  0.93056339,  0.        ])}\n",
      "cos(angle):  -0.3661308232509908   sin(angle)  0.930563388633763\n",
      "theta dot:  3.99721754651\n",
      "Angle:  111.47693139514598\n",
      "action:  [-0.76082416]\n",
      "state:  {1: array([ 5.31628752]), 2: array([ 4.58101646]), 3: array([-0.56785578,  0.82312807,  0.        ])}\n",
      "cos(angle):  -0.5678557774849664   sin(angle)  0.8231280677858971\n",
      "theta dot:  4.58101646457\n",
      "Angle:  124.6005461714293\n",
      "action:  [-0.31119438]\n",
      "state:  {1: array([ 5.57387169]), 2: array([ 5.15168336]), 3: array([-0.7588091 ,  0.65131309,  0.        ])}\n",
      "cos(angle):  -0.7588091030914694   sin(angle)  0.6513130929633764\n",
      "theta dot:  5.1516833583\n",
      "Angle:  139.3589973506713\n",
      "action:  [-0.43303426]\n",
      "state:  {1: array([ 5.85263234]), 2: array([ 5.57521304]), 3: array([-0.90873509,  0.41737337,  0.        ])}\n",
      "cos(angle):  -0.9087350945422222   sin(angle)  0.41737336755875837\n",
      "theta dot:  5.57521303843\n",
      "Angle:  155.33076885112496\n",
      "action:  [-0.09969775]\n",
      "state:  {1: array([ 6.14629676]), 2: array([ 5.8732884]), 3: array([-0.99064538,  0.13646143,  0.        ])}\n",
      "cos(angle):  -0.9906453840397221   sin(angle)  0.13646143440837605\n",
      "theta dot:  5.87328840216\n",
      "Angle:  172.15646137068745\n",
      "action:  [ 28.69546899]\n",
      "state:  {1: array([ 0.17689318]), 2: array([ 6.27563448]), 3: array([-0.98439516, -0.17597209,  0.        ])}\n",
      "cos(angle):  -0.9843951570507945   sin(angle)  -0.17597208521507499\n",
      "theta dot:  6.27563447797\n",
      "Angle:  190.13562963188824\n",
      "action:  [ 0.4210236]\n",
      "state:  {1: array([ 0.48723362]), 2: array([ 6.20680895]), 3: array([-0.88363141, -0.46818323,  0.        ])}\n",
      "cos(angle):  -0.8836314090185329   sin(angle)  -0.46818322588055433\n",
      "theta dot:  6.20680895465\n",
      "Angle:  207.91678591907885\n",
      "action:  [ 0.48439792]\n",
      "state:  {1: array([ 0.78365018]), 2: array([ 5.92833122]), 3: array([-0.70834171, -0.70586969,  0.        ])}\n",
      "cos(angle):  -0.708341707682186   sin(angle)  -0.7058696941772502\n",
      "theta dot:  5.92833122328\n",
      "Angle:  224.9001641370317\n",
      "action:  [ 0.5796596]\n",
      "state:  {1: array([ 1.05794408]), 2: array([ 5.48587789]), 3: array([-0.49066454, -0.87134856,  0.        ])}\n",
      "cos(angle):  -0.49066454079276783   sin(angle)  -0.8713485573572852\n",
      "theta dot:  5.48587789331\n",
      "Angle:  240.61600989708427\n",
      "action:  [ 0.40343522]\n",
      "state:  {1: array([ 1.30258817]), 2: array([ 4.89288176]), 3: array([-0.26500409, -0.96424729,  0.        ])}\n",
      "cos(angle):  -0.2650040854029997   sin(angle)  -0.9642472891949034\n",
      "theta dot:  4.89288175869\n",
      "Angle:  254.63305084060775\n",
      "action:  [ 0.42800049]\n",
      "state:  {1: array([ 1.51428299]), 2: array([ 4.23389637]), 3: array([-0.05648326, -0.99840355,  0.        ])}\n",
      "cos(angle):  -0.05648326407639479   sin(angle)  -0.9984035461071221\n",
      "theta dot:  4.23389636549\n",
      "Angle:  266.7622421091886\n",
      "action:  [ 0.25967849]\n",
      "state:  {1: array([ 1.69048526]), 2: array([ 3.52404548]), 3: array([ 0.11940337, -0.99284583,  0.        ])}\n",
      "cos(angle):  0.11940337151064233   sin(angle)  -0.9928458263355351\n",
      "theta dot:  3.52404547877\n",
      "Angle:  276.8578651385238\n",
      "action:  [ 0.08872957]\n",
      "state:  {1: array([ 1.83012129]), 2: array([ 2.79272054]), 3: array([ 0.25642814, -0.9665633 ,  0.        ])}\n",
      "cos(angle):  0.25642814170788236   sin(angle)  -0.9665632975342289\n",
      "theta dot:  2.79272054451\n",
      "Angle:  284.858401457784\n",
      "action:  [-0.04215442]\n",
      "state:  {1: array([ 1.93319503]), 2: array([ 2.06147491]), 3: array([ 0.35451816, -0.93504913,  0.        ])}\n",
      "cos(angle):  0.35451815864394143   sin(angle)  -0.935049129827791\n",
      "theta dot:  2.06147490777\n",
      "Angle:  290.7640782371142\n",
      "action:  [-0.11358107]\n",
      "state:  {1: array([ 2.00035258]), 2: array([ 1.3431509]), 3: array([ 0.41646741, -0.90915065,  0.        ])}\n",
      "cos(angle):  0.4164674084404241   sin(angle)  -0.9091506463215636\n",
      "theta dot:  1.34315090052\n",
      "Angle:  294.6119131316448\n",
      "action:  [-0.10382224]\n",
      "state:  {1: array([ 2.03263831]), 2: array([ 0.64571458]), 3: array([ 0.44559786, -0.89523323,  0.        ])}\n",
      "cos(angle):  0.44559786334815626   sin(angle)  -0.8952332345146475\n",
      "theta dot:  0.645714579576\n",
      "Angle:  296.4617448149235\n",
      "action:  [-0.09201637]\n",
      "state:  {1: array([ 2.03066267]), 2: array([-0.0395128]), 3: array([ 0.44382834, -0.89611183,  0.        ])}\n",
      "cos(angle):  0.44382833619162393   sin(angle)  -0.896111827839235\n",
      "theta dot:  -0.0395128022659\n",
      "Angle:  296.3485492392954\n",
      "action:  [-0.24404713]\n",
      "state:  {1: array([ 1.99325248]), 2: array([-0.74820374]), 3: array([ 0.41000191, -0.91208467,  0.        ])}\n",
      "cos(angle):  0.4100019064614731   sin(angle)  -0.9120846653123588\n",
      "theta dot:  -0.748203742161\n",
      "Angle:  294.20510841950613\n",
      "action:  [-0.36933166]\n",
      "state:  {1: array([ 1.91886913]), 2: array([-1.48766699]), 3: array([ 0.34108682, -0.9400318 ,  0.        ])}\n",
      "cos(angle):  0.34108681544092534   sin(angle)  -0.9400317996389101\n",
      "theta dot:  -1.48766699001\n",
      "Angle:  289.9432663931305\n",
      "action:  [-0.37878909]\n",
      "state:  {1: array([ 1.80639367]), 2: array([-2.2495092]), 3: array([ 0.23342387, -0.97237508,  0.        ])}\n",
      "cos(angle):  0.2334238683265586   sin(angle)  -0.9723750807664013\n",
      "theta dot:  -2.24950920321\n",
      "Angle:  283.4989122968309\n",
      "action:  [-0.35373737]\n",
      "state:  {1: array([ 1.65480111]), 2: array([-3.03185112]), 3: array([ 0.08390602, -0.99647367,  0.        ])}\n",
      "cos(angle):  0.08390602078475161   sin(angle)  -0.996473672344668\n",
      "theta dot:  -3.03185111861\n",
      "Angle:  274.8133189471148\n",
      "action:  [-0.3594276]\n",
      "state:  {1: array([ 1.46314509]), 2: array([-3.83312051]), 3: array([-0.10744343, -0.9942112 ,  0.        ])}\n",
      "cos(angle):  -0.10744343465128411   sin(angle)  -0.9942111990670469\n",
      "theta dot:  -3.83312051218\n",
      "Angle:  263.83226323994336\n",
      "action:  [-0.18017633]\n",
      "state:  {1: array([ 1.23285482]), 2: array([-4.60580536]), 3: array([-0.33154573, -0.94343915,  0.        ])}\n",
      "cos(angle):  -0.3315457329304219   sin(angle)  -0.9434391485282076\n",
      "theta dot:  -4.60580536115\n",
      "Angle:  250.63763367210763\n",
      "action:  [-0.30181415]\n",
      "state:  {1: array([ 0.96492198]), 2: array([-5.35865684]), 3: array([-0.56948101, -0.82200449,  0.        ])}\n",
      "cos(angle):  -0.5694810127028811   sin(angle)  -0.8220044867097144\n",
      "theta dot:  -5.35865684437\n",
      "Angle:  235.28624851827246\n",
      "action:  [-0.45732464]\n",
      "state:  {1: array([ 0.66273403]), 2: array([-6.04375891]), 3: array([-0.788313  , -0.61527442,  0.        ])}\n",
      "cos(angle):  -0.7883129996558479   sin(angle)  -0.6152744221675392\n",
      "theta dot:  -6.0437589058\n",
      "Angle:  217.97219512121745\n",
      "action:  [-0.41135953]\n",
      "state:  {1: array([ 0.3343881]), 2: array([-6.56691865]), 3: array([-0.94461131, -0.32819122,  0.        ])}\n",
      "cos(angle):  -0.9446113067159011   sin(angle)  -0.32819122356406466\n",
      "theta dot:  -6.56691865149\n",
      "Angle:  199.15940295689762\n",
      "action:  [-0.34781974]\n",
      "state:  {1: array([ 6.27431166]), 2: array([-6.86523503]), 3: array([-0.99996063,  0.00887354,  0.        ])}\n",
      "cos(angle):  -0.9999606294114443   sin(angle)  0.008873535206910593\n",
      "theta dot:  -6.8652350305\n",
      "Angle:  179.49115748255997\n",
      "action:  [-0.4944769]\n",
      "state:  {1: array([ 5.92767408]), 2: array([-6.93275141]), 3: array([-0.93746867,  0.34806967,  0.        ])}\n",
      "cos(angle):  -0.9374686703361925   sin(angle)  0.34806966563906583\n",
      "theta dot:  -6.93275141386\n",
      "Angle:  159.63033410441903\n",
      "action:  [ 0.13252529]\n",
      "state:  {1: array([ 5.59508307]), 2: array([-6.65182037]), 3: array([-0.77245262,  0.6350724 ,  0.        ])}\n",
      "cos(angle):  -0.7724526196678921   sin(angle)  0.6350723977376207\n",
      "theta dot:  -6.65182037147\n",
      "Angle:  140.5743169974694\n",
      "action:  [-0.72613468]\n",
      "state:  {1: array([ 5.28086125]), 2: array([-6.28443628]), 3: array([-0.53834522,  0.8427244 ,  0.        ])}\n",
      "cos(angle):  -0.5383452238889425   sin(angle)  0.8427244032991832\n",
      "theta dot:  -6.28443627533\n",
      "Angle:  122.57077533782791\n",
      "action:  [-0.72627048]\n",
      "state:  {1: array([ 4.99279458]), 2: array([-5.76133354]), 3: array([-0.27674542,  0.96094327,  0.        ])}\n",
      "cos(angle):  -0.2767454249638231   sin(angle)  0.9609432708342325\n",
      "theta dot:  -5.76133354492\n",
      "Angle:  106.06580910907621\n",
      "action:  [-0.37925533]\n",
      "state:  {1: array([ 4.73791886]), 2: array([-5.09751439]), 3: array([-0.0255271 ,  0.99967413,  0.        ])}\n",
      "cos(angle):  -0.025527102089462343   sin(angle)  0.99967413043397\n",
      "theta dot:  -5.09751439139\n",
      "Angle:  91.46254022618808\n",
      "action:  [-0.5008082]\n",
      "state:  {1: array([ 4.51677485]), 2: array([-4.42288002]), 3: array([ 0.19436899,  0.98092849,  0.        ])}\n",
      "cos(angle):  0.19436898514633108   sin(angle)  0.9809284875123085\n",
      "theta dot:  -4.42288002328\n",
      "Angle:  78.79195192419547\n",
      "action:  [-0.07956064]\n",
      "state:  {1: array([ 4.33181897]), 2: array([-3.69911775]), 3: array([ 0.37144976,  0.92845306,  0.        ])}\n",
      "cos(angle):  0.3714497605275477   sin(angle)  0.9284530550351092\n",
      "theta dot:  -3.69911775293\n",
      "Angle:  68.19478494673064\n",
      "action:  [ 0.27374125]\n",
      "state:  {1: array([ 4.18373313]), 2: array([-2.96171677]), 3: array([ 0.50437314,  0.8634858 ,  0.        ])}\n",
      "cos(angle):  0.5043731446509404   sin(angle)  0.8634858024049507\n",
      "theta dot:  -2.96171677361\n",
      "Angle:  59.710111225541084\n",
      "action:  [ 0.06489703]\n",
      "state:  {1: array([ 4.06851473]), 2: array([-2.30436787]), 3: array([ 0.60029847,  0.79977606,  0.        ])}\n",
      "cos(angle):  0.6002984676286287   sin(angle)  0.7997760622591303\n",
      "theta dot:  -2.30436786688\n",
      "Angle:  53.10859900184465\n",
      "action:  [ 0.28508255]\n",
      "state:  {1: array([ 3.98542606]), 2: array([-1.66177344]), 3: array([ 0.66460341,  0.7471963 ,  0.        ])}\n",
      "cos(angle):  0.6646034067172624   sin(angle)  0.7471963007000297\n",
      "theta dot:  -1.66177343709\n",
      "Angle:  48.347979911624066\n",
      "action:  [-0.32744445]\n",
      "state:  {1: array([ 3.92790142]), 2: array([-1.15049288]), 3: array([ 0.7064626 ,  0.70775038,  0.        ])}\n",
      "cos(angle):  0.7064625952519881   sin(angle)  0.7077503807910142\n",
      "theta dot:  -1.1504928796\n",
      "Angle:  45.052068300839494\n",
      "action:  [ 0.16230355]\n",
      "state:  {1: array([ 3.89813469]), 2: array([-0.59533456]), 3: array([ 0.72721394,  0.68641088,  0.        ])}\n",
      "cos(angle):  0.7272139370091075   sin(angle)  0.6864108753652682\n",
      "theta dot:  -0.59533456124\n",
      "Angle:  43.34656440118346\n",
      "action:  [ 0.28942282]\n",
      "state:  {1: array([ 3.89627904]), 2: array([-0.03711298]), 3: array([ 0.72848642,  0.68506024,  0.        ])}\n",
      "cos(angle):  0.7284864219326972   sin(angle)  0.6850602404604258\n",
      "theta dot:  -0.0371129816374\n",
      "Angle:  43.24024378915885\n",
      "action:  [ 0.33792631]\n",
      "state:  {1: array([ 3.9226476]), 2: array([ 0.52737114]), 3: array([ 0.71017122,  0.704029  ,  0.        ])}\n",
      "cos(angle):  0.7101712211067503   sin(angle)  0.7040290027489969\n",
      "theta dot:  0.527371144473\n",
      "Angle:  44.751047297006494\n",
      "action:  [ 0.77019828]\n",
      "state:  {1: array([ 3.98119373]), 2: array([ 1.17092264]), 3: array([ 0.66775983,  0.74437679,  0.        ])}\n",
      "cos(angle):  0.667759827663507   sin(angle)  0.7443767947476624\n",
      "theta dot:  1.17092263899\n",
      "Angle:  48.10548572038766\n",
      "action:  [ 0.25221202]\n",
      "state:  {1: array([ 4.06954558]), 2: array([ 1.76703704]), 3: array([ 0.5994737 ,  0.80039445,  0.        ])}\n",
      "cos(angle):  0.5994737011864112   sin(angle)  0.8003944537450678\n",
      "theta dot:  1.76703703814\n",
      "Angle:  53.16766210925954\n",
      "action:  [ 0.14747236]\n",
      "state:  {1: array([ 4.18901827]), 2: array([ 2.38945373]), 3: array([ 0.49980248,  0.86613941,  0.        ])}\n",
      "cos(angle):  0.49980247713711934   sin(angle)  0.8661394136324703\n",
      "theta dot:  2.38945373185\n",
      "Angle:  60.0129268109057\n",
      "action:  [-0.00586635]\n",
      "state:  {1: array([ 4.34092719]), 2: array([ 3.03817834]), 3: array([ 0.36297792,  0.93179774,  0.        ])}\n",
      "cos(angle):  0.36297791552805564   sin(angle)  0.9317977424521416\n",
      "theta dot:  3.03817833955\n",
      "Angle:  68.71664627103996\n",
      "action:  [-0.08968316]\n",
      "state:  {1: array([ 4.5271059]), 2: array([ 3.72357417]), 3: array([ 0.18422478,  0.98288414,  0.        ])}\n",
      "cos(angle):  0.18422478090313732   sin(angle)  0.9828841386965155\n",
      "theta dot:  3.72357417265\n",
      "Angle:  79.38387556626788\n",
      "action:  [-0.0976616]\n",
      "state:  {1: array([ 4.7494103]), 2: array([ 4.44608804]), 3: array([-0.03701286,  0.99931479,  0.        ])}\n",
      "cos(angle):  -0.037012860383728335   sin(angle)  0.9993147893262736\n",
      "theta dot:  4.44608803631\n",
      "Angle:  92.12094977265343\n",
      "action:  [-0.07954106]\n",
      "state:  {1: array([ 5.00859245]), 2: array([ 5.18364297]), 3: array([-0.29189112,  0.95645155,  0.        ])}\n",
      "cos(angle):  -0.2918911172494596   sin(angle)  0.9564515542728038\n",
      "theta dot:  5.18364296978\n",
      "Angle:  106.9709582804253\n",
      "action:  [-0.05965923]\n",
      "state:  {1: array([ 5.30319408]), 2: array([ 5.89203275]), 3: array([-0.55702984,  0.83049248,  0.        ])}\n",
      "cos(angle):  -0.5570298350380856   sin(angle)  0.830492482131803\n",
      "theta dot:  5.89203275106\n",
      "Angle:  123.85034927849\n",
      "action:  [-0.03399574]\n",
      "state:  {1: array([ 5.62868422]), 2: array([ 6.50980275]), 3: array([-0.79335175,  0.60876351,  0.        ])}\n",
      "cos(angle):  -0.7933517470785432   sin(angle)  0.6087635053182995\n",
      "theta dot:  6.50980275102\n",
      "Angle:  142.49951682343453\n",
      "action:  [-0.01921042]\n",
      "state:  {1: array([ 5.97685891]), 2: array([ 6.96349382]), 3: array([-0.95344781,  0.30155809,  0.        ])}\n",
      "cos(angle):  -0.9534478060156307   sin(angle)  0.3015580892696797\n",
      "theta dot:  6.9634938167\n",
      "Angle:  162.44841049237436\n",
      "action:  [-0.33091787]\n",
      "state:  {1: array([ 0.05067484]), 2: array([ 7.1400247]), 3: array([-0.99871631, -0.05065315,  0.        ])}\n",
      "cos(angle):  -0.9987163050679616   sin(angle)  -0.05065315381492306\n",
      "theta dot:  7.14002470291\n",
      "Angle:  182.9038685500113\n",
      "action:  [-0.27564411]\n",
      "state:  {1: array([ 0.40370925]), 2: array([ 7.06068822]), 3: array([-0.91961021, -0.3928321 ,  0.        ])}\n",
      "cos(angle):  -0.9196102109436816   sin(angle)  -0.3928321014480838\n",
      "theta dot:  7.06068822153\n",
      "Angle:  203.1312030272763\n",
      "action:  [-0.10952053]\n",
      "state:  {1: array([ 0.74119105]), 2: array([ 6.74963607]), 3: array([-0.73766492, -0.67516699,  0.        ])}\n",
      "cos(angle):  -0.737664922003178   sin(angle)  -0.6751669888598267\n",
      "theta dot:  6.74963606564\n",
      "Angle:  222.46744080126416\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "action:  [-0.21393482]\n",
      "state:  {1: array([ 1.05174958]), 2: array([ 6.2111706]), 3: array([-0.49605266, -0.86829244,  0.        ])}\n",
      "cos(angle):  -0.4960526575077903   sin(angle)  -0.8682924397802038\n",
      "theta dot:  6.21117060165\n",
      "Angle:  240.26109225749468\n",
      "action:  [ 0.04036461]\n",
      "state:  {1: array([ 1.33004988]), 2: array([ 5.56600596]), 3: array([-0.23842761, -0.97116027,  0.        ])}\n",
      "cos(angle):  -0.23842761036867194   sin(angle)  -0.9711602723618202\n",
      "theta dot:  5.56600596306\n",
      "Angle:  256.20648749161006\n",
      "action:  [-0.36430203]\n",
      "state:  {1: array([ 1.5691994]), 2: array([ 4.78299045]), 3: array([-0.00159692, -0.99999872,  0.        ])}\n",
      "cos(angle):  -0.001596921437836507   sin(angle)  -0.9999987249201477\n",
      "theta dot:  4.7829904547\n",
      "Angle:  269.908713775126\n",
      "action:  [ 0.37490026]\n",
      "state:  {1: array([ 1.77366073]), 2: array([ 4.08922645]), 3: array([ 0.20147581, -0.97949349,  0.        ])}\n",
      "cos(angle):  0.20147581498864744   sin(angle)  -0.9794934895009054\n",
      "theta dot:  4.0892264494\n",
      "Angle:  281.6234572321526\n",
      "action:  [ 0.00830412]\n",
      "state:  {1: array([ 1.94145332]), 2: array([ 3.35585195]), 3: array([ 0.36222789, -0.93208956,  0.        ])}\n",
      "cos(angle):  0.3622278909422101   sin(angle)  -0.9320895638422084\n",
      "theta dot:  3.35585195003\n",
      "Angle:  291.23724242132306\n",
      "action:  [-0.01652651]\n",
      "state:  {1: array([ 2.07416861]), 2: array([ 2.6543058]), 3: array([ 0.48238227, -0.87596081,  0.        ])}\n",
      "cos(angle):  0.4823822679797173   sin(angle)  -0.8759608139287649\n",
      "theta dot:  2.65430580134\n",
      "Angle:  298.84125063753135\n",
      "action:  [ 0.0006515]\n",
      "state:  {1: array([ 2.17404026]), 2: array([ 1.99743292]), 3: array([ 0.56731683, -0.82349961,  0.        ])}\n",
      "cos(angle):  0.567316831807318   sin(angle)  -0.8234996128402898\n",
      "theta dot:  1.99743291523\n",
      "Angle:  304.56346105168745\n",
      "action:  [ 0.07355324]\n",
      "state:  {1: array([ 2.24358232]), 2: array([ 1.39084119]), 3: array([ 0.6231673 , -0.78208856,  0.        ])}\n",
      "cos(angle):  0.6231672961484012   sin(angle)  -0.7820885634064028\n",
      "theta dot:  1.39084119226\n",
      "Angle:  308.5479182487611\n",
      "action:  [ 0.12246877]\n",
      "state:  {1: array([ 2.28471457]), 2: array([ 0.82264509]), 3: array([ 0.65480021, -0.75580202,  0.        ])}\n",
      "cos(angle):  0.6548002093162135   sin(angle)  -0.7558020150009147\n",
      "theta dot:  0.822645085632\n",
      "Angle:  310.90461730996856\n",
      "action:  [ 0.15713825]\n",
      "state:  {1: array([ 2.29868279]), 2: array([ 0.27936431]), 3: array([ 0.66529319, -0.74658219,  0.        ])}\n",
      "cos(angle):  0.6652931931406297   sin(angle)  -0.7465821904926107\n",
      "theta dot:  0.279364311137\n",
      "Angle:  311.7049352372139\n",
      "action:  [ 0.15140337]\n",
      "state:  {1: array([ 2.2857897]), 2: array([-0.25786183]), 3: array([ 0.65561241, -0.75509759,  0.        ])}\n",
      "cos(angle):  0.6556124117993777   sin(angle)  -0.7550975867360479\n",
      "theta dot:  -0.257861826022\n",
      "Angle:  310.96621724822876\n",
      "action:  [ 0.07074228]\n",
      "state:  {1: array([ 2.24511101]), 2: array([-0.81357367]), 3: array([ 0.62436214, -0.78113502,  0.        ])}\n",
      "cos(angle):  0.6243621422635707   sin(angle)  -0.7811350173357002\n",
      "theta dot:  -0.813573674673\n",
      "Angle:  308.63550580436123\n",
      "action:  [-0.05700803]\n",
      "state:  {1: array([ 2.17471221]), 2: array([-1.40797614]), 3: array([ 0.56787005, -0.82311822,  0.        ])}\n",
      "cos(angle):  0.5678700520277759   sin(angle)  -0.8231182199477614\n",
      "theta dot:  -1.40797614243\n",
      "Angle:  304.60196070573966\n",
      "action:  [-0.19035337]\n",
      "state:  {1: array([ 2.07201882]), 2: array([-2.05386781]), 3: array([ 0.48049802, -0.87699581,  0.        ])}\n",
      "cos(angle):  0.48049801631148836   sin(angle)  -0.876995813171719\n",
      "theta dot:  -2.0538678124\n",
      "Angle:  298.71807659839556\n",
      "action:  [-0.3408005]\n",
      "state:  {1: array([ 1.93388208]), 2: array([-2.76273475]), 3: array([ 0.3551605 , -0.93480534,  0.        ])}\n",
      "cos(angle):  0.35516049796003424   sin(angle)  -0.9348053383933901\n",
      "theta dot:  -2.76273474738\n",
      "Angle:  290.80344305928963\n",
      "action:  [-0.23260703]\n",
      "state:  {1: array([ 1.75894559]), 2: array([-3.49872981]), 3: array([ 0.18704114, -0.98235208,  0.        ])}\n",
      "cos(angle):  0.18704114072245556   sin(angle)  -0.9823520813217849\n",
      "theta dot:  -3.49872980544\n",
      "Angle:  280.78034392224345\n",
      "action:  [-0.19276284]\n",
      "state:  {1: array([ 1.54572517]), 2: array([-4.26440829]), 3: array([-0.02506853, -0.99968574,  0.        ])}\n",
      "cos(angle):  -0.02506852553631015   sin(angle)  -0.9996857351325142\n",
      "theta dot:  -4.26440829273\n",
      "Angle:  268.5637426252685\n",
      "action:  [-1.71294573]\n",
      "state:  {1: array([ 1.28216945]), 2: array([-5.27111445]), 3: array([-0.28463617, -0.95863562,  0.        ])}\n",
      "cos(angle):  -0.28463616696594635   sin(angle)  -0.95863562027234\n",
      "theta dot:  -5.27111445425\n",
      "Angle:  253.46314735907396\n",
      "action:  [ 2.0507992]\n",
      "state:  {1: array([ 0.99766489]), 2: array([-5.69009117]), 3: array([-0.54226576, -0.84020703,  0.        ])}\n",
      "cos(angle):  -0.542265755229891   sin(angle)  -0.8402070284786696\n",
      "theta dot:  -5.69009116946\n",
      "Angle:  237.16227502487635\n",
      "action:  [ 0.06666349]\n",
      "state:  {1: array([ 0.68215255]), 2: array([-6.31024692]), 3: array([-0.77621741, -0.63046533,  0.        ])}\n",
      "cos(angle):  -0.7762174113296018   sin(angle)  -0.630465328427164\n",
      "theta dot:  -6.31024691782\n",
      "Angle:  219.08479149408385\n",
      "action:  [-0.12968862]\n",
      "state:  {1: array([ 0.34202509]), 2: array([-6.80254921]), 3: array([-0.94207739, -0.33539557,  0.        ])}\n",
      "cos(angle):  -0.9420773923654849   sin(angle)  -0.33539556764192346\n",
      "theta dot:  -6.80254920751\n",
      "Angle:  199.59696908907932\n",
      "action:  [-0.05291405]\n",
      "state:  {1: array([ 6.27210875]), 2: array([-7.06203299]), 3: array([-0.99993866,  0.01107634,  0.        ])}\n",
      "cos(angle):  -0.9999386555130262   sin(angle)  0.011076335621562901\n",
      "theta dot:  -7.06203299085\n",
      "Angle:  179.36494030588028\n",
      "action:  [ 0.06728312]\n",
      "state:  {1: array([ 5.91992708]), 2: array([-7.04363327]), 3: array([-0.93474407,  0.35532172,  0.        ])}\n",
      "cos(angle):  -0.9347440689981942   sin(angle)  0.3553217210820345\n",
      "theta dot:  -7.04363327154\n",
      "Angle:  159.18646454709017\n",
      "action:  [ 0.20939625]\n",
      "state:  {1: array([ 5.58264045]), 2: array([-6.74573254]), 3: array([-0.76449107,  0.64463432,  0.        ])}\n",
      "cos(angle):  -0.7644910699076245   sin(angle)  0.644634318068388\n",
      "theta dot:  -6.74573254385\n",
      "Angle:  139.86140951313155\n",
      "action:  [ 0.31181875]\n",
      "state:  {1: array([ 5.27186625]), 2: array([-6.21548399]), 3: array([-0.53074324,  0.84753266,  0.        ])}\n",
      "cos(angle):  -0.530743243348137   sin(angle)  0.8475326599254451\n",
      "theta dot:  -6.21548399217\n",
      "Angle:  122.05540113220405\n",
      "action:  [ 0.62299005]\n",
      "state:  {1: array([ 4.99754696]), 2: array([-5.48638599]), 3: array([-0.28130905,  0.95961722,  0.        ])}\n",
      "cos(angle):  -0.28130905014206964   sin(angle)  0.9596172248913452\n",
      "theta dot:  -5.48638599002\n",
      "Angle:  106.33809978568729\n",
      "action:  [ 0.04891988]\n",
      "state:  {1: array([ 4.7595802]), 2: array([-4.75933509]), 3: array([-0.04717371,  0.9988867 ,  0.        ])}\n",
      "cos(angle):  -0.04717370641086903   sin(angle)  0.9988867009943926\n",
      "theta dot:  -4.75933508881\n",
      "Angle:  92.70364097510293\n",
      "action:  [-0.16725821]\n",
      "state:  {1: array([ 4.55781726]), 2: array([-4.0352588]), 3: array([ 0.15395694,  0.98807756,  0.        ])}\n",
      "cos(angle):  0.15395693897518978   sin(angle)  0.9880775581609926\n",
      "theta dot:  -4.03525879516\n",
      "Angle:  81.1435030974431\n",
      "action:  [-0.31598845]\n",
      "state:  {1: array([ 4.39073732]), 2: array([-3.34159889]), 3: array([ 0.31613395,  0.94871457,  0.        ])}\n",
      "cos(angle):  0.3161339489295243   sin(angle)  0.9487145652588164\n",
      "theta dot:  -3.34159889465\n",
      "Angle:  71.57054980872726\n",
      "action:  [-0.43682035]\n",
      "state:  {1: array([ 4.25595802]), 2: array([-2.69558602]), 3: array([ 0.44074724,  0.89763125,  0.        ])}\n",
      "cos(angle):  0.44074724380397845   sin(angle)  0.897631253399299\n",
      "theta dot:  -2.69558602356\n",
      "Angle:  63.84828274352276\n",
      "action:  [ 0.20327077]\n",
      "state:  {1: array([ 4.15636442]), 2: array([-1.99187197]), 3: array([ 0.5278138 ,  0.84936011,  0.        ])}\n",
      "cos(angle):  0.5278138006243602   sin(angle)  0.8493601072987053\n",
      "theta dot:  -1.99187196796\n",
      "Angle:  58.14200323255443\n",
      "action:  [ 0.04536771]\n",
      "state:  {1: array([ 4.08896208]), 2: array([-1.34804673]), 3: array([ 0.58382082,  0.81188253,  0.        ])}\n",
      "cos(angle):  0.5838208236573716   sin(angle)  0.8118825320599208\n",
      "theta dot:  -1.34804673142\n",
      "Angle:  54.28014284843442\n",
      "action:  [ 0.19429343]\n",
      "state:  {1: array([ 4.05346254]), 2: array([-0.70999082]), 3: array([ 0.6122684 ,  0.79064999,  0.        ])}\n",
      "cos(angle):  0.6122683956835909   sin(angle)  0.7906499931366862\n",
      "theta dot:  -0.709990817803\n",
      "Angle:  52.246173737080085\n",
      "action:  [ 0.27787025]\n",
      "state:  {1: array([ 4.0496964]), 2: array([-0.07532278]), 3: array([ 0.61524174,  0.7883385 ,  0.        ])}\n",
      "cos(angle):  0.6152417444721637   sin(angle)  0.7883385033466581\n",
      "theta dot:  -0.0753227849536\n",
      "Angle:  52.0303903577248\n",
      "action:  [ 0.14234392]\n",
      "state:  {1: array([ 4.07656053]), 2: array([ 0.53728268]), 3: array([ 0.59384427,  0.80458   ,  0.        ])}\n",
      "cos(angle):  0.5938442694404201   sin(angle)  0.80458000450718\n",
      "theta dot:  0.537282681156\n",
      "Angle:  53.56958826019752\n",
      "action:  [ 0.37814433]\n",
      "state:  {1: array([ 4.1364325]), 2: array([ 1.19743933]), 3: array([ 0.54463721,  0.83867175,  0.        ])}\n",
      "cos(angle):  0.5446372121788263   sin(angle)  0.8386717517062776\n",
      "theta dot:  1.19743933367\n",
      "Angle:  56.99999124053909\n",
      "action:  [ 0.1909994]\n",
      "state:  {1: array([ 4.22918715]), 2: array([ 1.85509306]), 3: array([ 0.46461681,  0.88551184,  0.        ])}\n",
      "cos(angle):  0.4646168078058211   sin(angle)  0.8855118417640324\n",
      "theta dot:  1.85509305733\n",
      "Angle:  62.314428952513765\n",
      "action:  [ 0.49513619]\n",
      "state:  {1: array([ 4.35886202]), 2: array([ 2.59349737]), 3: array([ 0.3462088 ,  0.93815749,  0.        ])}\n",
      "cos(angle):  0.3462087964011736   sin(angle)  0.9381574864032428\n",
      "theta dot:  2.59349736693\n",
      "Angle:  69.744234243579\n",
      "action:  [ 0.73204789]\n",
      "state:  {1: array([ 4.52920816]), 2: array([ 3.40692266]), 3: array([ 0.1821581 ,  0.98326925,  0.        ])}\n",
      "cos(angle):  0.1821580973088736   sin(angle)  0.9832692548762068\n",
      "theta dot:  3.40692266471\n",
      "Angle:  79.50432591100548\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "3\n",
      "1\n",
      "Reset!\n",
      "min:  -1.71294573449  self.max:  28.6954689906\n",
      "state:  {1: array([ 5.22606273]), 2: array([-0.43794659]), 3: array([-0.49138019,  0.87094518,  0.        ])}\n",
      "cos(angle):  -0.4913801871654936   sin(angle)  0.8709451829255412\n",
      "theta dot:  -0.437946593191\n",
      "Angle:  119.43105842169945\n",
      "action:  [ 0.20333771]\n",
      "state:  {1: array([ 5.23835087]), 2: array([ 0.24576295]), 3: array([-0.50204512,  0.86484143,  0.        ])}\n",
      "cos(angle):  -0.502045122308997   sin(angle)  0.8648414277575655\n",
      "theta dot:  0.245762949855\n",
      "Angle:  120.13511576467567\n",
      "action:  [ 0.14282079]\n",
      "state:  {1: array([ 5.28414173]), 2: array([ 0.91581714]), 3: array([-0.54110686,  0.84095384,  0.        ])}\n",
      "cos(angle):  -0.5411068615396598   sin(angle)  0.8409538420119735\n",
      "theta dot:  0.91581713909\n",
      "Angle:  122.75873247329764\n",
      "action:  [ 0.0427599]\n",
      "state:  {1: array([ 5.36178906]), 2: array([ 1.5529465]), 3: array([-0.60470871,  0.79644672,  0.        ])}\n",
      "cos(angle):  -0.6047087069904151   sin(angle)  0.7964467211872871\n",
      "theta dot:  1.55294650492\n",
      "Angle:  127.20758609701721\n",
      "action:  [-0.09645471]\n",
      "state:  {1: array([ 5.46857972]), 2: array([ 2.13581334]), 3: array([-0.68615537,  0.72745502,  0.        ])}\n",
      "cos(angle):  -0.6861553667455009   sin(angle)  0.7274550245110327\n",
      "theta dot:  2.13581333888\n",
      "Angle:  133.32622629624348\n",
      "action:  [-0.34800113]\n",
      "state:  {1: array([ 5.60003995]), 2: array([ 2.62920444]), 3: array([-0.77559109,  0.63123566,  0.        ])}\n",
      "cos(angle):  -0.7755910938939299   sin(angle)  0.6312356573201622\n",
      "theta dot:  2.62920443752\n",
      "Angle:  140.8583245702739\n",
      "action:  [-0.60915471]\n",
      "state:  {1: array([ 5.75060284]), 2: array([ 3.01125797]), 3: array([-0.86149867,  0.50775982,  0.        ])}\n",
      "cos(angle):  -0.8614986735220799   sin(angle)  0.507759820702364\n",
      "theta dot:  3.01125797327\n",
      "Angle:  149.4849230421995\n",
      "action:  [-0.74877613]\n",
      "state:  {1: array([ 5.91459091]), 2: array([ 3.27976142]), 3: array([-0.93283471,  0.36030459,  0.        ])}\n",
      "cos(angle):  -0.9328347138015876   sin(angle)  0.3603045888227211\n",
      "theta dot:  3.27976141932\n",
      "Angle:  158.8807254275681\n",
      "action:  [-0.86788524]\n",
      "state:  {1: array([ 6.08558127]), 2: array([ 3.41980708]), 3: array([-0.98053977,  0.19632056,  0.        ])}\n",
      "cos(angle):  -0.980539768318111   sin(angle)  0.1963205611918052\n",
      "theta dot:  3.41980707551\n",
      "Angle:  168.67772812670512\n",
      "action:  [-0.80878211]\n",
      "state:  {1: array([ 6.25786778]), 2: array([ 3.44573018]), 3: array([-0.99967953,  0.02531483,  0.        ])}\n",
      "cos(angle):  -0.9996795284636195   sin(angle)  0.025314825118008585\n",
      "theta dot:  3.44573017987\n",
      "Angle:  178.54899487575176\n",
      "action:  [-0.91964842]\n",
      "state:  {1: array([ 0.14102092]), 2: array([ 3.32676904]), 3: array([-0.99007302, -0.14055398,  0.        ])}\n",
      "cos(angle):  -0.9900730175414274   sin(angle)  -0.14055397517115018\n",
      "theta dot:  3.32676903644\n",
      "Angle:  188.08030568413022\n",
      "action:  [-0.57762415]\n",
      "state:  {1: array([ 0.29775642]), 2: array([ 3.13470993]), 3: array([-0.95599711, -0.29337609,  0.        ])}\n",
      "cos(angle):  -0.9559971077360206   sin(angle)  -0.29337608968755996\n",
      "theta dot:  3.13470993315\n",
      "Angle:  197.0605671427249\n",
      "action:  [-0.16206745]\n",
      "state:  {1: array([ 0.44227481]), 2: array([ 2.89036775]), 3: array([-0.90378039, -0.4279965 ,  0.        ])}\n",
      "cos(angle):  -0.9037803933796305   sin(angle)  -0.4279964959465913\n",
      "theta dot:  2.89036774882\n",
      "Angle:  205.34084144225739\n",
      "action:  [ 0.08530735]\n",
      "state:  {1: array([ 0.57138313]), 2: array([ 2.58216648]), 3: array([-0.84115379, -0.54079599,  0.        ])}\n",
      "cos(angle):  -0.841153788714463   sin(angle)  -0.5407959908609757\n",
      "theta dot:  2.58216647955\n",
      "Angle:  212.7381862079783\n",
      "action:  [ 0.1728267]\n",
      "state:  {1: array([ 0.6815078]), 2: array([ 2.20249349]), 3: array([-0.77662374, -0.62996474,  0.        ])}\n",
      "cos(angle):  -0.7766237380942059   sin(angle)  -0.6299647366548244\n",
      "theta dot:  2.20249349124\n",
      "Angle:  219.04785052589693\n",
      "action:  [ 0.32686645]\n",
      "state:  {1: array([ 0.7704603]), 2: array([ 1.77904991]), 3: array([-0.71759016, -0.69646562,  0.        ])}\n",
      "cos(angle):  -0.7175901624824954   sin(angle)  -0.696465619186149\n",
      "theta dot:  1.77904990565\n",
      "Angle:  224.14444116468644\n",
      "action:  [ 0.28909155]\n",
      "state:  {1: array([ 0.83546352]), 2: array([ 1.30006442]), 3: array([-0.670834  , -0.74160754,  0.        ])}\n",
      "cos(angle):  -0.670834003905886   sin(angle)  -0.7416075371809525\n",
      "theta dot:  1.30006442341\n",
      "Angle:  227.8688426832331\n",
      "action:  [ 0.41745002]\n",
      "state:  {1: array([ 0.87578733]), 2: array([ 0.80647627]), 3: array([-0.64039235, -0.76804794,  0.        ])}\n",
      "cos(angle):  -0.6403923457768534   sin(angle)  -0.7680479434712517\n",
      "theta dot:  0.806476273227\n",
      "Angle:  230.1792216172297\n",
      "action:  [ 0.3553141]\n",
      "state:  {1: array([ 0.88997421]), 2: array([ 0.28373743]), 3: array([-0.62943207, -0.77705551,  0.        ])}\n",
      "cos(angle):  -0.6294320698245687   sin(angle)  -0.7770555124805173\n",
      "theta dot:  0.283737430802\n",
      "Angle:  230.9920675801848\n",
      "action:  [ 0.2580668]\n",
      "state:  {1: array([ 0.876957]), 2: array([-0.26034418]), 3: array([-0.63949355, -0.76879646,  0.        ])}\n",
      "cos(angle):  -0.6394935511420594   sin(angle)  -0.7687964607408896\n",
      "theta dot:  -0.260344183448\n",
      "Angle:  230.2462381776414\n",
      "action:  [ 0.17244942]\n",
      "state:  {1: array([ 0.83640329]), 2: array([-0.81107412]), 3: array([-0.67013677, -0.74223764,  0.        ])}\n",
      "cos(angle):  -0.6701367671215612   sin(angle)  -0.7422376394065868\n",
      "theta dot:  -0.811074116748\n",
      "Angle:  227.92268742301593\n",
      "action:  [ 0.14072064]\n",
      "state:  {1: array([ 0.76907108]), 2: array([-1.34664425]), 3: array([-0.71855701, -0.69546806,  0.        ])}\n",
      "cos(angle):  -0.7185570144939835   sin(angle)  -0.6954680560036479\n",
      "theta dot:  -1.3466442504\n",
      "Angle:  224.06484484165867\n",
      "action:  [ 0.12108154]\n",
      "state:  {1: array([ 0.67656693]), 2: array([-1.85008306]), 3: array([-0.77972683, -0.62611986,  0.        ])}\n",
      "cos(angle):  -0.7797268253734438   sin(angle)  -0.6261198589671557\n",
      "theta dot:  -1.85008306141\n",
      "Angle:  218.76475967719597\n",
      "action:  [ 0.09416115]\n",
      "state:  {1: array([ 0.56128949]), 2: array([-2.30554878]), 3: array([-0.84656945, -0.53227828,  0.        ])}\n",
      "cos(angle):  -0.8465694492734473   sin(angle)  -0.5322782801851416\n",
      "theta dot:  -2.30554878265\n",
      "Angle:  212.15986438695464\n",
      "action:  [-0.12675982]\n",
      "state:  {1: array([ 0.42510091]), 2: array([-2.72377147]), 3: array([-0.91099712, -0.41241272,  0.        ])}\n",
      "cos(angle):  -0.9109971196210715   sin(angle)  -0.4124127156649163\n",
      "theta dot:  -2.72377146569\n",
      "Angle:  204.35685216667534\n",
      "action:  [-0.21867221]\n",
      "state:  {1: array([ 0.27180682]), 2: array([-3.06588183]), 3: array([-0.96328739, -0.26847236,  0.        ])}\n",
      "cos(angle):  -0.9632873873129363   sin(angle)  -0.26847236253256523\n",
      "theta dot:  -3.06588183394\n",
      "Angle:  195.57376822680874\n",
      "action:  [-0.27040073]\n",
      "state:  {1: array([ 0.10641701]), 2: array([-3.30779622]), 3: array([-0.99434305, -0.10621627,  0.        ])}\n",
      "cos(angle):  -0.9943430514643481   sin(angle)  -0.10621626996166246\n",
      "theta dot:  -3.30779621566\n",
      "Angle:  186.09765225375423\n",
      "action:  [-0.12384191]\n",
      "state:  {1: array([ 6.21930058]), 2: array([-3.40603471]), 3: array([-0.99796006,  0.06384128,  0.        ])}\n",
      "cos(angle):  -0.9979600649317661   sin(angle)  0.06384127819354157\n",
      "theta dot:  -3.40603470518\n",
      "Angle:  176.33926256240636\n",
      "action:  [-0.23519917]\n",
      "state:  {1: array([ 6.0496289]), 2: array([-3.39343362]), 3: array([-0.97284946,  0.23143883,  0.        ])}\n",
      "cos(angle):  -0.9728494587340479   sin(angle)  0.23143882699510468\n",
      "theta dot:  -3.39343362266\n",
      "Angle:  166.61781406358045\n",
      "action:  [ 0.14250174]\n",
      "state:  {1: array([ 5.88970494]), 2: array([-3.19847924]), 3: array([-0.92358027,  0.38340513,  0.        ])}\n",
      "cos(angle):  -0.923580265431483   sin(angle)  0.3834051294720918\n",
      "theta dot:  -3.19847924101\n",
      "Angle:  157.45486742202786\n",
      "action:  [ 0.32968715]\n",
      "state:  {1: array([ 5.74663132]), 2: array([-2.86147232]), 3: array([-0.85947531,  0.51117727,  0.        ])}\n",
      "cos(angle):  -0.859475306265447   sin(angle)  0.5111772666305849\n",
      "theta dot:  -2.86147232158\n",
      "Angle:  149.2573722303273\n",
      "action:  [ 0.43877763]\n",
      "state:  {1: array([ 5.62601769]), 2: array([-2.41227273]), 3: array([-0.79172564,  0.61087684,  0.        ])}\n",
      "cos(angle):  -0.7917256402920761   sin(angle)  0.6108768374264179\n",
      "theta dot:  -2.41227272765\n",
      "Angle:  142.34673607395962\n",
      "action:  [ 0.50129935]\n",
      "state:  {1: array([ 5.53207168]), 2: array([-1.8789202]), 3: array([-0.73092932,  0.68245317,  0.        ])}\n",
      "cos(angle):  -0.7309293220847641   sin(angle)  0.6824531677094825\n",
      "theta dot:  -1.87892019643\n",
      "Angle:  136.96403879618748\n",
      "action:  [ 0.46673345]\n",
      "state:  {1: array([ 5.46721816]), 2: array([-1.2970703]), 3: array([-0.68516426,  0.72838859,  0.        ])}\n",
      "cos(angle):  -0.6851642566295488   sin(angle)  0.7283885923305484\n",
      "theta dot:  -1.29707030379\n",
      "Angle:  133.2482147784458\n",
      "action:  [ 0.2701704]\n",
      "state:  {1: array([ 5.4317055]), 2: array([-0.7102533]), 3: array([-0.65887067,  0.75225623,  0.        ])}\n",
      "cos(angle):  -0.6588706714229565   sin(angle)  0.7522562318377046\n",
      "theta dot:  -0.710253299765\n",
      "Angle:  131.21349371342058\n",
      "action:  [ 0.43118794]\n",
      "state:  {1: array([ 5.42763635]), 2: array([-0.08138294]), 3: array([-0.65580418,  0.75493104,  0.        ])}\n",
      "cos(angle):  -0.655804184079424   sin(angle)  0.7549310380054067\n",
      "theta dot:  -0.0813829355823\n",
      "Angle:  130.98034932195117\n",
      "action:  [-0.17844586]\n",
      "state:  {1: array([ 5.45053877]), 2: array([ 0.45804846]), 3: array([-0.67292044,  0.73971487,  0.        ])}\n",
      "cos(angle):  -0.6729204387665384   sin(angle)  0.7397148660735766\n",
      "theta dot:  0.458048464348\n",
      "Angle:  132.29255844441377\n",
      "action:  [-0.26991912]\n",
      "state:  {1: array([ 5.49915611]), 2: array([ 0.97234675]), 3: array([-0.70807412,  0.70613811,  0.        ])}\n",
      "cos(angle):  -0.7080741242664714   sin(angle)  0.7061381129384461\n",
      "theta dot:  0.972346746172\n",
      "Angle:  135.0781201695055\n",
      "action:  [-0.21447384]\n",
      "state:  {1: array([ 5.57264507]), 2: array([ 1.46977926]), 3: array([-0.75800962,  0.65224337,  0.        ])}\n",
      "cos(angle):  -0.7580096229225515   sin(angle)  0.6522433683501975\n",
      "theta dot:  1.46977925522\n",
      "Angle:  139.28871773028902\n",
      "action:  [-0.18259149]\n",
      "state:  {1: array([ 5.66922373]), 2: array([ 1.93157306]), 3: array([-0.81737213,  0.57611006,  0.        ])}\n",
      "cos(angle):  -0.8173721313675115   sin(angle)  0.5761100579435596\n",
      "theta dot:  1.93157305815\n",
      "Angle:  144.82225399312574\n",
      "action:  [-0.20240757]\n",
      "state:  {1: array([ 5.78588845]), 2: array([ 2.33329447]), 3: array([-0.87887531,  0.47705156,  0.        ])}\n",
      "cos(angle):  -0.878875309747256   sin(angle)  0.47705155897100354\n",
      "theta dot:  2.33329446642\n",
      "Angle:  151.50663462649192\n",
      "action:  [-0.0869059]\n",
      "state:  {1: array([ 5.91979081]), 2: array([ 2.67804725]), 3: array([-0.93469564,  0.35544909,  0.        ])}\n",
      "cos(angle):  -0.9346956409799452   sin(angle)  0.35544909443278844\n",
      "theta dot:  2.67804725072\n",
      "Angle:  159.17865692611724\n",
      "action:  [-0.41975754]\n",
      "state:  {1: array([ 6.06387433]), 2: array([ 2.88167044]), 3: array([-0.97604758,  0.21755715,  0.        ])}\n",
      "cos(angle):  -0.9760475838291088   sin(angle)  0.21755715134501746\n",
      "theta dot:  2.88167044015\n",
      "Angle:  167.43401532989492\n",
      "action:  [-0.28727607]\n",
      "state:  {1: array([ 6.21396168]), 2: array([ 3.00174689]), 3: array([-0.997605  ,  0.06916836,  0.        ])}\n",
      "cos(angle):  -0.997605001277016   sin(angle)  0.06916835567717981\n",
      "theta dot:  3.00174689385\n",
      "Angle:  176.03336663007747\n",
      "action:  [-0.19672095]\n",
      "state:  {1: array([ 0.08198212]), 2: array([ 3.02411502]), 3: array([-0.99664135, -0.08189032,  0.        ])}\n",
      "cos(angle):  -0.9966413475392084   sin(angle)  -0.08189031917895363\n",
      "theta dot:  3.02411501811\n",
      "Angle:  184.69763957084024\n",
      "action:  [-0.11896851]\n",
      "state:  {1: array([ 0.22922472]), 2: array([ 2.944852]), 3: array([-0.97384285, -0.22722259,  0.        ])}\n",
      "cos(angle):  -0.9738428481078409   sin(angle)  -0.22722259392324665\n",
      "theta dot:  2.94485200175\n",
      "Angle:  193.1339993925218\n",
      "action:  [ 0.12540091]\n",
      "state:  {1: array([ 0.36888698]), 2: array([ 2.79324519]), 3: array([-0.93272925, -0.36057751,  0.        ])}\n",
      "cos(angle):  -0.9327292522917137   sin(angle)  -0.36057751166613367\n",
      "theta dot:  2.79324519292\n",
      "Angle:  201.13603871524995\n",
      "action:  [-0.46637655]\n",
      "state:  {1: array([ 0.49152976]), 2: array([ 2.45285558]), 3: array([-0.88161188, -0.4719751 ,  0.        ])}\n",
      "cos(angle):  -0.881611881125121   sin(angle)  -0.4719750958038205\n",
      "theta dot:  2.4528555772\n",
      "Angle:  208.1629358997406\n",
      "action:  [-0.21306709]\n",
      "state:  {1: array([ 0.59487547]), 2: array([ 2.06691419]), 3: array([-0.82821829, -0.56040562,  0.        ])}\n",
      "cos(angle):  -0.8282182919849344   sin(angle)  -0.5604056216898239\n",
      "theta dot:  2.06691419113\n",
      "Angle:  214.08419504163638\n",
      "action:  [-0.13920458]\n",
      "state:  {1: array([ 0.67616194]), 2: array([ 1.62572929]), 3: array([-0.77998033, -0.62580403,  0.        ])}\n",
      "cos(angle):  -0.7799803339402124   sin(angle)  -0.6258040257672642\n",
      "theta dot:  1.6257292879\n",
      "Angle:  218.74155549208663\n",
      "action:  [ 0.03491746]\n",
      "state:  {1: array([ 0.73424263]), 2: array([ 1.16161389]), 3: array([-0.74233842, -0.67002512,  0.        ])}\n",
      "cos(angle):  -0.7423384233109351   sin(angle)  -0.6700251228694599\n",
      "theta dot:  1.16161388741\n",
      "Angle:  222.0693263689305\n",
      "action:  [ 0.26855294]\n",
      "state:  {1: array([ 0.76921153]), 2: array([ 0.69937799]), 3: array([-0.71845933, -0.69556897,  0.        ])}\n",
      "cos(angle):  -0.7184593287563636   sin(angle)  -0.6955689706441449\n",
      "theta dot:  0.699377986213\n",
      "Angle:  224.07289202844044\n",
      "action:  [ 0.47556074]\n",
      "state:  {1: array([ 0.7816633]), 2: array([ 0.24903537]), 3: array([-0.70974279, -0.70446091,  0.        ])}\n",
      "cos(angle):  -0.7097427921916996   sin(angle)  -0.7044609066030065\n",
      "theta dot:  0.24903536942\n",
      "Angle:  224.78632414098735\n",
      "action:  [ 0.58174727]\n",
      "state:  {1: array([ 0.77206089]), 2: array([-0.19204822]), 3: array([-0.71647449, -0.69761329,  0.        ])}\n",
      "cos(angle):  -0.7164744902749947   sin(angle)  -0.6976132917205539\n",
      "theta dot:  -0.192048220154\n",
      "Angle:  224.2361478036473\n",
      "action:  [ 0.63215038]\n",
      "state:  {1: array([ 0.74103911]), 2: array([-0.62043563]), 3: array([-0.7377675 , -0.67505489,  0.        ])}\n",
      "cos(angle):  -0.7377675042318675   sin(angle)  -0.6750548938415907\n",
      "theta dot:  -0.62043563159\n",
      "Angle:  222.45873480252902\n",
      "action:  [ 0.64331901]\n",
      "state:  {1: array([ 0.68952766]), 2: array([-1.03022895]), 3: array([-0.77154659, -0.63617282,  0.        ])}\n",
      "cos(angle):  -0.7715465925115349   sin(angle)  -0.6361728189602567\n",
      "theta dot:  -1.0302289501\n",
      "Angle:  219.50735316548682\n",
      "action:  [ 0.57957735]\n",
      "state:  {1: array([ 0.61850656]), 2: array([-1.42042196]), 3: array([-0.81474529, -0.57981903,  0.        ])}\n",
      "cos(angle):  -0.8147452902025083   sin(angle)  -0.579819033917334\n",
      "theta dot:  -1.42042196163\n",
      "Angle:  215.43815350458905\n",
      "action:  [ 0.3804452]\n",
      "state:  {1: array([ 0.52859559]), 2: array([-1.79821946]), 3: array([-0.8635162 , -0.50432111,  0.        ])}\n",
      "cos(angle):  -0.8635161971922392   sin(angle)  -0.5043211052361918\n",
      "theta dot:  -1.79821945657\n",
      "Angle:  210.28664627607202\n",
      "action:  [ 0.0057422]\n",
      "state:  {1: array([ 0.41981564]), 2: array([-2.17559896]), 3: array([-0.9131641 , -0.40759211,  0.        ])}\n",
      "cos(angle):  -0.9131640999868871   sin(angle)  -0.40759210798927203\n",
      "theta dot:  -2.17559895521\n",
      "Angle:  204.0540289483017\n",
      "action:  [-0.25941985]\n",
      "state:  {1: array([ 0.29380534]), 2: array([-2.52020601]), 3: array([-0.9571488 , -0.28959659,  0.        ])}\n",
      "cos(angle):  -0.957148795264417   sin(angle)  -0.2895965879009542\n",
      "theta dot:  -2.52020601374\n",
      "Angle:  196.83418742689\n",
      "action:  [-0.17492372]\n",
      "state:  {1: array([ 0.15562324]), 2: array([-2.76364201]), 3: array([-0.98791512, -0.15499584,  0.        ])}\n",
      "cos(angle):  -0.9879151234316303   sin(angle)  -0.15499583508941955\n",
      "theta dot:  -2.76364201326\n",
      "Angle:  188.91695476858013\n",
      "action:  [-0.1546677]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "state:  {1: array([ 0.01046879]), 2: array([-2.90308904]), 3: array([-0.9999452 , -0.01046859,  0.        ])}\n",
      "cos(angle):  -0.9999452027682754   sin(angle)  -0.010468594017947487\n",
      "theta dot:  -2.90308904491\n",
      "Angle:  180.60023672549676\n",
      "action:  [ 0.01071485]\n",
      "state:  {1: array([ 6.14818743]), 2: array([-2.90933326]), 3: array([-0.99090162,  0.13458821,  0.        ])}\n",
      "cos(angle):  -0.9909016168127929   sin(angle)  0.1345882082420036\n",
      "theta dot:  -2.90933326331\n",
      "Angle:  172.26478852155515\n",
      "action:  [ 0.25730456]\n",
      "state:  {1: array([ 6.00969761]), 2: array([-2.76979642]), 3: array([-0.96283476,  0.27009115,  0.        ])}\n",
      "cos(angle):  -0.962834757596263   sin(angle)  0.2700911504743268\n",
      "theta dot:  -2.76979642299\n",
      "Angle:  164.3299248193373\n",
      "action:  [-0.34939135]\n",
      "state:  {1: array([ 5.87871577]), 2: array([-2.61963676]), 3: array([-0.91931128,  0.39353116,  0.        ])}\n",
      "cos(angle):  -0.9193112801480852   sin(angle)  0.3935311553009353\n",
      "theta dot:  -2.61963676193\n",
      "Angle:  156.82523585276863\n",
      "action:  [-0.30682039]\n",
      "state:  {1: array([ 5.7601902]), 2: array([-2.37051145]), 3: array([-0.86632708,  0.49947712,  0.        ])}\n",
      "cos(angle):  -0.8663270788179593   sin(angle)  0.4994771191023081\n",
      "theta dot:  -2.37051145463\n",
      "Angle:  150.03423665119215\n",
      "action:  [-0.53674964]\n",
      "state:  {1: array([ 5.65636939]), 2: array([-2.07641606]), 3: array([-0.8098993 ,  0.58656895,  0.        ])}\n",
      "cos(angle):  -0.8098992972812467   sin(angle)  0.5865689458736651\n",
      "theta dot:  -2.07641606097\n",
      "Angle:  144.08575672098044\n",
      "action:  [-2.68980994]\n",
      "state:  {1: array([ 5.55954493]), 2: array([-1.93648935]), 3: array([-0.74940034,  0.66211716,  0.        ])}\n",
      "cos(angle):  -0.7494003417217571   sin(angle)  0.6621171556660602\n",
      "theta dot:  -1.93648935157\n",
      "Angle:  138.53813634788844\n",
      "action:  [-0.48050648]\n",
      "state:  {1: array([ 5.48394605]), 2: array([-1.51197746]), 3: array([-0.69725223,  0.71682587,  0.        ])}\n",
      "cos(angle):  -0.6972522339220153   sin(angle)  0.7168258660865687\n",
      "theta dot:  -1.51197745668\n",
      "Angle:  134.20665012744405\n",
      "action:  [-0.37396653]\n",
      "state:  {1: array([ 5.4324234]), 2: array([-1.03045304]), 3: array([-0.65941055,  0.75178303,  0.        ])}\n",
      "cos(angle):  -0.6594105499337516   sin(angle)  0.7517830316228661\n",
      "theta dot:  -1.03045303667\n",
      "Angle:  131.25462653116978\n",
      "action:  [-0.21393185]\n",
      "state:  {1: array([ 5.40748812]), 2: array([-0.49870554]), 3: array([-0.64046158,  0.76799021,  0.        ])}\n",
      "cos(angle):  -0.6404615847590219   sin(angle)  0.7679902072604586\n",
      "theta dot:  -0.498705539959\n",
      "Angle:  129.82594373907955\n",
      "action:  [-0.1677881]\n",
      "state:  {1: array([ 5.41009407]), 2: array([ 0.0521189]), 3: array([-0.64246075,  0.76631859,  0.        ])}\n",
      "cos(angle):  -0.6424607480872704   sin(angle)  0.766318593776208\n",
      "theta dot:  0.0521189005902\n",
      "Angle:  129.97525304176338\n",
      "action:  [-0.21157829]\n",
      "state:  {1: array([ 5.43985013]), 2: array([ 0.5951211]), 3: array([-0.6649756 ,  0.74686508,  0.        ])}\n",
      "cos(angle):  -0.6649755978341562   sin(angle)  0.7468650843928284\n",
      "theta dot:  0.595121102182\n",
      "Angle:  131.68014542769384\n",
      "action:  [-0.23525409]\n",
      "state:  {1: array([ 5.49584922]), 2: array([ 1.1199818]), 3: array([-0.70573513,  0.70847577,  0.        ])}\n",
      "cos(angle):  -0.7057351318116152   sin(angle)  0.708475774975293\n",
      "theta dot:  1.11998180164\n",
      "Angle:  134.88864944309546\n",
      "action:  [-0.15212033]\n",
      "state:  {1: array([ 5.57727524]), 2: array([ 1.62852058]), 3: array([-0.76102148,  0.64872668,  0.        ])}\n",
      "cos(angle):  -0.7610214849816912   sin(angle)  0.6487266754159733\n",
      "theta dot:  1.62852058378\n",
      "Angle:  139.5540063485139\n",
      "action:  [ 0.10287002]\n",
      "state:  {1: array([ 5.68380005]), 2: array([ 2.13049609]), 3: array([-0.82568257,  0.564135  ,  0.        ])}\n",
      "cos(angle):  -0.8256825683231984   sin(angle)  0.5641349983534143\n",
      "theta dot:  2.1304960937\n",
      "Angle:  145.657413797991\n",
      "action:  [ 0.24964059]\n",
      "state:  {1: array([ 5.81335222]), 2: array([ 2.59104343]), 3: array([-0.89164387,  0.45273746,  0.        ])}\n",
      "cos(angle):  -0.8916438685956211   sin(angle)  0.4527374643166112\n",
      "theta dot:  2.59104343139\n",
      "Angle:  153.08018909800649\n",
      "action:  [ 0.38190092]\n",
      "state:  {1: array([ 5.9627463]), 2: array([ 2.98788167]), 3: array([-0.94909723,  0.31498325,  0.        ])}\n",
      "cos(angle):  -0.9490972309153267   sin(angle)  0.3149832476003429\n",
      "theta dot:  2.98788166826\n",
      "Angle:  161.63981954565304\n",
      "action:  [ 0.38685963]\n",
      "state:  {1: array([ 6.12685371]), 2: array([ 3.28214805]), 3: array([-0.98780508,  0.1556956 ,  0.        ])}\n",
      "cos(angle):  -0.9878050822389617   sin(angle)  0.15569559885519602\n",
      "theta dot:  3.28214804886\n",
      "Angle:  171.04245910501618\n",
      "action:  [ 0.51125359]\n",
      "state:  {1: array([ 0.01744879]), 2: array([ 3.47560779]), 3: array([-0.99984777, -0.0174479 ,  0.        ])}\n",
      "cos(angle):  -0.9998477737502162   sin(angle)  -0.017447903218914786\n",
      "theta dot:  3.47560778585\n",
      "Angle:  181.00016052478944\n",
      "action:  [ 0.46071047]\n",
      "state:  {1: array([ 0.19403021]), 2: array([ 3.53162843]), 3: array([-0.98123512, -0.19281503,  0.        ])}\n",
      "cos(angle):  -0.9812351210265954   sin(angle)  -0.19281503381200002\n",
      "theta dot:  3.53162842846\n",
      "Angle:  191.11750705398143\n",
      "action:  [ 0.32698493]\n",
      "state:  {1: array([ 0.36583345]), 2: array([ 3.43606489]), 3: array([-0.93382594, -0.35772772,  0.        ])}\n",
      "cos(angle):  -0.9338259357126466   sin(angle)  -0.3577277201872955\n",
      "theta dot:  3.4360648919\n",
      "Angle:  200.96108485737486\n",
      "action:  [ 0.25938467]\n",
      "state:  {1: array([ 0.52616729]), 2: array([ 3.2066768]), 3: array([-0.86473829, -0.50222275,  0.        ])}\n",
      "cos(angle):  -0.8647382889272676   sin(angle)  -0.5022227510409514\n",
      "theta dot:  3.2066768017\n",
      "Angle:  210.14751572549963\n",
      "action:  [ 0.22745548]\n",
      "state:  {1: array([ 0.6693737]), 2: array([ 2.86412806]), 3: array([-0.78421044, -0.62049496,  0.        ])}\n",
      "cos(angle):  -0.7842104370370401   sin(angle)  -0.6204949560167066\n",
      "theta dot:  2.86412806096\n",
      "Angle:  218.35261903230267\n",
      "action:  [ 0.34296475]\n",
      "state:  {1: array([ 0.79188378]), 2: array([ 2.45020156]), 3: array([-0.70250592, -0.7116779 ,  0.        ])}\n",
      "cos(angle):  -0.7025059214734573   sin(angle)  -0.7116778978545903\n",
      "theta dot:  2.45020155588\n",
      "Angle:  225.37191302355447\n",
      "action:  [ 0.4881861]\n",
      "state:  {1: array([ 0.89136733]), 2: array([ 1.98967105]), 3: array([-0.62834893, -0.77793163,  0.        ])}\n",
      "cos(angle):  -0.6283489266387859   sin(angle)  -0.7779316334947985\n",
      "theta dot:  1.98967104718\n",
      "Angle:  231.0718873756802\n",
      "action:  [ 0.61285738]\n",
      "state:  {1: array([ 0.96627487]), 2: array([ 1.49815093]), 3: array([-0.5683684 , -0.82277418,  0.        ])}\n",
      "cos(angle):  -0.5683684047324439   sin(angle)  -0.8227741831765851\n",
      "theta dot:  1.49815092977\n",
      "Angle:  235.36376360687734\n",
      "action:  [ 0.55587447]\n",
      "state:  {1: array([ 1.01449745]), 2: array([ 0.96445146]), 3: array([-0.52804677, -0.84921529,  0.        ])}\n",
      "cos(angle):  -0.5280467734014533   sin(angle)  -0.8492152878394937\n",
      "theta dot:  0.964451463629\n",
      "Angle:  238.1267070664723\n",
      "action:  [ 0.40556162]\n",
      "state:  {1: array([ 1.03391616]), 2: array([ 0.38837424]), 3: array([-0.51145759, -0.85930852,  0.        ])}\n",
      "cos(angle):  -0.5114575861355579   sin(angle)  -0.859308522932473\n",
      "theta dot:  0.38837424124\n",
      "Angle:  239.239314709445\n",
      "action:  [ 0.17648335]\n",
      "state:  {1: array([ 1.02243443]), 2: array([-0.22963465]), 3: array([-0.52129001, -0.85337959,  0.        ])}\n",
      "cos(angle):  -0.5212900074622014   sin(angle)  -0.8533795920456839\n",
      "theta dot:  -0.229634647965\n",
      "Angle:  238.58146143987403\n",
      "action:  [-0.07423358]\n",
      "state:  {1: array([ 0.97839421]), 2: array([-0.88080438]), 3: array([-0.55835543, -0.82960184,  0.        ])}\n",
      "cos(angle):  -0.5583554337860073   sin(angle)  -0.8296018379690583\n",
      "theta dot:  -0.880804378311\n",
      "Angle:  236.05814866778326\n",
      "action:  [-0.14636362]\n",
      "state:  {1: array([ 0.90214619]), 2: array([-1.5249603]), 3: array([-0.61992737, -0.7846592 ,  0.        ])}\n",
      "cos(angle):  -0.6199273669534159   sin(angle)  -0.7846591997180719\n",
      "theta dot:  -1.52496029999\n",
      "Angle:  231.68946942792587\n",
      "action:  [-0.30063342]\n",
      "state:  {1: array([ 0.79421871]), 2: array([-2.15854971]), 3: array([-0.70084229, -0.71331626,  0.        ])}\n",
      "cos(angle):  -0.7008422884079459   sin(angle)  -0.713316259999107\n",
      "theta dot:  -2.1585497133\n",
      "Angle:  225.50569446621358\n",
      "action:  [-0.22206199]\n",
      "state:  {1: array([ 0.6578764]), 2: array([-2.72684621]), 3: array([-0.79129247, -0.61143784,  0.        ])}\n",
      "cos(angle):  -0.7912924658097643   sin(angle)  -0.6114378411193594\n",
      "theta dot:  -2.72684620739\n",
      "Angle:  217.69387378042273\n",
      "action:  [-0.00997007]\n",
      "state:  {1: array([ 0.49853039]), 2: array([-3.1869201]), 3: array([-0.87828618, -0.47813532,  0.        ])}\n",
      "cos(angle):  -0.8782861813886832   sin(angle)  -0.4781353193204671\n",
      "theta dot:  -3.18692009869\n",
      "Angle:  208.56404156492582\n",
      "action:  [ 0.05921421]\n",
      "state:  {1: array([ 0.32169842]), 2: array([-3.53663946]), 3: array([-0.94869978, -0.31617831,  0.        ])}\n",
      "cos(angle):  -0.9486997832516725   sin(angle)  -0.3161783061157574\n",
      "theta dot:  -3.53663945667\n",
      "Angle:  198.43233953092508\n",
      "action:  [ 0.45720093]\n",
      "state:  {1: array([ 0.13643877]), 2: array([-3.70519305]), 3: array([-0.99070666, -0.13601585,  0.        ])}\n",
      "cos(angle):  -0.990706661534321   sin(angle)  -0.13601584757490753\n",
      "theta dot:  -3.70519304612\n",
      "Angle:  187.81776816122203\n",
      "action:  [ 0.16678431]\n",
      "state:  {1: array([ 6.23051471]), 2: array([-3.78218728]), 3: array([-0.99861322,  0.05264625,  0.        ])}\n",
      "cos(angle):  -0.9986132247477676   sin(angle)  0.05264624733886216\n",
      "theta dot:  -3.78218728495\n",
      "Angle:  176.98178322607373\n",
      "action:  [ 0.29588782]\n",
      "state:  {1: array([ 6.04559874]), 2: array([-3.69831943]), 3: array([-0.97190882,  0.23535768,  0.        ])}\n",
      "cos(angle):  -0.9719088244860172   sin(angle)  0.2353576786217272\n",
      "theta dot:  -3.6983194266\n",
      "Angle:  166.38690327974166\n",
      "action:  [ 0.33745176]\n",
      "state:  {1: array([ 5.87203957]), 2: array([-3.4711834]), 3: array([-0.91666352,  0.39965985,  0.        ])}\n",
      "cos(angle):  -0.9166635188823643   sin(angle)  0.3996598468075087\n",
      "theta dot:  -3.47118340409\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  156.44271858505329\n",
      "Reset!\n",
      "min:  -2.68980993741  self.max:  28.6954689906\n",
      "state:  {1: array([ 2.38757557]), 2: array([ 0.50856519]), 3: array([ 0.72894478, -0.6845725 ,  0.        ])}\n",
      "cos(angle):  0.7289447755333067   sin(angle)  -0.6845725047229819\n",
      "theta dot:  0.508565191547\n",
      "Angle:  316.7981046871613\n",
      "action:  [ 0.58239951]\n",
      "state:  {1: array([ 2.39170036]), 2: array([ 0.08249574]), 3: array([ 0.73176228, -0.68155995,  0.        ])}\n",
      "cos(angle):  0.7317622822251897   sin(angle)  -0.6815599477027547\n",
      "theta dot:  0.0824957402089\n",
      "Angle:  317.0344370216025\n",
      "action:  [ 0.35365254]\n",
      "state:  {1: array([ 2.37291904]), 2: array([-0.37562634]), 3: array([ 0.71883339, -0.6951824 ,  0.        ])}\n",
      "cos(angle):  0.7188333850149418   sin(angle)  -0.6951823966326827\n",
      "theta dot:  -0.375626339048\n",
      "Angle:  315.9583493428934\n",
      "action:  [ 0.03377819]\n",
      "state:  {1: array([ 2.32832172]), 2: array([-0.89194641]), 3: array([ 0.68712566, -0.7265386 ,  0.        ])}\n",
      "cos(angle):  0.687125656143102   sin(angle)  -0.7265385968205074\n",
      "theta dot:  -0.89194640851\n",
      "Angle:  313.40311708016515\n",
      "action:  [-0.27144826]\n",
      "state:  {1: array([ 2.25444334]), 2: array([-1.4775676]), 3: array([ 0.63162466, -0.77527433,  0.        ])}\n",
      "cos(angle):  0.6316246570415228   sin(angle)  -0.7752743337794556\n",
      "theta dot:  -1.47756759582\n",
      "Angle:  309.1702076192462\n",
      "action:  [-0.51940269]\n",
      "state:  {1: array([ 2.14759666]), 2: array([-2.13693375]), 3: array([ 0.54534473, -0.83821186,  0.        ])}\n",
      "cos(angle):  0.545344731281685   sin(angle)  -0.8382118610848374\n",
      "theta dot:  -2.13693375003\n",
      "Angle:  303.0483576860161\n",
      "action:  [-0.56652385]\n",
      "state:  {1: array([ 2.0050681]), 2: array([-2.85057122]), 3: array([ 0.42074988, -0.90717669,  0.        ])}\n",
      "cos(angle):  0.4207498782892276   sin(angle)  -0.9071766861640571\n",
      "theta dot:  -2.85057122356\n",
      "Angle:  294.8820917667381\n",
      "action:  [-0.4726495]\n",
      "state:  {1: array([ 1.82497554]), 2: array([-3.60185116]), 3: array([ 0.25145108, -0.96787001,  0.        ])}\n",
      "cos(angle):  0.2514510756896621   sin(angle)  -0.9678700101431554\n",
      "theta dot:  -3.60185116259\n",
      "Angle:  284.5635723933758\n",
      "action:  [-0.59332949]\n",
      "state:  {1: array([ 1.60413788]), 2: array([-4.41675309]), 3: array([ 0.03333538, -0.99944422,  0.        ])}\n",
      "cos(angle):  0.03333537853102612   sin(angle)  -0.9994442218244064\n",
      "theta dot:  -4.41675309409\n",
      "Angle:  271.9105364095495\n",
      "action:  [-0.36234364]\n",
      "state:  {1: array([ 1.34310349]), 2: array([-5.22068781]), 3: array([-0.22573051, -0.97418978,  0.        ])}\n",
      "cos(angle):  -0.22573050947534015   sin(angle)  -0.9741897849454199\n",
      "theta dot:  -5.22068780656\n",
      "Angle:  256.95440250994085\n",
      "action:  [-0.30179648]\n",
      "state:  {1: array([ 1.04327351]), 2: array([-5.99659962]), 3: array([-0.50339446, -0.86405672,  0.        ])}\n",
      "cos(angle):  -0.5033944601098563   sin(angle)  -0.8640567212461843\n",
      "theta dot:  -5.99659961787\n",
      "Angle:  239.77545020511627\n",
      "action:  [-0.13403806]\n",
      "state:  {1: array([ 0.71003612]), 2: array([-6.66474787]), 3: array([-0.75833833, -0.65186116,  0.        ])}\n",
      "cos(angle):  -0.7583383326894675   sin(angle)  -0.6518611609643258\n",
      "theta dot:  -6.66474786725\n",
      "Angle:  220.68239863735548\n",
      "action:  [ 0.58482056]\n",
      "state:  {1: array([ 0.35674009]), 2: array([-7.06592065]), 3: array([-0.93704023, -0.34922142,  0.        ])}\n",
      "cos(angle):  -0.937040232746342   sin(angle)  -0.3492214229034084\n",
      "theta dot:  -7.06592065328\n",
      "Angle:  200.4400743823506\n",
      "action:  [ 0.34985946]\n",
      "state:  {1: array([ 6.2761575]), 2: array([-7.2753578]), 3: array([-0.99997531,  0.00702775,  0.        ])}\n",
      "cos(angle):  -0.9999753050805286   sin(angle)  0.007027747085930001\n",
      "theta dot:  -7.27535780159\n",
      "Angle:  179.59691646218926\n",
      "action:  [ 0.43523365]\n",
      "state:  {1: array([ 5.91591741]), 2: array([-7.20480194]), 3: array([-0.93331183,  0.35906688,  0.        ])}\n",
      "cos(angle):  -0.933311833523623   sin(angle)  0.35906687595039044\n",
      "theta dot:  -7.20480194354\n",
      "Angle:  158.95672754830753\n",
      "action:  [ 0.43049543]\n",
      "state:  {1: array([ 5.57237103]), 2: array([-6.87092747]), 3: array([-0.75783085,  0.65245107,  0.        ])}\n",
      "cos(angle):  -0.7578308522214298   sin(angle)  0.6524510705189634\n",
      "theta dot:  -6.87092747215\n",
      "Angle:  139.27301630265424\n",
      "action:  [-0.20285808]\n",
      "state:  {1: array([ 5.25177014]), 2: array([-6.41201788]), 3: array([-0.51360511,  0.85802669,  0.        ])}\n",
      "cos(angle):  -0.5136051062644675   sin(angle)  0.8580266865424787\n",
      "theta dot:  -6.41201788056\n",
      "Angle:  120.9039811215165\n",
      "action:  [ 0.22285557]\n",
      "state:  {1: array([ 4.96501666]), 2: array([-5.73506953]), 3: array([-0.24994909,  0.96825898,  0.        ])}\n",
      "cos(angle):  -0.24994909447870323   sin(angle)  0.9682589788735637\n",
      "theta dot:  -5.73506953087\n",
      "Angle:  104.47425557472138\n",
      "action:  [ 0.19899808]\n",
      "state:  {1: array([ 4.71606538]), 2: array([-4.97902558]), 3: array([-0.00367639,  0.99999324,  0.        ])}\n",
      "cos(angle):  -0.0036763930080501114   sin(angle)  0.9999932420443902\n",
      "theta dot:  -4.97902558426\n",
      "Angle:  90.2104313264669\n",
      "action:  [ 0.24368393]\n",
      "state:  {1: array([ 4.50644148]), 2: array([-4.19247806]), 3: array([ 0.20449473,  0.97886766,  0.        ])}\n",
      "cos(angle):  0.20449473051607925   sin(angle)  0.9788676648000771\n",
      "theta dot:  -4.19247806347\n",
      "Angle:  78.19989447541877\n",
      "action:  [ 0.35600582]\n",
      "state:  {1: array([ 4.33619516]), 2: array([-3.40492644]), 3: array([ 0.36738313,  0.93006969,  0.        ])}\n",
      "cos(angle):  0.3673831301487955   sin(angle)  0.9300696939918391\n",
      "theta dot:  -3.40492644241\n",
      "Angle:  68.44552155026008\n",
      "action:  [ 0.40989649]\n",
      "state:  {1: array([ 4.20390067]), 2: array([-2.6458897]), 3: array([ 0.48685737,  0.87348148,  0.        ])}\n",
      "cos(angle):  0.48685736950679176   sin(angle)  0.8734814833509221\n",
      "theta dot:  -2.64588969837\n",
      "Angle:  60.865623636680255\n",
      "action:  [ 0.40561178]\n",
      "state:  {1: array([ 4.10740383]), 2: array([-1.92993682]), 3: array([ 0.56874986,  0.82251054,  0.        ])}\n",
      "cos(angle):  0.5687498619585301   sin(angle)  0.8225105437148834\n",
      "theta dot:  -1.92993681956\n",
      "Angle:  55.336774841137924\n",
      "action:  [ 0.36053814]\n",
      "state:  {1: array([ 4.04445517]), 2: array([-1.25897319]), 3: array([ 0.61936514,  0.78510307,  0.        ])}\n",
      "cos(angle):  0.6193651377183548   sin(angle)  0.7851030672332923\n",
      "theta dot:  -1.25897319065\n",
      "Angle:  51.730090757911455\n",
      "action:  [ 0.11995289]\n",
      "state:  {1: array([ 4.01184752]), 2: array([-0.65215296]), 3: array([ 0.64463172,  0.76449326,  0.        ])}\n",
      "cos(angle):  0.6446317221168584   sin(angle)  0.7644932588588689\n",
      "theta dot:  -0.652152955979\n",
      "Angle:  49.86181452802606\n",
      "action:  [-0.08618896]\n",
      "state:  {1: array([ 4.00726196]), 2: array([-0.09171136]), 3: array([ 0.64813057,  0.76152923,  0.        ])}\n",
      "cos(angle):  0.6481305680453414   sin(angle)  0.7615292290944735\n",
      "theta dot:  -0.0917113561595\n",
      "Angle:  49.599081460342234\n",
      "action:  [-0.1650709]\n",
      "state:  {1: array([ 4.0299957]), 2: array([ 0.45467493]), 3: array([ 0.63065217,  0.77606562,  0.        ])}\n",
      "cos(angle):  0.6306521695185504   sin(angle)  0.7760656164793963\n",
      "theta dot:  0.454674930796\n",
      "Angle:  50.90162614367807\n",
      "action:  [-0.05117358]\n",
      "state:  {1: array([ 4.08144811]), 2: array([ 1.02904811]), 3: array([ 0.58990475,  0.80747284,  0.        ])}\n",
      "cos(angle):  0.5899047484567185   sin(angle)  0.8074728402542192\n",
      "theta dot:  1.02904810683\n",
      "Angle:  53.849624921826944\n",
      "action:  [-0.22519807]\n",
      "state:  {1: array([ 4.16149176]), 2: array([ 1.60087303]), 3: array([ 0.52345192,  0.85205521,  0.        ])}\n",
      "cos(angle):  0.52345192204587   sin(angle)  0.8520552125927547\n",
      "theta dot:  1.60087302683\n",
      "Angle:  58.43577759609991\n",
      "action:  [-0.03328635]\n",
      "state:  {1: array([ 4.27323783]), 2: array([ 2.23492148]), 3: array([ 0.42517131,  0.9051129 ,  0.        ])}\n",
      "cos(angle):  0.4251713111071035   sin(angle)  0.9051128969424016\n",
      "theta dot:  2.2349214838\n",
      "Angle:  64.83834105236383\n",
      "action:  [ 0.08355159]\n",
      "state:  {1: array([ 4.41955228]), 2: array([ 2.9262889]), 3: array([ 0.28866932,  0.95742886,  0.        ])}\n",
      "cos(angle):  0.2886693240835608   sin(angle)  0.957428859672164\n",
      "theta dot:  2.92628889549\n",
      "Angle:  73.22152161622466\n",
      "action:  [ 0.19159266]\n",
      "state:  {1: array([ 4.60320725]), 2: array([ 3.67309944]), 3: array([ 0.10896494,  0.99404559,  0.        ])}\n",
      "cos(angle):  0.10896494058848905   sin(angle)  0.9940455933821885\n",
      "theta dot:  3.67309943958\n",
      "Angle:  83.74415179073549\n",
      "action:  [ 0.27562501]\n",
      "state:  {1: array([ 4.82620612]), 2: array([ 4.45997739]), 3: array([-0.11357156,  0.99352982,  0.        ])}\n",
      "cos(angle):  -0.11357155985506959   sin(angle)  0.9935298187734912\n",
      "theta dot:  4.4599773856\n",
      "Angle:  96.52101595879878\n",
      "action:  [ 0.4574107]\n",
      "state:  {1: array([ 5.08989294]), 2: array([ 5.27373636]), 3: array([-0.36860133,  0.92958758,  0.        ])}\n",
      "cos(angle):  -0.36860132871604606   sin(angle)  0.9295875754703079\n",
      "theta dot:  5.27373635507\n",
      "Angle:  111.62912239997644\n",
      "action:  [ 0.52388938]\n",
      "state:  {1: array([ 5.39236846]), 2: array([ 6.04951044]), 3: array([-0.62877707,  0.77758562,  0.        ])}\n",
      "cos(angle):  -0.6287770668008572   sin(angle)  0.7775856224656617\n",
      "theta dot:  6.04951044355\n",
      "Angle:  128.95965270045866\n",
      "action:  [ 0.5325719]\n",
      "state:  {1: array([ 5.72799773]), 2: array([ 6.71258545]), 3: array([-0.84980158,  0.52710271,  0.        ])}\n",
      "cos(angle):  -0.849801583319096   sin(angle)  0.5271027119910858\n",
      "theta dot:  6.712585446\n",
      "Angle:  148.18974851597795\n",
      "action:  [ 0.30283382]\n",
      "state:  {1: array([ 6.08566461]), 2: array([ 7.15333755]), 3: array([-0.98055613,  0.19623884,  0.        ])}\n",
      "cos(angle):  -0.9805561262980454   sin(angle)  0.19623884217802443\n",
      "theta dot:  7.15333755247\n",
      "Angle:  168.68250315444016\n",
      "action:  [ 0.10372442]\n",
      "state:  {1: array([ 0.16828307]), 2: array([ 7.31607535]), 3: array([-0.98587379, -0.16748992,  0.        ])}\n",
      "cos(angle):  -0.9858737885323674   sin(angle)  -0.16748991935288768\n",
      "theta dot:  7.31607534777\n",
      "Angle:  189.64230797923773\n",
      "action:  [-0.17037979]\n",
      "state:  {1: array([ 0.52652812]), 2: array([ 7.16490094]), 3: array([-0.86455702, -0.50253473,  0.        ])}\n",
      "cos(angle):  -0.8645570200806064   sin(angle)  -0.50253473415212\n",
      "theta dot:  7.16490093906\n",
      "Angle:  210.16818920268088\n",
      "action:  [ 0.43636568]\n",
      "state:  {1: array([ 0.86920085]), 2: array([ 6.85345474]), 3: array([-0.64543715, -0.76381338,  0.        ])}\n",
      "cos(angle):  -0.6454371525508629   sin(angle)  -0.7638133817281771\n",
      "theta dot:  6.85345474114\n",
      "Angle:  229.8018448782276\n",
      "action:  [ 0.42086312]\n",
      "state:  {1: array([ 1.18638706]), 2: array([ 6.34372417]), 3: array([-0.37501158, -0.92702013,  0.        ])}\n",
      "cos(angle):  -0.37501157927200646   sin(angle)  -0.9270201267566501\n",
      "theta dot:  6.34372417227\n",
      "Angle:  247.97523345424884\n",
      "action:  [ 0.7000544]\n",
      "state:  {1: array([ 1.47406042]), 2: array([ 5.75346724]), 3: array([-0.0965851 , -0.99532473,  0.        ])}\n",
      "cos(angle):  -0.0965851009637332   sin(angle)  -0.9953247300614134\n",
      "theta dot:  5.75346723651\n",
      "Angle:  264.4576644220946\n",
      "action:  [ 0.16172549]\n",
      "state:  {1: array([ 1.72562205]), 2: array([ 5.03123251]), 3: array([ 0.15420791, -0.98803842,  0.        ])}\n",
      "cos(angle):  0.1542079084336963   sin(angle)  -0.9880384207997707\n",
      "theta dot:  5.03123251306\n",
      "Angle:  278.8710501547035\n",
      "action:  [ 0.2070641]\n",
      "state:  {1: array([ 1.94168521]), 2: array([ 4.32126331]), 3: array([ 0.36244402, -0.93200554,  0.        ])}\n",
      "cos(angle):  0.3624440234059481   sin(angle)  -0.9320055417739256\n",
      "theta dot:  4.32126331292\n",
      "Angle:  291.25052870584494\n",
      "action:  [ 0.08637902]\n",
      "state:  {1: array([ 2.12344602]), 2: array([ 3.63521601]), 3: array([ 0.52494432, -0.85113657,  0.        ])}\n",
      "cos(angle):  0.5249443156779168   sin(angle)  -0.8511365727293381\n",
      "theta dot:  3.63521600965\n",
      "Angle:  301.6646311017196\n",
      "action:  [-0.40405655]\n",
      "state:  {1: array([ 2.27025877]), 2: array([ 2.9362551]), 3: array([ 0.64380645, -0.76518838,  0.        ])}\n",
      "cos(angle):  0.643806448047146   sin(angle)  -0.7651883803697737\n",
      "theta dot:  2.93625509786\n",
      "Angle:  310.07636266549736\n",
      "action:  [-0.33302586]\n",
      "state:  {1: array([ 2.38587927]), 2: array([ 2.31240993]), 3: array([ 0.72778248, -0.68580803,  0.        ])}\n",
      "cos(angle):  0.7277824819032579   sin(angle)  -0.6858080336615591\n",
      "theta dot:  2.3124099329\n",
      "Angle:  316.7009136573761\n",
      "action:  [-0.7119311]\n",
      "state:  {1: array([ 2.47044248]), 2: array([ 1.69126424]), 3: array([ 0.78310691, -0.62188711,  0.        ])}\n",
      "cos(angle):  0.7831069051088962   sin(angle)  -0.621887108059627\n",
      "theta dot:  1.69126424216\n",
      "Angle:  321.546017483278\n",
      "action:  [-0.40590899]\n",
      "state:  {1: array([ 2.52864061]), 2: array([ 1.16396256]), 3: array([ 0.81795332, -0.5752846 ,  0.        ])}\n",
      "cos(angle):  0.8179533173545663   sin(angle)  -0.5752845996797238\n",
      "theta dot:  1.1639625628\n",
      "Angle:  324.8805168037539\n",
      "action:  [-0.87047464]\n",
      "state:  {1: array([ 2.558737]), 2: array([ 0.60192792]), 3: array([ 0.83489428, -0.55041034,  0.        ])}\n",
      "cos(angle):  0.8348942764583454   sin(angle)  -0.5504103443223938\n",
      "theta dot:  0.601927917487\n",
      "Angle:  326.6049092335304\n",
      "action:  [-0.83492639]\n",
      "state:  {1: array([ 2.56193106]), 2: array([ 0.0638812]), 3: array([ 0.83664806, -0.54774084,  0.        ])}\n",
      "cos(angle):  0.8366480583336543   sin(angle)  -0.5477408387974428\n",
      "theta dot:  0.0638812000117\n",
      "Angle:  326.78791496312846\n",
      "action:  [-0.74227908]\n",
      "state:  {1: array([ 2.53901775]), 2: array([-0.45826629]), 3: array([ 0.82387898, -0.56676576,  0.        ])}\n",
      "cos(angle):  0.823878979541311   sin(angle)  -0.5667657603189946\n",
      "theta dot:  -0.458266290361\n",
      "Angle:  325.4750818165638\n",
      "action:  [-0.59881038]\n",
      "state:  {1: array([ 2.49035964]), 2: array([-0.97316217]), 3: array([ 0.79533699, -0.60616753,  0.        ])}\n",
      "cos(angle):  0.7953369904461957   sin(angle)  -0.6061675276917992\n",
      "theta dot:  -0.973162167116\n",
      "Angle:  322.68718408800487\n",
      "action:  [-0.42111185]\n",
      "state:  {1: array([ 2.41581191]), 2: array([-1.49095459]), 3: array([ 0.74798146, -0.66371963,  0.        ])}\n",
      "cos(angle):  0.7479814553554701   sin(angle)  -0.6637196263817372\n",
      "theta dot:  -1.4909545911\n",
      "Angle:  318.41592380027566\n",
      "action:  [-0.24524643]\n",
      "state:  {1: array([ 2.31453535]), 2: array([-2.02553128]), 3: array([ 0.67704434, -0.73594223,  0.        ])}\n",
      "cos(angle):  0.6770443403788167   sin(angle)  -0.7359422267821116\n",
      "theta dot:  -2.02553127501\n",
      "Angle:  312.6132177030294\n",
      "action:  [-0.10295213]\n",
      "state:  {1: array([ 2.18488881]), 2: array([-2.59293076]), 3: array([ 0.57621705, -0.81729671,  0.        ])}\n",
      "cos(angle):  0.576217047814607   sin(angle)  -0.8172967109977984\n",
      "theta dot:  -2.59293076472\n",
      "Angle:  305.18503560395806\n",
      "action:  [-0.02990358]\n",
      "state:  {1: array([ 2.02436937]), 2: array([-3.21038883]), 3: array([ 0.43818008, -0.89888721,  0.        ])}\n",
      "cos(angle):  0.43818008407302905   sin(angle)  -0.898887208676235\n",
      "theta dot:  -3.21038883498\n",
      "Angle:  295.9879705686957\n",
      "action:  [-0.03840555]\n",
      "state:  {1: array([ 1.82985361]), 2: array([-3.89031507]), 3: array([ 0.25616941, -0.9666319 ,  0.        ])}\n",
      "cos(angle):  0.25616940848748243   sin(angle)  -0.9666319020988151\n",
      "theta dot:  -3.89031507456\n",
      "Angle:  284.8430648929121\n",
      "action:  [-0.1527582]\n",
      "state:  {1: array([ 1.59794348]), 2: array([-4.63820273]), 3: array([ 0.02714382, -0.99963154,  0.        ])}\n",
      "cos(angle):  0.02714381519206487   sin(angle)  -0.9996315387665693\n",
      "theta dot:  -4.63820273122\n",
      "Angle:  271.5556239134725\n",
      "action:  [-0.27218935]\n",
      "state:  {1: array([ 1.32650574]), 2: array([-5.42875479]), 3: array([-0.24186804, -0.97030915,  0.        ])}\n",
      "cos(angle):  -0.2418680385663505   sin(angle)  -0.9703091527549693\n",
      "theta dot:  -5.42875478737\n",
      "Angle:  256.0034234148832\n",
      "action:  [-0.22240127]\n",
      "state:  {1: array([ 1.01701339]), 2: array([-6.18984684]), 3: array([-0.52590852, -0.85054114,  0.        ])}\n",
      "cos(angle):  -0.5259085235652761   sin(angle)  -0.8505411364780608\n",
      "theta dot:  -6.18984684268\n",
      "Angle:  238.27085988542007\n",
      "action:  [-0.23625246]\n",
      "state:  {1: array([ 0.67385387]), 2: array([-6.86319056]), 3: array([-0.78142265, -0.62400211,  0.        ])}\n",
      "cos(angle):  -0.7814226538813087   sin(angle)  -0.6240021121767877\n",
      "theta dot:  -6.86319056368\n",
      "Angle:  218.60931319803407\n",
      "action:  [-0.07482397]\n",
      "state:  {1: array([ 0.30673308]), 2: array([-7.34241574]), 3: array([-0.95332509, -0.30194582,  0.        ])}\n",
      "cos(angle):  -0.9533250884385714   sin(angle)  -0.30194581592330416\n",
      "theta dot:  -7.34241574387\n",
      "Angle:  197.5748907079599\n",
      "action:  [ 0.25345657]\n",
      "state:  {1: array([ 6.21337556]), 2: array([-7.53085662]), 3: array([-0.99756429,  0.06975306,  0.        ])}\n",
      "cos(angle):  -0.9975642887167925   sin(angle)  0.06975306356684169\n",
      "theta dot:  -7.53085661977\n",
      "Angle:  175.9997843018068\n",
      "action:  [ 0.05132422]\n",
      "state:  {1: array([ 5.8398334]), 2: array([-7.47084319]), 3: array([-0.90331887,  0.42896971,  0.        ])}\n",
      "cos(angle):  -0.9033188721367261   sin(angle)  0.42896971366476805\n",
      "theta dot:  -7.47084318857\n",
      "Angle:  154.59744514434226\n",
      "action:  [ 0.56202451]\n",
      "state:  {1: array([ 5.48659278]), 2: array([-7.06481223]), 3: array([-0.69914703,  0.71497792,  0.        ])}\n",
      "cos(angle):  -0.6991470347767026   sin(angle)  0.7149779183743679\n",
      "theta dot:  -7.06481222674\n",
      "Angle:  134.35829629003183\n",
      "action:  [ 0.61066154]\n",
      "state:  {1: array([ 5.16474381]), 2: array([-6.43697956]), 3: array([-0.43708472,  0.89942034,  0.        ])}\n",
      "cos(angle):  -0.4370847229355819   sin(angle)  0.8994203383159209\n",
      "theta dot:  -6.43697955747\n",
      "Angle:  115.91775133929195\n",
      "action:  [-1.14084218]\n",
      "state:  {1: array([ 4.86806678]), 2: array([-5.93354063]), 3: array([-0.15504973,  0.98790667,  0.        ])}\n",
      "cos(angle):  -0.15504973308653755   sin(angle)  0.9879066657684791\n",
      "theta dot:  -5.93354063094\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  98.91944930259973\n",
      "action:  [ 0.60357806]\n",
      "state:  {1: array([ 4.61296308]), 2: array([-5.10207392]), 3: array([ 0.09926217,  0.99506132,  0.        ])}\n",
      "cos(angle):  0.09926216939550864   sin(angle)  0.9950613155614569\n",
      "theta dot:  -5.10207392198\n",
      "Angle:  84.30311835727406\n",
      "action:  [ 0.39963259]\n",
      "state:  {1: array([ 4.39817143]), 2: array([-4.29583305]), 3: array([ 0.30907243,  0.9510385 ,  0.        ])}\n",
      "cos(angle):  0.309072429112106   sin(angle)  0.9510385026710234\n",
      "theta dot:  -4.29583304754\n",
      "Angle:  71.99649197967307\n",
      "action:  [ 0.26870154]\n",
      "state:  {1: array([ 4.22105898]), 2: array([-3.54224894]), 3: array([ 0.47179897,  0.88170615,  0.        ])}\n",
      "cos(angle):  0.4717989746700298   sin(angle)  0.8817061457766463\n",
      "theta dot:  -3.54224893994\n",
      "Angle:  61.848719997419494\n",
      "action:  [ 0.22296584]\n",
      "state:  {1: array([ 4.07868276]), 2: array([-2.84752445]), 3: array([ 0.59213544,  0.80583846,  0.        ])}\n",
      "cos(angle):  0.5921354350943979   sin(angle)  0.8058384617934093\n",
      "theta dot:  -2.84752445467\n",
      "Angle:  53.69118240763504\n",
      "action:  [ 0.25699436]\n",
      "state:  {1: array([ 3.96845293]), 2: array([-2.20459645]), 3: array([ 0.67718933,  0.73580882,  0.        ])}\n",
      "cos(angle):  0.6771893270576537   sin(angle)  0.7358088170980299\n",
      "theta dot:  -2.2045964538\n",
      "Angle:  47.37549355983663\n",
      "action:  [ 0.28891471]\n",
      "state:  {1: array([ 3.8879828]), 2: array([-1.60940263]), 3: array([ 0.73414471,  0.67899304,  0.        ])}\n",
      "cos(angle):  0.7341447104190866   sin(angle)  0.6789930369036751\n",
      "theta dot:  -1.60940263427\n",
      "Angle:  42.764905417350576\n",
      "action:  [ 0.19789634]\n",
      "state:  {1: array([ 3.83445913]), 2: array([-1.07047341]), 3: array([ 0.76941823,  0.63874532,  0.        ])}\n",
      "cos(angle):  0.769418228542089   sin(angle)  0.6387453245129499\n",
      "theta dot:  -1.07047340619\n",
      "Angle:  39.69823217576845\n",
      "action:  [ 0.06124222]\n",
      "state:  {1: array([ 3.80534773]), 2: array([-0.58222808]), 3: array([ 0.78768437,  0.616079  ,  0.        ])}\n",
      "cos(angle):  0.787684367527363   sin(angle)  0.616079002363348\n",
      "theta dot:  -0.582228079887\n",
      "Angle:  38.03027549160146\n",
      "action:  [-0.26131538]\n",
      "state:  {1: array([ 3.79737942]), 2: array([-0.15936614]), 3: array([ 0.79256842,  0.609783  ,  0.        ])}\n",
      "cos(angle):  0.7925684156191474   sin(angle)  0.6097829995686618\n",
      "theta dot:  -0.159366135561\n",
      "Angle:  37.57372621096397\n",
      "action:  [-0.45434059]\n",
      "state:  {1: array([ 3.80887042]), 2: array([ 0.22982003]), 3: array([ 0.78550923,  0.61884995,  0.        ])}\n",
      "cos(angle):  0.7855092265731357   sin(angle)  0.6188499454378857\n",
      "theta dot:  0.22982002552\n",
      "Angle:  38.23211054686908\n",
      "action:  [-0.60677909]\n",
      "state:  {1: array([ 3.83901745]), 2: array([ 0.60294062]), 3: array([ 0.76649864,  0.64224593,  0.        ])}\n",
      "cos(angle):  0.7664986384683623   sin(angle)  0.6422459320432843\n",
      "theta dot:  0.602940620719\n",
      "Angle:  39.95940415091593\n",
      "action:  [-0.42466607]\n",
      "state:  {1: array([ 3.89006371]), 2: array([ 1.02092516]), 3: array([ 0.7327302 ,  0.68051925,  0.        ])}\n",
      "cos(angle):  0.7327301990881026   sin(angle)  0.6805192542054264\n",
      "theta dot:  1.02092515951\n",
      "Angle:  42.88413245355241\n",
      "action:  [-0.05073229]\n",
      "state:  {1: array([ 3.96624895]), 2: array([ 1.52370476]), 3: array([ 0.67880939,  0.73431452,  0.        ])}\n",
      "cos(angle):  0.6788093923739854   sin(angle)  0.7343145162836295\n",
      "theta dot:  1.52370475634\n",
      "Angle:  47.249214834209226\n",
      "action:  [ 0.47652047]\n",
      "state:  {1: array([ 4.07354489]), 2: array([ 2.14591871]), 3: array([ 0.5962679 ,  0.80278552,  0.        ])}\n",
      "cos(angle):  0.5962678959635421   sin(angle)  0.8027855231898557\n",
      "theta dot:  2.14591871477\n",
      "Angle:  53.39680473518274\n",
      "action:  [ 0.50106819]\n",
      "state:  {1: array([ 4.21470329]), 2: array([ 2.82316809]), 3: array([ 0.47739326,  0.87868975,  0.        ])}\n",
      "cos(angle):  0.47739325888424755   sin(angle)  0.8786897497819568\n",
      "theta dot:  2.82316808554\n",
      "Angle:  61.484566630358415\n",
      "action:  [ 0.57745749]\n",
      "state:  {1: array([ 4.39314349]), 2: array([ 3.56880402]), 3: array([ 0.31385026,  0.94947249,  0.        ])}\n",
      "cos(angle):  0.31385026305071834   sin(angle)  0.9494724916410138\n",
      "theta dot:  3.56880402088\n",
      "Angle:  71.7084131378561\n",
      "action:  [ 0.49553718]\n",
      "state:  {1: array([ 4.61090544]), 2: array([ 4.35523897]), 3: array([ 0.10130944,  0.99485496,  0.        ])}\n",
      "cos(angle):  0.10130943591041101   sin(angle)  0.9948549633969337\n",
      "theta dot:  4.35523896605\n",
      "Angle:  84.18522453792144\n",
      "action:  [ 0.32159548]\n",
      "state:  {1: array([ 4.86838641]), 2: array([ 5.14961951]), 3: array([-0.1553655 ,  0.98785706,  0.        ])}\n",
      "cos(angle):  -0.15536549897959812   sin(angle)  0.9878570553105447\n",
      "theta dot:  5.14961951101\n",
      "Angle:  98.93776324402148\n",
      "action:  [ 0.58415804]\n",
      "state:  {1: array([ 5.16729322]), 2: array([ 5.97813601]), 3: array([-0.43937629,  0.89830311,  0.        ])}\n",
      "cos(angle):  -0.4393762896451091   sin(angle)  0.8983031092552765\n",
      "theta dot:  5.97813600888\n",
      "Angle:  116.06382132904878\n",
      "action:  [-0.03367667]\n",
      "state:  {1: array([ 5.49963381]), 2: array([ 6.64681184]), 3: array([-0.70841136,  0.70579979,  0.        ])}\n",
      "cos(angle):  -0.7084113629801189   sin(angle)  0.7057997880423671\n",
      "theta dot:  6.64681183987\n",
      "Angle:  135.10549008345936\n",
      "action:  [-0.08449229]\n",
      "state:  {1: array([ 5.8578082]), 2: array([ 7.16348784]), 3: array([-0.91088318,  0.41266431,  0.        ])}\n",
      "cos(angle):  -0.9108831786698617   sin(angle)  0.41266431250629\n",
      "theta dot:  7.16348783792\n",
      "Angle:  155.62732307978627\n",
      "action:  [-0.11520529]\n",
      "state:  {1: array([ 6.23059346]), 2: array([ 7.45570528]), 3: array([-0.99861737,  0.0525676 ,  0.        ])}\n",
      "cos(angle):  -0.9986173676915095   sin(angle)  0.052567603567981534\n",
      "theta dot:  7.45570527813\n",
      "Angle:  176.98629541974253\n",
      "action:  [-0.15059768]\n",
      "state:  {1: array([ 0.32103522]), 2: array([ 7.47254133]), 3: array([-0.94890926, -0.31554906,  0.        ])}\n",
      "cos(angle):  -0.9489092632058317   sin(angle)  -0.3155490614819914\n",
      "theta dot:  7.4725413284\n",
      "Angle:  198.39434121274212\n",
      "action:  [ 0.00276184]\n",
      "state:  {1: array([ 0.68284991]), 2: array([ 7.23629381]), 3: array([-0.77577756, -0.63100648,  0.        ])}\n",
      "cos(angle):  -0.7757775582927382   sin(angle)  -0.6310064817807794\n",
      "theta dot:  7.23629380756\n",
      "Angle:  219.124747460536\n",
      "action:  [-0.00836968]\n",
      "state:  {1: array([ 1.02093909]), 2: array([ 6.76178349]), 3: array([-0.52256552, -0.85259913,  0.        ])}\n",
      "cos(angle):  -0.5225655167847225   sin(angle)  -0.8525991324576374\n",
      "theta dot:  6.76178349371\n",
      "Angle:  238.49578497117346\n",
      "action:  [ 0.16963244]\n",
      "state:  {1: array([ 1.32832804]), 2: array([ 6.14777901]), 3: array([-0.24009944, -0.9707483 ,  0.        ])}\n",
      "cos(angle):  -0.24009944275539377   sin(angle)  -0.970748297752074\n",
      "theta dot:  6.14777900976\n",
      "Angle:  256.107833318468\n",
      "action:  [ 0.38115256]\n",
      "state:  {1: array([ 1.60217257]), 2: array([ 5.47689067]), 3: array([ 0.0313711 , -0.99950781,  0.        ])}\n",
      "cos(angle):  0.031371096622944634   sin(angle)  -0.9995078060208804\n",
      "theta dot:  5.4768906709\n",
      "Angle:  271.79793264304016\n",
      "action:  [ 0.48211066]\n",
      "state:  {1: array([ 1.84215139]), 2: array([ 4.79957642]), 3: array([ 0.26803716, -0.96340857,  0.        ])}\n",
      "cos(angle):  0.26803716390540583   sin(angle)  -0.9634085731223003\n",
      "theta dot:  4.79957641558\n",
      "Angle:  285.5476740933366\n",
      "action:  [ 0.58793637]\n",
      "state:  {1: array([ 2.05041191]), 2: array([ 4.16521044]), 3: array([ 0.46143817, -0.88717237,  0.        ])}\n",
      "cos(angle):  0.4614381693964527   sin(angle)  -0.8871723709764922\n",
      "theta dot:  4.1652104417\n",
      "Angle:  297.48009514481373\n",
      "action:  [ 0.4251622]\n",
      "state:  {1: array([ 2.22859219]), 2: array([ 3.56360549]), 3: array([ 0.61137411, -0.79134171,  0.        ])}\n",
      "cos(angle):  0.6113741121909929   sin(angle)  -0.7913417055499319\n",
      "theta dot:  3.56360549307\n",
      "Angle:  307.68904900197623\n",
      "action:  [ 0.21449127]\n",
      "state:  {1: array([ 2.37870583]), 2: array([ 3.0022729]), 3: array([ 0.7228442 , -0.69101104,  0.        ])}\n",
      "cos(angle):  0.7228442013436727   sin(angle)  -0.6910110423023846\n",
      "theta dot:  3.00227290483\n",
      "Angle:  316.2899072090965\n",
      "action:  [ 0.0049252]\n",
      "state:  {1: array([ 2.5029435]), 2: array([ 2.4847534]), 3: array([ 0.80290175, -0.59611139,  0.        ])}\n",
      "cos(angle):  0.8029017476550975   sin(angle)  -0.5961113852396969\n",
      "theta dot:  2.48475340311\n",
      "Angle:  323.4081847199238\n",
      "Reset!\n",
      "min:  -2.68980993741  self.max:  28.6954689906\n",
      "state:  {1: array([ 5.60982431]), 2: array([-0.01298641]), 3: array([-0.78173011,  0.6236169 ,  0.        ])}\n",
      "cos(angle):  -0.7817301088492439   sin(angle)  0.6236168991604937\n",
      "theta dot:  -0.0129864114926\n",
      "Angle:  141.41892592038727\n",
      "action:  [-0.29700986]\n",
      "state:  {1: array([ 5.63033305]), 2: array([ 0.41017478]), 3: array([-0.79435441,  0.60745458,  0.        ])}\n",
      "cos(angle):  -0.7943544133046815   sin(angle)  0.6074545794241536\n",
      "theta dot:  0.410174784527\n",
      "Angle:  142.59398737338654\n",
      "action:  [-0.34193129]\n",
      "state:  {1: array([ 5.67105685]), 2: array([ 0.81447603]), 3: array([-0.81842684,  0.57461075,  0.        ])}\n",
      "cos(angle):  -0.8184268370133327   sin(angle)  0.574610748643246\n",
      "theta dot:  0.814476025312\n",
      "Angle:  144.92728385537288\n",
      "action:  [-0.36831271]\n",
      "state:  {1: array([ 5.73056621]), 2: array([ 1.19018718]), 3: array([-0.85115263,  0.52491828,  0.        ])}\n",
      "cos(angle):  -0.85115262929856   sin(angle)  0.5249182809144184\n",
      "theta dot:  1.19018718042\n",
      "Angle:  148.3369109956268\n",
      "action:  [-0.35248804]\n",
      "state:  {1: array([ 5.80711634]), 2: array([ 1.53100268]), 3: array([-0.88880333,  0.45828881,  0.        ])}\n",
      "cos(angle):  -0.888803334326056   sin(angle)  0.45828880947595174\n",
      "theta dot:  1.53100268485\n",
      "Angle:  152.72290035252422\n",
      "action:  [-0.34294416]\n",
      "state:  {1: array([ 5.89828022]), 2: array([ 1.82327767]), 3: array([-0.92683408,  0.37547117,  0.        ])}\n",
      "cos(angle):  -0.9268340755656127   sin(angle)  0.3754711658309278\n",
      "theta dot:  1.82327766851\n",
      "Angle:  157.94619390248943\n",
      "action:  [-0.27433221]\n",
      "state:  {1: array([ 6.00146679]), 2: array([ 2.06373121]), 3: array([-0.9605791 ,  0.27800683,  0.        ])}\n",
      "cos(angle):  -0.9605790961015845   sin(angle)  0.27800683468696036\n",
      "theta dot:  2.06373121139\n",
      "Angle:  163.8583345004232\n",
      "action:  [-0.22551816]\n",
      "state:  {1: array([ 6.11338722]), 2: array([ 2.23840861]), 3: array([-0.98561891,  0.16898335,  0.        ])}\n",
      "cos(angle):  -0.9856189061826053   sin(angle)  0.16898334762752415\n",
      "theta dot:  2.2384086132\n",
      "Angle:  170.2708878231997\n",
      "action:  [-0.28247133]\n",
      "state:  {1: array([ 6.22952599]), 2: array([ 2.32277543]), 3: array([-0.99856068,  0.05363357,  0.        ])}\n",
      "cos(angle):  -0.9985606840824409   sin(angle)  0.053633573485343214\n",
      "theta dot:  2.32277542509\n",
      "Angle:  176.9251336933978\n",
      "action:  [-0.333241]\n",
      "state:  {1: array([ 0.0619914]), 2: array([ 2.31301445]), 3: array([-0.99807915, -0.06195171,  0.        ])}\n",
      "cos(angle):  -0.9980791482660788   sin(angle)  -0.0619517053555319\n",
      "theta dot:  2.31301445465\n",
      "Angle:  183.5522583431202\n",
      "action:  [-0.69703581]\n",
      "state:  {1: array([ 0.17009117]), 2: array([ 2.1619953]), 3: array([-0.98556934, -0.1692722 ,  0.        ])}\n",
      "cos(angle):  -0.985569338844844   sin(angle)  -0.16927220187891762\n",
      "theta dot:  2.16199530361\n",
      "Angle:  189.74590417088945\n",
      "action:  [-0.53437737]\n",
      "state:  {1: array([ 0.2678354]), 2: array([ 1.95488455]), 3: array([-0.96434601, -0.26464463,  0.        ])}\n",
      "cos(angle):  -0.9643460062169559   sin(angle)  -0.2646446301994562\n",
      "theta dot:  1.95488454706\n",
      "Angle:  195.34622277399865\n",
      "action:  [-0.25580326]\n",
      "state:  {1: array([ 0.35373692]), 2: array([ 1.71803058]), 3: array([-0.93808477, -0.34640577,  0.        ])}\n",
      "cos(angle):  -0.9380847737003253   sin(angle)  -0.34640576980126847\n",
      "theta dot:  1.71803058481\n",
      "Angle:  200.26800634392643\n",
      "action:  [ 0.01198415]\n",
      "state:  {1: array([ 0.42673812]), 2: array([ 1.46002388]), 3: array([-0.91032069, -0.41390365,  0.        ])}\n",
      "cos(angle):  -0.9103206948516291   sin(angle)  -0.4139036512581727\n",
      "theta dot:  1.46002388063\n",
      "Angle:  204.45065688049377\n",
      "action:  [ 0.26758365]\n",
      "state:  {1: array([ 0.4862248]), 2: array([ 1.18973369]), 3: array([-0.88410327, -0.46729156,  0.        ])}\n",
      "cos(angle):  -0.884103272350218   sin(angle)  -0.4672915618964634\n",
      "theta dot:  1.18973369032\n",
      "Angle:  207.85898487034586\n",
      "action:  [ 0.60280705]\n",
      "state:  {1: array([ 0.53270911]), 2: array([ 0.92968608]), 3: array([-0.86143436, -0.50786892,  0.        ])}\n",
      "cos(angle):  -0.8614343622752576   sin(angle)  -0.5078689195958147\n",
      "theta dot:  0.929686075717\n",
      "Angle:  210.52233306281295\n",
      "action:  [ 0.6243211]\n",
      "state:  {1: array([ 0.56483073]), 2: array([ 0.64243255]), 3: array([-0.84467922, -0.53527285,  0.        ])}\n",
      "cos(angle):  -0.8446792158977893   sin(angle)  -0.5352728483963071\n",
      "theta dot:  0.642432551543\n",
      "Angle:  212.3627624503508\n",
      "action:  [ 0.77985881]\n",
      "state:  {1: array([ 0.58272857]), 2: array([ 0.35795674]), 3: array([-0.83496422, -0.55030424,  0.        ])}\n",
      "cos(angle):  -0.8349642157613903   sin(angle)  -0.550304241668158\n",
      "theta dot:  0.357956736118\n",
      "Angle:  213.38823056375313\n",
      "action:  [ 0.5630087]\n",
      "state:  {1: array([ 0.58421256]), 2: array([ 0.02967986]), 3: array([-0.83414665, -0.55154272,  0.        ])}\n",
      "cos(angle):  -0.8341466490396585   sin(angle)  -0.5515427162930436\n",
      "theta dot:  0.0296798594681\n",
      "Angle:  213.47325689912788\n",
      "action:  [ 0.24028708]\n",
      "state:  {1: array([ 0.56681586]), 2: array([-0.34793412]), 3: array([-0.84361497, -0.53694859,  0.        ])}\n",
      "cos(angle):  -0.8436149692898318   sin(angle)  -0.5369485856114309\n",
      "theta dot:  -0.347934115307\n",
      "Angle:  212.47650141218966\n",
      "action:  [-0.01338401]\n",
      "state:  {1: array([ 0.5291832]), 2: array([-0.75265316]), 3: array([-0.8632197 , -0.50482843,  0.        ])}\n",
      "cos(angle):  -0.8632197020499043   sin(angle)  -0.5048284322350263\n",
      "theta dot:  -0.752653155398\n",
      "Angle:  210.32031399221768\n",
      "action:  [-0.06462499]\n",
      "state:  {1: array([ 0.47213479]), 2: array([-1.14096823]), 3: array([-0.89059944, -0.45478856,  0.        ])}\n",
      "cos(angle):  -0.8905994406670905   sin(angle)  -0.4547885621730889\n",
      "theta dot:  -1.14096822845\n",
      "Angle:  207.0516884332377\n",
      "action:  [ 0.04881446]\n",
      "state:  {1: array([ 0.39839791]), 2: array([-1.47473748]), 3: array([-0.92168369, -0.38794223,  0.        ])}\n",
      "cos(angle):  -0.9216836929993821   sin(angle)  -0.38794222515604143\n",
      "theta dot:  -1.47473748145\n",
      "Angle:  202.82688663380716\n",
      "action:  [ 0.28682106]\n",
      "state:  {1: array([ 0.31226437]), 2: array([-1.72267099]), 3: array([-0.95164036, -0.30721428,  0.        ])}\n",
      "cos(angle):  -0.9516403647824525   sin(angle)  -0.3072142837120707\n",
      "theta dot:  -1.72267099149\n",
      "Angle:  197.8918093090658\n",
      "action:  [ 0.32463351]\n",
      "state:  {1: array([ 0.21704503]), 2: array([-1.90438668]), 3: array([-0.97653805, -0.21534493,  0.        ])}\n",
      "cos(angle):  -0.9765380493698375   sin(angle)  -0.21534492827311483\n",
      "theta dot:  -1.90438667816\n",
      "Angle:  192.4361561057906\n",
      "action:  [ 0.44323614]\n",
      "state:  {1: array([ 0.11707453]), 2: array([-1.99940995]), 3: array([-0.9931546 , -0.11680727,  0.        ])}\n",
      "cos(angle):  -0.9931546009635684   sin(angle)  -0.1168072711131252\n",
      "theta dot:  -1.99940995262\n",
      "Angle:  186.70828190995914\n",
      "action:  [ 0.43087776]\n",
      "state:  {1: array([ 0.01595535]), 2: array([-2.02238374]), 3: array([-0.99987272, -0.01595467,  0.        ])}\n",
      "cos(angle):  -0.9998727161553459   sin(angle)  -0.01595466979072287\n",
      "theta dot:  -2.02238374218\n",
      "Angle:  180.9145928089744\n",
      "action:  [ 0.35948462]\n",
      "state:  {1: array([ 6.2001193]), 2: array([-1.98042705]), 3: array([-0.996552  ,  0.08297051,  0.        ])}\n",
      "cos(angle):  -0.9965520026099093   sin(angle)  0.08297051340192656\n",
      "theta dot:  -1.98042705143\n",
      "Angle:  175.24025865740714\n",
      "action:  [-0.06406288]\n",
      "state:  {1: array([ 6.10372887]), 2: array([-1.9278086]), 3: array([-0.98394086,  0.17849476,  0.        ])}\n",
      "cos(angle):  -0.9839408615209441   sin(angle)  0.17849476471152423\n",
      "theta dot:  -1.92780859787\n",
      "Angle:  169.7175067536532\n",
      "action:  [-0.10224038]\n",
      "state:  {1: array([ 6.01326519]), 2: array([-1.80927358]), 3: array([-0.9637922 ,  0.26665444,  0.        ])}\n",
      "cos(angle):  -0.9637922011862613   sin(angle)  0.26665444480177175\n",
      "theta dot:  -1.80927358157\n",
      "Angle:  164.5343318637449\n",
      "action:  [-0.26305561]\n",
      "state:  {1: array([ 5.93082814]), 2: array([-1.64874109]), 3: array([-0.93856184,  0.34511111,  0.        ])}\n",
      "cos(angle):  -0.9385618357305628   sin(angle)  0.345111113281036\n",
      "theta dot:  -1.64874108943\n",
      "Angle:  159.81104761213214\n",
      "action:  [-0.3424011]\n",
      "state:  {1: array([ 5.85876474]), 2: array([-1.44126792]), 3: array([-0.91127749,  0.41179283,  0.        ])}\n",
      "cos(angle):  -0.911277493006716   sin(angle)  0.41179282502175124\n",
      "theta dot:  -1.44126791894\n",
      "Angle:  155.6821288221862\n",
      "action:  [-0.24596496]\n",
      "state:  {1: array([ 5.80029884]), 2: array([-1.16931804]), 3: array([-0.88565832,  0.46433753,  0.        ])}\n",
      "cos(angle):  -0.8856583190100733   sin(angle)  0.4643375302172454\n",
      "theta dot:  -1.16931804379\n",
      "Angle:  152.33228721468447\n",
      "action:  [-0.05145438]\n",
      "state:  {1: array([ 5.75885969]), 2: array([-0.82878305]), 3: array([-0.86566175,  0.50062933,  0.        ])}\n",
      "cos(angle):  -0.8656617528278724   sin(angle)  0.5006293336301575\n",
      "theta dot:  -0.828783052978\n",
      "Angle:  149.95800421340948\n",
      "action:  [ 0.08519528]\n",
      "state:  {1: array([ 5.7368331]), 2: array([-0.44053176]), 3: array([-0.8544255 ,  0.51957393,  0.        ])}\n",
      "cos(angle):  -0.8544255000805112   sin(angle)  0.5195739262243326\n",
      "theta dot:  -0.440531760453\n",
      "Angle:  148.69597663380853\n",
      "action:  [ 0.4625585]\n",
      "state:  {1: array([ 5.73775972]), 2: array([ 0.01853246]), 3: array([-0.85490658,  0.51878197,  0.        ])}\n",
      "cos(angle):  -0.8549065823244224   sin(angle)  0.5187819729890154\n",
      "theta dot:  0.0185324592407\n",
      "Angle:  148.74906809458219\n",
      "action:  [ 0.02055604]\n",
      "state:  {1: array([ 5.75829484]), 2: array([ 0.41070235]), 3: array([-0.86537884,  0.50111822,  0.        ])}\n",
      "cos(angle):  -0.8653788353762639   sin(angle)  0.5011182208649183\n",
      "theta dot:  0.410702345494\n",
      "Angle:  149.92564089489042\n",
      "action:  [-0.05789962]\n",
      "state:  {1: array([ 5.79718764]), 2: array([ 0.77785607]), 3: array([-0.88420939,  0.46709074,  0.        ])}\n",
      "cos(angle):  -0.8842093894303736   sin(angle)  0.46709073598516787\n",
      "theta dot:  0.777856068817\n",
      "Angle:  152.1540291745414\n",
      "action:  [ 0.02172299]\n",
      "state:  {1: array([ 5.85375927]), 2: array([ 1.13143257]), 3: array([-0.90920487,  0.41634902,  0.        ])}\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "cos(angle):  -0.9092048687867388   sin(angle)  0.41634902014354397\n",
      "theta dot:  1.13143256862\n",
      "Angle:  155.39533714422078\n",
      "action:  [ 0.10357647]\n",
      "state:  {1: array([ 5.92672081]), 2: array([ 1.4592308]), 3: array([-0.93713644,  0.34896317,  0.        ])}\n",
      "cos(angle):  -0.9371364388887065   sin(angle)  0.34896317127598686\n",
      "theta dot:  1.45923080475\n",
      "Angle:  159.5757156910538\n",
      "action:  [ 0.16777933]\n",
      "state:  {1: array([ 6.01402682]), 2: array([ 1.74612008]), 3: array([-0.96399501,  0.26592032,  0.        ])}\n",
      "cos(angle):  -0.963995011845545   sin(angle)  0.2659203210303932\n",
      "theta dot:  1.74612008274\n",
      "Angle:  164.57796955680897\n",
      "action:  [ 0.44727268]\n",
      "state:  {1: array([ 6.11465938]), 2: array([ 2.01265123]), 3: array([-0.98583308,  0.16772935,  0.        ])}\n",
      "cos(angle):  -0.9858330826097088   sin(angle)  0.1677293451732256\n",
      "theta dot:  2.01265122526\n",
      "Angle:  170.34377711579702\n",
      "action:  [ 0.11707829]\n",
      "state:  {1: array([ 6.22245988]), 2: array([ 2.15600998]), 3: array([-0.99815678,  0.06068812,  0.        ])}\n",
      "cos(angle):  -0.9981567775107006   sin(angle)  0.06068811670709303\n",
      "theta dot:  2.15600997749\n",
      "Angle:  176.52027628737483\n",
      "action:  [ 0.2048343]\n",
      "state:  {1: array([ 0.05088713]), 2: array([ 2.23225121]), 3: array([-0.99870553, -0.05086517,  0.        ])}\n",
      "cos(angle):  -0.9987055294219994   sin(angle)  -0.05086516983087613\n",
      "theta dot:  2.23225120955\n",
      "Angle:  182.91603182392927\n",
      "action:  [ 0.38567593]\n",
      "state:  {1: array([ 0.16348482]), 2: array([ 2.25195372]), 3: array([-0.9866661 , -0.16275754,  0.        ])}\n",
      "cos(angle):  -0.9866660955999151   sin(angle)  -0.16275753682585337\n",
      "theta dot:  2.25195372162\n",
      "Angle:  189.36738893324303\n",
      "action:  [ 0.01518883]\n",
      "state:  {1: array([ 0.27009301]), 2: array([ 2.13216389]), 3: array([-0.96374608, -0.26682108,  0.        ])}\n",
      "cos(angle):  -0.9637460835635856   sin(angle)  -0.2668210756590457\n",
      "theta dot:  2.13216389361\n",
      "Angle:  195.47557426635336\n",
      "action:  [ 0.17666565]\n",
      "state:  {1: array([ 0.36802041]), 2: array([ 1.95854793]), 3: array([-0.93304137, -0.3597691 ,  0.        ])}\n",
      "cos(angle):  -0.933041369905298   sin(angle)  -0.3597690954560229\n",
      "theta dot:  1.958547934\n",
      "Angle:  201.08638767544224\n",
      "action:  [-0.65288542]\n",
      "state:  {1: array([ 0.44755982]), 2: array([ 1.5907883]), 3: array([-0.90150581, -0.43276699,  0.        ])}\n",
      "cos(angle):  -0.9015058140171286   sin(angle)  -0.43276698960677956\n",
      "theta dot:  1.5907882996\n",
      "Angle:  205.64364980188242\n",
      "action:  [-0.52557839]\n",
      "state:  {1: array([ 0.50692864]), 2: array([ 1.1873763]), 3: array([-0.87423976, -0.48549443,  0.        ])}\n",
      "cos(angle):  -0.8742397587088572   sin(angle)  -0.4854944328132704\n",
      "theta dot:  1.18737629828\n",
      "Angle:  209.04522437679583\n",
      "action:  [-0.64779687]\n",
      "state:  {1: array([ 0.54323293]), 2: array([ 0.72608594]), 3: array([-0.85604203, -0.51690622,  0.        ])}\n",
      "cos(angle):  -0.856042034428462   sin(angle)  -0.5169062151798716\n",
      "theta dot:  0.726085943906\n",
      "Angle:  211.125302520147\n",
      "action:  [-0.48366689]\n",
      "state:  {1: array([ 0.55652575]), 2: array([ 0.26585625]), 3: array([-0.84909547, -0.52823942,  0.        ])}\n",
      "cos(angle):  -0.8490954696920547   sin(angle)  -0.5282394185863347\n",
      "theta dot:  0.265856249088\n",
      "Angle:  211.88692279064398\n",
      "action:  [-0.18853055]\n",
      "state:  {1: array([ 0.5485956]), 2: array([-0.1586029]), 3: array([-0.85325774, -0.52148943,  0.        ])}\n",
      "cos(angle):  -0.8532577424164626   sin(angle)  -0.5214894294291703\n",
      "theta dot:  -0.158602897099\n",
      "Angle:  211.43256002202708\n",
      "action:  [ 0.12416641]\n",
      "state:  {1: array([ 0.52204085]), 2: array([-0.53109501]), 3: array([-0.86680331, -0.49865019,  0.        ])}\n",
      "cos(angle):  -0.8668033148789688   sin(angle)  -0.49865019133138944\n",
      "theta dot:  -0.531095007014\n",
      "Angle:  209.91108845877164\n",
      "action:  [ 0.34517308]\n",
      "state:  {1: array([ 0.47937552]), 2: array([-0.85330669]), 3: array([-0.88728312, -0.46122517,  0.        ])}\n",
      "cos(angle):  -0.8872831233280238   sin(angle)  -0.46122517175157185\n",
      "theta dot:  -0.853306688496\n",
      "Angle:  207.46655058111028\n",
      "action:  [ 0.47596718]\n",
      "state:  {1: array([ 0.42298399]), 2: array([-1.12783049]), 3: array([-0.91186812, -0.41048328,  0.        ])}\n",
      "cos(angle):  -0.911868123076297   sin(angle)  -0.4104832836027689\n",
      "theta dot:  -1.12783049052\n",
      "Angle:  204.23556178092915\n",
      "action:  [ 0.50417848]\n",
      "state:  {1: array([ 0.35498068]), 2: array([-1.36006618]), 3: array([-0.9376532 , -0.34757225,  0.        ])}\n",
      "cos(angle):  -0.9376532031061534   sin(angle)  -0.3475722524954639\n",
      "theta dot:  -1.36006618083\n",
      "Angle:  200.33926829114432\n",
      "action:  [ 0.43613029]\n",
      "state:  {1: array([ 0.27721439]), 2: array([-1.55532583]), 3: array([-0.96182153, -0.27367746,  0.        ])}\n",
      "cos(angle):  -0.9618215272479331   sin(angle)  -0.2736774556342804\n",
      "theta dot:  -1.55532582638\n",
      "Angle:  195.8835984294796\n",
      "action:  [ 0.31055694]\n",
      "state:  {1: array([ 0.19151437]), 2: array([-1.71400038]), 3: array([-0.98171711, -0.1903458 ,  0.        ])}\n",
      "cos(angle):  -0.9817171064538912   sin(angle)  -0.1903457982089422\n",
      "theta dot:  -1.7140003772\n",
      "Angle:  190.97336052688584\n",
      "action:  [ 0.12955096]\n",
      "state:  {1: array([ 0.09964802]), 2: array([-1.83732708]), 3: array([-0.99503924, -0.09948319,  0.        ])}\n",
      "cos(angle):  -0.9950392431547161   sin(angle)  -0.09948318743430815\n",
      "theta dot:  -1.83732708258\n",
      "Angle:  185.70981846448055\n",
      "action:  [-0.00693844]\n",
      "state:  {1: array([ 0.00399901]), 2: array([-1.91298024]), 3: array([-0.999992, -0.003999,  0.      ])}\n",
      "cos(angle):  -0.9999920039839569   sin(angle)  -0.003998995892694874\n",
      "theta dot:  -1.91298023944\n",
      "Angle:  180.22954657916048\n",
      "action:  [ 0.01982168]\n",
      "state:  {1: array([ 6.191534]), 2: array([-1.91300623]), 3: array([-0.99580296,  0.09152305,  0.        ])}\n",
      "cos(angle):  -0.9958029582816744   sin(angle)  0.09152304779379833\n",
      "theta dot:  -1.91300623416\n",
      "Angle:  174.74835838997046\n",
      "action:  [-0.14734087]\n",
      "state:  {1: array([ 6.09821075]), 2: array([-1.86646508]), 3: array([-0.98294093,  0.18392153,  0.        ])}\n",
      "cos(angle):  -0.9829409300796219   sin(angle)  0.1839215266743069\n",
      "theta dot:  -1.86646507863\n",
      "Angle:  169.40134231291097\n",
      "action:  [-0.0719001]\n",
      "state:  {1: array([ 6.0112453]), 2: array([-1.73930895]), 3: array([-0.96325162,  0.26860066,  0.        ])}\n",
      "cos(angle):  -0.9632516223348818   sin(angle)  0.26860065537749206\n",
      "theta dot:  -1.73930894816\n",
      "Angle:  164.41860086479085\n",
      "action:  [ 0.0978514]\n",
      "state:  {1: array([ 5.93508626]), 2: array([-1.52318075]), 3: array([-0.94002285,  0.34111148,  0.        ])}\n",
      "cos(angle):  -0.9400228488921525   sin(angle)  0.34111148259869756\n",
      "theta dot:  -1.52318074616\n",
      "Angle:  160.05501965922667\n",
      "action:  [-0.57380791]\n",
      "state:  {1: array([ 5.86741535]), 2: array([-1.35341832]), 3: array([-0.91480561,  0.40389441,  0.        ])}\n",
      "cos(angle):  -0.9148056092204774   sin(angle)  0.40389441360181166\n",
      "theta dot:  -1.35341832035\n",
      "Angle:  156.1777708423267\n",
      "action:  [-0.52756328]\n",
      "state:  {1: array([ 5.81093375]), 2: array([-1.129632]), 3: array([-0.89054633,  0.45489256,  0.        ])}\n",
      "cos(angle):  -0.8905463285394659   sin(angle)  0.4548925551433851\n",
      "theta dot:  -1.12963200185\n",
      "Angle:  152.94162110440936\n",
      "action:  [-0.72915403]\n",
      "state:  {1: array([ 5.76604196]), 2: array([-0.89783569]), 3: array([-0.86923505,  0.49439905,  0.        ])}\n",
      "cos(angle):  -0.8692350522744772   sin(angle)  0.4943990532933766\n",
      "theta dot:  -0.897835689405\n",
      "Angle:  150.36951733414958\n",
      "action:  [-2.99897218]\n",
      "state:  {1: array([ 5.72469014]), 2: array([-0.8270364]), 3: array([-0.8480535 ,  0.52991062,  0.        ])}\n",
      "cos(angle):  -0.84805349931859   sin(angle)  0.5299106172681336\n",
      "theta dot:  -0.827036399435\n",
      "Angle:  148.00023811498812\n",
      "action:  [-0.86526188]\n",
      "state:  {1: array([ 5.69672051]), 2: array([-0.55939272]), 3: array([-0.83290233,  0.55342001,  0.        ])}\n",
      "cos(angle):  -0.8329023299489228   sin(angle)  0.5534200111720354\n",
      "theta dot:  -0.559392718286\n",
      "Angle:  146.39769977001384\n",
      "action:  [-0.86824766]\n",
      "state:  {1: array([ 5.68299226]), 2: array([-0.27456486]), 3: array([-0.8252266 ,  0.56480179,  0.        ])}\n",
      "cos(angle):  -0.8252265991487794   sin(angle)  0.5648017882915561\n",
      "theta dot:  -0.274564858353\n",
      "Angle:  145.61113123004031\n",
      "action:  [-0.66584967]\n",
      "state:  {1: array([ 5.68545022]), 2: array([ 0.04915903]), 3: array([-0.82661236,  0.56277172,  0.        ])}\n",
      "cos(angle):  -0.8266123603957367   sin(angle)  0.5627717171651297\n",
      "theta dot:  0.0491590319416\n",
      "Angle:  145.75196115347882\n",
      "action:  [-0.39866309]\n",
      "state:  {1: array([ 5.70602213]), 2: array([ 0.41143836]), 3: array([-0.83801393,  0.54564884,  0.        ])}\n",
      "cos(angle):  -0.8380139307648196   sin(angle)  0.5456488356480717\n",
      "theta dot:  0.411438356617\n",
      "Angle:  146.930642465407\n",
      "action:  [-0.1758082]\n",
      "state:  {1: array([ 5.74573732]), 2: array([ 0.79430375]), 3: array([-0.85901797,  0.51194544,  0.        ])}\n",
      "cos(angle):  -0.859017969107394   sin(angle)  0.5119454353254929\n",
      "theta dot:  0.794303753239\n",
      "Angle:  149.206149779881\n",
      "action:  [-0.08410401]\n",
      "state:  {1: array([ 5.80401968]), 2: array([ 1.16564723]), 3: array([-0.88737991,  0.46103893,  0.        ])}\n",
      "cos(angle):  -0.887379911133225   sin(angle)  0.46103892820150194\n",
      "theta dot:  1.16564722864\n",
      "Angle:  152.54547530119055\n",
      "action:  [-0.12093378]\n",
      "state:  {1: array([ 5.878684]), 2: array([ 1.49328636]), 3: array([-0.91929878,  0.39356036,  0.        ])}\n",
      "cos(angle):  -0.9192987773433725   sin(angle)  0.39356036128525496\n",
      "theta dot:  1.49328635719\n",
      "Angle:  156.82341559107908\n",
      "action:  [-0.16323176]\n",
      "state:  {1: array([ 5.96688259]), 2: array([ 1.76397186]), 3: array([-0.95039197,  0.31105482,  0.        ])}\n",
      "cos(angle):  -0.950391970128041   sin(angle)  0.31105482332884804\n",
      "theta dot:  1.76397186483\n",
      "Angle:  161.8768109257809\n",
      "action:  [-0.25309657]\n",
      "state:  {1: array([ 6.06484752]), 2: array([ 1.9592985]), 3: array([-0.97625884,  0.21660717,  0.        ])}\n",
      "cos(angle):  -0.9762588447310394   sin(angle)  0.21660717459127782\n",
      "theta dot:  1.95929849711\n",
      "Angle:  167.48977453475413\n",
      "action:  [-0.28985191]\n",
      "state:  {1: array([ 6.16876132]), 2: array([ 2.07827609]), 3: array([-0.99346072,  0.11417446,  0.        ])}\n",
      "cos(angle):  -0.9934607154250832   sin(angle)  0.11417445820796293\n",
      "theta dot:  2.07827609201\n",
      "Angle:  173.44358304890272\n",
      "action:  [-0.28881011]\n",
      "state:  {1: array([ 6.27479059]), 2: array([ 2.12058542]), 3: array([-0.99996476,  0.00839461,  0.        ])}\n",
      "cos(angle):  -0.9999647646025619   sin(angle)  0.008394614544032583\n",
      "theta dot:  2.12058541937\n",
      "Angle:  179.518598574208\n",
      "action:  [-0.22879337]\n",
      "state:  {1: array([ 0.09623341]), 2: array([ 2.09256238]), 3: array([-0.99537314, -0.09608494,  0.        ])}\n",
      "cos(angle):  -0.9953731382046601   sin(angle)  -0.0960849402383459\n",
      "theta dot:  2.09256237534\n",
      "Angle:  185.51417601429443\n",
      "action:  [-0.17560298]\n",
      "state:  {1: array([ 0.19594132]), 2: array([ 1.99415822]), 3: array([-0.98086484, -0.19468993,  0.        ])}\n",
      "cos(angle):  -0.9808648392434592   sin(angle)  -0.19468992561481682\n",
      "theta dot:  1.99415822311\n",
      "Angle:  191.22700514849532\n",
      "action:  [-0.03168216]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "state:  {1: array([ 0.28811074]), 2: array([ 1.84338845]), 3: array([-0.9587824 , -0.28414134,  0.        ])}\n",
      "cos(angle):  -0.9587824032947515   sin(angle)  -0.28414134358157117\n",
      "theta dot:  1.84338845496\n",
      "Angle:  196.50791172306012\n",
      "action:  [-0.08772627]\n",
      "state:  {1: array([ 0.36896691]), 2: array([ 1.61712351]), 3: array([-0.93270043, -0.36065207,  0.        ])}\n",
      "cos(angle):  -0.9327004275591506   sin(angle)  -0.3606520656130747\n",
      "theta dot:  1.61712350603\n",
      "Angle:  201.14061848211088\n",
      "action:  [-0.05572068]\n",
      "state:  {1: array([ 0.43588073]), 2: array([ 1.33827636]), 3: array([-0.90649854, -0.42220895,  0.        ])}\n",
      "cos(angle):  -0.9064985407434754   sin(angle)  -0.4222089478326456\n",
      "theta dot:  1.3382763554\n",
      "Angle:  204.97448886618966\n",
      "action:  [-0.00258077]\n",
      "state:  {1: array([ 0.48694236]), 2: array([ 1.02123253]), 3: array([-0.88376774, -0.46792584,  0.        ])}\n",
      "cos(angle):  -0.8837677368095063   sin(angle)  -0.46792583533568993\n",
      "theta dot:  1.02123252942\n",
      "Angle:  207.90009771669725\n",
      "action:  [-0.01210784]\n",
      "state:  {1: array([ 0.52036596]), 2: array([ 0.66847198]), 3: array([-0.86763728, -0.49719769,  0.        ])}\n",
      "cos(angle):  -0.8676372843321187   sin(angle)  -0.49719768989486074\n",
      "theta dot:  0.668471977623\n",
      "Angle:  209.81512439056038\n",
      "action:  [ 0.07267749]\n",
      "state:  {1: array([ 0.53568972]), 2: array([ 0.30647533]), 3: array([-0.85991677, -0.51043427,  0.        ])}\n",
      "cos(angle):  -0.8599167747497751   sin(angle)  -0.5104342665847821\n",
      "theta dot:  0.306475333816\n",
      "Angle:  210.69310949507553\n",
      "action:  [-0.1618614]\n",
      "state:  {1: array([ 0.53065825]), 2: array([-0.10062958]), 3: array([-0.86247412, -0.50610117,  0.        ])}\n",
      "cos(angle):  -0.8624741184086848   sin(angle)  -0.5061011707901514\n",
      "theta dot:  -0.100629576492\n",
      "Angle:  210.4048276678439\n",
      "action:  [-0.18782599]\n",
      "state:  {1: array([ 0.50523928]), 2: array([-0.50837935]), 3: array([-0.87505868, -0.48401684,  0.        ])}\n",
      "cos(angle):  -0.8750586848041988   sin(angle)  -0.4840168366376792\n",
      "theta dot:  -0.50837935255\n",
      "Angle:  208.94843150889608\n",
      "action:  [-0.12298128]\n",
      "state:  {1: array([ 0.46074732]), 2: array([-0.88983917]), 3: array([-0.89572048, -0.44461762,  0.        ])}\n",
      "cos(angle):  -0.8957204762382508   sin(angle)  -0.4446176204869992\n",
      "theta dot:  -0.889839172743\n",
      "Angle:  206.39923601784423\n",
      "action:  [-0.07871404]\n",
      "state:  {1: array([ 0.39899184]), 2: array([-1.23510949]), 3: array([-0.92145312, -0.38848957,  0.        ])}\n",
      "cos(angle):  -0.9214531199817962   sin(angle)  -0.3884895721583958\n",
      "theta dot:  -1.23510949464\n",
      "Angle:  202.8609162280148\n",
      "action:  [ 0.25866361]\n",
      "state:  {1: array([ 0.32460799]), 2: array([-1.48767713]), 3: array([-0.94777583, -0.31893727,  0.        ])}\n",
      "cos(angle):  -0.9477758264155904   sin(angle)  -0.31893727104596054\n",
      "theta dot:  -1.48767713285\n",
      "Angle:  198.59904514459043\n",
      "action:  [ 0.13276183]\n",
      "state:  {1: array([ 0.2392597]), 2: array([-1.70696581]), 3: array([-0.97151368, -0.23698348,  0.        ])}\n",
      "cos(angle):  -0.9715136804939886   sin(angle)  -0.23698347751061505\n",
      "theta dot:  -1.70696581185\n",
      "Angle:  193.70895974013516\n",
      "action:  [ 0.06116715]\n",
      "state:  {1: array([ 0.14548328]), 2: array([-1.87552835]), 3: array([-0.98943596, -0.14497062,  0.        ])}\n",
      "cos(angle):  -0.989435959988755   sin(angle)  -0.1449706214414863\n",
      "theta dot:  -1.87552834752\n",
      "Angle:  188.33597937098656\n",
      "action:  [ 0.03952347]\n",
      "state:  {1: array([ 0.04656689]), 2: array([-1.97832779]), 3: array([-0.99891596, -0.04655006,  0.        ])}\n",
      "cos(angle):  -0.9989159582672759   sin(angle)  -0.04655006250232087\n",
      "theta dot:  -1.9783277935\n",
      "Angle:  182.6685009709592\n",
      "action:  [-0.078789]\n",
      "state:  {1: array([ 6.22849926]), 2: array([-2.02505869]), 3: array([-0.99850509,  0.05465879,  0.        ])}\n",
      "cos(angle):  -0.9985050909029887   sin(angle)  0.05465879106616137\n",
      "theta dot:  -2.02505869081\n",
      "Angle:  176.86630688991613\n",
      "action:  [ 0.15933757]\n",
      "state:  {1: array([ 6.13049107]), 2: array([-1.96016396]), 3: array([-0.98836487,  0.15210158,  0.        ])}\n",
      "cos(angle):  -0.9883648671969272   sin(angle)  0.1521015755697502\n",
      "theta dot:  -1.96016396261\n",
      "Angle:  171.25086391070226\n",
      "action:  [ 0.09880966]\n",
      "state:  {1: array([ 6.03892775]), 2: array([-1.83126633]), 3: array([-0.97031714,  0.24183599,  0.        ])}\n",
      "cos(angle):  -0.9703171413995328   sin(angle)  0.24183598802957126\n",
      "theta dot:  -1.83126633217\n",
      "Angle:  166.00468457865372\n",
      "action:  [ 0.27304528]\n",
      "state:  {1: array([ 5.95848112]), 2: array([-1.60893255]), 3: array([-0.94774514,  0.31902844,  0.        ])}\n",
      "cos(angle):  -0.9477451409060578   sin(angle)  0.31902844369892286\n",
      "theta dot:  -1.60893254851\n",
      "Angle:  161.39544312951227\n",
      "action:  [ 0.35102192]\n",
      "state:  {1: array([ 5.89263072]), 2: array([-1.31700793]), 3: array([-0.92469807,  0.3807013 ,  0.        ])}\n",
      "cos(angle):  -0.9246980717700325   sin(angle)  0.38070129506580846\n",
      "theta dot:  -1.31700792782\n",
      "Angle:  157.6225021598135\n",
      "Reset!\n",
      "min:  -2.99897217737  self.max:  28.6954689906\n",
      "state:  {1: array([ 4.38304344]), 2: array([ 0.15320082]), 3: array([ 0.32342381,  0.94625422,  0.        ])}\n",
      "cos(angle):  0.323423809349051   sin(angle)  0.9462542150744422\n",
      "theta dot:  0.153200818082\n",
      "Angle:  71.1297243951276\n",
      "action:  [ 0.48848909]\n",
      "state:  {1: array([ 4.42985169]), 2: array([ 0.93616484]), 3: array([ 0.27879324,  0.96035115,  0.        ])}\n",
      "cos(angle):  0.2787932373854414   sin(angle)  0.9603511497302145\n",
      "theta dot:  0.936164843512\n",
      "Angle:  73.8116328467463\n",
      "action:  [ 0.40017249]\n",
      "state:  {1: array([ 4.51567439]), 2: array([ 1.71645408]), 3: array([ 0.19544834,  0.980714  ,  0.        ])}\n",
      "cos(angle):  0.19544834466990385   sin(angle)  0.9807139973334705\n",
      "theta dot:  1.71645407999\n",
      "Angle:  78.72890007361644\n",
      "action:  [ 0.30757947]\n",
      "state:  {1: array([ 4.64058071]), 2: array([ 2.4981265]), 3: array([ 0.07174657,  0.99742289,  0.        ])}\n",
      "cos(angle):  0.07174656946880795   sin(angle)  0.9974228941474411\n",
      "theta dot:  2.49812649872\n",
      "Angle:  85.88548859171556\n",
      "action:  [ 0.19913316]\n",
      "state:  {1: array([ 4.8043839]), 2: array([ 3.27606364]), 3: array([-0.09186521,  0.99577145,  0.        ])}\n",
      "cos(angle):  -0.09186521132515231   sin(angle)  0.9957714511614526\n",
      "theta dot:  3.27606364311\n",
      "Angle:  95.27069765333698\n",
      "action:  [-0.11843884]\n",
      "state:  {1: array([ 5.00464022]), 2: array([ 4.0051264]), 3: array([-0.28810873,  0.9575977 ,  0.        ])}\n",
      "cos(angle):  -0.28810873224974815   sin(angle)  0.9575977017523815\n",
      "theta dot:  4.00512640484\n",
      "Angle:  106.74451279326644\n",
      "action:  [-0.53736667]\n",
      "state:  {1: array([ 5.2367762]), 2: array([ 4.64271968]), 3: array([-0.50068266,  0.86563091,  0.        ])}\n",
      "cos(angle):  -0.5006826578298605   sin(angle)  0.8656309121955077\n",
      "theta dot:  4.64271968014\n",
      "Angle:  120.04489384790497\n",
      "action:  [-0.65143532]\n",
      "state:  {1: array([ 5.49648758]), 2: array([ 5.19422757]), 3: array([-0.70618725,  0.70802511,  0.        ])}\n",
      "cos(angle):  -0.7061872534346241   sin(angle)  0.7080251147286105\n",
      "theta dot:  5.19422756701\n",
      "Angle:  134.9252249222319\n",
      "action:  [-0.77364987]\n",
      "state:  {1: array([ 5.77694753]), 2: array([ 5.60919892]), 3: array([-0.87457496,  0.48489034,  0.        ])}\n",
      "cos(angle):  -0.8745749560994982   sin(angle)  0.48489034447342916\n",
      "theta dot:  5.60919892268\n",
      "Angle:  150.9943585815579\n",
      "action:  [-0.21972474]\n",
      "state:  {1: array([ 6.07394292]), 2: array([ 5.93990797]), 3: array([-0.97818857,  0.20771887,  0.        ])}\n",
      "cos(angle):  -0.9781885667155485   sin(angle)  0.2077188675758195\n",
      "theta dot:  5.93990796946\n",
      "Angle:  168.01090165678477\n",
      "action:  [-0.14975741]\n",
      "state:  {1: array([ 0.09441929]), 2: array([ 6.07323351]), 3: array([-0.99554581, -0.09427906,  0.        ])}\n",
      "cos(angle):  -0.9955458092099752   sin(angle)  -0.09427906323492934\n",
      "theta dot:  6.07323350855\n",
      "Angle:  185.41023520804694\n",
      "action:  [ 0.28845424]\n",
      "state:  {1: array([ 0.39670891]), 2: array([ 6.04579235]), 3: array([-0.92233761, -0.38638494,  0.        ])}\n",
      "cos(angle):  -0.9223376146014823   sin(angle)  -0.3863849436653133\n",
      "theta dot:  6.04579234649\n",
      "Angle:  202.7301139699497\n",
      "action:  [ 0.52612872]\n",
      "state:  {1: array([ 0.68845506]), 2: array([ 5.83492295]), 3: array([-0.77222851, -0.63534489,  0.        ])}\n",
      "cos(angle):  -0.7722285079911242   sin(angle)  -0.6353448917287383\n",
      "theta dot:  5.8349229472\n",
      "Angle:  219.44589781409636\n",
      "action:  [ 0.612493]\n",
      "state:  {1: array([ 0.96096947]), 2: array([ 5.45028823]), 3: array([-0.57272554, -0.81974719,  0.        ])}\n",
      "cos(angle):  -0.5727255365013763   sin(angle)  -0.8197471926388102\n",
      "theta dot:  5.45028822865\n",
      "Angle:  235.05978693361087\n",
      "action:  [ 0.46562689]\n",
      "state:  {1: array([ 1.20623556]), 2: array([ 4.90532187]), 3: array([-0.35653896, -0.93428046,  0.        ])}\n",
      "cos(angle):  -0.35653896038740346   sin(angle)  -0.9342804556052051\n",
      "theta dot:  4.90532186795\n",
      "Angle:  249.11246608167963\n",
      "action:  [ 0.30560449]\n",
      "state:  {1: array([ 1.41875817]), 2: array([ 4.2504522]), 3: array([-0.15145309, -0.98846445,  0.        ])}\n",
      "cos(angle):  -0.15145308960361137   sin(angle)  -0.9884644463254713\n",
      "theta dot:  4.25045219986\n",
      "Angle:  261.2890862111526\n",
      "action:  [-0.02039722]\n",
      "state:  {1: array([ 1.59406039]), 2: array([ 3.50604428]), 3: array([ 0.02326196, -0.9997294 ,  0.        ])}\n",
      "cos(angle):  0.023261960499972815   sin(angle)  -0.9997294039857474\n",
      "theta dot:  3.5060442825\n",
      "Angle:  271.3331397324373\n",
      "action:  [-0.13074527]\n",
      "state:  {1: array([ 1.73089216]), 2: array([ 2.73663544]), 3: array([ 0.15941281, -0.98721201,  0.        ])}\n",
      "cos(angle):  0.15941281273068691   sin(angle)  -0.9872120112403875\n",
      "theta dot:  2.73663543887\n",
      "Angle:  279.1730044350901\n",
      "action:  [-0.19152924]\n",
      "state:  {1: array([ 1.82926701]), 2: array([ 1.96749704]), 3: array([ 0.25560234, -0.96678201,  0.        ])}\n",
      "cos(angle):  0.2556023353202975   sin(angle)  -0.9667820055104512\n",
      "theta dot:  1.96749704481\n",
      "Angle:  284.80945509821936\n",
      "action:  [-0.13839526]\n",
      "state:  {1: array([ 1.89034957]), 2: array([ 1.22165125]), 3: array([ 0.31414245, -0.94937586,  0.        ])}\n",
      "cos(angle):  0.3141424542147037   sin(angle)  -0.9493758573188823\n",
      "theta dot:  1.22165125142\n",
      "Angle:  288.30921995141716\n",
      "action:  [-0.06002322]\n",
      "state:  {1: array([ 1.91538037]), 2: array([ 0.50061588]), 3: array([ 0.3378052 , -0.94121605,  0.        ])}\n",
      "cos(angle):  0.33780519773076795   sin(angle)  -0.9412160476671001\n",
      "theta dot:  0.500615875234\n",
      "Angle:  289.7433754381462\n",
      "action:  [-0.00901479]\n",
      "state:  {1: array([ 1.90504795]), 2: array([-0.20664838]), 3: array([ 0.3280623 , -0.94465609,  0.        ])}\n",
      "cos(angle):  0.32806230055826274   sin(angle)  -0.9446560892475209\n",
      "theta dot:  -0.206648378821\n",
      "Angle:  289.1513728250212\n",
      "action:  [ 0.09066272]\n",
      "state:  {1: array([ 1.8599709]), 2: array([-0.90154104]), 3: array([ 0.28516116, -0.95847958,  0.        ])}\n",
      "cos(angle):  0.28516116324098334   sin(angle)  -0.9584795829745406\n",
      "theta dot:  -0.901541037081\n",
      "Angle:  286.5686540404122\n",
      "action:  [ 0.23524036]\n",
      "state:  {1: array([ 1.78071516]), 2: array([-1.58511467]), 3: array([ 0.20838052, -0.97804783,  0.        ])}\n",
      "cos(angle):  0.2083805173885776   sin(angle)  -0.9780478311273272\n",
      "theta dot:  -1.58511467072\n",
      "Angle:  282.0276456254254\n",
      "action:  [ 0.29422088]\n",
      "state:  {1: array([ 1.66698929]), 2: array([-2.27451741]), 3: array([ 0.09604469, -0.99537702,  0.        ])}\n",
      "cos(angle):  0.09604468615777181   sin(angle)  -0.9953770231730563\n",
      "theta dot:  -2.27451741204\n",
      "Angle:  275.5116484557052\n",
      "action:  [ 0.32936816]\n",
      "state:  {1: array([ 1.51840704]), 2: array([-2.97164496]), 3: array([-0.05236532, -0.998628  ,  0.        ])}\n",
      "cos(angle):  -0.0523653216581154   sin(angle)  -0.998627995345435\n",
      "theta dot:  -2.97164495611\n",
      "Angle:  266.9985326532427\n",
      "action:  [ 0.27668992]\n",
      "state:  {1: array([ 1.33445142]), 2: array([-3.67911247]), 3: array([-0.23415072, -0.97220031,  0.        ])}\n",
      "cos(angle):  -0.23415071545931676   sin(angle)  -0.9722003098384047\n",
      "theta dot:  -3.67911246534\n",
      "Angle:  256.4586764691206\n",
      "action:  [ 0.13886915]\n",
      "state:  {1: array([ 1.1150798]), 2: array([-4.38743232]), 3: array([-0.44010583, -0.89794591,  0.        ])}\n",
      "cos(angle):  -0.44010582543670146   sin(angle)  -0.8979459128570494\n",
      "theta dot:  -4.38743232449\n",
      "Angle:  243.88963810635875\n",
      "action:  [-0.1137494]\n",
      "state:  {1: array([ 0.8611821]), 2: array([-5.07795417]), 3: array([-0.65154117, -0.75861328,  0.        ])}\n",
      "cos(angle):  -0.6515411702374566   sin(angle)  -0.7586132766341528\n",
      "theta dot:  -5.07795416985\n",
      "Angle:  229.34240499945304\n",
      "action:  [-0.06093505]\n",
      "state:  {1: array([ 0.57837938]), 2: array([-5.65605438]), 3: array([-0.83734969, -0.54666763,  0.        ])}\n",
      "cos(angle):  -0.8373496916672953   sin(angle)  -0.5466676265014104\n",
      "theta dot:  -5.65605438471\n",
      "Angle:  213.13904064293598\n",
      "action:  [-0.14342187]\n",
      "state:  {1: array([ 0.27400096]), 2: array([-6.08756838]), 3: array([-0.962696 , -0.2705853,  0.       ])}\n",
      "cos(angle):  -0.9626960043453763   sin(angle)  -0.27058529748943727\n",
      "theta dot:  -6.08756838465\n",
      "Angle:  195.6994826273325\n",
      "action:  [-0.21843643]\n",
      "state:  {1: array([ 6.24102262]), 2: array([-6.32327282]), 3: array([-0.99911129,  0.04215019,  0.        ])}\n",
      "cos(angle):  -0.9991112857020001   sin(angle)  0.0421501931537253\n",
      "theta dot:  -6.32327282266\n",
      "Angle:  177.5838408805863\n",
      "action:  [-0.17000277]\n",
      "state:  {1: array([ 5.92516459]), 2: array([-6.31716059]), 3: array([-0.93659224,  0.35042114,  0.        ])}\n",
      "cos(angle):  -0.9365922415450761   sin(angle)  0.35042113674487413\n",
      "theta dot:  -6.31716059336\n",
      "Angle:  159.48655117463773\n",
      "action:  [-0.03906768]\n",
      "state:  {1: array([ 5.62215435]), 2: array([-6.06020489]), 3: array([-0.78935971,  0.61393098,  0.        ])}\n",
      "cos(angle):  -0.7893597137972194   sin(angle)  0.6139309751380623\n",
      "theta dot:  -6.06020489328\n",
      "Angle:  142.12538360411537\n",
      "action:  [ 0.08349759]\n",
      "state:  {1: array([ 5.34279275]), 2: array([-5.58723202]), 3: array([-0.58947096,  0.80778957,  0.        ])}\n",
      "cos(angle):  -0.589470964900231   sin(angle)  0.8077895651341324\n",
      "theta dot:  -5.58723202386\n",
      "Angle:  126.11918032719564\n",
      "action:  [ 0.70094349]\n",
      "state:  {1: array([ 5.09898033]), 2: array([-4.87624833]), 3: array([-0.37703352,  0.92619961,  0.        ])}\n",
      "cos(angle):  -0.3770335220770373   sin(angle)  0.9261996130587533\n",
      "theta dot:  -4.87624832641\n",
      "Angle:  112.14979054564088\n",
      "action:  [-0.03919333]\n",
      "state:  {1: array([ 4.88960645]), 2: array([-4.18747762]), 3: array([-0.17629131,  0.98433804,  0.        ])}\n",
      "cos(angle):  -0.17629130935096826   sin(angle)  0.984338038606312\n",
      "theta dot:  -4.18747761625\n",
      "Angle:  100.1535788871713\n",
      "action:  [-0.16990397]\n",
      "state:  {1: array([ 4.71587097]), 2: array([-3.47470968]), 3: array([-0.00348198,  0.99999394,  0.        ])}\n",
      "cos(angle):  -0.003481978905694372   sin(angle)  0.9999939378930756\n",
      "theta dot:  -3.47470968215\n",
      "Angle:  90.19929217360493\n",
      "action:  [-0.16892224]\n",
      "state:  {1: array([ 4.57836834]), 2: array([-2.75005256]), 3: array([ 0.1336198 ,  0.99103267,  0.        ])}\n",
      "cos(angle):  0.13361979973081997   sin(angle)  0.9910326680386957\n",
      "theta dot:  -2.75005256408\n",
      "Angle:  82.32099032845701\n",
      "action:  [-0.4702136]\n",
      "state:  {1: array([ 4.47450283]), 2: array([-2.0773101]), 3: array([ 0.23564883,  0.97183827,  0.        ])}\n",
      "cos(angle):  0.23564883157641142   sin(angle)  0.9718382726445136\n",
      "theta dot:  -2.07731010266\n",
      "Angle:  76.3699491634535\n",
      "action:  [-0.39182594]\n",
      "state:  {1: array([ 4.40414257]), 2: array([-1.40720529]), 3: array([ 0.30338817,  0.95286705,  0.        ])}\n",
      "cos(angle):  0.3033881673778917   sin(angle)  0.952867052581358\n",
      "theta dot:  -1.40720528867\n",
      "Angle:  72.33861239300484\n",
      "action:  [-0.2154452]\n",
      "state:  {1: array([ 4.36789898]), 2: array([-0.72487178]), 3: array([ 0.33771669,  0.94124781,  0.        ])}\n",
      "cos(angle):  0.3377166855368722   sin(angle)  0.9412478102550833\n",
      "theta dot:  -0.724871778965\n",
      "Angle:  70.26201256785623\n",
      "action:  [-0.34108326]\n",
      "state:  {1: array([ 4.36439406]), 2: array([-0.07009841]), 3: array([ 0.3410136 ,  0.94005836,  0.        ])}\n",
      "cos(angle):  0.3410136031842562   sin(angle)  0.940058361190033\n",
      "theta dot:  -0.0700984097613\n",
      "Angle:  70.06119588595794\n",
      "action:  [ 0.1148569]\n",
      "state:  {1: array([ 4.39700275]), 2: array([ 0.6521739]), 3: array([ 0.31018367,  0.95067665,  0.        ])}\n",
      "cos(angle):  0.31018367079567827   sin(angle)  0.9506766486938228\n",
      "theta dot:  0.652173896834\n",
      "Angle:  71.92953210683527\n",
      "action:  [-0.07505772]\n",
      "state:  {1: array([ 4.46469889]), 2: array([ 1.35392273]), 3: array([ 0.2451652 ,  0.96948132,  0.        ])}\n",
      "cos(angle):  0.2451652008520457   sin(angle)  0.9694813171439541\n",
      "theta dot:  1.35392272571\n",
      "Angle:  75.80822593527648\n",
      "action:  [ 0.52751753]\n",
      "state:  {1: array([ 4.57270696]), 2: array([ 2.16016134]), 3: array([ 0.13922824,  0.99026032,  0.        ])}\n",
      "cos(angle):  0.13922824168795275   sin(angle)  0.9902603176521217\n",
      "theta dot:  2.16016134266\n",
      "Angle:  81.99661786421234\n",
      "action:  [ 0.62696262]\n",
      "state:  {1: array([ 4.72255201]), 2: array([ 2.99690097]), 3: array([-0.01016285,  0.99994836,  0.        ])}\n",
      "cos(angle):  -0.010162850728896219   sin(angle)  0.9999483568990262\n",
      "theta dot:  2.99690097369\n",
      "Angle:  90.5820866581999\n",
      "action:  [ 0.38072319]\n",
      "state:  {1: array([ 4.91275054]), 2: array([ 3.80397072]), 3: array([-0.19902367,  0.97999468,  0.        ])}\n",
      "cos(angle):  -0.19902367227703155   sin(angle)  0.9799946825740253\n",
      "theta dot:  3.80397071913\n",
      "Angle:  101.47963455487138\n",
      "action:  [ 0.53480826]\n",
      "state:  {1: array([ 5.14370994]), 2: array([ 4.61918797]), 3: array([-0.41807115,  0.90841429,  0.        ])}\n",
      "cos(angle):  -0.4180711459280332   sin(angle)  0.9084142870642343\n",
      "theta dot:  4.61918797075\n",
      "Angle:  114.71260238550502\n",
      "action:  [-0.22346923]\n",
      "state:  {1: array([ 5.40705886]), 2: array([ 5.2669783]), 3: array([-0.64013185,  0.76826507,  0.        ])}\n",
      "cos(angle):  -0.6401318511086862   sin(angle)  0.7682650670153933\n",
      "theta dot:  5.26697830165\n",
      "Angle:  129.80134847503007\n",
      "action:  [-0.3532579]\n",
      "state:  {1: array([ 5.69656828]), 2: array([ 5.79018842]), 3: array([-0.83281807,  0.5535468 ,  0.        ])}\n",
      "cos(angle):  -0.832818072915956   sin(angle)  0.5535467978631557\n",
      "theta dot:  5.79018841619\n",
      "Angle:  146.3889776275871\n",
      "action:  [-0.31532065]\n",
      "state:  {1: array([ 6.0044708]), 2: array([ 6.15805042]), 3: array([-0.9614099 ,  0.27511999,  0.        ])}\n",
      "cos(angle):  -0.9614098966019081   sin(angle)  0.27511999330457315\n",
      "theta dot:  6.15805041683\n",
      "Angle:  164.03045131981457\n",
      "action:  [-0.1348281]\n",
      "state:  {1: array([ 0.0384938]), 2: array([ 6.3441662]), 3: array([-0.99925921, -0.03848429,  0.        ])}\n",
      "cos(angle):  -0.999259205162824   sin(angle)  -0.03848429416477975\n",
      "theta dot:  6.34416619717\n",
      "Angle:  182.20594803555747\n",
      "action:  [-0.10665555]\n",
      "state:  {1: array([ 0.35345903]), 2: array([ 6.29930464]), 3: array([-0.938181  , -0.34614507,  0.        ])}\n",
      "cos(angle):  -0.9381810009215653   sin(angle)  -0.3461450700353971\n",
      "theta dot:  6.29930464386\n",
      "Angle:  200.2520843338559\n",
      "action:  [-0.11790107]\n",
      "state:  {1: array([ 0.65455957]), 2: array([ 6.02201068]), 3: array([-0.79331615, -0.6088099 ,  0.        ])}\n",
      "cos(angle):  -0.7933161455004705   sin(angle)  -0.6088098991378808\n",
      "theta dot:  6.02201068055\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Angle:  217.5038338006667\n",
      "action:  [-0.17649915]\n",
      "state:  {1: array([ 0.93150599]), 2: array([ 5.53892838]), 3: array([-0.59662608, -0.80251936,  0.        ])}\n",
      "cos(angle):  -0.5966260768775006   sin(angle)  -0.8025193607569618\n",
      "theta dot:  5.53892838412\n",
      "Angle:  233.37165766656972\n",
      "action:  [-0.20863592]\n",
      "state:  {1: array([ 1.17679316]), 2: array([ 4.90574348]), 3: array([-0.38388792, -0.92337969,  0.        ])}\n",
      "cos(angle):  -0.383887924927957   sin(angle)  -0.9233796949762905\n",
      "theta dot:  4.9057434752\n",
      "Angle:  247.4255446276167\n",
      "action:  [-0.25431963]\n",
      "state:  {1: array([ 1.3855462]), 2: array([ 4.17506076]), 3: array([-0.18419239, -0.98289021,  0.        ])}\n",
      "cos(angle):  -0.18419238930983542   sin(angle)  -0.9828902093928569\n",
      "theta dot:  4.17506075898\n",
      "Angle:  259.38618469343743\n",
      "action:  [-0.57145667]\n",
      "state:  {1: array([ 1.55315493]), 2: array([ 3.3521746]), 3: array([-0.01764048, -0.99984439,  0.        ])}\n",
      "cos(angle):  -0.017640484544002657   sin(angle)  -0.9998443945459977\n",
      "theta dot:  3.35217460206\n",
      "Angle:  268.98943508129935\n",
      "action:  [-0.77393811]\n",
      "state:  {1: array([ 1.67746496]), 2: array([ 2.48620059]), 3: array([ 0.10646646, -0.99431629,  0.        ])}\n",
      "cos(angle):  0.10646646219702748   sin(angle)  -0.9943162939564296\n",
      "theta dot:  2.48620059015\n",
      "Angle:  276.11185846790636\n",
      "action:  [-0.60239005]\n",
      "state:  {1: array([ 1.7599702]), 2: array([ 1.65010486]), 3: array([ 0.18804757, -0.98215992,  0.        ])}\n",
      "cos(angle):  0.1880475704322938   sin(angle)  -0.9821599214254833\n",
      "theta dot:  1.65010486204\n",
      "Angle:  280.83904963112315\n",
      "action:  [-0.58774724]\n",
      "state:  {1: array([ 1.80123634]), 2: array([ 0.82532284]), 3: array([ 0.22840593, -0.97356599,  0.        ])}\n",
      "cos(angle):  0.2284059290523514   sin(angle)  -0.9735659872724253\n",
      "theta dot:  0.825322835051\n",
      "Angle:  283.2034198614076\n",
      "action:  [-0.34481013]\n",
      "state:  {1: array([ 1.80340768]), 2: array([ 0.04342683]), 3: array([ 0.23051933, -0.97306775,  0.        ])}\n",
      "cos(angle):  0.23051933298409633   sin(angle)  -0.9730677453911251\n",
      "theta dot:  0.0434268258263\n",
      "Angle:  283.3278282623616\n",
      "action:  [-0.52239863]\n",
      "state:  {1: array([ 1.76517099]), 2: array([-0.76473378]), 3: array([ 0.19315302, -0.98116865,  0.        ])}\n",
      "cos(angle):  0.19315301578073482   sin(angle)  -0.9811686463064376\n",
      "theta dot:  -0.76473377837\n",
      "Angle:  281.13703248781184\n",
      "action:  [ 0.03644956]\n",
      "state:  {1: array([ 1.69041385]), 2: array([-1.49514283]), 3: array([ 0.11933247, -0.99285435,  0.        ])}\n",
      "cos(angle):  0.1193324747173764   sin(angle)  -0.992854350082542\n",
      "theta dot:  -1.49514282971\n",
      "Angle:  276.8537738083393\n",
      "action:  [-0.56211605]\n",
      "state:  {1: array([ 1.5742088]), 2: array([-2.324101]), 3: array([ 0.00341247, -0.99999418,  0.        ])}\n",
      "cos(angle):  0.003412469031421733   sin(angle)  -0.9999941775106042\n",
      "theta dot:  -2.32410100017\n",
      "Angle:  270.1957304541384\n",
      "action:  [-0.79642916]\n",
      "state:  {1: array([ 1.41453075]), 2: array([-3.19356101]), 3: array([-0.15563038, -0.98781536,  0.        ])}\n",
      "cos(angle):  -0.15563037774778926   sin(angle)  -0.9878153600355081\n",
      "theta dot:  -3.19356100765\n",
      "Angle:  261.04687348036447\n",
      "action:  [-0.73979713]\n",
      "state:  {1: array([ 1.21226115]), 2: array([-4.0453921]), 3: array([-0.35090294, -0.93641184,  0.        ])}\n",
      "cos(angle):  -0.3509029350728685   sin(angle)  -0.9364118378989269\n",
      "theta dot:  -4.04539209753\n",
      "Angle:  249.45770589766312\n",
      "action:  [-0.37594068]\n",
      "state:  {1: array([ 0.97205654]), 2: array([-4.80409208]), 3: array([-0.56360192, -0.82604653,  0.        ])}\n",
      "cos(angle):  -0.5636019237391083   sin(angle)  -0.8260465311092204\n",
      "theta dot:  -4.80409207781\n",
      "Angle:  235.6950280582549\n",
      "action:  [-0.0013906]\n",
      "state:  {1: array([ 0.70086477]), 2: array([-5.42383557]), 3: array([-0.7642848 , -0.64487886,  0.        ])}\n",
      "cos(angle):  -0.7642848044449717   sin(angle)  -0.6448788550530334\n",
      "theta dot:  -5.42383556541\n",
      "Angle:  220.15692005957692\n",
      "action:  [ 0.23403457]\n",
      "state:  {1: array([ 0.40724529]), 2: array([-5.87238952]), 3: array([-0.9182154 , -0.39608142,  0.        ])}\n",
      "cos(angle):  -0.918215395183798   sin(angle)  -0.3960814159329641\n",
      "theta dot:  -5.872389521\n",
      "Angle:  203.33380263883836\n",
      "action:  [ 0.22605242]\n",
      "state:  {1: array([ 0.10046815]), 2: array([-6.13554272]), 3: array([-0.99495732, -0.10029922,  0.        ])}\n",
      "cos(angle):  -0.9949573189294113   sin(angle)  -0.10029921988130135\n",
      "theta dot:  -6.13554271946\n",
      "Angle:  185.75680859913183\n",
      "action:  [ 0.00751015]\n",
      "state:  {1: array([ 6.07317143]), 2: array([-6.20964061]), 3: array([-0.97802802,  0.20847347,  0.        ])}\n",
      "cos(angle):  -0.978028021712986   sin(angle)  0.20847347251912673\n",
      "theta dot:  -6.20964061155\n",
      "Angle:  167.96669839681977\n",
      "action:  [-0.32304526]\n",
      "state:  {1: array([ 5.76808431]), 2: array([-6.1017423]), 3: array([-0.87024298,  0.49262274,  0.        ])}\n",
      "cos(angle):  -0.8702429755853089   sin(angle)  0.49262273947152224\n",
      "theta dot:  -6.10174229651\n",
      "Angle:  150.486535209721\n",
      "action:  [-0.52768721]\n",
      "state:  {1: array([ 5.4775129]), 2: array([-5.81142832]), 3: array([-0.69262639,  0.72129653,  0.        ])}\n",
      "cos(angle):  -0.69262638566116   sin(angle)  0.7212965339483881\n",
      "theta dot:  -5.81142832364\n",
      "Angle:  133.8380583467221\n",
      "action:  [-0.4747604]\n",
      "state:  {1: array([ 5.2104294]), 2: array([-5.34166998]), 3: array([-0.47770493,  0.87852035,  0.        ])}\n",
      "cos(angle):  -0.4777049251724735   sin(angle)  0.8785203494888217\n",
      "theta dot:  -5.34166998277\n",
      "Angle:  118.53533685291382\n",
      "action:  [-0.5569251]\n",
      "state:  {1: array([ 4.97211348]), 2: array([-4.76631849]), 3: array([-0.2568143 ,  0.96646077,  0.        ])}\n",
      "cos(angle):  -0.2568142966983721   sin(angle)  0.9664607684801906\n",
      "theta dot:  -4.76631848507\n",
      "Angle:  104.88087213250827\n",
      "action:  [-0.49868607]\n",
      "state:  {1: array([ 4.76629968]), 2: array([-4.11627582]), 3: array([-0.05388459,  0.99854717,  0.        ])}\n",
      "cos(angle):  -0.05388459363705487   sin(angle)  0.9985471699266738\n",
      "theta dot:  -4.11627581921\n",
      "Angle:  93.08863812025089\n",
      "action:  [-0.57387102]\n",
      "state:  {1: array([ 4.59362738]), 2: array([-3.45344609]), 3: array([ 0.11848262,  0.99295613,  0.        ])}\n",
      "cos(angle):  0.11848262243429125   sin(angle)  0.9929561260101541\n",
      "theta dot:  -3.45344609426\n",
      "Angle:  83.19526695640513\n",
      "action:  [-0.36282433]\n",
      "state:  {1: array([ 4.45546975]), 2: array([-2.76315265]), 3: array([ 0.25410211,  0.9671774 ,  0.        ])}\n",
      "cos(angle):  0.2541021145563204   sin(angle)  0.9671773960230908\n",
      "theta dot:  -2.76315264921\n",
      "Angle:  75.27943621956274\n",
      "action:  [-0.03883447]\n",
      "state:  {1: array([ 4.35329001]), 2: array([-2.04359477]), 3: array([ 0.35143082,  0.93621385,  0.        ])}\n",
      "cos(angle):  0.3514308210679844   sin(angle)  0.9362138527086011\n",
      "theta dot:  -2.04359477208\n",
      "Angle:  69.42498213605035\n",
      "action:  [ 0.46340065]\n",
      "state:  {1: array([ 4.28969379]), 2: array([-1.27192429]), 3: array([ 0.41021991,  0.91198663,  0.        ])}\n",
      "cos(angle):  0.41021991378821193   sin(angle)  0.9119866349522848\n",
      "theta dot:  -1.27192428546\n",
      "Angle:  65.7811959859689\n",
      "action:  [ 0.1359952]\n",
      "state:  {1: array([ 4.26131704]), 2: array([-0.56753503]), 3: array([ 0.43593051,  0.89998033,  0.        ])}\n",
      "cos(angle):  0.4359305071961935   sin(angle)  0.8999803291715156\n",
      "theta dot:  -0.567535029662\n",
      "Angle:  64.15533169167429\n",
      "action:  [ 0.16625528]\n",
      "state:  {1: array([ 4.26793647]), 2: array([ 0.13238851]), 3: array([ 0.42996365,  0.9028462 ,  0.        ])}\n",
      "cos(angle):  0.42996364746347193   sin(angle)  0.902846200556832\n",
      "theta dot:  0.132388509863\n",
      "Angle:  64.53459594834902\n",
      "action:  [ 0.11106809]\n",
      "state:  {1: array([ 4.30924564]), 2: array([ 0.82618337]), 3: array([ 0.39231162,  0.91983237,  0.        ])}\n",
      "cos(angle):  0.3923116247820785   sin(angle)  0.9198323700875316\n",
      "theta dot:  0.826183373614\n",
      "Angle:  66.90143143425496\n",
      "action:  [ 0.0072208]\n",
      "state:  {1: array([ 4.38510268]), 2: array([ 1.51714077]), 3: array([ 0.32147457,  0.94691821,  0.        ])}\n",
      "cos(angle):  0.32147456798356133   sin(angle)  0.9469182130151382\n",
      "theta dot:  1.51714077068\n",
      "Angle:  71.24770942513214\n",
      "action:  [ 0.36797908]\n",
      "state:  {1: array([ 4.49922899]), 2: array([ 2.28252629]), 3: array([ 0.21154942,  0.9773673 ,  0.        ])}\n",
      "cos(angle):  0.2115494221821187   sin(angle)  0.9773673014657344\n",
      "theta dot:  2.28252629186\n",
      "Angle:  77.78665029179461\n",
      "action:  [-0.11076385]\n",
      "state:  {1: array([ 4.64917585]), 2: array([ 2.99893719]), 3: array([ 0.06317104,  0.99800272,  0.        ])}\n",
      "cos(angle):  0.06317104034039578   sin(angle)  0.9980027152579857\n",
      "theta dot:  2.99893719096\n",
      "Angle:  86.37795240494125\n",
      "action:  [ 0.00057787]\n",
      "state:  {1: array([ 4.83655214]), 2: array([ 3.74752591]), 3: array([-0.12384438,  0.99230165,  0.        ])}\n",
      "cos(angle):  -0.12384438365902826   sin(angle)  0.992301652037381\n",
      "theta dot:  3.74752590767\n",
      "Angle:  97.11379820613172\n",
      "action:  [ 0.13253928]\n",
      "state:  {1: array([ 5.0621338]), 2: array([ 4.51163304]), 3: array([-0.34265808,  0.93946018,  0.        ])}\n",
      "cos(angle):  -0.3426580838680731   sin(angle)  0.9394601841269594\n",
      "theta dot:  4.51163303856\n",
      "Angle:  110.03864457327359\n",
      "action:  [ 0.1212552]\n",
      "state:  {1: array([ 5.32385462]), 2: array([ 5.23441646]), 3: array([-0.57406815,  0.81880752,  0.        ])}\n",
      "cos(angle):  -0.5740681527232836   sin(angle)  0.8188075207451851\n",
      "theta dot:  5.23441645603\n",
      "Angle:  125.0341080645727\n",
      "action:  [ 0.55646643]\n",
      "state:  {1: array([ 5.62045422]), 2: array([ 5.93199206]), 3: array([-0.78831481,  0.6152721 ,  0.        ])}\n",
      "cos(angle):  -0.7883148134895931   sin(angle)  0.6152720982076695\n",
      "theta dot:  5.93199206155\n",
      "Angle:  142.02797378710676\n",
      "action:  [ 0.34224427]\n",
      "state:  {1: array([ 5.94269336]), 2: array([ 6.44478278]), 3: array([-0.94259049,  0.33395084,  0.        ])}\n",
      "cos(angle):  -0.9425904939451145   sin(angle)  0.33395083578919976\n",
      "theta dot:  6.44478277627\n",
      "Angle:  160.4908732607671\n",
      "action:  [ 0.36121511]\n",
      "state:  {1: array([ 6.28016477]), 2: array([ 6.74942817]), 3: array([-0.99999544,  0.00302053,  0.        ])}\n",
      "cos(angle):  -0.9999954381810973   sin(angle)  0.0030205325681715235\n",
      "theta dot:  6.74942816901\n",
      "Angle:  179.8265154561825\n",
      "action:  [ 0.40612635]\n",
      "state:  {1: array([ 0.33761009]), 2: array([ 6.81261252]), 3: array([-0.94354898, -0.33123304,  0.        ])}\n",
      "cos(angle):  -0.9435489769201673   sin(angle)  -0.33123304206088156\n",
      "theta dot:  6.81261252121\n",
      "Angle:  199.34400889856593\n",
      "action:  [ 0.35745514]\n",
      "state:  {1: array([ 0.66850039]), 2: array([ 6.61780601]), 3: array([-0.78475202, -0.61980986,  0.        ])}\n",
      "cos(angle):  -0.7847520213499872   sin(angle)  -0.6198098619634163\n",
      "theta dot:  6.6178060103\n",
      "Angle:  218.30258226650486\n",
      "action:  [ 0.48130334]\n",
      "state:  {1: array([ 0.9797576]), 2: array([ 6.22514412]), 3: array([-0.55722385, -0.83036232,  0.        ])}\n",
      "cos(angle):  -0.5572238469773267   sin(angle)  -0.8303623211344483\n",
      "theta dot:  6.225144115\n",
      "Angle:  236.1362647961093\n",
      "action:  [ 0.33178094]\n",
      "state:  {1: array([ 1.26236457]), 2: array([ 5.65213951]), 3: array([-0.30356477, -0.95281081,  0.        ])}\n",
      "cos(angle):  -0.30356477041897667   sin(angle)  -0.9528108050187477\n",
      "theta dot:  5.65213951472\n",
      "Angle:  252.32841390245693\n",
      "action:  [ 0.22921984]\n",
      "state:  {1: array([ 1.51096029]), 2: array([ 4.97191439]), 3: array([-0.05980034, -0.99821036,  0.        ])}\n",
      "cos(angle):  -0.059800337285449943   sin(angle)  -0.998210358421784\n",
      "theta dot:  4.97191438657\n",
      "Angle:  266.5718661176154\n",
      "action:  [ 0.0775832]\n",
      "state:  {1: array([ 1.722705]), 2: array([ 4.2348941]), 3: array([ 0.1513251 , -0.98848405,  0.        ])}\n",
      "cos(angle):  0.1513250951345766   sin(angle)  -0.9884840492301893\n",
      "theta dot:  4.23489409725\n",
      "Angle:  278.703915670478\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reset!\n",
      "min:  -2.99897217737  self.max:  28.6954689906\n",
      "state:  {1: array([ 0.52447612]), 2: array([-0.92486436]), 3: array([-0.8655864 , -0.50075961,  0.        ])}\n",
      "cos(angle):  -0.8655864008340288   sin(angle)  -0.5007596056903871\n",
      "theta dot:  -0.924864356584\n",
      "Angle:  210.0506184924177\n",
      "action:  [ 0.27004705]\n",
      "state:  {1: array([ 0.46147976]), 2: array([-1.259927]), 3: array([-0.89539458, -0.44527357,  0.        ])}\n",
      "cos(angle):  -0.8953945779147359   sin(angle)  -0.4452735674177079\n",
      "theta dot:  -1.25992700313\n",
      "Angle:  206.44120194411084\n",
      "action:  [ 0.31595169]\n",
      "state:  {1: array([ 0.38415529]), 2: array([-1.54648943]), 3: array([-0.92711534, -0.37477613,  0.        ])}\n",
      "cos(angle):  -0.9271153392069544   sin(angle)  -0.37477613025268003\n",
      "theta dot:  -1.54648942582\n",
      "Angle:  202.01084644614176\n",
      "action:  [ 0.32315895]\n",
      "state:  {1: array([ 0.29520041]), 2: array([-1.77909768]), 3: array([-0.95674386, -0.2909316 ,  0.        ])}\n",
      "cos(angle):  -0.9567438560694201   sin(angle)  -0.29093159655392664\n",
      "theta dot:  -1.77909768061\n",
      "Angle:  196.91411894248733\n",
      "action:  [ 0.39164056]\n",
      "state:  {1: array([ 0.19827289]), 2: array([-1.93855029]), 3: array([-0.98040824, -0.19697636,  0.        ])}\n",
      "cos(angle):  -0.9804082387783551   sin(angle)  -0.19697635730087962\n",
      "theta dot:  -1.9385502936\n",
      "Angle:  191.36059441919548\n",
      "action:  [ 0.59436176]\n",
      "state:  {1: array([ 0.09841648]), 2: array([-1.9971283]), 3: array([-0.99516101, -0.09825768,  0.        ])}\n",
      "cos(angle):  -0.9951610059233656   sin(angle)  -0.09825768310720025\n",
      "theta dot:  -1.99712829775\n",
      "Angle:  185.63925666781662\n",
      "action:  [ 0.62625176]\n",
      "state:  {1: array([ 6.2827576]), 2: array([-1.9768838]), 3: array([ -9.99999909e-01,   4.27709863e-04,   0.00000000e+00])}\n",
      "cos(angle):  -0.9999999085321326   sin(angle)  0.00042770986253316427\n",
      "theta dot:  -1.97688379682\n",
      "Angle:  179.97507316926396\n",
      "action:  [ 0.36313411]\n",
      "state:  {1: array([ 6.18665295]), 2: array([-1.9220929]), 3: array([-0.99534437,  0.0963825 ,  0.        ])}\n",
      "cos(angle):  -0.9953443692131888   sin(angle)  0.09638250191605965\n",
      "theta dot:  -1.92209289845\n",
      "Angle:  174.46869549990151\n",
      "action:  [ 0.58523065]\n",
      "state:  {1: array([ 6.09855188]), 2: array([-1.76202142]), 3: array([-0.98300361,  0.1835862 ,  0.        ])}\n",
      "cos(angle):  -0.9830036146041757   sin(angle)  0.18358620230051345\n",
      "theta dot:  -1.76202142397\n",
      "Angle:  169.4208877536248\n",
      "action:  [ 0.43130877]\n",
      "state:  {1: array([ 6.02057011]), 2: array([-1.55963546]), 3: array([-0.96571436,  0.25960697,  0.        ])}\n",
      "cos(angle):  -0.9657143574449619   sin(angle)  0.25960697183370157\n",
      "theta dot:  -1.55963545673\n",
      "Angle:  164.95287173932275\n",
      "action:  [ 0.13642903]\n",
      "state:  {1: array([ 5.95334681]), 2: array([-1.34446587]), 3: array([-0.94609467,  0.32389023,  0.        ])}\n",
      "cos(angle):  -0.9460946665644503   sin(angle)  0.3238902312486465\n",
      "theta dot:  -1.34446587362\n",
      "Angle:  161.10126973314664\n",
      "action:  [ 0.37478453]\n",
      "state:  {1: array([ 5.90108029]), 2: array([-1.04533052]), 3: array([-0.92788178,  0.3728745 ,  0.        ])}\n",
      "cos(angle):  -0.9278817841845676   sin(angle)  0.3728745024515134\n",
      "theta dot:  -1.04533052052\n",
      "Angle:  158.1066253848237\n",
      "action:  [ 28.70395536]\n",
      "state:  {1: array([ 5.87779656]), 2: array([-0.46567464]), 3: array([-0.91894915,  0.39437603,  0.        ])}\n",
      "cos(angle):  -0.9189491526405651   sin(angle)  0.39437603231077245\n",
      "theta dot:  -0.465674643683\n",
      "Angle:  156.77256891896434\n",
      "action:  [ 0.28293046]\n",
      "state:  {1: array([ 5.8714239]), 2: array([-0.12745305]), 3: array([-0.91641729,  0.40022413,  0.        ])}\n",
      "cos(angle):  -0.916417288711852   sin(angle)  0.40022412839560056\n",
      "theta dot:  -0.127453050503\n",
      "Angle:  156.40744367879074\n",
      "action:  [ 0.39100496]\n",
      "state:  {1: array([ 5.88299219]), 2: array([ 0.23136579]), 3: array([-0.92098577,  0.3895962 ,  0.        ])}\n",
      "cos(angle):  -0.920985774798341   sin(angle)  0.3895962045748129\n",
      "theta dot:  0.23136579029\n",
      "Angle:  157.0702562942122\n",
      "action:  [ 0.22602673]\n",
      "state:  {1: array([ 5.91086554]), 2: array([ 0.55746695]), 3: array([-0.93148597,  0.36377724,  0.        ])}\n",
      "cos(angle):  -0.931485974570931   sin(angle)  0.3637772383996048\n",
      "theta dot:  0.55746695298\n",
      "Angle:  158.667277740871\n",
      "action:  [-0.52882541]\n",
      "state:  {1: array([ 5.94841434]), 2: array([ 0.75097607]), 3: array([-0.94448559,  0.32855286,  0.        ])}\n",
      "cos(angle):  -0.9444855853158849   sin(angle)  0.3285528559159245\n",
      "theta dot:  0.750976070874\n",
      "Angle:  160.81866067882277\n",
      "action:  [-0.25693903]\n",
      "state:  {1: array([ 5.99635684]), 2: array([ 0.95884986]), 3: array([-0.95914596,  0.28291169,  0.        ])}\n",
      "cos(angle):  -0.9591459608656363   sin(angle)  0.2829116925033946\n",
      "theta dot:  0.958849858329\n",
      "Angle:  163.5655567588343\n",
      "action:  [-0.42984531]\n",
      "state:  {1: array([ 6.05168468]), 2: array([ 1.10655683]), 3: array([-0.97332319,  0.22943838,  0.        ])}\n",
      "cos(angle):  -0.9733231893668942   sin(angle)  0.2294383774364197\n",
      "theta dot:  1.10655683078\n",
      "Angle:  166.73560115565817\n",
      "action:  [-0.64728976]\n",
      "state:  {1: array([ 6.11076179]), 2: array([ 1.18154215]), 3: array([-0.98517186,  0.17157043,  0.        ])}\n",
      "cos(angle):  -0.9851718560134617   sin(angle)  0.1715704348627439\n",
      "theta dot:  1.18154214992\n",
      "Angle:  170.1204621657564\n",
      "action:  [-0.85946413]\n",
      "state:  {1: array([ 6.1698268]), 2: array([ 1.18130036]), 3: array([-0.9935818 ,  0.11311588,  0.        ])}\n",
      "cos(angle):  -0.9935818022424767   sin(angle)  0.11311587975431185\n",
      "theta dot:  1.18130035624\n",
      "Angle:  173.50463048957897\n",
      "action:  [-0.8520647]\n",
      "state:  {1: array([ 6.22674318]), 2: array([ 1.13832756]), 3: array([-0.99840757,  0.05641216,  0.        ])}\n",
      "cos(angle):  -0.998407566100411   sin(angle)  0.056412161396753625\n",
      "theta dot:  1.13832756164\n",
      "Angle:  176.76569111307992\n",
      "action:  [-0.56919061]\n",
      "state:  {1: array([ 6.28150609]), 2: array([ 1.09525809]), 3: array([-0.99999859,  0.00167922,  0.        ])}\n",
      "cos(angle):  -0.9999985901101385   sin(angle)  0.0016792193826976054\n",
      "theta dot:  1.09525809049\n",
      "Angle:  179.90336707894903\n",
      "action:  [-0.19649489]\n",
      "state:  {1: array([ 0.05167294]), 2: array([ 1.06704327]), 3: array([-0.99866525, -0.05164995,  0.        ])}\n",
      "cos(angle):  -0.9986652504928197   sin(angle)  -0.05164995119178398\n",
      "theta dot:  1.06704327111\n",
      "Angle:  182.9610555649514\n",
      "action:  [-0.19067057]\n",
      "state:  {1: array([ 0.1016582]), 2: array([ 0.99970522]), 3: array([-0.99483725, -0.1014832 ,  0.        ])}\n",
      "cos(angle):  -0.994837253180295   sin(angle)  -0.10148319902666462\n",
      "theta dot:  0.999705222425\n",
      "Angle:  185.82499336792517\n",
      "action:  [-0.02224827]\n",
      "state:  {1: array([ 0.14767098]), 2: array([ 0.92025558]), 3: array([-0.98911644, -0.14713487,  0.        ])}\n",
      "cos(angle):  -0.9891164397795906   sin(angle)  -0.14713486520110525\n",
      "theta dot:  0.920255581941\n",
      "Angle:  188.4613252489645\n",
      "action:  [ 0.13412389]\n",
      "state:  {1: array([ 0.18917213]), 2: array([ 0.83002302]), 3: array([-0.98216025, -0.18804586,  0.        ])}\n",
      "cos(angle):  -0.9821602483593582   sin(angle)  -0.18804586287042793\n",
      "theta dot:  0.830023016875\n",
      "Angle:  190.83916047683482\n",
      "action:  [ 0.19604121]\n",
      "state:  {1: array([ 0.22509187]), 2: array([ 0.7183948]), 3: array([-0.97477361, -0.22319592,  0.        ])}\n",
      "cos(angle):  -0.9747736051747727   sin(angle)  -0.22319591988783372\n",
      "theta dot:  0.718394800611\n",
      "Angle:  192.8972051691886\n",
      "action:  [ 0.17906106]\n",
      "state:  {1: array([ 0.25398473]), 2: array([ 0.57785702]), 3: array([-0.9679189 , -0.25126283,  0.        ])}\n",
      "cos(angle):  -0.9679188952631282   sin(angle)  -0.2512628348813358\n",
      "theta dot:  0.577857019486\n",
      "Angle:  194.55263971698986\n",
      "action:  [ 0.11219448]\n",
      "state:  {1: array([ 0.27429668]), 2: array([ 0.40623907]), 3: array([-0.96261594, -0.27086998,  0.        ])}\n",
      "cos(angle):  -0.9626159443101284   sin(angle)  -0.270869975744673\n",
      "theta dot:  0.406239065736\n",
      "Angle:  195.71642619255047\n",
      "action:  [-0.01706255]\n",
      "state:  {1: array([ 0.28432304]), 2: array([ 0.2005272]), 3: array([-0.95985177, -0.28050773,  0.        ])}\n",
      "cos(angle):  -0.9598517654214397   sin(angle)  -0.28050773325765116\n",
      "theta dot:  0.200527202105\n",
      "Angle:  196.290892967105\n",
      "action:  [-0.14719784]\n",
      "state:  {1: array([ 0.28272638]), 2: array([-0.03193327]), 3: array([-0.96029842, -0.27897482,  0.        ])}\n",
      "cos(angle):  -0.9602984182604799   sin(angle)  -0.2789748158641215\n",
      "theta dot:  -0.0319332744052\n",
      "Angle:  196.19941108855676\n",
      "action:  [-0.14589045]\n",
      "state:  {1: array([ 0.26957398]), 2: array([-0.26304795]), 3: array([-0.96388444, -0.26632082,  0.        ])}\n",
      "cos(angle):  -0.9638844425131959   sin(angle)  -0.2663208243472999\n",
      "theta dot:  -0.263047953728\n",
      "Angle:  195.44583597283398\n",
      "action:  [-0.08122895]\n",
      "state:  {1: array([ 0.24582533]), 2: array([-0.47497291]), 3: array([-0.96993681, -0.24335693,  0.        ])}\n",
      "cos(angle):  -0.9699368051539292   sin(angle)  -0.24335692718266452\n",
      "theta dot:  -0.474972914534\n",
      "Angle:  194.0851419854364\n",
      "action:  [-0.21996084]\n",
      "state:  {1: array([ 0.21130109]), 2: array([-0.69048474]), 3: array([-0.97775886, -0.20973223,  0.        ])}\n",
      "cos(angle):  -0.9777588608492599   sin(angle)  -0.20973223412427014\n",
      "theta dot:  -0.690484735208\n",
      "Angle:  192.10705355378485\n",
      "action:  [-0.27238259]\n",
      "state:  {1: array([ 0.16686903]), 2: array([-0.8886413]), 3: array([-0.98610964, -0.16609569,  0.        ])}\n",
      "cos(angle):  -0.9861096400781104   sin(angle)  -0.1660956885202608\n",
      "theta dot:  -0.888641299102\n",
      "Angle:  189.56128970991057\n",
      "action:  [-0.53449931]\n",
      "state:  {1: array([ 0.11219963]), 2: array([-1.09338796]), 3: array([-0.99371222, -0.11196437,  0.        ])}\n",
      "cos(angle):  -0.993712221732927   sin(angle)  -0.11196437102315249\n",
      "theta dot:  -1.09338796221\n",
      "Angle:  186.42897125439399\n",
      "action:  [-0.61322477]\n",
      "state:  {1: array([ 0.04873238]), 2: array([-1.26934496]), 3: array([-0.99881281, -0.0487131 ,  0.        ])}\n",
      "cos(angle):  -0.9988128123457534   sin(angle)  -0.0487130977660724\n",
      "theta dot:  -1.26934495524\n",
      "Angle:  182.7925743237942\n",
      "action:  [-0.43991694]\n",
      "state:  {1: array([ 6.26332433]), 2: array([-1.37186732]), 3: array([-0.99980278,  0.01985968,  0.        ])}\n",
      "cos(angle):  -0.999802777182818   sin(angle)  0.01985967616866955\n",
      "theta dot:  -1.37186731973\n",
      "Angle:  178.86163130582645\n",
      "action:  [-0.31504616]\n",
      "state:  {1: array([ 6.19311285]), 2: array([-1.40422949]), 3: array([-0.99594622,  0.08995071,  0.        ])}\n",
      "cos(angle):  -0.9959462181369   sin(angle)  0.08995071194163193\n",
      "theta dot:  -1.40422948728\n",
      "Angle:  174.8388195584689\n",
      "action:  [-0.31601731]\n",
      "state:  {1: array([ 6.1239044]), 2: array([-1.38416905]), 3: array([-0.98734159,  0.15860826,  0.        ])}\n",
      "cos(angle):  -0.9873415924595911   sin(angle)  0.15860825892543784\n",
      "theta dot:  -1.38416904925\n",
      "Angle:  170.87347659844443\n",
      "action:  [-0.12053011]\n",
      "state:  {1: array([ 6.05973978]), 2: array([-1.28329237]), 3: array([-0.97513974,  0.2215908 ,  0.        ])}\n",
      "cos(angle):  -0.9751397419500971   sin(angle)  0.22159080231250125\n",
      "theta dot:  -1.28329237121\n",
      "Angle:  167.19712335776342\n",
      "action:  [-0.60111414]\n",
      "state:  {1: array([ 5.99937646]), 2: array([-1.20726639]), 3: array([-0.95999587,  0.28001415,  0.        ])}\n",
      "cos(angle):  -0.9599958733706104   sin(angle)  0.2800141480557705\n",
      "theta dot:  -1.20726639092\n",
      "Angle:  163.73856799799637\n",
      "action:  [-0.55514661]\n",
      "state:  {1: array([ 5.94535007]), 2: array([-1.08052777]), 3: array([-0.94347438,  0.33144547,  0.        ])}\n",
      "cos(angle):  -0.9434743770668352   sin(angle)  0.3314454703542457\n",
      "theta dot:  -1.08052777077\n",
      "Angle:  160.6430911909797\n",
      "action:  [-0.56599664]\n",
      "state:  {1: array([ 5.89950791]), 2: array([-0.91684316]), 3: array([-0.92729434,  0.37433302,  0.        ])}\n",
      "cos(angle):  -0.927294338841318   sin(angle)  0.37433301905234445\n",
      "theta dot:  -0.916843164546\n",
      "Angle:  158.01653514281622\n",
      "action:  [-0.56817545]\n",
      "state:  {1: array([ 5.86344193]), 2: array([-0.72131972]), 3: array([-0.91319355,  0.40752612,  0.        ])}\n",
      "cos(angle):  -0.9131935499467407   sin(angle)  0.40752612227398327\n",
      "theta dot:  -0.721319718254\n",
      "Angle:  155.95011119823732\n",
      "action:  [ 0.19463164]\n",
      "state:  {1: array([ 5.84411791]), 2: array([-0.38648038]), 3: array([-0.9051485 ,  0.42509551,  0.        ])}\n",
      "cos(angle):  -0.9051485014884122   sin(angle)  0.4250955072137107\n",
      "theta dot:  -0.386480380759\n",
      "Angle:  154.84292905320706\n",
      "action:  [ 0.35795454]\n",
      "state:  {1: array([ 5.84341963]), 2: array([-0.01396557]), 3: array([-0.90485145,  0.42572745,  0.        ])}\n",
      "cos(angle):  -0.9048514457854249   sin(angle)  0.42572744926775186\n",
      "theta dot:  -0.0139655700944\n",
      "Angle:  154.80292073551868\n",
      "action:  [ 0.87100768]\n",
      "state:  {1: array([ 5.86521869]), 2: array([ 0.43598117]), 3: array([-0.91391618,  0.40590295,  0.        ])}\n",
      "cos(angle):  -0.9139161846293425   sin(angle)  0.40590295326906084\n",
      "theta dot:  0.435981169065\n",
      "Angle:  156.0519118615657\n",
      "action:  [ 0.34869925]\n",
      "state:  {1: array([ 5.90485435]), 2: array([ 0.79271327]), 3: array([-0.92928242,  0.36936997,  0.        ])}\n",
      "cos(angle):  -0.9292824247403498   sin(angle)  0.3693699704479182\n",
      "theta dot:  0.792713270877\n",
      "Angle:  158.3228627903577\n",
      "action:  [ 0.30713467]\n",
      "state:  {1: array([ 5.9606449]), 2: array([ 1.11581095]), 3: array([-0.94843323,  0.31697699,  0.        ])}\n",
      "cos(angle):  -0.9484332286406982   sin(angle)  0.31697698782432315\n",
      "theta dot:  1.11581094874\n",
      "Angle:  161.51941822029676\n",
      "action:  [ 0.32960584]\n",
      "state:  {1: array([ 6.03079413]), 2: array([ 1.40298457]), 3: array([-0.96831807,  0.24972009,  0.        ])}\n",
      "cos(angle):  -0.9683180650488448   sin(angle)  0.24972009310438212\n",
      "theta dot:  1.40298456515\n",
      "Angle:  165.53866353680053\n",
      "action:  [ 0.38481611]\n",
      "state:  {1: array([ 6.11319398]), 2: array([ 1.64799705]), 3: array([-0.98558623,  0.1691738 ,  0.        ])}\n",
      "cos(angle):  -0.9855862342291224   sin(angle)  0.16917380086188732\n",
      "theta dot:  1.64799705106\n",
      "Angle:  170.25981628046804\n",
      "action:  [ 0.36766263]\n",
      "state:  {1: array([ 6.20469532]), 2: array([ 1.8300268]), 3: array([-0.99692124,  0.07840942,  0.        ])}\n",
      "cos(angle):  -0.9969212420616625   sin(angle)  0.07840941988200179\n",
      "theta dot:  1.83002679581\n",
      "Angle:  175.50244461072077\n",
      "action:  [ 0.26469564]\n",
      "state:  {1: array([ 0.01793692]), 2: array([ 1.92853821]), 3: array([-0.99983914, -0.01793596,  0.        ])}\n",
      "cos(angle):  -0.9998391377038444   sin(angle)  -0.01793596151960851\n",
      "theta dot:  1.92853820611\n",
      "Angle:  181.02812851785833\n",
      "action:  [ 0.09713994]\n",
      "state:  {1: array([ 0.11441978]), 2: array([ 1.92965723]), 3: array([-0.99346119, -0.11417029,  0.        ])}\n",
      "cos(angle):  -0.9934611948935613   sin(angle)  -0.11417028615474892\n",
      "theta dot:  1.92965722548\n",
      "Angle:  186.55617633723026\n",
      "action:  [ 0.0176544]\n",
      "state:  {1: array([ 0.20675367]), 2: array([ 1.84667767]), 3: array([-0.97870249, -0.20528379,  0.        ])}\n",
      "cos(angle):  -0.9787024899825879   sin(angle)  -0.20528379405564973\n",
      "theta dot:  1.8466776708\n",
      "Angle:  191.84650579902205\n",
      "action:  [-0.06889518]\n",
      "state:  {1: array([ 0.2908727]), 2: array([ 1.68238055]), 3: array([-0.95799396, -0.28678837,  0.        ])}\n",
      "cos(angle):  -0.9579939614203432   sin(angle)  -0.28678837124639117\n",
      "theta dot:  1.68238054771\n",
      "Angle:  196.66615977450078\n",
      "action:  [-0.27835398]\n",
      "state:  {1: array([ 0.3621495]), 2: array([ 1.42553617]), 3: array([-0.93513745, -0.35428513,  0.        ])}\n",
      "cos(angle):  -0.9351374472451549   sin(angle)  -0.354285132005021\n",
      "theta dot:  1.42553617238\n",
      "Angle:  200.75001053572356\n",
      "action:  [-0.4163301]\n",
      "state:  {1: array([ 0.41701814]), 2: array([ 1.09737281]), 3: array([-0.91430076, -0.40503595,  0.        ])}\n",
      "cos(angle):  -0.9143007618790755   sin(angle)  -0.4050359451052982\n",
      "theta dot:  1.09737280899\n",
      "Angle:  203.89374470966973\n",
      "action:  [-0.20615999]\n",
      "state:  {1: array([ 0.45515174]), 2: array([ 0.76267185]), 3: array([-0.89819434, -0.43959861,  0.        ])}\n",
      "cos(angle):  -0.8981943350771204   sin(angle)  -0.43959860831828107\n",
      "theta dot:  0.762671851298\n",
      "Angle:  206.07863351208928\n",
      "action:  [-0.18820657]\n",
      "state:  {1: array([ 0.47538883]), 2: array([ 0.40474191]), 3: array([-0.88911483, -0.4576842 ,  0.        ])}\n",
      "cos(angle):  -0.8891148263028322   sin(angle)  -0.4576841986003935\n",
      "theta dot:  0.404741908956\n",
      "Angle:  207.23813095944288\n",
      "action:  [ 0.04161551]\n",
      "state:  {1: array([ 0.47877489]), 2: array([ 0.06772109]), 3: array([-0.88755999, -0.46069216,  0.        ])}\n",
      "cos(angle):  -0.8875599887040064   sin(angle)  -0.46069216018046577\n",
      "theta dot:  0.067721086171\n",
      "Angle:  207.43213712685412\n",
      "action:  [ 0.01411271]\n",
      "state:  {1: array([ 0.46499083]), 2: array([-0.27568113]), 3: array([-0.89382568, -0.44841461,  0.        ])}\n",
      "cos(angle):  -0.893825677378894   sin(angle)  -0.4484146055361728\n",
      "theta dot:  -0.27568112807\n",
      "Angle:  206.64237071718009\n",
      "action:  [ 0.03069779]\n",
      "state:  {1: array([ 0.43462146]), 2: array([-0.60738741]), 3: array([-0.9070295 , -0.42106708,  0.        ])}\n",
      "cos(angle):  -0.9070294980837721   sin(angle)  -0.4210670844484291\n",
      "theta dot:  -0.607387413406\n",
      "Angle:  204.90233802025782\n",
      "action:  [ 0.08081064]\n",
      "state:  {1: array([ 0.38906815]), 2: array([-0.91106613]), 3: array([-0.92526294, -0.37932638,  0.        ])}\n",
      "cos(angle):  -0.9252629355495393   sin(angle)  -0.37932637674995534\n",
      "theta dot:  -0.911066130229\n",
      "Angle:  202.29233191761531\n",
      "action:  [-0.08772602]\n",
      "state:  {1: array([ 0.32863216]), 2: array([-1.20871982]), 3: array([-0.9464847 , -0.32274869,  0.        ])}\n",
      "cos(angle):  -0.9464846965638126   sin(angle)  -0.32274869352254165\n",
      "theta dot:  -1.2087198153\n",
      "Angle:  198.82961281343748\n",
      "action:  [-0.09163797]\n",
      "state:  {1: array([ 0.25540581]), 2: array([-1.46452703]), 3: array([-0.96756085, -0.25263808,  0.        ])}\n",
      "cos(angle):  -0.9675608520497565   sin(angle)  -0.2526380762686996\n",
      "theta dot:  -1.46452703109\n",
      "Angle:  194.6340617312463\n",
      "action:  [ 0.12910638]\n",
      "state:  {1: array([ 0.17367383]), 2: array([-1.63463963]), 3: array([-0.98495657, -0.17280207,  0.        ])}\n",
      "cos(angle):  -0.9849565699885788   sin(angle)  -0.17280206953718388\n",
      "theta dot:  -1.6346396311\n",
      "Angle:  189.95117508753728\n",
      "action:  [ 0.36753712]\n",
      "state:  {1: array([ 0.0882183]), 2: array([-1.70911062]), 3: array([-0.99611129, -0.08810392,  0.        ])}\n",
      "cos(angle):  -0.9961112888515092   sin(angle)  -0.08810391718070827\n",
      "theta dot:  -1.7091106154\n",
      "Angle:  185.0549452878727\n",
      "action:  [ 0.54627189]\n",
      "state:  {1: array([ 0.00355591]), 2: array([-1.69324777]), 3: array([-0.99999368, -0.0035559 ,  0.        ])}\n",
      "cos(angle):  -0.9999936777579055   sin(angle)  -0.003555902729013646\n",
      "theta dot:  -1.69324776916\n",
      "Angle:  180.20415908898008\n",
      "action:  [ 0.58450601]\n",
      "state:  {1: array([ 6.20632928]), 2: array([-1.6082388]), 3: array([-0.99704803,  0.07678039,  0.        ])}\n",
      "cos(angle):  -0.9970480288616493   sin(angle)  0.07678038905280268\n",
      "theta dot:  -1.60823879525\n",
      "Angle:  175.59606325725053\n",
      "action:  [ 0.4257602]\n",
      "state:  {1: array([ 6.1319898]), 2: array([-1.48678947]), 3: array([-0.98859172,  0.1506201 ,  0.        ])}\n",
      "cos(angle):  -0.9885917176210797   sin(angle)  0.1506201044051011\n",
      "theta dot:  -1.48678947352\n",
      "Angle:  171.33673512454413\n",
      "action:  [ 0.16234618]\n",
      "state:  {1: array([ 6.06451618]), 2: array([-1.34947247]), 3: array([-0.97618702,  0.21693063,  0.        ])}\n",
      "cos(angle):  -0.9761870209782255   sin(angle)  0.2169306342443534\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "theta dot:  -1.34947246775\n",
      "Angle:  167.4707903162597\n",
      "action:  [-0.11667208]\n",
      "state:  {1: array([ 6.00430242]), 2: array([-1.2042753]), 3: array([-0.96136356,  0.27528187,  0.        ])}\n",
      "cos(angle):  -0.9613635577461681   sin(angle)  0.27528187342727495\n",
      "theta dot:  -1.20427530366\n",
      "Angle:  164.02080377024402\n",
      "action:  [-0.30686686]\n",
      "state:  {1: array([ 5.95211022]), 2: array([-1.04384393]), 3: array([-0.94569342,  0.32505992,  0.        ])}\n",
      "cos(angle):  -0.9456934220413239   sin(angle)  0.32505991987289123\n",
      "theta dot:  -1.04384392786\n",
      "Angle:  161.03041818623205\n",
      "action:  [-0.50435349]\n",
      "state:  {1: array([ 5.90832512]), 2: array([-0.87570201]), 3: array([-0.93055882,  0.36614243,  0.        ])}\n",
      "cos(angle):  -0.9305588218435549   sin(angle)  0.36614243005848857\n",
      "theta dot:  -0.875702011101\n",
      "Angle:  158.5217225852922\n",
      "action:  [-0.62065443]\n",
      "state:  {1: array([ 5.87361545]), 2: array([-0.69419335]), 3: array([-0.9172922,  0.3982148,  0.       ])}\n",
      "cos(angle):  -0.9172921972257341   sin(angle)  0.3982147974508042\n",
      "theta dot:  -0.694193352931\n",
      "Angle:  156.53300977131778\n",
      "action:  [-0.72685097]\n",
      "state:  {1: array([ 5.84838746]), 2: array([-0.5045599]), 3: array([-0.90695521,  0.42122707,  0.        ])}\n",
      "cos(angle):  -0.9069552112878563   sin(angle)  0.42122707025759853\n",
      "theta dot:  -0.504559900586\n",
      "Angle:  155.08755551066224\n",
      "action:  [-0.41416628]\n",
      "state:  {1: array([ 5.83584923]), 2: array([-0.25076454]), 3: array([-0.90160262,  0.43256527,  0.        ])}\n",
      "cos(angle):  -0.9016026200741631   sin(angle)  0.43256527308072734\n",
      "theta dot:  -0.250764539324\n",
      "Angle:  154.3691697028214\n",
      "action:  [-0.17269131]\n",
      "state:  {1: array([ 5.83823701]), 2: array([ 0.04775572]), 3: array([-0.90263292,  0.43041121,  0.        ])}\n",
      "cos(angle):  -0.9026329221262006   sin(angle)  0.43041120790926946\n",
      "theta dot:  0.0477557191979\n",
      "Angle:  154.50597944078322\n",
      "action:  [ 0.3206087]\n",
      "state:  {1: array([ 5.85916979]), 2: array([ 0.41865543]), 3: array([-0.91144421,  0.41142368,  0.        ])}\n",
      "cos(angle):  -0.9114442126009723   sin(angle)  0.41142368346534636\n",
      "theta dot:  0.418655430786\n",
      "Angle:  155.7053360988782\n",
      "action:  [ 0.38095826]\n",
      "state:  {1: array([ 5.89838813]), 2: array([ 0.78436693]), 3: array([-0.92687459,  0.37537115,  0.        ])}\n",
      "cos(angle):  -0.9268745866595496   sin(angle)  0.3753711504693575\n",
      "theta dot:  0.784366931842\n",
      "Angle:  157.95237658352812\n",
      "action:  [ 0.5871299]\n",
      "state:  {1: array([ 5.95608637]), 2: array([ 1.15396478]), 3: array([-0.94697843,  0.32129714,  0.        ])}\n",
      "cos(angle):  -0.9469784309812793   sin(angle)  0.3212971385746136\n",
      "theta dot:  1.15396478017\n",
      "Angle:  161.2582344335068\n",
      "action:  [ 0.69382116]\n",
      "state:  {1: array([ 6.03103691]), 2: array([ 1.49901081]), 3: array([-0.96837866,  0.24948499,  0.        ])}\n",
      "cos(angle):  -0.9683786646719327   sin(angle)  0.24948499315230274\n",
      "theta dot:  1.49901080852\n",
      "Angle:  165.55257403011205\n",
      "action:  [ 0.71048758]\n",
      "state:  {1: array([ 6.1206718]), 2: array([ 1.79269769]), 3: array([-0.98682372,  0.16179911,  0.        ])}\n",
      "cos(angle):  -0.9868237173261715   sin(angle)  0.16179910668034136\n",
      "theta dot:  1.79269769105\n",
      "Angle:  170.68826260263666\n",
      "action:  [ 0.85531929]\n",
      "state:  {1: array([ 6.22278904]), 2: array([ 2.04234491]), 3: array([-0.9981767 ,  0.06035955,  0.        ])}\n",
      "cos(angle):  -0.9981766999542846   sin(angle)  0.06035955324862872\n",
      "theta dot:  2.04234491497\n",
      "Angle:  176.53913611765188\n",
      "action:  [ 0.47613344]\n",
      "state:  {1: array([ 0.04755547]), 2: array([ 2.1590346]), 3: array([-0.99886945, -0.04753754,  0.        ])}\n",
      "cos(angle):  -0.9988694519483023   sin(angle)  -0.04753754268468356\n",
      "theta dot:  2.15903459663\n",
      "Angle:  182.72514199917018\n",
      "action:  [ 0.28816499]\n",
      "state:  {1: array([ 0.15588577]), 2: array([ 2.16660619]), 3: array([-0.9878744 , -0.15525519,  0.        ])}\n",
      "cos(angle):  -0.9878743972167178   sin(angle)  -0.15525519419235695\n",
      "theta dot:  2.16660618758\n",
      "Angle:  188.93199700560638\n",
      "action:  [ 0.04700091]\n",
      "state:  {1: array([ 0.25874652]), 2: array([ 2.05721493]), 3: array([-0.96671146, -0.255869  ,  0.        ])}\n",
      "cos(angle):  -0.9667114639001175   sin(angle)  -0.2558690007876918\n",
      "theta dot:  2.05721492855\n",
      "Angle:  194.82546987195735\n",
      "action:  [-0.25366292]\n",
      "state:  {1: array([ 0.35010971]), 2: array([ 1.82726374]), 3: array([-0.93933509, -0.34300086,  0.        ])}\n",
      "cos(angle):  -0.9393350885197562   sin(angle)  -0.3430008622082192\n",
      "theta dot:  1.82726374068\n",
      "Angle:  200.06018265083392\n",
      "action:  [-0.1610423]\n",
      "state:  {1: array([ 0.42740255]), 2: array([ 1.54585675]), 3: array([-0.91004549, -0.4145084 ,  0.        ])}\n",
      "cos(angle):  -0.9100454850665427   sin(angle)  -0.4145084017363231\n",
      "theta dot:  1.54585674962\n",
      "Angle:  204.48872566922245\n",
      "action:  [-0.39545637]\n",
      "state:  {1: array([ 0.4861854]), 2: array([ 1.17565699]), 3: array([-0.88412169, -0.46725672,  0.        ])}\n",
      "cos(angle):  -0.8841216865152131   sin(angle)  -0.46725672112179983\n",
      "theta dot:  1.1756569926\n",
      "Angle:  207.8567269849282\n",
      "action:  [-0.44198421]\n",
      "state:  {1: array([ 0.52413124]), 2: array([ 0.75891682]), 3: array([-0.86575905, -0.50046105,  0.        ])}\n",
      "cos(angle):  -0.8657590507140922   sin(angle)  -0.5004610535362707\n",
      "theta dot:  0.758916820003\n",
      "Angle:  210.03085844024562\n",
      "action:  [-0.43110818]\n",
      "state:  {1: array([ 0.54007648]), 2: array([ 0.3189048]), 3: array([-0.85766936, -0.51420158,  0.        ])}\n",
      "cos(angle):  -0.857669359607884   sin(angle)  -0.514201584585075\n",
      "theta dot:  0.318904802474\n",
      "Angle:  210.94445126627835\n",
      "action:  [-0.5034407]\n",
      "state:  {1: array([ 0.53296335]), 2: array([-0.14226249]), 3: array([-0.86130521, -0.50808792,  0.        ])}\n",
      "cos(angle):  -0.861305211235347   sin(angle)  -0.5080879186704151\n",
      "theta dot:  -0.142262491623\n",
      "Angle:  210.53690020166\n",
      "action:  [-0.27386577]\n",
      "state:  {1: array([ 0.50474294]), 2: array([-0.5644083]), 3: array([-0.87529881, -0.48358245,  0.        ])}\n",
      "cos(angle):  -0.875298814530006   sin(angle)  -0.4835824493117655\n",
      "theta dot:  -0.564408296743\n",
      "Angle:  208.91999331641574\n",
      "action:  [-0.34365543]\n",
      "state:  {1: array([ 0.45581076]), 2: array([-0.97864345]), 3: array([-0.89790443, -0.44019045,  0.        ])}\n",
      "cos(angle):  -0.8979044325219167   sin(angle)  -0.44019044748551145\n",
      "theta dot:  -0.978643447801\n",
      "Angle:  206.1163929120971\n",
      "action:  [-0.16848235]\n",
      "state:  {1: array([ 0.38910783]), 2: array([-1.33405864]), 3: array([-0.92524788, -0.37936309,  0.        ])}\n",
      "cos(angle):  -0.9252478833794171   sin(angle)  -0.37936309032628435\n",
      "theta dot:  -1.33405863654\n",
      "Angle:  202.2946053742563\n",
      "action:  [-0.03690365]\n",
      "state:  {1: array([ 0.30790201]), 2: array([-1.6241165]), 3: array([-0.95297148, -0.30305998,  0.        ])}\n",
      "cos(angle):  -0.9529714842221146   sin(angle)  -0.3030599779903309\n",
      "theta dot:  -1.62411650156\n",
      "Angle:  197.64186520553082\n",
      "3\n",
      "1\n"
     ]
    }
   ],
   "source": [
    "exp_name = \"box2d_arm_example\"\n",
    "\n",
    "import random\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "#print(config)\n",
    "#print(\"------------------------------\")\n",
    "#print(config_gps)\n",
    "\n",
    "seed = config_gps.get('random_seed', 0)\n",
    "random.seed(seed)\n",
    "np.random.seed(seed)\n",
    "\n",
    "gps = GPSMain(config_gps)\n",
    "if config_gps['gui_on']:\n",
    "    run_gps = threading.Thread(\n",
    "        #target=lambda: gps.run(itr_load=resume_training_itr) # no iteration load used at this stage\n",
    "        target=lambda: gps.run()\n",
    "    )\n",
    "    run_gps.daemon = True\n",
    "    run_gps.start()\n",
    "\n",
    "    plt.ioff()\n",
    "    plt.show()\n",
    "else:\n",
    "    gps.run()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "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.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}