diff --git a/img/quicklatex1.png b/img/quicklatex1.png index af86f1a..bfc5db6 100644 Binary files a/img/quicklatex1.png and b/img/quicklatex1.png differ diff --git a/mpc_demo/mpc_demo_pybullet.py b/mpc_demo/mpc_demo_pybullet.py index 7ae683f..e54ae5f 100644 --- a/mpc_demo/mpc_demo_pybullet.py +++ b/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) diff --git a/notebooks/MPC_racecar.ipynb b/notebooks/MPC_racecar.ipynb index 85b9446..d6559d7 100644 --- a/notebooks/MPC_racecar.ipynb +++ b/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", - " #[a,d]\n", - " \n", - " L=0.3\n", + " L=0.3 #vehicle wheelbase\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,