Path_Following (kinematic bicycle model).ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Learning Lyapunov function for Circle Path Following"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# -*- coding: utf-8 -*-\n",
    "from dreal import *\n",
    "from Functions import *\n",
    "import torch \n",
    "import torch.nn.functional as F\n",
    "import numpy as np\n",
    "import timeit "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Neural network model\n",
    "Building NN with random parameters for Lyapunov function and initializing parameters of NN controller to LQR solution\n",
    "\n",
    "LQR solution is obtained by minimizing the cost function J = ∫(xᵀQx + uᵀRu)dt, where Q is 2×2 identity matrix and R is 1×1 identity matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Net(torch.nn.Module):\n",
    "    \n",
    "    def __init__(self,n_input,n_hidden,n_output,lqr):\n",
    "        super(Net, self).__init__()\n",
    "        torch.manual_seed(2)\n",
    "        self.layer1 = torch.nn.Linear(n_input, n_hidden)\n",
    "        self.layer2 = torch.nn.Linear(n_hidden,n_output)\n",
    "        self.control = torch.nn.Linear(n_input,1,bias=False)\n",
    "        self.control.weight = torch.nn.Parameter(lqr)\n",
    "\n",
    "    def forward(self,x):\n",
    "        sigmoid = torch.nn.Tanh()\n",
    "        h_1 = sigmoid(self.layer1(x))\n",
    "        out = sigmoid(self.layer2(h_1))\n",
    "        u = self.control(x)\n",
    "        return out,u"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Dynamical system"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def f_value(x,u):\n",
    "    v = 6\n",
    "    l = 1\n",
    "    y = [] \n",
    "    \n",
    "    for r in range(0,len(x)): \n",
    "        f = [v*torch.sin(x[r][1]),\n",
    "             v*torch.tan(u[r][0])/l -(torch.cos(x[r][1])/(1-x[r][0]))]\n",
    "        y.append(f) \n",
    "    y = torch.tensor(y)    \n",
    "    return y"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Options"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "'''\n",
    "For learning \n",
    "'''\n",
    "N = 500            # sample size\n",
    "D_in = 2            # input dimension\n",
    "H1 = 6              # hidden dimension\n",
    "D_out = 1           # output dimension\n",
    "torch.manual_seed(10)  \n",
    "\n",
    "lqr = torch.tensor([[-0.8471 , -1.6414]])  # lqr solution\n",
    "x = torch.Tensor(N, D_in).uniform_(-1, 1)           \n",
    "x_0 = torch.zeros([1, 2])\n",
    "x = torch.cat((x, x_0), 0)\n",
    "\n",
    "'''\n",
    "For verifying \n",
    "'''\n",
    "x1 = Variable(\"x1\")\n",
    "x2 = Variable(\"x2\")\n",
    "vars_ = [x1,x2]\n",
    "v = 6\n",
    "l = 1\n",
    "config = Config()\n",
    "config.use_polytope_in_forall = True\n",
    "config.use_local_optimization = True\n",
    "config.precision = 1e-2\n",
    "epsilon = 0\n",
    "# Checking candidate V within a ball around the origin (ball_lb ≤ sqrt(∑xᵢ²) ≤ ball_ub)\n",
    "ball_lb = 0.2\n",
    "ball_ub = 0.8"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Learning and Falsification"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "out_iters = 0\n",
    "valid = False\n",
    "while out_iters < 2 and not valid: \n",
    "    start = timeit.default_timer()\n",
    "    lqr = torch.tensor([[-23.58639732,  -5.31421063]])    # lqr solution\n",
    "    model = Net(D_in,H1, D_out,lqr)\n",
    "    L = []\n",
    "    i = 0 \n",
    "    t = 0\n",
    "    max_iters = 2000\n",
    "    learning_rate = 0.01\n",
    "    optimizer = torch.optim.Adam(model.parameters(), lr=learning_rate)\n",
    "\n",
    "    while i < max_iters and not valid: \n",
    "        V_candidate, u = model(x)\n",
    "        X0,u0 = model(x_0)\n",
    "        f = f_value(x,u)\n",
    "        Circle_Tuning = Tune(x)\n",
    "        # Compute lie derivative of V : L_V = ∑∂V/∂xᵢ*fᵢ\n",
    "        L_V = torch.diagonal(torch.mm(torch.mm(torch.mm(dtanh(V_candidate),model.layer2.weight)\\\n",
    "                            *dtanh(torch.tanh(torch.mm(x,model.layer1.weight.t())+model.layer1.bias)),model.layer1.weight),f.t()),0)\n",
    "\n",
    "        # With tuning\n",
    "        Lyapunov_risk = (F.relu(-V_candidate)+ 2*F.relu(L_V+0.8)).mean()\\\n",
    "                    +1.5*((Circle_Tuning-V_candidate).pow(2)).mean()+ 1.2*(X0).pow(2)\n",
    "\n",
    "\n",
    "        print(i, \"Lyapunov Risk=\",Lyapunov_risk.item()) \n",
    "        L.append(Lyapunov_risk.item())\n",
    "        optimizer.zero_grad()\n",
    "        Lyapunov_risk.backward()\n",
    "        optimizer.step() \n",
    "\n",
    "        w1 = model.layer1.weight.data.numpy()\n",
    "        w2 = model.layer2.weight.data.numpy()\n",
    "        b1 = model.layer1.bias.data.numpy()\n",
    "        b2 = model.layer2.bias.data.numpy()\n",
    "        q = model.control.weight.data.numpy()\n",
    "\n",
    "        # Falsification\n",
    "        if i % 10 == 0:\n",
    "            u_NN = (q.item(0)*x1 + q.item(1)*x2) \n",
    "            f = [v*sin(x2),\n",
    "                 v*tan(u_NN)/l -(cos(x2)/(1-x1))]\n",
    "\n",
    "            # Candidate V\n",
    "            z1 = np.dot(vars_,w1.T)+b1\n",
    "\n",
    "            a1 = []\n",
    "            for j in range(0,len(z1)):\n",
    "                a1.append(tanh(z1[j]))\n",
    "            z2 = np.dot(a1,w2.T)+b2\n",
    "            V_learn = tanh(z2.item(0))\n",
    "\n",
    "            print('===========Verifying==========')        \n",
    "            start_ = timeit.default_timer() \n",
    "            result= CheckLyapunov(vars_, f, V_learn, ball_lb, ball_ub, config,epsilon)\n",
    "            stop_ = timeit.default_timer() \n",
    "\n",
    "            if (result): \n",
    "                print(\"Not a Lyapunov function. Found counterexample: \")\n",
    "                print(result)\n",
    "                x = AddCounterexamples(x,result,10)\n",
    "            else:  \n",
    "                valid = True\n",
    "                print(\"Satisfy conditions!!\")\n",
    "                print(V_learn, \" is a Lyapunov function.\")\n",
    "            t += (stop_ - start_)\n",
    "            print('==============================') \n",
    "        i += 1\n",
    "\n",
    "    stop = timeit.default_timer()\n",
    "\n",
    "\n",
    "    np.savetxt(\"w1_p.txt\", model.layer1.weight.data, fmt=\"%s\")\n",
    "    np.savetxt(\"w2_p.txt\", model.layer2.weight.data, fmt=\"%s\")\n",
    "    np.savetxt(\"b1_p.txt\", model.layer1.bias.data, fmt=\"%s\")\n",
    "    np.savetxt(\"b2_p.txt\", model.layer2.bias.data, fmt=\"%s\")\n",
    "    np.savetxt(\"q_p.txt\", model.control.weight.data, fmt=\"%s\")\n",
    "\n",
    "    print('\\n')\n",
    "    print(\"Total time: \", stop - start)\n",
    "    print(\"Verified time: \", t)\n",
    "    \n",
    "    out_iters+=1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Checking result with smaller epsilon"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "('Satisfy conditions with epsilon= ', -1e-05)\n",
      "(<Expression \"tanh((0.099664619999999995 + 0.6251369 * tanh((-1.2775723999999999 - 2.5249655 * x1 - 0.18407556 * x2)) - 1.0490067000000001 * tanh((-0.46410936000000003 - 0.47741896 * x1 - 0.69643679999999997 * x2)) - 1.0708085000000001 * tanh((-0.3698631 - 0.52394079999999998 * x1 - 0.58617014000000001 * x2)) + 0.46437746000000002 * tanh((0.91936094000000002 - 0.023182843000000002 * x1 - 0.50320260000000006 * x2)) + 0.70194889999999999 * tanh((0.97582674000000003 - 0.062661019999999998 * x1 - 0.56203824000000002 * x2)) - 1.1286860000000001 * tanh((1.3282121 + 1.3561938 * x1 + 2.5183802000000002 * x2))))\">, ' is a Lyapunov function.')\n"
     ]
    }
   ],
   "source": [
    "epsilon = -0.00001\n",
    "start_ = timeit.default_timer() \n",
    "result = CheckLyapunov(vars_, f, V_learn, ball_lb, ball_ub, config, epsilon)\n",
    "stop_ = timeit.default_timer() \n",
    "\n",
    "if (result): \n",
    "    print(\"Not a Lyapunov function. Found counterexample: \")\n",
    "    print(result)\n",
    "else:  \n",
    "    print(\"Satisfy conditions with epsilon= \",epsilon)\n",
    "    print(V_learn, \" is a Lyapunov function.\")\n",
    "t += (stop_ - start_)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "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.15"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}