added input delay in sim

mpc_demo/mpc_demo_pybullet.py

@@ -56,6 +56,7 @@ def run_sim():
     """
     """
     p.connect(p.GUI)
+    p.resetDebugVisualizerCamera(cameraDistance=1.0, cameraYaw=-90, cameraPitch=-45, cameraTargetPosition=[-0.1,-0.0,0.65])
 
     p.resetSimulation()
 
@@ -113,12 +114,19 @@ def run_sim():
     y_history=[]
 
     time.sleep(0.5)
+    input("Press Enter to continue...")
     while (1):
 
         state =  get_state(car)
         x_history.append(state[0])
         y_history.append(state[1])
 
+        #add 1 timestep delay to input
+        state[0]=state[0]+state[2]*np.cos(state[3])*P.dt
+        state[1]=state[1]+state[2]*np.sin(state[3])*P.dt
+        state[2]=state[2]+opt_u[0,0]*P.dt
+        state[3]=state[3]+opt_u[0,0]*np.tan(opt_u[1,0])/0.3*P.dt
+
         #track path in bullet
         p.addUserDebugLine([state[0],state[1],0],[state[0],state[1],0.5],[1,0,0])
 

notebooks/MPC_racecar.ipynb

@@ -32,7 +32,7 @@
     "\n",
     "The inputs of the model are:\n",
     "\n",
-    "* $v$ linear velocity of the robot\n",
+    "* $a$ acceleration of the robot\n",
     "* $\\delta$ angular velocity of the robot\n",
     "\n",
     "These are the differential equations f(x,u) of the model:\n",
@@ -715,7 +715,6 @@
     "u_bar[0,:]=0.5\n",
     "u_bar[1,:]=0.1\n",
     "\n",
-    "    \n",
     "K=road_curve(x0,track)\n",
     "\n",
     "# dynamics starting state w.r.t vehicle frame\n",
@@ -728,7 +727,6 @@
     "    A,B,C=get_linear_model(xt,ut)\n",
     "    xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
     "    x_bar[:,t]= xt_plus_one\n",
-    "    \n",
     "\n",
     "x = cp.Variable((N, T+1))\n",
     "u = cp.Variable((M, T))\n",

notebooks/equations.ipynb

@@ -350,7 +350,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 43,
+   "execution_count": 11,
    "metadata": {},
    "outputs": [
     {
@@ -409,7 +409,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 44,
+   "execution_count": 12,
    "metadata": {},
    "outputs": [
     {
@@ -419,7 +419,7 @@
        "       -0.58083993])"
       ]
      },
-     "execution_count": 44,
+     "execution_count": 12,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -430,7 +430,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 40,
+   "execution_count": 13,
    "metadata": {},
    "outputs": [
     {
@@ -475,12 +475,12 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "## With BSLINES"
+    "## With SPLINES"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 18,
+   "execution_count": 17,
    "metadata": {},
    "outputs": [
     {
@@ -523,10 +523,9 @@
     "\n",
     "#fit poly\n",
     "import scipy\n",
-    "from scipy.interpolate import BSpline\n",
+    "from scipy.interpolate import CubicSpline\n",
     "from scipy.interpolate import PPoly,splrep\n",
     "spl=splrep(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:])\n",
-    "coeff\n",
     "print( spl)\n",
     "print(PPoly.from_spline(spl).c)\n",
     "#coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
@@ -536,6 +535,30 @@
     "\n"
    ]
   },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def spline_planning(qs, qf, ts, tf, dqs=0.0, dqf=0.0, ddqs=0.0, ddqf=0.0):\n",
+    "    \n",
+    "    bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T \n",
+    "    \n",
+    "    C = np.array([[1,    xs,    xs**2,   xs**3,    xs**4,    xs**5],    #f(xs)=ys\n",
+    "                  [0,     1,  2*xs**1, 3*xs**2,  4*xs**3,  5**xs^4],    #df(xs)=dys\n",
+    "                  [0,     0,        1, 6*xs**1, 12*xs**2, 20**xs^3],    #ddf(xs)=ddys\n",
+    "                  [1,    xf,    xf**2,   xf**3,    xf**4,    xf**5],    #f(xf)=yf\n",
+    "                  [0,     1,  2*xf**1, 3*xf**2,  4*xf**3,  5**xf^4],    #df(xf)=dyf\n",
+    "                  [0,     0,        1, 6*xf**1, 12*xf**2, 20**xf^3]])   #ddf(xf)=ddyf\n",
+    "    \n",
+    "    #To compute the polynomial coefficients we solve:\n",
+    "    #Ax = B. \n",
+    "    #Matrices A and B must have the same number of rows\n",
+    "    a = np.linalg.lstsq(C,bc)[0]\n",
+    "    return a"
+   ]
+  },
   {
    "cell_type": "markdown",
    "metadata": {},
@@ -577,7 +600,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 45,
+   "execution_count": 15,
    "metadata": {},
    "outputs": [
     {