12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

reformatted notebooks with black

master
mcarfagno 2022-08-02 09:33:49 +01:00
parent 04609f8cf6
commit 7f7cdf31b4
17 changed files with 2794 additions and 2446 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
env.yml
View File

@ -20,3 +20,4 @@ dependencies:
- pip:
- pybullet
- black==22.3.0
- 'black[jupyter]'

115
mpc_pybullet_demo/racecar/racecar.py
View File

@ -11,13 +11,13 @@ p.resetSimulation()
p.setGravity(0, 0, -10)
useRealTimeSim = 0
p.setTimeStep(1./120.)
p.setTimeStep(1.0 / 120.0)
p.setRealTimeSimulation(useRealTimeSim) # either this
track = p.loadURDF("plane.urdf")
# track = p.loadSDF("f10_racecar/meshes/barca_track.sdf", globalScaling=1)
# otherCar = p.loadURDF("f10_racecar/racecar_differential.urdf", [0,1,.3])
car = p.loadURDF("f10_racecar/racecar_differential.urdf", [0,0,.3])
car = p.loadURDF("f10_racecar/racecar_differential.urdf", [0, 0, 0.3])
for wheel in range(p.getNumJoints(car)):
print("joint[", wheel, "]=", p.getJointInfo(car, wheel))
@ -28,28 +28,100 @@ wheels = [8,15]
print("----------------")
# p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(
car,
9,
car,
11,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(
car,
10,
car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(
car,
9,
car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(
car,
16,
car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(
car,
16,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(
car,
17,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(
car,
1,
car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(
car,
3,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
@ -81,7 +153,6 @@ steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
# rayIds.append(-1)
# def getCarYaw(car):
# carPos,carOrn = p.getBasePositionAndOrientation(car)
# carEuler = p.getEulerFromQuaternion(carOrn)
@ -101,12 +172,14 @@ lastTime = time.time()
lastControlTime = time.time()
# lastLidarTime = time.time()
def getCarVel(car):
linVel, angVel = p.getBaseVelocity(car)
return linVel[0]
# frame=0
while (True):
while True:
# print (getCarVel(car))
nowTime = time.time()
# render Camera at 10Hertz
@ -149,7 +222,7 @@ while (True):
# lastLidarTime = nowLidarTime
# control at 100Hz
if (nowControlTime-lastControlTime>.01):
if nowControlTime - lastControlTime > 0.01:
carPos, carOrn = p.getBasePositionAndOrientation(car)
# Keep the previous orientation of the camera set by the user.
@ -181,15 +254,23 @@ while (True):
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
# print(targetVelocity)
gearRatio=1./21
gearRatio = 1.0 / 21
for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity/gearRatio,force=maxForce)
p.setJointMotorControl2(
car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity / gearRatio,
force=maxForce,
)
for steer in steering:
p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=steeringAngle)
p.setJointMotorControl2(
car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle
)
if (useRealTimeSim==0):
if useRealTimeSim == 0:
frame += 1
p.stepSimulation()
lastControlTime = nowControlTime

39
notebooks/1.0-lti-system-modelling.ipynb
View File

@ -21,6 +21,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -193,16 +194,13 @@
" dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"\n",
"def predict(x0, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x_ = np.zeros((N, T + 1))\n",
"\n",
@ -212,10 +210,7 @@
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:, t] = x_next[1]\n",
@ -274,13 +269,13 @@
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel('theta(t) [deg]')\n",
"plt.ylabel(\"theta(t) [deg]\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
@ -345,8 +340,12 @@
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)"
]
@ -413,13 +412,13 @@
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel('theta(t)')\n",
"plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",

68
notebooks/1.1-parametrized-path-curves.ipynb
View File

@ -23,13 +23,19 @@
"import numpy as np\n",
"from scipy.interpolate import interp1d\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" final_xp = []\n",
" final_yp = []\n",
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -41,6 +47,7 @@
"\n",
" return np.vstack((final_xp, final_yp))\n",
"\n",
"\n",
"def get_nn_idx(state, path):\n",
"\n",
" dx = state[0] - path[0, :]\n",
@ -49,12 +56,13 @@
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
@ -90,7 +98,9 @@
"state = [3.5, 0.5, np.radians(30)]\n",
"\n",
"# given vehicle pos find lookahead waypoints\n",
"nn_idx=get_nn_idx(state,track)-1 #index ox closest wp, take the previous to have a straighter line\n",
"nn_idx = (\n",
" get_nn_idx(state, track) - 1\n",
") # index ox closest wp, take the previous to have a straighter line\n",
"LOOKAHED = 6\n",
"lk_wp = track[:, nn_idx : nn_idx + LOOKAHED]\n",
"\n",
@ -98,16 +108,36 @@
"dx = lk_wp[0, :] - state[0]\n",
"dy = lk_wp[1, :] - state[1]\n",
"\n",
"wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
"wp_vehicle_frame = np.vstack(\n",
" (\n",
" dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]),\n",
" )\n",
")\n",
"\n",
"# fit poly\n",
"coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
"coeff = np.polyfit(\n",
" wp_vehicle_frame[0, :],\n",
" wp_vehicle_frame[1, :],\n",
" 5,\n",
" rcond=None,\n",
" full=False,\n",
" w=None,\n",
" cov=False,\n",
")\n",
"\n",
"# def f(x,coeff):\n",
"# return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n",
"def f(x, coeff):\n",
" return coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0\n",
" return (\n",
" coeff[0] * x**5\n",
" + coeff[1] * x**4\n",
" + coeff[2] * x**3\n",
" + coeff[3] * x**2\n",
" + coeff[4] * x**1\n",
" + coeff[5] * x**0\n",
" )\n",
"\n",
"\n",
"def f(x, coeff):\n",
" y = 0\n",
@ -116,6 +146,7 @@
" y += coeff[k] * x ** (j - k - 1)\n",
" return y\n",
"\n",
"\n",
"# def df(x,coeff):\n",
"# return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
"def df(x, coeff):\n",
@ -165,25 +196,26 @@
],
"source": [
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"x = np.arange(-1, 2, 0.001) # interp range of curve\n",
"\n",
"# VEHICLE REF FRAME\n",
"plt.subplot(2, 1, 1)\n",
"plt.title('parametrized curve, vehicle ref frame')\n",
"plt.title(\"parametrized curve, vehicle ref frame\")\n",
"plt.scatter(0, 0)\n",
"plt.scatter(wp_vehicle_frame[0, :], wp_vehicle_frame[1, :])\n",
"plt.plot(x, [f(xs, coeff) for xs in x])\n",
"plt.axis('equal')\n",
"plt.axis(\"equal\")\n",
"\n",
"# MAP REF FRAME\n",
"plt.subplot(2, 1, 2)\n",
"plt.title('waypoints, map ref frame')\n",
"plt.title(\"waypoints, map ref frame\")\n",
"plt.scatter(state[0], state[1])\n",
"plt.scatter(track[0, :], track[1, :])\n",
"plt.scatter(track[0, nn_idx : nn_idx + LOOKAHED], track[1, nn_idx : nn_idx + LOOKAHED])\n",
"plt.axis('equal')\n",
"plt.axis(\"equal\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()\n",
@ -252,12 +284,16 @@
"\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",
" C = np.array(\n",
" [\n",
" [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",
" [0, 0, 1, 6 * xf**1, 12 * xf**2, 20**xf ^ 3],\n",
" ]\n",
" ) # ddf(xf)=ddyf\n",
"\n",
" # To compute the polynomial coefficients we solve:\n",
" # Ax = B.\n",

103
notebooks/2.0-MPC-base.ipynb
View File

@ -125,6 +125,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -145,6 +146,7 @@
"T = 20 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"\n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
@ -173,15 +175,22 @@
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\n",
@ -193,16 +202,13 @@
" dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"\n",
"def predict(x0, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x_ = np.zeros((N, T + 1))\n",
"\n",
@ -212,16 +218,14 @@
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:, t] = x_next[1]\n",
"\n",
" return x_\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
@ -232,7 +236,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -260,12 +269,13 @@
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
@ -277,6 +287,7 @@
"\n",
" return target_idx\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" Adapted from pythonrobotics\n",
@ -377,8 +388,7 @@
"MAX_ACC = 1.0\n",
"REF_VEL = 1.0\n",
"\n",
"track = compute_path_from_wp([0,3,6],\n",
" [0,0,0],0.05)\n",
"track = compute_path_from_wp([0, 3, 6], [0, 0, 0], 0.05)\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
@ -488,23 +498,23 @@
"plt.plot(x_ref[0, :], x_ref[1, :], \"g+\")\n",
"plt.plot(x_traj[0, :], x_traj[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"# plot v(t)\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(a_mpc)\n",
"plt.ylabel('a_in(t)')\n",
"plt.ylabel(\"a_in(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(theta_mpc)\n",
"plt.ylabel('theta(t)')\n",
"plt.ylabel(\"theta(t)\")\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(delta_mpc)\n",
"plt.ylabel('d_in(t)')\n",
"plt.ylabel(\"d_in(t)\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -539,8 +549,9 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
@ -614,8 +625,12 @@
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
@ -636,8 +651,9 @@
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
@ -646,14 +662,15 @@
"\n",
" # move simulation to t+1\n",
" tspan = [0, DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -682,24 +699,24 @@
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"

92
notebooks/2.1-MPC-with-iterative-linearization.ipynb
View File

@ -30,6 +30,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -50,6 +51,7 @@
"T = 20 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"\n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
@ -78,15 +80,22 @@
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\n",
@ -98,16 +107,13 @@
" dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"\n",
"def predict(x0, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x_ = np.zeros((N, T + 1))\n",
"\n",
@ -117,16 +123,14 @@
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:, t] = x_next[1]\n",
"\n",
" return x_\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
@ -137,7 +141,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -165,12 +174,13 @@
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
@ -182,6 +192,7 @@
"\n",
" return target_idx\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" Adapted from pythonrobotics\n",
@ -246,8 +257,9 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
@ -324,8 +336,12 @@
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
@ -346,8 +362,9 @@
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" # check how this solution differs from previous\n",
" # if the solutions are very\n",
@ -355,29 +372,28 @@
" if delta_u < 0.05:\n",
" break\n",
"\n",
" \n",
" # select u from best iteration\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
" \n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time() - iter_start)\n",
"\n",
" # move simulation to t+1\n",
" tspan = [0, DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
" # reset u_bar? -> this simulates that we don use previous solution!\n",
" u_bar[0, :] = MAX_ACC / 2 # a\n",
" u_bar[1, :] = 0.0 # delta\n",
"\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -406,24 +422,24 @@
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"

92
notebooks/2.2-MPC-v2-car-reference-frame.ipynb
View File

@ -12,6 +12,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -45,6 +46,7 @@
"T = 20 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"\n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
@ -73,15 +75,22 @@
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\n",
@ -93,16 +102,13 @@
" dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"\n",
"def predict(x0, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x_ = np.zeros((N, T + 1))\n",
"\n",
@ -112,10 +118,7 @@
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:, t] = x_next[1]\n",
@ -126,6 +129,8 @@
"\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\n",
"\"\"\"\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
@ -136,7 +141,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -165,12 +175,13 @@
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
@ -182,6 +193,7 @@
"\n",
" return target_idx\n",
"\n",
"\n",
"def normalize_angle(angle):\n",
" \"\"\"\n",
" Normalize an angle to [-pi, pi]\n",
@ -194,6 +206,7 @@
"\n",
" return angle\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" modified reference in robot frame\n",
@ -258,8 +271,9 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
@ -334,8 +348,12 @@
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
@ -356,8 +374,9 @@
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
@ -366,14 +385,15 @@
"\n",
" # move simulation to t+1\n",
" tspan = [0, DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -402,24 +422,24 @@
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"

128
notebooks/2.3-MPC-simplified.ipynb
View File

@ -25,6 +25,7 @@
"from scipy.integrate import odeint\n",
"from scipy.interpolate import interp1d\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"N = 4 # number of state variables\n",
@ -39,6 +40,7 @@
"MAX_STEER = np.radians(30) # rad\n",
"MAX_D_STEER = np.radians(30) # rad/s\n",
"\n",
"\n",
"def get_linear_model_matrices(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
@ -71,22 +73,34 @@
" B_lin = DT * B\n",
"\n",
" f_xu = np.array([v * ct, v * st, a, v * td / L]).reshape(N, 1)\n",
" C_lin = DT*(f_xu - np.dot(A, x_bar.reshape(N,1)) - np.dot(B, u_bar.reshape(M,1)))#.flatten()\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" ) # .flatten()\n",
"\n",
" # return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)\n",
" return A_lin, B_lin, C_lin\n",
"\n",
"class MPC():\n",
"\n",
"class MPC:\n",
" def __init__(self, N, M, Q, R):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
" self.state_len = N\n",
" self.action_len = M\n",
" self.state_cost = Q\n",
" self.action_cost = R\n",
"\n",
" def optimize_linearized_model(self, A, B, C, initial_state, target, time_horizon=10, Q=None, R=None, verbose=False):\n",
" def optimize_linearized_model(\n",
" self,\n",
" A,\n",
" B,\n",
" C,\n",
" initial_state,\n",
" target,\n",
" time_horizon=10,\n",
" Q=None,\n",
" R=None,\n",
" verbose=False,\n",
" ):\n",
" \"\"\"\n",
" Optimisation problem defined for the linearised model,\n",
" :param A:\n",
@ -103,25 +117,30 @@
"\n",
" assert len(initial_state) == self.state_len\n",
"\n",
" if (Q == None or R==None):\n",
" if Q == None or R == None:\n",
" Q = self.state_cost\n",
" R = self.action_cost\n",
"\n",
" # Create variables\n",
" x = opt.Variable((self.state_len, time_horizon + 1), name='states')\n",
" u = opt.Variable((self.action_len, time_horizon), name='actions')\n",
" x = opt.Variable((self.state_len, time_horizon + 1), name=\"states\")\n",
" u = opt.Variable((self.action_len, time_horizon), name=\"actions\")\n",
"\n",
" # Loop through the entire time_horizon and append costs\n",
" cost_function = []\n",
"\n",
" for t in range(time_horizon):\n",
"\n",
" _cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) +\\\n",
" opt.quad_form(u[:, t], R)\n",
" _cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) + opt.quad_form(\n",
" u[:, t], R\n",
" )\n",
"\n",
" _constraints = [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,\n",
" u[0, t] >= -MAX_ACC, u[0, t] <= MAX_ACC,\n",
" u[1, t] >= -MAX_STEER, u[1, t] <= MAX_STEER]\n",
" _constraints = [\n",
" x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,\n",
" u[0, t] >= -MAX_ACC,\n",
" u[0, t] <= MAX_ACC,\n",
" u[1, t] >= -MAX_STEER,\n",
" u[1, t] <= MAX_STEER,\n",
" ]\n",
" # opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]\n",
"\n",
" # Actuation rate of change\n",
@ -130,13 +149,14 @@
" _constraints += [opt.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC]\n",
" _constraints += [opt.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER]\n",
"\n",
" \n",
" if t == 0:\n",
" # _constraints += [opt.norm(target[:, time_horizon] - x[:, time_horizon], 1) <= 0.01,\n",
" # x[:, 0] == initial_state]\n",
" _constraints += [x[:, 0] == initial_state]\n",
"\n",
" cost_function.append(opt.Problem(opt.Minimize(_cost), constraints=_constraints))\n",
" cost_function.append(\n",
" opt.Problem(opt.Minimize(_cost), constraints=_constraints)\n",
" )\n",
"\n",
" # Add final cost\n",
" problem = sum(cost_function)\n",
@ -160,6 +180,8 @@
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\n",
@ -171,16 +193,13 @@
" dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"\n",
"def predict(x0, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x_ = np.zeros((N, T + 1))\n",
"\n",
@ -190,10 +209,7 @@
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:, t] = x_next[1]\n",
@ -204,6 +220,8 @@
"\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\n",
"\"\"\"\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
@ -214,7 +232,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -243,12 +266,13 @@
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
@ -260,6 +284,7 @@
"\n",
" return target_idx\n",
"\n",
"\n",
"def normalize_angle(angle):\n",
" \"\"\"\n",
" Normalize an angle to [-pi, pi]\n",
@ -272,6 +297,7 @@
"\n",
" return angle\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" For each step in the time horizon\n",
@ -288,7 +314,6 @@
" dx = path[0, ind] - state[0]\n",
" dy = path[1, ind] - state[1]\n",
"\n",
" \n",
" xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X\n",
" xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y\n",
" xref[2, 0] = target_v # V\n",
@ -338,8 +363,9 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
@ -389,11 +415,14 @@
" # Get Reference_traj -> inputs are in worldframe\n",
" target, _ = get_ref_trajectory(x_sim[:, sim_time], track, REF_VEL)\n",
"\n",
" x_mpc, u_mpc = mpc.optimize_linearized_model(A, B, C, start_state, target, time_horizon=T, verbose=False)\n",
" x_mpc, u_mpc = mpc.optimize_linearized_model(\n",
" A, B, C, start_state, target, time_horizon=T, verbose=False\n",
" )\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u_mpc.value[0,:]).flatten(),\n",
" (np.array(u_mpc.value[1,:]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u_mpc.value[0, :]).flatten(), (np.array(u_mpc.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
@ -402,14 +431,15 @@
"\n",
" # move simulation to t+1\n",
" tspan = [0, DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -453,24 +483,24 @@
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"

166
notebooks/3.0-MPC-v3-track-constrains.ipynb
View File

@ -12,6 +12,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -32,6 +33,7 @@
"T = 20 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"\n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
@ -60,15 +62,22 @@
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\n",
@ -80,16 +89,13 @@
" dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"\n",
"def predict(x0, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x_ = np.zeros((N, T + 1))\n",
"\n",
@ -99,10 +105,7 @@
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:, t] = x_next[1]\n",
@ -113,6 +116,8 @@
"\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\n",
"\"\"\"\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
@ -123,7 +128,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -152,12 +162,13 @@
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
@ -169,6 +180,7 @@
"\n",
" return target_idx\n",
"\n",
"\n",
"def normalize_angle(angle):\n",
" \"\"\"\n",
" Normalize an angle to [-pi, pi]\n",
@ -181,6 +193,7 @@
"\n",
" return angle\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" modified reference in robot frame\n",
@ -247,6 +260,8 @@
"outputs": [],
"source": [
"from scipy.signal import savgol_filter\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
@ -257,7 +272,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -277,7 +297,7 @@
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))\n"
" return np.vstack((final_xp, final_yp, theta))"
]
},
{
@ -343,8 +363,10 @@
"\n",
" return bounds_low, bounds_upp\n",
"\n",
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"lower, upper = generate_track_bounds(track)\n",
"\n",
@ -432,12 +454,15 @@
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"\n",
"def get_coeff(x, y, theta):\n",
" m = np.sin(theta) / np.cos(theta)\n",
" return (-m, 1, y - m * x)\n",
"\n",
"\n",
"# test -> assume point 10,1,pi/6\n",
"# coeff = get_coeff(1,-1, np.pi/2)\n",
"coeff = get_coeff(1, -1, np.pi / 4)\n",
@ -571,8 +596,7 @@
" coeff_upp[1, idx] = f[1]\n",
" coeff_upp[2, idx] = f[2]\n",
"\n",
" return coeff_low, coeff_upp\n",
" "
" return coeff_low, coeff_upp"
]
},
{
@ -607,8 +631,7 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,3,0],\n",
" [0,0,1,1],0.05)\n",
"track = compute_path_from_wp([0, 3, 3, 0], [0, 0, 1, 1], 0.05)\n",
"\n",
"track_lower, track_upper = generate_track_bounds(track, 0.12)\n",
"\n",
@ -683,8 +706,12 @@
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
@ -705,8 +732,9 @@
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
@ -715,14 +743,15 @@
"\n",
" # move simulation to t+1\n",
" tspan = [0, DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -753,24 +782,24 @@
"plt.plot(track_upper[0, :], track_upper[1, :], \"r-\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -804,8 +833,7 @@
"\n",
"computed_coeff = []\n",
"\n",
"track = compute_path_from_wp([0,3,3,0],\n",
" [0,0,-1,-1],0.05)\n",
"track = compute_path_from_wp([0, 3, 3, 0], [0, 0, -1, -1], 0.05)\n",
"\n",
"track_lower, track_upper = generate_track_bounds(track, WIDTH)\n",
"\n",
@ -879,8 +907,12 @@
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A, B, C = get_linear_model(x_bar[:, t], u_bar[:, t])\n",
@ -901,15 +933,18 @@
" computed_coeff.append((low, upp))\n",
" for ii in range(low.shape[1]):\n",
" # constr += [low[0,ii]*x[0,ii] + x[1,ii] >= low[2,ii]]\n",
" constr += [upp[0,ii]*x[0,ii] + x[1,ii] <= upp[2,ii]] #<-- CAUSES ISSUES Y?\n",
" constr += [\n",
" upp[0, ii] * x[0, ii] + x[1, ii] <= upp[2, ii]\n",
" ] # <-- CAUSES ISSUES Y?\n",
"\n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.ECOS, verbose=False)\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
@ -918,14 +953,15 @@
"\n",
" # move simulation to t+1\n",
" tspan = [0, DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -959,25 +995,25 @@
"plt.plot(track_upper[0, :], track_upper[1, :], \"r.\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -1009,6 +1045,7 @@
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"\n",
@ -1044,6 +1081,7 @@
" plt.xlim((-2, 2))\n",
" plt.ylim((-2, 2))\n",
"\n",
"\n",
"def plot_lines():\n",
" \"\"\"\n",
" sample randomly from computed coeff\n",
@ -1056,12 +1094,12 @@
" for i in range(len(indices)):\n",
" plt.subplot(3, 3, i + 1)\n",
" plt.title(\"t = \" + str(indices[i]))\n",
" plot_low_upp(computed_coeff[indices[i]][0]\n",
" ,computed_coeff[indices[i]][1])\n",
" plot_low_upp(computed_coeff[indices[i]][0], computed_coeff[indices[i]][1])\n",
"\n",
" plt.tight_layout()\n",
" plt.show()\n",
"\n",
"\n",
"plot_lines()"
]
},

49
notebooks/3.1-better-track.ipynb
View File

@ -36,11 +36,12 @@
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"\n",
"def shrink_polygon(coords, shrink_value_x, shrink_value_y):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" def det(a, b):\n",
" return a[0] * b[1] - a[1] * b[0]\n",
@ -51,7 +52,7 @@
"\n",
" div = det(xdiff, ydiff)\n",
" if div == 0:\n",
" raise Exception('lines do not intersect')\n",
" raise Exception(\"lines do not intersect\")\n",
"\n",
" d = (det(*line1), det(*line2))\n",
" x = det(d, xdiff) / div\n",
@ -77,8 +78,9 @@
" new_dx = dy * shrink_value_x * factor\n",
" new_dy = dx * shrink_value_y * factor\n",
"\n",
" new_lines.append([(i[0][0] + new_dx, i[0][1] - new_dy),\n",
" (i[1][0] + new_dx, i[1][1] - new_dy)])\n",
" new_lines.append(\n",
" [(i[0][0] + new_dx, i[0][1] - new_dy), (i[1][0] + new_dx, i[1][1] - new_dy)]\n",
" )\n",
"\n",
" # find position of intersection of all the lines\n",
" new_coords = []\n",
@ -87,8 +89,21 @@
"\n",
" return new_coords\n",
"\n",
"\n",
"# must be clockwise!\n",
"coords = [(0, 0), (0, 10), (2, 10), (3, 8), (4, 10), (6, 10), (6, 0), (4, 1), (4, 4), (2, 4), (2, 1)]\n",
"coords = [\n",
" (0, 0),\n",
" (0, 10),\n",
" (2, 10),\n",
" (3, 8),\n",
" (4, 10),\n",
" (6, 10),\n",
" (6, 0),\n",
" (4, 1),\n",
" (4, 4),\n",
" (2, 4),\n",
" (2, 1),\n",
"]\n",
"\n",
"# how much the coordinates are moved as an absolute value\n",
"SHIFT = 0.5 # [m]\n",
@ -99,9 +114,12 @@
"\"\"\"\n",
"the last point is out of order for my needs\n",
"\"\"\"\n",
"\n",
"\n",
"def rotate(l, n):\n",
" return l[n:] + l[:n]\n",
"\n",
"\n",
"up = rotate(up, 1)\n",
"lo = rotate(lo, 1)\n",
"\n",
@ -131,6 +149,8 @@
"source": [
"from scipy.interpolate import interp1d\n",
"from scipy.signal import savgol_filter\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1, smooth_factor=7):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
@ -141,7 +161,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -201,8 +226,8 @@
"plt.plot(track_lower[0, :], track_lower[1, :], \"g-\")\n",
"plt.plot(track_upper[0, :], track_upper[1, :], \"r-\")\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')"
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")"
]
},
{
@ -250,15 +275,15 @@
"metadata": {},
"outputs": [],
"source": [
"with open('tracks/test.npy', 'wb') as f:\n",
"with open(\"tracks/test.npy\", \"wb\") as f:\n",
" np.save(f, track)\n",
" np.save(f, track_lower)\n",
" np.save(f, track_upper)\n",
"\n",
"with open('tracks/test.npy', 'rb') as f:\n",
"with open(\"tracks/test.npy\", \"rb\") as f:\n",
" a = np.load(f)\n",
" b = np.load(f)\n",
" c = np.load(f)\n"
" c = np.load(f)"
]
},
{

5
notebooks/models/ackerman_model.ipynb
View File

@ -50,10 +50,7 @@
"source": [
"x, y, theta, v, delta, L, a = sp.symbols(\"x y theta v delta L a\")\n",
"\n",
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
" [ sp.sin(theta)*v],\n",
" [a],\n",
" [ v*sp.tan(delta)/L]])\n",
"gs = sp.Matrix([[sp.cos(theta) * v], [sp.sin(theta) * v], [a], [v * sp.tan(delta) / L]])\n",
"\n",
"X = sp.Matrix([x, y, v, theta])\n",
"\n",

17
notebooks/models/differential_model.ipynb
View File

@ -38,11 +38,7 @@
"\n",
"x, y, theta, psi, cte, v, w = sp.symbols(\"x y theta psi cte v w\")\n",
"\n",
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
" [ sp.sin(theta)*v],\n",
" [w],\n",
" [-w],\n",
" [ v*sp.sin(psi)]])\n",
"gs = sp.Matrix([[sp.cos(theta) * v], [sp.sin(theta) * v], [w], [-w], [v * sp.sin(psi)]])\n",
"\n",
"state = sp.Matrix([x, y, theta, psi, cte])\n",
"\n",
@ -108,9 +104,9 @@
"\n",
"x, y, theta, psi, cte, v, w, dt = sp.symbols(\"x y theta psi cte v w dt\")\n",
"\n",
"gs = sp.Matrix([[x + sp.cos(theta)*v*dt],\n",
" [y+ sp.sin(theta)*v*dt],\n",
" [theta + w*dt]])\n",
"gs = sp.Matrix(\n",
" [[x + sp.cos(theta) * v * dt], [y + sp.sin(theta) * v * dt], [theta + w * dt]]\n",
")\n",
"\n",
"state = sp.Matrix([x, y, theta])\n",
"\n",
@ -197,10 +193,7 @@
"source": [
"x, y, theta, v, delta, L, a = sp.symbols(\"x y theta v delta L a\")\n",
"\n",
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
" [ sp.sin(theta)*v],\n",
" [a],\n",
" [ v*sp.tan(delta)/L]])\n",
"gs = sp.Matrix([[sp.cos(theta) * v], [sp.sin(theta) * v], [a], [v * sp.tan(delta) / L]])\n",
"\n",
"X = sp.Matrix([x, y, v, theta])\n",
"\n",

82
notebooks/models/motion_model.ipynb
View File

@ -20,9 +20,26 @@
"import matplotlib.pyplot as plt\n",
"import time\n",
"from scipy.stats import norm\n",
"from sympy import Symbol, symbols, Matrix, sin, cos, integrate, diff, Q, refine, simplify, factor, expand_trig, exp, latex, Integral\n",
"from sympy import (\n",
" Symbol,\n",
" symbols,\n",
" Matrix,\n",
" sin,\n",
" cos,\n",
" integrate,\n",
" diff,\n",
" Q,\n",
" refine,\n",
" simplify,\n",
" factor,\n",
" expand_trig,\n",
" exp,\n",
" latex,\n",
" Integral,\n",
")\n",
"from sympy import init_printing\n",
"from sympy.utilities.codegen import codegen\n",
"\n",
"init_printing(use_latex=True)"
]
},
@ -674,15 +691,18 @@
}
],
"source": [
"speed, yaw, dt, x, y, acceleration, t, d_sin, d_cos = symbols('v \\\\theta \\Delta{t} x y a t d_{sin} d_{cos}')\n",
"turn_rate_non_zero = Symbol('\\hat{\\omega}', nonzero=True, finite=True)\n",
"zero_turn_rate = Symbol('\\omega_0', zero=True)\n",
"turn_rate = Symbol('\\omega')\n",
"speed, yaw, dt, x, y, acceleration, t, d_sin, d_cos = symbols(\n",
" \"v \\\\theta \\Delta{t} x y a t d_{sin} d_{cos}\"\n",
")\n",
"turn_rate_non_zero = Symbol(\"\\hat{\\omega}\", nonzero=True, finite=True)\n",
"zero_turn_rate = Symbol(\"\\omega_0\", zero=True)\n",
"turn_rate = Symbol(\"\\omega\")\n",
"\n",
"state_catr = Matrix([x, y, yaw, speed, turn_rate, acceleration])\n",
"print(\"CART state is:\")\n",
"display(state_catr)\n",
"\n",
"\n",
"def get_next_state(turn_rate):\n",
" # Specify the functions for yaw, speed, x, and y.\n",
" yaw_func = yaw + turn_rate * t\n",
@ -698,26 +718,36 @@
"\n",
" return Matrix([next_x, next_y, next_yaw, next_speed, turn_rate, acceleration])\n",
"\n",
"\n",
"# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not\n",
"print(\"Assuming a non-zero turn rate, the next state is:\")\n",
"next_state = get_next_state(turn_rate_non_zero)\n",
"display(next_state)\n",
"\n",
"substitutes = {x:42, y:23, yaw:0.5, speed:2, acceleration:2, dt:0.1, turn_rate_non_zero:2, zero_turn_rate:0}\n",
"display('Plugging in the numbers:', substitutes)\n",
"substitutes = {\n",
" x: 42,\n",
" y: 23,\n",
" yaw: 0.5,\n",
" speed: 2,\n",
" acceleration: 2,\n",
" dt: 0.1,\n",
" turn_rate_non_zero: 2,\n",
" zero_turn_rate: 0,\n",
"}\n",
"display(\"Plugging in the numbers:\", substitutes)\n",
"display(next_state.evalf(subs=substitutes))\n",
"\n",
"state = Matrix([x, y, yaw, speed, turn_rate_non_zero, acceleration])\n",
"print(\"Jacobian of the next state with respect to the previous state:\")\n",
"J = next_state.jacobian(state)\n",
"display(J)\n",
"display('Plugging in the numbers:', substitutes)\n",
"display(\"Plugging in the numbers:\", substitutes)\n",
"display(J.evalf(subs=substitutes))\n",
"\n",
"print(\"Assuming a zero turn rate, state is:\")\n",
"next_state = get_next_state(zero_turn_rate)\n",
"display(next_state)\n",
"display('Plugging in the numbers:', substitutes)\n",
"display(\"Plugging in the numbers:\", substitutes)\n",
"display(next_state.evalf(subs=substitutes))\n",
"\n",
"state = Matrix([x, y, yaw, speed, zero_turn_rate, acceleration])\n",
@ -725,8 +755,8 @@
"J = next_state.jacobian(state)\n",
"display(J)\n",
"\n",
"display('Plugging in the numbers:', substitutes)\n",
"display(J.evalf(subs=substitutes))\n"
"display(\"Plugging in the numbers:\", substitutes)\n",
"display(J.evalf(subs=substitutes))"
]
},
{
@ -1117,14 +1147,15 @@
}
],
"source": [
"speed, yaw, dt, x, y, t = symbols('v \\\\theta \\Delta{t} x y t')\n",
"turn_rate_non_zero = Symbol('\\omega', nonzero=True, finite=True)\n",
"zero_turn_rate = Symbol('\\omega_0', zero=True)\n",
"speed, yaw, dt, x, y, t = symbols(\"v \\\\theta \\Delta{t} x y t\")\n",
"turn_rate_non_zero = Symbol(\"\\omega\", nonzero=True, finite=True)\n",
"zero_turn_rate = Symbol(\"\\omega_0\", zero=True)\n",
"\n",
"state_cvtr = Matrix([x, y, yaw, speed, turn_rate_non_zero])\n",
"print(\"CVRT state is:\")\n",
"display(state_cvtr)\n",
"\n",
"\n",
"def get_next_state(turn_rate):\n",
" # Specify the functions for yaw, x, and y.\n",
" yaw_func = yaw + turn_rate * t\n",
@ -1138,12 +1169,21 @@
"\n",
" return Matrix([next_x, next_y, next_yaw, speed, turn_rate])\n",
"\n",
"\n",
"# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not\n",
"print(\"Assuming a non-zero turn rate, next state is:\")\n",
"next_state = get_next_state(turn_rate_non_zero)\n",
"display(next_state)\n",
"substitutes = {x:42, y:23, yaw:0.5, speed:2, dt:0.1, turn_rate_non_zero:2, zero_turn_rate:0}\n",
"display(Markdown('Plugging in the numbers:'), substitutes)\n",
"substitutes = {\n",
" x: 42,\n",
" y: 23,\n",
" yaw: 0.5,\n",
" speed: 2,\n",
" dt: 0.1,\n",
" turn_rate_non_zero: 2,\n",
" zero_turn_rate: 0,\n",
"}\n",
"display(Markdown(\"Plugging in the numbers:\"), substitutes)\n",
"display(next_state.evalf(subs=substitutes))\n",
"\n",
"state = Matrix([x, y, yaw, speed, turn_rate_non_zero])\n",
@ -1151,21 +1191,23 @@
"J = next_state.jacobian(state)\n",
"display(J)\n",
"\n",
"display(Markdown('Plugging in the numbers:'), substitutes)\n",
"display(Markdown(\"Plugging in the numbers:\"), substitutes)\n",
"display(J.evalf(subs=substitutes))\n",
"\n",
"print(\"Assuming a zero turn rate, next state is:\")\n",
"next_state = get_next_state(zero_turn_rate)\n",
"display(next_state)\n",
"display(Markdown('Plugging in the numbers:'), substitutes)\n",
"display(Markdown(\"Plugging in the numbers:\"), substitutes)\n",
"display(next_state.evalf(subs=substitutes))\n",
"\n",
"state = Matrix([x, y, yaw, speed, zero_turn_rate])\n",
"print(\"Jacobian of the next state with respect to the previous one with zero turn rate:\")\n",
"print(\n",
" \"Jacobian of the next state with respect to the previous one with zero turn rate:\"\n",
")\n",
"J = next_state.jacobian(state)\n",
"display(J)\n",
"\n",
"display(Markdown('Plugging in the numbers:'), substitutes)\n",
"display(Markdown(\"Plugging in the numbers:\"), substitutes)\n",
"display(J.evalf(subs=substitutes))"
]
}

20
notebooks/models/numerical_jacobian.ipynb
View File

@ -62,12 +62,15 @@
" L = 0.3\n",
"\n",
" # vector of ackerman equations\n",
" return np.array([\n",
" return np.array(\n",
" [\n",
" xx + np.cos(theta) * v * dt,\n",
" xy + np.sin(theta) * v * dt,\n",
" v + a * dt,\n",
" theta + v*np.arctan(delta)/L*dt\n",
" ])\n",
" theta + v * np.arctan(delta) / L * dt,\n",
" ]\n",
" )\n",
"\n",
"\n",
"def Jacobians(f, x, u, epsilon=1e-4):\n",
" \"\"\"\n",
@ -93,15 +96,18 @@
" for i in range(x.shape[0]):\n",
"\n",
" # each coloumn of the jac is given by perturbing a variable\n",
" jac_x[:,i]= (f(x+perturb_x[i,:], u)-f(x-perturb_x[i,:], u))/2*epsilon\n",
" jac_x[:, i] = (\n",
" (f(x + perturb_x[i, :], u) - f(x - perturb_x[i, :], u)) / 2 * epsilon\n",
" )\n",
"\n",
" for i in range(u.shape[0]):\n",
"\n",
" # each coloumn of the jac is given by perturbing a variable\n",
" jac_u[:,i]= (f(x, u+perturb_u[i,:])-f(x, u-perturb_u[i,:]))/2*epsilon\n",
" jac_u[:, i] = (\n",
" (f(x, u + perturb_u[i, :]) - f(x, u - perturb_u[i, :])) / 2 * epsilon\n",
" )\n",
"\n",
" return jac_x, jac_u\n",
" "
" return jac_x, jac_u"
]
},
{

183
notebooks/old_scipy_implementation/MPC_cte_cvxpy.ipynb
View File

@ -12,6 +12,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -194,8 +195,7 @@
"outputs": [],
"source": [
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
@ -216,7 +216,9 @@
" B_lin = dt * B\n",
"\n",
" f_xu = np.array([v * np.cos(theta), v * np.sin(theta), w]).reshape(N, 1)\n",
" C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" C_lin = dt * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return A_lin, B_lin, C_lin"
]
@ -237,22 +239,19 @@
"# Define process model\n",
"# This uses the continuous model\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" dxdt = u[0] * np.cos(x[2])\n",
" dydt = u[0] * np.sin(x[2])\n",
" dthetadt = u[1]\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dthetadt]\n",
" dqdt = [dxdt, dydt, dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"\n",
"def predict(x0, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x_bar = np.zeros((N, T + 1))\n",
"\n",
@ -262,10 +261,7 @@
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, dt]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_bar[:, t] = x_next[1]\n",
@ -337,13 +333,13 @@
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel('theta(t) [deg]')\n",
"plt.ylabel(\"theta(t) [deg]\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
@ -419,13 +415,13 @@
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel('theta(t)')\n",
"plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
@ -470,7 +466,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
@ -482,6 +483,7 @@
"\n",
" return np.vstack((final_xp, final_yp))\n",
"\n",
"\n",
"def get_nn_idx(state, path):\n",
"\n",
" dx = state[0] - path[0, :]\n",
@ -490,12 +492,13 @@
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
@ -507,6 +510,7 @@
"\n",
" return target_idx\n",
"\n",
"\n",
"def road_curve(state, track):\n",
"\n",
" # given vehicle pos find lookahead waypoints\n",
@ -518,20 +522,39 @@
" dx = lk_wp[0, :] - state[0]\n",
" dy = lk_wp[1, :] - state[1]\n",
"\n",
" wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
" wp_vehicle_frame = np.vstack(\n",
" (\n",
" dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]),\n",
" )\n",
" )\n",
"\n",
" # fit poly\n",
" return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n",
" return np.polyfit(\n",
" wp_vehicle_frame[0, :],\n",
" wp_vehicle_frame[1, :],\n",
" 3,\n",
" rcond=None,\n",
" full=False,\n",
" w=None,\n",
" cov=False,\n",
" )\n",
"\n",
"\n",
"def f(x, coeff):\n",
" return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n",
" return round(\n",
" coeff[0] * x**3 + coeff[1] * x**2 + coeff[2] * x**1 + coeff[3] * x**0, 6\n",
" )\n",
"\n",
"\n",
"# def f(x,coeff):\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",
"\n",
"def df(x, coeff):\n",
" return round(3 * coeff[0] * x**2 + 2 * coeff[1] * x**1 + coeff[2] * x**0, 6)\n",
"\n",
"\n",
"# def df(x,coeff):\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)"
]
@ -766,13 +789,13 @@
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_traj[0, :], x_traj[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"# plot v(t)\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(v_mpc)\n",
"plt.ylabel('v(t)')\n",
"plt.ylabel(\"v(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"# plot w(t)\n",
@ -834,8 +857,7 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
" [0,0,2,4,3,3],0.25)\n",
"track = compute_path_from_wp([0, 3, 4, 6, 10, 13], [0, 0, 2, 4, 3, 3], 0.25)\n",
"\n",
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
"# [0,0,2.5,2.5,0,0,5,10],0.5)\n",
@ -889,7 +911,9 @@
" for t in range(T):\n",
"\n",
" # cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
" cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n",
" cost += 50 * cp.sum_squares(\n",
" x[2, t] - np.arctan2(df(x_bar[0, t], K), x_bar[0, t])\n",
" ) # psi\n",
" cost += 20 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
"\n",
" # Actuation rate of change\n",
@ -914,8 +938,9 @@
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n",
" (np.array(u.value[1, :]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
@ -924,14 +949,15 @@
"\n",
" # move simulation to t+1\n",
" tspan = [0, dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -960,16 +986,16 @@
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('w(t) [deg/s]')\n",
"plt.ylabel(\"w(t) [deg/s]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -1020,18 +1046,22 @@
],
"source": [
"import dccp\n",
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
" [0,0,2,4,3,3],0.25)\n",
"\n",
"track = compute_path_from_wp([0, 3, 4, 6, 10, 13], [0, 0, 2, 4, 3, 3], 0.25)\n",
"\n",
"obstacles = np.array([[4, 6], [2, 4]])\n",
"obstacle_radius = 0.5\n",
"\n",
"\n",
"def to_vehic_frame(pt, pos_x, pos_y, theta):\n",
" dx = pt[0] - pos_x\n",
" dy = pt[1] - pos_y\n",
"\n",
" return [dx * np.cos(-theta) - dy * np.sin(-theta),\n",
" dy * np.cos(-theta) + dx * np.sin(-theta)]\n",
" return [\n",
" dx * np.cos(-theta) - dy * np.sin(-theta),\n",
" dy * np.cos(-theta) + dx * np.sin(-theta),\n",
" ]\n",
"\n",
"\n",
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
"# [0,0,2.5,2.5,0,0,5,10],0.5)\n",
@ -1068,7 +1098,14 @@
" # compute opstacles in ref frame\n",
" o_ = []\n",
" for j in range(2):\n",
" o_.append(to_vehic_frame(obstacles[:,j],x_sim[0,sim_time],x_sim[1,sim_time],x_sim[2,sim_time]) )\n",
" o_.append(\n",
" to_vehic_frame(\n",
" obstacles[:, j],\n",
" x_sim[0, sim_time],\n",
" x_sim[1, sim_time],\n",
" x_sim[2, sim_time],\n",
" )\n",
" )\n",
"\n",
" # dynamics starting state w.r.t vehicle frame\n",
" x_bar = np.zeros((N, T + 1))\n",
@ -1091,7 +1128,9 @@
" for t in range(T):\n",
"\n",
" # cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
" cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n",
" cost += 50 * cp.sum_squares(\n",
" x[2, t] - np.arctan2(df(x_bar[0, t], K), x_bar[0, t])\n",
" ) # psi\n",
" cost += 20 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
"\n",
" # Actuation rate of change\n",
@ -1120,8 +1159,9 @@
" solution = prob.solve(method=\"dccp\", verbose=False)\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n",
" (np.array(u.value[1, :]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
@ -1130,14 +1170,15 @@
"\n",
" # move simulation to t+1\n",
" tspan = [0, dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -1166,22 +1207,22 @@
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"# obstacles\n",
"circle1=plt.Circle((obstacles[0,0], obstacles[1,0]), obstacle_radius, color='r')\n",
"circle2=plt.Circle((obstacles[0,1], obstacles[1,1]), obstacle_radius, color='r')\n",
"circle1 = plt.Circle((obstacles[0, 0], obstacles[1, 0]), obstacle_radius, color=\"r\")\n",
"circle2 = plt.Circle((obstacles[0, 1], obstacles[1, 1]), obstacle_radius, color=\"r\")\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"ax.add_artist(circle1)\n",
"ax.add_artist(circle2)\n",
"\n",
"plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('w(t) [deg/s]')\n",
"plt.ylabel(\"w(t) [deg/s]\")\n",
"\n",
"\n",
"plt.tight_layout()\n",

148
notebooks/old_scipy_implementation/MPC_tracking_cvxpy.ipynb
View File

@ -12,6 +12,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -233,8 +234,7 @@
"outputs": [],
"source": [
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
@ -259,8 +259,12 @@
" B[4, 0] = -np.sin(-psi)\n",
" B_lin = dt * B\n",
"\n",
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)\n",
" C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), w, -w, v * np.sin(-psi)]\n",
" ).reshape(N, 1)\n",
" C_lin = dt * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return A_lin, B_lin, C_lin"
]
@ -280,8 +284,7 @@
"source": [
"# Define process model\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" dxdt = u[0] * np.cos(x[2])\n",
" dydt = u[0] * np.sin(x[2])\n",
@ -289,17 +292,13 @@
" dpsidt = -u[1]\n",
" dctedt = u[0] * np.sin(-x[3])\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dthetadt,\n",
" dpsidt,\n",
" dctedt]\n",
" dqdt = [dxdt, dydt, dthetadt, dpsidt, dctedt]\n",
"\n",
" return dqdt\n",
"\n",
"\n",
"def predict(x0, u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
"\n",
" x_bar = np.zeros((N, T + 1))\n",
"\n",
@ -309,10 +308,7 @@
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, dt]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_bar[:, t] = x_next[1]\n",
@ -386,23 +382,23 @@
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel('theta(t) [deg]')\n",
"plt.ylabel(\"theta(t) [deg]\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(np.degrees(x_bar[3, :]))\n",
"plt.ylabel('psi(t) [deg]')\n",
"plt.ylabel(\"psi(t) [deg]\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4, :])\n",
"plt.ylabel('cte(t)')\n",
"plt.ylabel(\"cte(t)\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -487,23 +483,23 @@
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2, :])\n",
"plt.ylabel('theta(t)')\n",
"plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3, :])\n",
"plt.ylabel('psi(t)')\n",
"plt.ylabel(\"psi(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4, :])\n",
"plt.ylabel('cte(t)')\n",
"plt.ylabel(\"cte(t)\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -548,12 +544,13 @@
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
@ -565,8 +562,10 @@
"\n",
" # front_axle_vect = [np.cos(state[2] - np.pi / 2),\n",
" # np.sin(state[2] - np.pi / 2)]\n",
" path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2),\n",
" np.sin(path[2,target_idx] + np.pi / 2)]\n",
" path_ref_vect = [\n",
" np.cos(path[2, target_idx] + np.pi / 2),\n",
" np.sin(path[2, target_idx] + np.pi / 2),\n",
" ]\n",
"\n",
" # heading error w.r.t path frame\n",
" psi = path[2, target_idx] - state[2]\n",
@ -578,6 +577,7 @@
"\n",
" return target_idx, psi, cte\n",
"\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" \"\"\"\n",
" Interpolation range is computed to assure one point every fixed distance step [m].\n",
@ -593,7 +593,12 @@
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, int(section_len / delta))\n",
"\n",
@ -676,23 +681,23 @@
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(track[0, :], track[1, :], \"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2, :])\n",
"plt.ylabel('theta(t)')\n",
"plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3, :])\n",
"plt.ylabel('psi(t)')\n",
"plt.ylabel(\"psi(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4, :])\n",
"plt.ylabel('cte(t)')\n",
"plt.ylabel(\"cte(t)\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -757,23 +762,23 @@
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(track[0, :], track[1, :], \"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2, :])\n",
"plt.ylabel('theta(t)')\n",
"plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3, :])\n",
"plt.ylabel('psi(t)')\n",
"plt.ylabel(\"psi(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4, :])\n",
"plt.ylabel('cte(t)')\n",
"plt.ylabel(\"cte(t)\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -1003,25 +1008,25 @@
"plt.plot(track[0, :], track[1, :], \"b*\")\n",
"plt.plot(x_traj[0, :], x_traj[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"# plot v(t)\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(v_mpc)\n",
"plt.ylabel('v(t)')\n",
"plt.ylabel(\"v(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"# plot w(t)\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(w_mpc)\n",
"plt.ylabel('w(t)')\n",
"plt.ylabel(\"w(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(cte_mpc)\n",
"plt.ylabel('cte(t)')\n",
"plt.legend(loc='best')\n",
"plt.ylabel(\"cte(t)\")\n",
"plt.legend(loc=\"best\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -1056,9 +1061,9 @@
}
],
"source": [
"\n",
"track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
" [0,0,2.5,2.5,0,0,5,10])\n",
"track = compute_path_from_wp(\n",
" [0, 5, 7.5, 10, 12, 13, 13, 10], [0, 0, 2.5, 2.5, 0, 0, 5, 10]\n",
")\n",
"\n",
"sim_duration = 100\n",
"opt_time = []\n",
@ -1108,7 +1113,6 @@
"\n",
" x_bar[:, t] = xt_plus_one\n",
"\n",
"\n",
" # CVXPY Linear MPC problem statement\n",
" cost = 0\n",
" constr = []\n",
@ -1149,8 +1153,9 @@
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n",
" (np.array(u.value[1, :]).flatten())))\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
@ -1159,14 +1164,15 @@
"\n",
" # move simulation to t+1\n",
" tspan = [0, dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -1195,16 +1201,16 @@
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel('w(t) [deg/s]')\n",
"plt.ylabel(\"w(t) [deg/s]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"