2DofArm_simulation_data_generator_and_physics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# In this notebook I'm generating the movements and the states variables (torques and angles) in order to produce this movement using the 2dof simulator.\n",
    "\n",
    "### Some of the algorithms (or inspiration) to simulate the 2dof arm came from: http://www.gribblelab.org/compneuro/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Here starts the 2 joint arm study"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Main functions to the 2 joint arm simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Makes possible to show the output from matplotlib inline\n",
    "%matplotlib inline\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# Makes the figures in the PNG format:\n",
    "# For more information see %config InlineBackend\n",
    "%config InlineBackend.figure_formats=set([u'png'])\n",
    "\n",
    "plt.rcParams['figure.figsize'] = 20, 10\n",
    "\n",
    "import numpy\n",
    "import sys\n",
    "import save_load_file as slf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Loads the modules and starts the object to be used with the parallel processing iPython stuff...\n",
    "\n",
    "# Remember to execute at the shell: ipcluster start -n 4\n",
    "# or from the iPython notebook interface!\n",
    "# from IPython.parallel import Client\n",
    "from ipyparallel import Client\n",
    "\n",
    "# When using the ipython in my desktop, launch the cluster in the right profile :)\n",
    "cli = Client()\n",
    "\n",
    "# lbview = cli.load_balanced_view()\n",
    "dview = cli[:]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Overwriting simulation_2DoF_Arm_physics.py\n"
     ]
    }
   ],
   "source": [
    "%%file simulation_2DoF_Arm_physics.py\n",
    "\n",
    "# 2 DOF Simulator module\n",
    "# Strongly based on http://www.gribblelab.org/compneuro/index.html\n",
    "\n",
    "import numpy\n",
    "\n",
    "# forward kinematics\n",
    "def joints_to_hand(A,aparams):\n",
    "    \"\"\"\n",
    "    Given joint angles A=(a1,a2) and anthropometric params aparams,\n",
    "    returns hand position H=(hx,hy) and elbow position E=(ex,ey)\n",
    "    Note1: A must be type matrix (or array([[a1,a2],...]))\n",
    "    Note2: If A has multiple lines, H and E will have the same number of lines.\n",
    "    \"\"\"\n",
    "    l1 = aparams['l1']\n",
    "    l2 = aparams['l2']\n",
    "    n = numpy.shape(A)[0]\n",
    "    E = numpy.zeros((n,2))\n",
    "    H = numpy.zeros((n,2))\n",
    "    for i in range(n):\n",
    "        E[i,0] = l1 * numpy.cos(A[i,0])\n",
    "        E[i,1] = l1 * numpy.sin(A[i,0])\n",
    "        H[i,0] = E[i,0] + (l2 * numpy.cos(A[i,0]+A[i,1]))\n",
    "        H[i,1] = E[i,1] + (l2 * numpy.sin(A[i,0]+A[i,1]))\n",
    "    return H,E\n",
    "\n",
    "\n",
    "# I could do the inverse kinematics using all the possible values (workspace) of the arm and creating a numpy array. Then I \n",
    "# could use the argmin trick to find the value.\n",
    "# In order to solve the problem when multiple solutions appear I could use the minimum jerk criterion. The user should enter\n",
    "# the actual position and the next one, then the system solves according to the one that uses mininum energy.\n",
    "# One curious thing about inverse kinematics is that as human beings we cannot do a inverse kinematic of our hand position\n",
    "# without taking in account the actual position. The function, below, doesn't care about the actual position, and that is why \n",
    "# more than one solution appears.\n",
    "# So, I don't think the brain solves the problem of multiple solutions. Who solves this problem is the morphology of the limbs.\n",
    "# It is impossible to change trajectories instantaneously, therefore the continuity of the movements is guaranteed.\n",
    "# Summary: there are no positions, but trajectories :)\n",
    "\n",
    "# inverse kinematics\n",
    "def hand_to_joints(H,aparams):\n",
    "    \"\"\"\n",
    "    Given hand position H=(hx,hy) and anthropometric params aparams,\n",
    "    returns joint angles A=(a1,a2)\n",
    "    Note1: H must be type matrix (or array([[hx,hy],...]))\n",
    "    Note2: If H has multiple lines, A will have the same number of lines.\n",
    "    \"\"\"\n",
    "    l1 = aparams['l1']\n",
    "    l2 = aparams['l2']\n",
    "    n = numpy.shape(H)[0]\n",
    "    A = numpy.zeros((n,2))\n",
    "    for i in range(n):\n",
    "        A[i,1] = numpy.arccos(((H[i,0]*H[i,0])+(H[i,1]*H[i,1])-(l1*l1)-(l2*l2))/(2.0*l1*l2))\n",
    "        A[i,0] = numpy.arctan2(H[i,1],H[i,0]) - numpy.arctan2((l2*numpy.sin(A[i,1])),(l1+(l2*numpy.cos(A[i,1]))))\n",
    "#         if A[i,0] < 0:\n",
    "#             print \"<0:\",A[i,0]\n",
    "#             A[i,0] = A[i,0] + pi\n",
    "#         elif A[i,0] > pi:\n",
    "#             print \">0:\",A[i,0]\n",
    "#             A[i,0] = A[i,0] - pi\n",
    "    return A\n",
    "\n",
    "\n",
    "# inverse kinematics\n",
    "def hand_to_joints(H,aparams,ang_error=0.01):\n",
    "    \"\"\"\n",
    "    Given hand position H=(hx,hy) and anthropometric params aparams,\n",
    "    returns joint angles A=(a1,a2)\n",
    "    Note1: H must be type matrix (or array([[hx,hy],...]))\n",
    "    Note2: If H has multiple lines, A will have the same number of lines.\n",
    "    \"\"\"\n",
    "    l1 = aparams['l1']\n",
    "    l2 = aparams['l2']\n",
    "    n = numpy.shape(H)[0]\n",
    "    A = numpy.zeros((n,2))\n",
    "    t_bias=[0,0] \n",
    "    for i in range(n):\n",
    "        A[i,1] = numpy.arccos(((H[i,0]*H[i,0])+(H[i,1]*H[i,1])-(l1*l1)-(l2*l2))/(2.0*l1*l2)) + t_bias[1]\n",
    "        A[i,0] = numpy.arctan2(H[i,1],H[i,0]) - numpy.arctan2((l2*numpy.sin(A[i,1])),(l1+(l2*numpy.cos(A[i,1])))) + t_bias[0]\n",
    "        if i>0:\n",
    "            # Here I'm trying to avoid descontinuity problems when there's a 2pi difference between them!\n",
    "            if 0<=abs(abs((A[i,1]-A[i-1,1])/numpy.pi)-2)<=ang_error:\n",
    "                print \"Correction on Joint 2:\",(A[i,1],A[i-1,1])\n",
    "                if (A[i,1]-A[i-1,1])>0:\n",
    "                    A[i,1]-=2*numpy.pi\n",
    "                    t_bias[1]-=2*numpy.pi\n",
    "                else:\n",
    "                    A[i,1]+=2*numpy.pi\n",
    "                    t_bias[1]+=2*numpy.pi  \n",
    "                    \n",
    "            if 0<=abs(abs((A[i,0]-A[i-1,0])/numpy.pi)-2)<=ang_error:\n",
    "                print \"Correction on Joint 1:\",(A[i,0],A[i-1,0])\n",
    "                if (A[i,0]-A[i-1,0])>0:\n",
    "                    A[i,0]-=2*numpy.pi\n",
    "                    t_bias[0]-=2*numpy.pi\n",
    "                else:\n",
    "                    A[i,0]+=2*numpy.pi\n",
    "                    t_bias[0]+=2*numpy.pi\n",
    "    return A\n",
    "\n",
    "\n",
    "# Generates the movements according to:\n",
    "# Flash, Tamar and Neville Hogan. 1985. The Coordination of Arm Movements: An Experimentally Confirmed Mathematical Model. The Journal of Neuroscience 5 (7): 1688-1703\n",
    "def cartesian_movement_generation_training(xstart,ystart,xdest,ydest,MT,t):\n",
    "    '''\n",
    "    xstart,ystart: initial position of the trajectory\n",
    "    xdest,ydest: final position of the trajectory\n",
    "    MT: total time spent doing the trajectory\n",
    "    t: current time\n",
    "    \n",
    "    returns a matrix: [[x0,y0],[x1,y1],...]\n",
    "    '''\n",
    "    x_t=xstart+(xstart-xdest)*(15*(t/MT)**4-6*(t/MT)**5-10*(t/MT)**3)\n",
    "    y_t=ystart+(ystart-ydest)*(15*(t/MT)**4-6*(t/MT)**5-10*(t/MT)**3)    \n",
    "    return numpy.array([x_t,y_t]).T\n",
    "\n",
    "\n",
    "# Used to generate the velocities and the accelerations using the position and time vectors\n",
    "def derivator(v,t):\n",
    "    return numpy.array([(v[i+1]-v[i])/(t[i+1]-t[i]) for i in range(len(t)-1)])\n",
    "\n",
    "\n",
    "def twojointarm_torques(state, t, aparams):\n",
    "    \"\"\"\n",
    "    Calculates the necessaries torques to generate the accelerations\n",
    "    \"\"\"\n",
    "    import numpy\n",
    "\n",
    "    a1,a2,a1d,a2d,a1dd,a2dd = state # joint_angle_a1,joint_angle_a2,joint_vel_a1,joint_vel_a2,joint_acc_a1,joint_acc_a2\n",
    "\n",
    "    l1,l2 = aparams['l1'], aparams['l2'] # lenght link 1 and 2\n",
    "    m1,m2 = aparams['m1'], aparams['m2'] # mass link 1 and 2\n",
    "    i1,i2 = aparams['i1'], aparams['i2'] # moment of inertia link 1 and 2\n",
    "    lc1,lc2 = aparams['lc1'], aparams['lc2'] # distance to the center of mass of link 1 and 2\n",
    "\n",
    "    M11 = i1 + i2 + (m1*lc1*lc1) + (m2*((l1*l1) + (lc2*lc2) + (2*l1*lc2*numpy.cos(a2))))\n",
    "    M12 = i2 + (m2*((lc2*lc2) + (l1*lc2*numpy.cos(a2))))\n",
    "    M21 = M12\n",
    "    M22 = i2 + (m2*lc2*lc2)\n",
    "    M = numpy.matrix([[M11,M12],[M21,M22]]) # H matrix\n",
    "\n",
    "    C1 = -(m2*l1*a2d*a2d*lc2*numpy.sin(a2)) - (2*m2*l1*a1d*a2d*lc2*numpy.sin(a2))\n",
    "    C2 = m2*l1*a1d*a1d*lc2*numpy.sin(a2)\n",
    "    C = numpy.matrix([[C1],[C2]])\n",
    "\n",
    "    ACC = numpy.array([[a1dd],[a2dd]])\n",
    "\n",
    "    T = M*ACC + C\n",
    "\n",
    "    return numpy.array([T[0,0],T[1,0]])\n",
    "\n",
    "\n",
    "# forward dynamics equations of our two-joint arm\n",
    "def twojointarm(state, t, aparams, torque):\n",
    "    import numpy\n",
    "    \n",
    "    \"\"\"\n",
    "    two-joint arm in plane\n",
    "    X is fwd(+) and back(-)\n",
    "    Y is up(+) and down(-)\n",
    "    shoulder angle a1 relative to Y vert, +ve counter-clockwise\n",
    "    elbow angle a2 relative to upper arm, +ve counter-clockwise\n",
    "    \"\"\"\n",
    "    a1,a2,a1d,a2d = state # joint_angle_a1, joint_angle_a2, joint_velocity_a1, joint_velocity_a2\n",
    "\n",
    "    l1,l2 = aparams['l1'], aparams['l2'] # lenght link 1 and 2\n",
    "    m1,m2 = aparams['m1'], aparams['m2'] # mass link 1 and 2\n",
    "    i1,i2 = aparams['i1'], aparams['i2'] # moment of inertia link 1 and 2\n",
    "    lc1,lc2 = aparams['lc1'], aparams['lc2'] # distance to the center of mass of link 1 and 2\n",
    "\n",
    "    M11 = i1 + i2 + (m1*lc1*lc1) + (m2*((l1*l1) + (lc2*lc2) + (2*l1*lc2*numpy.cos(a2))))\n",
    "    M12 = i2 + (m2*((lc2*lc2) + (l1*lc2*numpy.cos(a2))))\n",
    "    M21 = M12\n",
    "    M22 = i2 + (m2*lc2*lc2)\n",
    "    M = numpy.matrix([[M11,M12],[M21,M22]]) # H matrix\n",
    "\n",
    "    C1 = -(m2*l1*a2d*a2d*lc2*numpy.sin(a2)) - (2*m2*l1*a1d*a2d*lc2*numpy.sin(a2))\n",
    "    C2 = m2*l1*a1d*a1d*lc2*numpy.sin(a2)\n",
    "    C = numpy.matrix([[C1],[C2]])\n",
    "\n",
    "    T = numpy.matrix([[torque[0]],[torque[1]]])\n",
    "\n",
    "    ACC = numpy.linalg.inv(M) * (T-C) # calculates the accelerations of joints 1 and 2\n",
    "\n",
    "    a1dd,a2dd = ACC[0,0], ACC[1,0]\n",
    "\n",
    "    return [a1d, a2d, a1dd, a2dd] # It returns the first and second derivatives of the joints\n",
    "\n",
    "\n",
    "def animatearm(state,t,aparams):\n",
    "    \"\"\"\n",
    "    animate the twojointarm\n",
    "    \"\"\"\n",
    "    import matplotlib.pyplot as plt\n",
    "    import numpy\n",
    "    import time\n",
    "\n",
    "    A = state[:,[0,1]] # Gets the angles a1 and a2 from the states matrix\n",
    "    A[:,0] = A[:,0]\n",
    "    H,E = joints_to_hand(A,aparams)\n",
    "    l1,l2 = aparams['l1'], aparams['l2']\n",
    "    plt.figure()\n",
    "    plt.plot(0,0,'b.')\n",
    "    plt.plot(H[:,0],H[:,1],'g.-');\n",
    "    p1, = plt.plot(E[0,0],E[0,1],'b.')\n",
    "    p2, = plt.plot(H[0,0],H[0,1],'b.')\n",
    "    p3, = plt.plot((0,E[0,0],H[0,0]),(0,E[0,1],H[0,1]),'b-')\n",
    "    plt.xlim([-l1-l2, l1+l2])\n",
    "    plt.ylim([-l1-l2, l1+l2])\n",
    "    dt = t[1]-t[0]\n",
    "    tt = plt.title(\"Click on this plot to continue...\")\n",
    "    plt.ginput(1)\n",
    "    for i in xrange(0,numpy.shape(state)[0]):\n",
    "        time.sleep(0.05)\n",
    "        p1.set_xdata((E[i,0]))\n",
    "        p1.set_ydata((E[i,1]))\n",
    "        p2.set_xdata((H[i,0]))\n",
    "        p2.set_ydata((H[i,1]))\n",
    "        p3.set_xdata((0,E[i,0],H[i,0]))\n",
    "        p3.set_ydata((0,E[i,1],H[i,1]))\n",
    "        tt.set_text(\"Current time:%4.2f sec - click to next slide!\" % (i*dt))\n",
    "        plt.draw()\n",
    "    tt.set_text(\"Current time:%4.2f sec - finished!\" % ((numpy.shape(state)[0]-1)*dt))\n",
    "    plt.draw()        \n",
    "\n",
    "    \n",
    "def animatearm_JS(state,t,aparams):\n",
    "    \"\"\"\n",
    "    animate the twojointarm\n",
    "    \"\"\"\n",
    "    import matplotlib.pyplot as plt\n",
    "    import numpy\n",
    "    from JSAnimation import IPython_display\n",
    "    from matplotlib import animation\n",
    "\n",
    "\n",
    "    A = state[:,[0,1]] # Gets the angles a1 and a2 from the states matrix\n",
    "    A[:,0] = A[:,0]\n",
    "    H,E = joints_to_hand(A,aparams)\n",
    "    l1,l2 = aparams['l1'], aparams['l2']\n",
    "\n",
    "    # Set up the axes, making sure the axis ratio is equal\n",
    "#     ax = fig.add_axes([0, 0, 1, 1], xlim=(-0.02, 13.02), ylim=(-0.02, 5.02),\n",
    "#                       xticks=range(14), yticks=range(6), aspect='equal', frameon=False)\n",
    "\n",
    "    fig = plt.figure(figsize=(6, 6),dpi=100)\n",
    "    ax = plt.axes(xlim=(-1, 1), ylim=(-1, 1), aspect='equal')\n",
    "    ax.plot(0,0,'b.')\n",
    "    ax.plot(H[:,0],H[:,1],'g.-');\n",
    "    p1, = ax.plot(E[0,0],E[0,1],'b.')\n",
    "    p2, = ax.plot(H[0,0],H[0,1],'b.')\n",
    "    p3, = ax.plot((0,E[0,0],H[0,0]),(0,E[0,1],H[0,1]),'b-')    \n",
    "    \n",
    "    def init():\n",
    "        p1.set_data([],[])\n",
    "        p2.set_data([],[])\n",
    "        p3.set_data([],[])        \n",
    "        return p1,p2,p3\n",
    "    \n",
    "    def animate(i):\n",
    "        p1.set_data([E[i,0]],[E[i,1]])\n",
    "        p2.set_data(H[i,0],H[i,1])\n",
    "        p3.set_data((0,E[i,0],H[i,0]),(0,E[i,1],H[i,1]))\n",
    "        return p1,p2,p3\n",
    "    anim = animation.FuncAnimation(fig, animate, init_func=init, frames=len(E[:,0]), interval=20, blit=True)\n",
    "    return anim\n",
    "    # In order to make the JSAnimation to work it is necessary that the function returns the animation object!\n",
    "    \n",
    "    \n",
    "def odeint_arms(twojointarm, state, t, aparams, torque):\n",
    "    '''\n",
    "    twojointarm: function object. Must receive (state,t,aparams,torque) and return [a1d,a2d,a1dd,a2dd]\n",
    "    state: current states => [a1(t),a2(t),a1d(t),a2d(t)]\n",
    "    t: array([t,t+1]) => current time step and next (t+1)\n",
    "    returns next states [a1(t+1),a2(t+1),a1d(t+1),a2d(t+1)]\n",
    "    '''\n",
    "    from scipy.integrate import odeint\n",
    "\n",
    "    return odeint(twojointarm, state, t, args=(aparams,torque))\n",
    "\n",
    "\n",
    "def moving_average (values,window=6):\n",
    "    weights = numpy.repeat(1.0, window)/window\n",
    "    sma = numpy.convolve(numpy.concatenate((numpy.zeros(int((window-1)/2.0)),values,numpy.zeros((window-1)-int((window-1)/2.0)))), weights, 'valid')\n",
    "    # I should try the function numpy.lib.pad instead of concatenating manually\n",
    "    return sma\n",
    "\n",
    "\n",
    "def moving_average (values, window=6):\n",
    "    weights = numpy.repeat(1.0, window)/window\n",
    "    sma = numpy.convolve(values, weights, 'valid')\n",
    "    # I should try the function numpy.lib.pad instead of concatenating manually\n",
    "    return numpy.lib.pad(sma, (int((window-1)/2.0),(window-1)-int((window-1)/2.0)), 'edge')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from simulation_2DoF_Arm_physics import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "@dview.parallel(block=True)\n",
    "def generate_trajectories(sim_inputs):\n",
    "    import numpy\n",
    "    import sys\n",
    "    import save_load_file as slf\n",
    "    \n",
    "    import simulation_2DoF_Arm_physics\n",
    "    reload(sys.modules['simulation_2DoF_Arm_physics']) # Makes sure the interpreter is going to reload the module\n",
    "    s2ap = simulation_2DoF_Arm_physics\n",
    "\n",
    "    tji,positions,sim_params = sim_inputs\n",
    "    \n",
    "    xstart,ystart = positions[0]\n",
    "    xdest,ydest = positions[1]\n",
    "\n",
    "    sim_set,base_dir,MT,time_step,Ninput,aparams = sim_params\n",
    "    \n",
    "    t_mov=numpy.arange(0, MT, time_step) # t starts in 0s and steps time_step(s) until reaches MT(s)\n",
    "\n",
    "    # Generates the movements according to:\n",
    "    # Flash, Tamar, and Neville Hogan. 1985\n",
    "    H_path=s2ap.cartesian_movement_generation_training(xstart, ystart, xdest, ydest, MT, t_mov)\n",
    "\n",
    "    # These are the values teta1 and teta2 can have because the system limits the resolution.\n",
    "    # According to Joshi/Maass paper there are 50 neurons to code the positions of each variable.\n",
    "    #\n",
    "    #\n",
    "    teta1=numpy.linspace(-numpy.pi/6,numpy.pi,num=Ninput)\n",
    "    teta2=numpy.linspace(0,numpy.pi,num=Ninput)\n",
    "    teta1_teta2 = numpy.array([teta1,teta2]).T # This is the matrix to use with the function\n",
    "                                               # to generate the x,y values of the workspace\n",
    "\n",
    "    # Joint's workspace: all the possible combinations between teta1 and teta2.\n",
    "    teta_workspace = numpy.array([[t1,t2] for t1 in teta1 for t2 in teta2])\n",
    "\n",
    "    # Arm's workspace: x,y points that the arm (endpoint) can reach\n",
    "    H_workspace = s2ap.joints_to_hand(teta_workspace,aparams)[0] # I'm getting the first because it returns the elbow's positions too.\n",
    "\n",
    "    # Generate the joint's positions according to the ORIGINAL (X,Y) values.\n",
    "    # I'm using the traditional geometric way to do the inverse kinematics here. I need to\n",
    "    # implement the minimum jerk way to generate the joint's positions taking into account the movement's dynamics.\n",
    "\n",
    "    Joints=s2ap.hand_to_joints(H_path, aparams,ang_error=0.1)\n",
    "\n",
    "    # Here I'm extending the Joints matrix because I need two extra positions to calculate the accelerations.\n",
    "    # Consequently, because the trajectory always finish with velocity ZERO, keeping the same position seems a good choice. \n",
    "    Joints_extended=numpy.concatenate((Joints,[Joints[-1],Joints[-1]]))\n",
    "\n",
    "    # But the time array (t_mov) must be extended too:\n",
    "    t_mov_extended=numpy.concatenate((t_mov,[t_mov[-1]+time_step],[t_mov[-1]+2*time_step]))\n",
    "\n",
    "\n",
    "    # Joint's velocities\n",
    "    teta1_d=s2ap.derivator(Joints_extended[:,0],t_mov_extended)\n",
    "    teta2_d=s2ap.derivator(Joints_extended[:,1],t_mov_extended)\n",
    "    \n",
    "\n",
    "    # Joint's accelerations\n",
    "    teta1_dd=s2ap.derivator(teta1_d,t_mov_extended[:251])\n",
    "    teta2_dd=s2ap.derivator(teta2_d,t_mov_extended[:251])\n",
    "\n",
    "    #\n",
    "    # WITH ORIGINAL JOINT'S VELOCITIES\n",
    "    #\n",
    "\n",
    "    # And generates a matrix with [teta1,teta2,teta1d,teta2d,teta1dd,teta2dd]\n",
    "    # Using this matrix I will generate the torques.\n",
    "    states_mov = numpy.array([[Joints[:,0][i],Joints[:,1][i],teta1_d[i],teta2_d[i],teta1_dd[i],teta2_dd[i]] for i in range(len(Joints[:,0]))])\n",
    "\n",
    "\n",
    "    # Applying the function to all lines of the states_mov matrix, I generate the torques matrix\n",
    "    T_mov=numpy.array([s2ap.twojointarm_torques(states_mov[i], t_mov, aparams) for i in range(numpy.shape(states_mov)[0])])\n",
    "\n",
    "    # Here I calculate the states using the calculated torques, just to make sure it is working!\n",
    "    state = states_mov[0,[0,1,2,3]] # This is the initial state. The rest of the states will be generated dynamically \n",
    "                                    # according to the input torques.\n",
    "    state_v2 = [state]\n",
    "    t = t_mov\n",
    "    torque = T_mov\n",
    "    for i in range(len(t)-1):\n",
    "        print \"state:\",state\n",
    "        state = s2ap.odeint_arms(s2ap.twojointarm, state, [t[i], t[i+1]], aparams, torque[i])[1]\n",
    "        state_v2.append(state)\n",
    "\n",
    "    state_v2=numpy.array(state_v2)\n",
    "\n",
    "    # slf.save_to_file_gz([numpy.array(state_v2),numpy.array(torque)],\"./\"+base_dir+\"/\"+sim_set+\"/States_Torques_movement\"+str(tji)+\".gzpickle\")\n",
    "    \n",
    "    return tji"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## End of the main functions!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Adjusting the parameters:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Experiment identifier\n",
    "sim_sets = [\"set_A\", \"set_B\", \"set_C\", \"set_D\"]\n",
    "\n",
    "sim_set = sim_sets[0]\n",
    "\n",
    "# Base dir to save / access\n",
    "base_dir = \"2DofArm_simulation_data\"\n",
    "\n",
    "# List with all trajectories to be generated\n",
    "# [[[start_x,start_y],[final_x,final_y]],...]\n",
    "trajectories = [[[0.75,0.25],[0.0,0.5]], [[0.25,0.60],[-0.25,0.60]], [[-0.10,0.75],[-0.10,0.25]],[[-0.75,0.50],[-0.40,0.00]]]\n",
    "\n",
    "\n",
    "# The values below must match the ones used with the SNN simulation:\n",
    "\n",
    "# Total time spent during the movement (in seconds)\n",
    "MT = 0.5\n",
    "\n",
    "# Simulation time step (in seconds)\n",
    "time_step = 2/1000.0\n",
    "\n",
    "# Number of neurons at the input layer (defines the resolution of the system)\n",
    "Ninput = 50\n",
    "\n",
    "\n",
    "# Arm parametres used with the 2 dof arm simulator\n",
    "# (according to Joshi/Maass 2006 paper)\n",
    "aparams = {\n",
    "    'l1' : 0.5, # metres\n",
    "    'l2' : 0.5,\n",
    "    'lc1' : 0.25,\n",
    "    'lc2' : 0.25,\n",
    "    'm1' : 1.0, # kg\n",
    "    'm2' : 1.0,\n",
    "    'i1' : 0.03, # kg*m*m\n",
    "    'i2' : 0.03\n",
    "}\n",
    "\n",
    "# Variable only used to pass the parameters\n",
    "sim_params = sim_set,base_dir,MT,time_step,Ninput,aparams"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CPU times: user 15.6 ms, sys: 6.15 ms, total: 21.7 ms\n",
      "Wall time: 1.69 s\n"
     ]
    }
   ],
   "source": [
    "%time results = generate_trajectories.map([(tji,positions,sim_params) for tji,positions in zip(range(1,len(trajectories)+1),trajectories)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}