AA277 Project_Final.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cvxpy as cp\n",
    "import matplotlib.pyplot as plt\n",
    "np.random.seed(5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# assume a very simple double integrator dynamics, dx=Ax+Bu, x = [x,dx,y,dy]\n",
    "dt = 0.2\n",
    "A = np.array([[1, dt, 0, 0],\n",
    "              [0, 1, 0, 0], \n",
    "              [0, 0, 1, dt], \n",
    "              [0, 0, 0, 1]])\n",
    "B = np.array([[0, 0],\n",
    "              [dt, 0],\n",
    "              [0, 0],\n",
    "              [0, dt]])\n",
    "# assume some random initial position\n",
    "num_agent = 5\n",
    "m,n = B.shape\n",
    "# px_init = np.random.rand(num_agent)*100\n",
    "# py_init = np.random.rand(num_agent)*100\n",
    "px_init = np.array([20, 20, 22, 22, 20]).T\n",
    "py_init = np.array([20, 18, 18, 16, 16]).T\n",
    "x_init = np.zeros((m,num_agent))\n",
    "x_init[0] = px_init\n",
    "x_init[2] = py_init\n",
    "priority = np.arange(num_agent)\n",
    "# np.random.shuffle(priority)\n",
    "plt.scatter(px_init,py_init)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "def Decentralized_CP(A,B,x_init,x_fin,Umax,Vmax,T,dt,R_col,x_nom,x_neigbs_p,collision_check):\n",
    "    m, n = B.shape\n",
    "    G = np.array([\n",
    "        [1,0,0,0],\n",
    "        [0,0,1,0]\n",
    "    ])\n",
    "    H = np.array([\n",
    "        [0,1,0,0],\n",
    "        [0,0,0,1]\n",
    "    ])\n",
    "    x = cp.Variable((T,m))\n",
    "    u = cp.Variable((T-1,n))\n",
    "    objective = cp.Minimize(cp.sum(cp.norm(u,axis=1))*dt)\n",
    "    constraints = [ x[0] == x_init, x[T-1] == x_fin ]\n",
    "    for k in range(T-1):\n",
    "        constraints.append( x[k+1] == A@x[k] + B@u[k] )\n",
    "        constraints.append( cp.norm(u[k]) <= Umax )\n",
    "        constraints.append( cp.norm(H@x[k+1]) <= Vmax)\n",
    "        if len(x_neigbs_p) != 0 and collision_check:\n",
    "            for x_neigb in x_neigbs_p:\n",
    "                x_neigb = x_neigb.T\n",
    "#                 print(R_col*np.linalg.norm(x_nom[k]-x_neigb[k]))\n",
    "#                 print(G@(x_nom[k]-x_neigb[k]))\n",
    "                constraints.append( (x_nom[k]-x_neigb[k]).T@G.T@G@(x[k]-x_neigb[k]) >= R_col*cp.norm(G@(x_nom[k]-x_neigb[k])) )\n",
    "    \n",
    "    prob = cp.Problem(objective,constraints)\n",
    "    prob.solve()\n",
    "    return x.value, u.value, prob.value, prob.status\n",
    "    \n",
    "def compute_laplacian(x,R_com):\n",
    "    _, num_agent = x.shape\n",
    "    L = np.zeros((num_agent,num_agent))\n",
    "    px, py = x[0].reshape(1,-1), x[2].reshape(1,-1)\n",
    "    dist = np.sqrt((px-px.T)**2+(py-py.T)**2)\n",
    "    L[dist <= R_com] = 1\n",
    "    L -= np.diag(np.ones((num_agent)))\n",
    "    return L\n",
    "\n",
    "\n",
    "def communicate_traj(L,priority,K,x_nom):\n",
    "    communication = {}\n",
    "    for agent_i in K:\n",
    "        communication[agent_i] = []\n",
    "        for agent_j in K:\n",
    "            if L[agent_i,agent_j] == 1 and priority[agent_i] < priority[agent_j]: \n",
    "                communication[agent_i].append(x_nom[agent_j,:,:])\n",
    "    return communication\n",
    "\n",
    "def check_collision(L,prioprity,x_i,x_nom,agent_i,K,R_col):\n",
    "    is_collision = []\n",
    "    for agent_j in K:\n",
    "            if L[agent_i,agent_j] == 1 and priority[agent_i] < priority[agent_j]:\n",
    "                px_i = x_i[0]\n",
    "                py_i = x_i[2]\n",
    "                px_j = x_nom[agent_j][0]\n",
    "                py_j = x_nom[agent_j][2]\n",
    "                is_collision.append(np.linalg.norm((px_i-px_j,py_i-py_j),axis=0))\n",
    "    return np.all(np.array(is_collision) > R_col)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "## Temporary\n",
    "# p_fin = np.loadtxt('S1.txt',delimiter='\\t')\n",
    "T = 30\n",
    "px_fin = np.array([22, 20, 20, 22, 22])\n",
    "py_fin = np.array([20, 20, 18, 18, 16])\n",
    "# px_fin = np.random.rand(num_agent)*100\n",
    "# py_fin = np.random.rand(num_agent)*100\n",
    "x_fin = np.zeros((m,num_agent))\n",
    "x_fin[0] = px_fin\n",
    "x_fin[2] = py_fin\n",
    "Umax = 5\n",
    "R_col = 0.01\n",
    "R_com = 10\n",
    "Vmax = 10\n",
    "plt.scatter(px_fin,py_fin)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "def Swarm_SCP(A,B,num_agent,x_init,x_fin,Umax,Vmax,T,dt,R_col,R_com,priority,collision_check):\n",
    "    m,n = B.shape\n",
    "    eps = 1e-3\n",
    "    K = np.arange(num_agent).tolist()\n",
    "    K_const = np.copy(K)\n",
    "    x_nom = np.zeros((num_agent,m,T))\n",
    "    u_nom = np.zeros((num_agent,n,T-1))\n",
    "    all_cost = np.zeros((num_agent,T-1))\n",
    "    x_neigbs_p = []\n",
    "    for agent in K:\n",
    "        x_agent, u_agent, _, _ = Decentralized_CP(A,B,x_init[:,agent],x_fin[:,agent],Umax,Vmax,T,dt,R_col,\n",
    "                                               x_nom[agent].T,x_neigbs_p,collision_check)\n",
    "        x_nom[agent] = x_agent.T\n",
    "\n",
    "    L = compute_laplacian(x_init,R_com)\n",
    "    neigbs_p_all = communicate_traj(L,priority,K,x_nom)\n",
    "    w = 0\n",
    "    while len(K) != 0:\n",
    "        print(w)\n",
    "        for agent in K:\n",
    "            x_nom_prev = np.copy(x_nom)\n",
    "            x_neigbs_p = neigbs_p_all[agent]\n",
    "            x_agent, u_agent, cost, status = Decentralized_CP(A,B,x_init[:,agent],x_fin[:,agent],Umax,Vmax,T,dt,\n",
    "                                                      R_col,x_nom[agent].T,x_neigbs_p,collision_check)\n",
    "            print(status)\n",
    "            if status == 'infeasible':\n",
    "                return x_nom, u_nom, all_cost\n",
    "            x_nom[agent] = x_agent.T\n",
    "            u_nom[agent] = u_agent.T\n",
    "            all_cost[agent] = cost\n",
    "    #     L = compute_laplacian(x_init,R_com)\n",
    "        neigbs_p_all = communicate_traj(L,priority,K_const,x_nom)\n",
    "        K_list = np.copy(K)\n",
    "        K_list = K_list.tolist()\n",
    "        for agent in K_const:\n",
    "            if np.linalg.norm(x_nom[agent]-x_nom_prev[agent],np.inf) < eps and \\\n",
    "            check_collision(L,priority,x_nom[agent],x_nom,agent,K_const,R_col):\n",
    "                K_list.remove(agent)\n",
    "        w += 1\n",
    "        K = np.copy(K_list)\n",
    "        K = K.tolist()\n",
    "    return x_nom, u_nom, all_cost"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "x_nom,u_nom,all_cost = Swarm_SCP(A,B,num_agent,x_init,x_fin,Umax,Vmax,T,dt,R_col,R_com,priority,True)\n",
    "# for i in range(T):\n",
    "#     plt.scatter(x_nom[:,0,i],x_nom[:,2,i])\n",
    "    \n",
    "for i in range(num_agent):\n",
    "    plt.scatter(x_nom[i,0,:],x_nom[i,2,:])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "def downwash(dx,dy,sigma):\n",
    "    x_dist = -1/(sigma*np.sqrt(2*np.pi))*np.exp(-(dx/sigma)**2/2)\n",
    "    y_dist = max(0, 6.5-dy/(1*sigma))\n",
    "    return x_dist*y_dist"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.02727222 -0.01698228  0.01493884  0.00657946]]\n",
      "[[ 4.16666667e+00 -5.15996662e-12 -4.16666667e+00 -7.84974792e-12\n",
      "   4.16666667e+00]\n",
      " [ 8.74952794e-12  4.16666667e+00 -3.50706695e-11  4.16666667e+00\n",
      "   1.14396753e-15]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.02727222 -0.01698228  0.01493884  0.00657946]]\n",
      "[[ 2.70934848e-12 -2.15367750e-12 -6.17657988e-11  8.98786552e-13\n",
      "   5.63340931e-11]\n",
      " [-5.63306114e-13  3.74193981e-11  1.03870593e-12  1.61645259e-11\n",
      "   5.61327376e-17]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.02341676 -0.02144536  0.01513493  0.00386007]]\n",
      "[[ 2.68122638e-12 -1.12799152e-12 -8.29029290e-11  5.54160058e-13\n",
      "   5.95542004e-11]\n",
      " [ 7.99883582e-13  2.75733724e-11  1.16044263e-12  2.34297045e-11\n",
      "   7.76636363e-17]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[0.         0.         0.         0.         0.        ]\n",
      " [0.         0.01356864 0.00571358 0.00712635 0.02259612]]\n",
      "[[ 3.66715501e-12  3.97194279e-13 -9.36074367e-11  2.33177372e-13\n",
      "   2.12809154e-10]\n",
      " [-1.85536566e-12 -8.93563803e-13  1.41059533e-12  2.81726007e-11\n",
      "   2.60972552e-16]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.0035888   0.01111108 -0.00454582  0.01337767]]\n",
      "[[ 1.36165058e-12  6.79832769e-13 -1.04226140e-10  1.45343540e-13\n",
      "   1.74469841e-10]\n",
      " [ 4.47888318e-13  6.97498087e-11  9.25465402e-13  2.56969834e-11\n",
      "   9.67254055e-17]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.         -0.0008713   0.0159961  -0.01472234  0.00560397]]\n",
      "[[ 2.80563524e-12  3.70977938e-13 -9.13947788e-11  1.21728752e-13\n",
      "   1.70825816e-10]\n",
      " [ 1.55685166e-12  9.70278241e-14 -6.29771365e-13  1.01932569e-11\n",
      "   3.97479795e-17]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.00257754  0.0324533  -0.01983442  0.01746672]]\n",
      "[[ 7.65906123e-11  4.42088714e-13 -7.98563692e-11 -2.76907468e-12\n",
      "   1.32469495e-10]\n",
      " [ 1.47990781e-11 -2.13668957e-12 -1.43171250e-12  1.12682239e-10\n",
      "   2.23117953e-17]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.01168132  0.03816247 -0.01906117  0.02619928]]\n",
      "[[ 4.32748471e-11 -2.93801352e-12 -7.90315917e-11 -1.16138883e-12\n",
      "   1.18062984e-10]\n",
      " [ 1.17601989e-11  3.62590510e-11 -1.97496998e-12  7.04118684e-11\n",
      "   1.11008552e-17]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.02089596  0.01511204 -0.01411005  0.01076519]]\n",
      "[[ 1.34262486e-11  8.71749182e-14 -7.77744920e-11 -3.56528029e-13\n",
      "   1.04895076e-10]\n",
      " [ 6.77917226e-12  3.76994964e-12 -3.11598274e-12  5.74853343e-11\n",
      "   3.80857065e-18]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.02440321 -0.00673187 -0.00814029 -0.00352463]]\n",
      "[[-3.49185850e-12  7.69512898e-13 -7.27033928e-11  1.90253163e-15\n",
      "   6.89886575e-11]\n",
      " [ 2.76837078e-12 -4.58572318e-12 -3.09838567e-12  3.40970874e-11\n",
      "   4.13309162e-18]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.01837701  0.00175953 -0.0047163   0.00811149]]\n",
      "[[ 1.34549257e-10  6.60639168e-13 -1.57466174e-10  2.59259962e-13\n",
      "   5.02082077e-11]\n",
      " [-2.84134958e-12  4.72482708e-11  2.51981976e-13  2.07451039e-11\n",
      "   1.29316917e-17]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.          0.00152773  0.02433193 -0.00769456  0.03083961]]\n",
      "[[ 7.43804936e-12  4.30048224e-13 -5.91309597e-11  3.43396955e-13\n",
      "   4.83023957e-11]\n",
      " [ 1.31763578e-12  6.01148431e-12 -1.74300954e-12  2.54893889e-11\n",
      "   1.05096865e-19]]\n",
      "0\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "optimal\n",
      "[[ 0.          0.          0.          0.          0.        ]\n",
      " [ 0.         -0.02536656  0.03039506 -0.02146227  0.03740016]]\n",
      "[[-4.16666667e+00  6.13014273e-12  4.16666667e+00  8.29358734e-12\n",
      "  -4.16666667e+00]\n",
      " [-3.89864890e-11 -4.16666667e+00  3.58475563e-11 -4.16666667e+00\n",
      "  -1.70684303e-15]]\n"
     ]
    }
   ],
   "source": [
    "k0 = 0\n",
    "x = x_init\n",
    "all_x = [x]\n",
    "all_u = []\n",
    "eps = 1e-3\n",
    "sigma = 0.8\n",
    "num_iter = 0\n",
    "while True:\n",
    "    x_nom, u_nom, all_cost = Swarm_SCP(A, B, num_agent, x, x_fin, Umax, Vmax, T, dt, R_col, R_com, priority, True)\n",
    "#     downwash_effct = downwash(x[0, 0] - x[0, 1], np.abs(x[2, 0] - x[2, 1]), sigma)\n",
    "#     disturbance = np.zeros((num_agent, 2))\n",
    "#     disturbance[1, 0] = downwash_effct\n",
    "#     compensator = np.zeros_like(disturbance)\n",
    "#     compensator[1,0] = nnet.predict(np.array([x[0, 0] - x[0, 1], np.abs(x[2, 0] - x[2, 1])]).reshape(1,2))\n",
    "#     pos_diff = compute_all_pos_diff(x)\n",
    "    dx, dy = compute_pos_diff_matrix(x[0,:],x[2,:])\n",
    "    W = compute_dist_comp_matrix(dy)\n",
    "    disturbance = compute_dist_comp_mat(W,dx,dy).T\n",
    "    dist = np.concatenate((np.zeros((1,5)),disturbance),axis=0)\n",
    "    print(dist)\n",
    "    print(u_nom[:, :, 0].T)\n",
    "#     print(compute_dist_comp_matrix(x,pos_diff,sigma))\n",
    "#     x = A@x+B@(u_nom[:,:,0].T + 2*compute_dist_comp_matrix(x,pos_diff,sigma))\n",
    "    x = A@x+B@(u_nom[:,:,0].T)# + 2*dist\n",
    "    all_x.append(x)\n",
    "    all_u.append(u_nom[:,:,0].T)\n",
    "    if np.all(x-x_fin <= eps): break\n",
    "    T -= 1\n",
    "    if T == 1: raise Exception(\"Planning horizon not long enough\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(14, 4, 5)\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "x = np.array(all_x)\n",
    "print(x.shape)\n",
    "for i in range(num_agent):\n",
    "    plt.scatter(x[:,0,i],x[:,2,i])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "np.savetxt('mpc_dist_no_comp_dist.txt',x.reshape(14,-1),delimiter='\\t')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "k0 = 0\n",
    "x = x_init\n",
    "all_x = [x]\n",
    "all_u = []\n",
    "eps = 1e-3\n",
    "sigma = 0.8\n",
    "K = np.zeros((2,4))\n",
    "K[1,3] = 1\n",
    "while True:\n",
    "    downwash_effct = downwash(x[0, 0] - x[0, 1], np.abs(x[2, 0] - x[2, 1]), sigma)\n",
    "#     print(x[0, 0] - x[0, 1],np.abs(x[2, 0] - x[2, 1]))\n",
    "    disturbance = np.zeros((num_agent, 2))\n",
    "    disturbance[1, 0] = downwash_effct\n",
    "    u_nom = np.array([[2, 0],[0, 0]])\n",
    "    print(downwash_effct,2.935402561181967**2*compensator[1,0])\n",
    "    x = A@x+B@(u_nom.T + 2*disturbance)\n",
    "    all_x.append(x)\n",
    "    if x[0,0] >= 30: break"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "x = np.array(all_x)\n",
    "print(x.shape)\n",
    "for i in range(num_agent):\n",
    "    plt.scatter(x[:,0,i],x[:,2,i])\n",
    "print(x[-1])\n",
    "plt.ylim([19,24])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "np.savetxt('swap_nc.txt',x.reshape(18,-1),delimiter='\\t')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7f91ff50e950>]"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import tensorflow as tf\n",
    "from tensorflow import keras\n",
    "from tensorflow.keras import layers\n",
    "import numpy as np\n",
    "from keras.models import Sequential\n",
    "from keras.layers import Dense, Conv2D,MaxPool2D,Dropout,Flatten\n",
    "from keras.callbacks import History\n",
    "import matplotlib.pyplot as plt\n",
    "from keras.utils.vis_utils import plot_model\n",
    "from keras.models import load_model\n",
    "from keras.initializers import glorot_uniform\n",
    "import os\n",
    "from matplotlib import pyplot as plt\n",
    "\n",
    "nnet = load_model('my_model_2.h5')\n",
    "\n",
    "N = 1000\n",
    "dx = np.linspace(-2, 2, N).reshape([-1, 1])\n",
    "dy = (np.ones(N)*4).reshape([-1, 1])\n",
    "data_in = np.concatenate((dx, dy), axis = 1)\n",
    "all_x = []\n",
    "for x in dx:\n",
    "    all_x.append(downwash(x,dy[0],0.8))\n",
    "\n",
    "data_out = np.array(all_x)-nnet.predict(data_in)*2.935402561181967**2\n",
    "plt.plot(dx, data_out)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "# def compute_all_pos_diff(x):\n",
    "#     m, num_agent = x.shape\n",
    "#     pos_diff = []\n",
    "#     for i in range(num_agent):\n",
    "#         for j in range(num_agent):\n",
    "#             if j < i:\n",
    "#                 pos_diff.append((x[0,i]-x[0,j],np.abs(x[2,i]-x[2,j])))\n",
    "                \n",
    "                \n",
    "#     return pos_diff\n",
    "\n",
    "# x = np.random.randint(4,size=(4,5))\n",
    "# compute_all_pos_diff(x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "# def compute_dist_comp_matrix(pos_diff):\n",
    "#     num_agent = 5\n",
    "#     sigma = 0.8\n",
    "#     dist_comp_matrix = np.zeros((2,5))\n",
    "#     counter = 0\n",
    "#     for i in range(1,num_agent):\n",
    "#         disturbance = 0\n",
    "#         compensation = 0\n",
    "#         for k in range(i):\n",
    "#             disturbance += downwash(pos_diff[counter][0],pos_diff[counter][1],sigma)\n",
    "#             compensation += nnet.predict(np.array(pos_diff[counter]).reshape(1,2))\n",
    "#             counter += 1\n",
    "#         dist_comp_matrix[1,i] = disturbance\n",
    "#     return dist_comp_matrix\n",
    "\n",
    "# compute_dist_comp_matrix(compute_all_pos_diff(x))\n",
    "\n",
    "# def compute_all_pos_diff(x):\n",
    "#     m, num_agent = x.shape\n",
    "#     pos_diff_dict = {}\n",
    "#     for i in range(num_agent):\n",
    "#         pos_diff_dict[i] = []\n",
    "#         for j in range(num_agent):\n",
    "#             if x[2, i] - x[2, j] < 0:\n",
    "#                 pos_diff_dict[i].append((x[0, i] - x[0, j], np.abs(x[2, i] - x[2, j])))\n",
    "#     return pos_diff_dict\n",
    "\n",
    "# def compute_dist_comp_matrix(x, dict_pos_diff, sigma):\n",
    "#     m, num_agent = x.shape\n",
    "#     dist_matrix = np.zeros((2, num_agent))\n",
    "#     for agent in range(num_agent):\n",
    "#         for i in range(len(dict_pos_diff[agent])):\n",
    "#             downwash_effect = downwash(dict_pos_diff[agent][i][0], dict_pos_diff[agent][i][1], sigma)\n",
    "#             dist_matrix[1, agent] += downwash_effect\n",
    "#             compensation = nnet.predict(np.array([dict_pos_diff[agent][i][0], dict_pos_diff[agent][i][1]]).reshape(1,2))\n",
    "#             dist_matrix[1, agent] -= 2.935402561181967**2*compensation\n",
    "#     return dist_matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "def compute_dist_comp_matrix(pos_diff_y):\n",
    "    num_agents = 5\n",
    "    W = np.zeros((num_agents, num_agents))\n",
    "    for ii in range(num_agents):\n",
    "        for jj in range(num_agents):\n",
    "            if pos_diff_y[ii, jj] > 0:\n",
    "                W[ii, jj] = 1\n",
    "            else:\n",
    "                W[ii, jj] = 0\n",
    "    return W\n",
    "\n",
    "\n",
    "def compute_pos_diff_matrix(x, y):\n",
    "    num_agents = 5\n",
    "    del_x = np.zeros((num_agents, num_agents))\n",
    "    del_y = np.zeros((num_agents, num_agents))\n",
    "    for ii in range(num_agents):\n",
    "        for jj in range(num_agents):\n",
    "            del_x[ii, jj] = x[jj] - x[ii]\n",
    "            del_y[ii, jj] = y[jj] - y[ii]\n",
    "            \n",
    "    return del_x, del_y\n",
    "\n",
    "\n",
    "def compute_dist_comp_mat(W, dx, dy):\n",
    "    num_agents = 5\n",
    "    dist_comp_mat = np.zeros((num_agents, num_agents))\n",
    "    for ii in range(num_agents):\n",
    "        for jj in range(num_agents):\n",
    "            dist = downwash(dx[ii, jj], dy[ii, jj], 0.8)\n",
    "            comp = nnet.predict(np.array([dx[ii, jj], dy[ii, jj]]).reshape(1,2))\n",
    "            dist_comp_mat[ii, jj] = W[ii, jj]*(dist - 2.935402561181967**2*comp)\n",
    "    \n",
    "    return dist_comp_mat.sum(axis=1).reshape(-1,1)\n",
    "\n",
    "# def compute_tot_dist_comp(M):\n",
    "#     return M.sum(axis=1).reshape(-1,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true,
    "jupyter": {
     "outputs_hidden": 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.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}