Added docstrings in utils functions

mpc_demo/mpc_demo_pybullet.py

@@ -69,7 +69,7 @@ def run_sim():
     plane = p.loadURDF("racecar/plane.urdf")
     #track = p.loadSDF("racecar/f10_racecar/meshes/barca_track.sdf", globalScaling=1)
 
-    car = p.loadURDF("racecar/f10_racecar/racecar_differential.urdf", [0,0,.3])
+    car = p.loadURDF("racecar/f10_racecar/racecar_differential.urdf", [0,0.3,.3])
     for wheel in range(p.getNumJoints(car)):
     	print("joint[",wheel,"]=", p.getJointInfo(car,wheel))
     	p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)

notebooks/MPC_racecar.ipynb

@@ -170,7 +170,9 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "# Control problem statement.\n",
+    "\"\"\"\n",
+    "Control problem statement.\n",
+    "\"\"\"\n",
     "\n",
     "N = 4 #number of state variables\n",
     "M = 2 #number of control variables\n",
@@ -186,8 +188,10 @@
    "source": [
     "def get_linear_model(x_bar,u_bar):\n",
     "    \"\"\"\n",
+    "    Computes the LTI approximated state space model x' = Ax + Bu + C\n",
     "    \"\"\"\n",
-    "    L=0.3\n",
+    "    \n",
+    "    L=0.3 #vehicle wheelbase\n",
     "    \n",
     "    x = x_bar[0]\n",
     "    y = x_bar[1]\n",
@@ -233,12 +237,10 @@
     "# This uses the continuous model \n",
     "def kinematics_model(x,t,u):\n",
     "    \"\"\"\n",
+    "    Returns the set of ODE of the vehicle model.\n",
     "    \"\"\"\n",
     "    \n",
-    "    #[x,y,v,th]\n",
+    "    L=0.3 #vehicle wheelbase\n",
-    "    #[a,d]\n",
-    "    \n",
-    "    L=0.3\n",
     "    dxdt = x[2]*np.cos(x[3])\n",
     "    dydt = x[2]*np.sin(x[3])\n",
     "    dvdt = u[0]\n",
@@ -255,9 +257,9 @@
     "    \"\"\"\n",
     "    \"\"\"\n",
     "    \n",
-    "    x_bar = np.zeros((N,T+1))\n",
+    "    x_ = np.zeros((N,T+1))\n",
     "    \n",
-    "    x_bar[:,0] = x0\n",
+    "    x_[:,0] = x0\n",
     "    \n",
     "    # solve ODE\n",
     "    for t in range(1,T+1):\n",
@@ -269,9 +271,9 @@
     "                         args=(u[:,t-1],))\n",
     "\n",
     "        x0 = x_next[1]\n",
-    "        x_bar[:,t]=x_next[1]\n",
+    "        x_[:,t]=x_next[1]\n",
     "        \n",
-    "    return x_bar"
+    "    return x_"
    ]
   },
   {
@@ -457,6 +459,10 @@
    "outputs": [],
    "source": [
     "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
+    "    \"\"\"\n",
+    "    Computes a reference path given a set of waypoints\n",
+    "    \"\"\"\n",
+    "    \n",
     "    final_xp=[]\n",
     "    final_yp=[]\n",
     "    delta = step #[m]\n",
@@ -475,6 +481,9 @@
     "    return np.vstack((final_xp,final_yp))\n",
     "\n",
     "def get_nn_idx(state,path):\n",
+    "    \"\"\"\n",
+    "    Computes the index of the waypoint closest to vehicle\n",
+    "    \"\"\"\n",
     "\n",
     "    dx = state[0]-path[0,:]\n",
     "    dy = state[1]-path[1,:]\n",
@@ -500,6 +509,9 @@
     "    return target_idx\n",
     "\n",
     "def road_curve(state,track):\n",
+    "    \"\"\"\n",
+    "    Computes the polynomial coefficents of the path ahead, in vehicle frame.\n",
+    "    \"\"\"\n",
     "    \n",
     "    #given vehicle pos find lookahead waypoints\n",
     "    nn_idx=get_nn_idx(state,track)-1\n",
@@ -517,14 +529,27 @@
     "    return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n",
     "\n",
     "def f(x,coeff):\n",
+    "    \"\"\"\n",
+    "    Evaluates rank 3 polynomial\n",
+    "    \"\"\"\n",
     "    return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n",
     "\n",
     "# def f(x,coeff):\n",
+    "#     \"\"\"\n",
+    "#     Evaluates rank 5 polynomial\n",
+    "#     \"\"\"\n",
     "#     return  round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)\n",
     "\n",
     "def df(x,coeff):\n",
+    "    \"\"\"\n",
+    "    Evaluates derivative of rank 3 polynomial\n",
+    "    \"\"\"\n",
     "    return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
+    "\n",
     "# def df(x,coeff):\n",
+    "#     \"\"\"\n",
+    "#     Evaluates derivative of rank 5 polynomial\n",
+    "#     \"\"\"\n",
     "#     return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)"
    ]
   },
@@ -1178,9 +1203,9 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Python [conda env:jupyter] *",
+   "display_name": "Python [conda env:.conda-jupyter] *",
    "language": "python",
-   "name": "conda-env-jupyter-py"
+   "name": "conda-env-.conda-jupyter-py"
   },
   "language_info": {
    "codemirror_mode": {
@@ -1192,7 +1217,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.3"
+   "version": "3.8.5"
   }
  },
  "nbformat": 4,