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

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

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

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

@ -23,13 +23,19 @@
"import numpy as np\n", "import numpy as np\n",
"from scipy.interpolate import interp1d\n", "from scipy.interpolate import interp1d\n",
"\n", "\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n", "def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" final_xp = []\n", " final_xp = []\n",
" final_yp = []\n", " final_yp = []\n",
" delta = step # [m]\n", " delta = step # [m]\n",
"\n", "\n",
" for idx in range(len(start_xp) - 1):\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", "\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n", " interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n", "\n",
@ -41,6 +47,7 @@
"\n", "\n",
" return np.vstack((final_xp, final_yp))\n", " return np.vstack((final_xp, final_yp))\n",
"\n", "\n",
"\n",
"def get_nn_idx(state, path):\n", "def get_nn_idx(state, path):\n",
"\n", "\n",
" dx = state[0] - path[0, :]\n", " dx = state[0] - path[0, :]\n",
@ -49,12 +56,13 @@
" nn_idx = np.argmin(dist)\n", " nn_idx = np.argmin(dist)\n",
"\n", "\n",
" try:\n", " try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n", " v = [\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \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", " v /= np.linalg.norm(v)\n",
"\n", "\n",
" d = [path[0,nn_idx] - state[0],\n", " d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
" path[1,nn_idx] - state[1]]\n",
"\n", "\n",
" if np.dot(d, v) > 0:\n", " if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
@ -90,7 +98,9 @@
"state = [3.5, 0.5, np.radians(30)]\n", "state = [3.5, 0.5, np.radians(30)]\n",
"\n", "\n",
"# given vehicle pos find lookahead waypoints\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", "LOOKAHED = 6\n",
"lk_wp = track[:, nn_idx : nn_idx + LOOKAHED]\n", "lk_wp = track[:, nn_idx : nn_idx + LOOKAHED]\n",
"\n", "\n",
@ -98,16 +108,36 @@
"dx = lk_wp[0, :] - state[0]\n", "dx = lk_wp[0, :] - state[0]\n",
"dy = lk_wp[1, :] - state[1]\n", "dy = lk_wp[1, :] - state[1]\n",
"\n", "\n",
"wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n", "wp_vehicle_frame = np.vstack(\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\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", "\n",
"# fit poly\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", "\n",
"# def f(x,coeff):\n", "# def f(x,coeff):\n",
"# return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n", "# return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n",
"def f(x, coeff):\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", "\n",
"def f(x, coeff):\n", "def f(x, coeff):\n",
" y = 0\n", " y = 0\n",
@ -116,6 +146,7 @@
" y += coeff[k] * x ** (j - k - 1)\n", " y += coeff[k] * x ** (j - k - 1)\n",
" return y\n", " return y\n",
"\n", "\n",
"\n",
"# def df(x,coeff):\n", "# def df(x,coeff):\n",
"# return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n", "# return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
"def df(x, coeff):\n", "def df(x, coeff):\n",
@ -165,25 +196,26 @@
], ],
"source": [ "source": [
"import matplotlib.pyplot as plt\n", "import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n", "plt.style.use(\"ggplot\")\n",
"\n", "\n",
"x = np.arange(-1, 2, 0.001) # interp range of curve\n", "x = np.arange(-1, 2, 0.001) # interp range of curve\n",
"\n", "\n",
"# VEHICLE REF FRAME\n", "# VEHICLE REF FRAME\n",
"plt.subplot(2, 1, 1)\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(0, 0)\n",
"plt.scatter(wp_vehicle_frame[0, :], wp_vehicle_frame[1, :])\n", "plt.scatter(wp_vehicle_frame[0, :], wp_vehicle_frame[1, :])\n",
"plt.plot(x, [f(xs, coeff) for xs in x])\n", "plt.plot(x, [f(xs, coeff) for xs in x])\n",
"plt.axis('equal')\n", "plt.axis(\"equal\")\n",
"\n", "\n",
"# MAP REF FRAME\n", "# MAP REF FRAME\n",
"plt.subplot(2, 1, 2)\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(state[0], state[1])\n",
"plt.scatter(track[0, :], track[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.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", "\n",
"plt.tight_layout()\n", "plt.tight_layout()\n",
"plt.show()\n", "plt.show()\n",
@ -252,12 +284,16 @@
"\n", "\n",
" bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T\n", " bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T\n",
"\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, 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", " [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", " [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, 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", "\n",
" # To compute the polynomial coefficients we solve:\n", " # To compute the polynomial coefficients we solve:\n",
" # Ax = B.\n", " # Ax = B.\n",

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

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

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

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

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

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

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

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

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

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

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

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

17
notebooks/models/differential_model.ipynb
View File

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

82
notebooks/models/motion_model.ipynb
View File

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

20
notebooks/models/numerical_jacobian.ipynb
View File

@ -62,12 +62,15 @@
" L = 0.3\n", " L = 0.3\n",
"\n", "\n",
" # vector of ackerman equations\n", " # vector of ackerman equations\n",
" return np.array([\n", " return np.array(\n",
" [\n",
" xx + np.cos(theta) * v * dt,\n", " xx + np.cos(theta) * v * dt,\n",
" xy + np.sin(theta) * v * dt,\n", " xy + np.sin(theta) * v * dt,\n",
" v + a * dt,\n", " v + a * dt,\n",
" theta + v*np.arctan(delta)/L*dt\n", " theta + v * np.arctan(delta) / L * dt,\n",
" ])\n", " ]\n",
" )\n",
"\n",
"\n", "\n",
"def Jacobians(f, x, u, epsilon=1e-4):\n", "def Jacobians(f, x, u, epsilon=1e-4):\n",
" \"\"\"\n", " \"\"\"\n",
@ -93,15 +96,18 @@
" for i in range(x.shape[0]):\n", " for i in range(x.shape[0]):\n",
"\n", "\n",
" # each coloumn of the jac is given by perturbing a variable\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", "\n",
" for i in range(u.shape[0]):\n", " for i in range(u.shape[0]):\n",
"\n", "\n",
" # each coloumn of the jac is given by perturbing a variable\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", "\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", "import cvxpy as cp\n",
"\n", "\n",
"import matplotlib.pyplot as plt\n", "import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n", "plt.style.use(\"ggplot\")\n",
"\n", "\n",
"import time" "import time"
@ -194,8 +195,7 @@
"outputs": [], "outputs": [],
"source": [ "source": [
"def get_linear_model(x_bar, u_bar):\n", "def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n", " \"\"\" \"\"\"\n",
" \"\"\"\n",
"\n", "\n",
" x = x_bar[0]\n", " x = x_bar[0]\n",
" y = x_bar[1]\n", " y = x_bar[1]\n",
@ -216,7 +216,9 @@
" B_lin = dt * B\n", " B_lin = dt * B\n",
"\n", "\n",
" f_xu = np.array([v * np.cos(theta), v * np.sin(theta), w]).reshape(N, 1)\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", "\n",
" return A_lin, B_lin, C_lin" " return A_lin, B_lin, C_lin"
] ]
@ -237,22 +239,19 @@
"# Define process model\n", "# Define process model\n",
"# This uses the continuous model\n", "# This uses the continuous model\n",
"def kinematics_model(x, t, u):\n", "def kinematics_model(x, t, u):\n",
" \"\"\"\n", " \"\"\" \"\"\"\n",
" \"\"\"\n",
"\n", "\n",
" dxdt = u[0] * np.cos(x[2])\n", " dxdt = u[0] * np.cos(x[2])\n",
" dydt = u[0] * np.sin(x[2])\n", " dydt = u[0] * np.sin(x[2])\n",
" dthetadt = u[1]\n", " dthetadt = u[1]\n",
"\n", "\n",
" dqdt = [dxdt,\n", " dqdt = [dxdt, dydt, dthetadt]\n",
" dydt,\n",
" dthetadt]\n",
"\n", "\n",
" return dqdt\n", " return dqdt\n",
"\n", "\n",
"\n",
"def predict(x0, u):\n", "def predict(x0, u):\n",
" \"\"\"\n", " \"\"\" \"\"\"\n",
" \"\"\"\n",
"\n", "\n",
" x_bar = np.zeros((N, T + 1))\n", " x_bar = np.zeros((N, T + 1))\n",
"\n", "\n",
@ -262,10 +261,7 @@
" for t in range(1, T + 1):\n", " for t in range(1, T + 1):\n",
"\n", "\n",
" tspan = [0, dt]\n", " tspan = [0, dt]\n",
" x_next = odeint(kinematics_model,\n", " x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"\n", "\n",
" x0 = x_next[1]\n", " x0 = x_next[1]\n",
" x_bar[:, t] = x_next[1]\n", " x_bar[:, t] = x_next[1]\n",
@ -337,13 +333,13 @@
"plt.subplot(2, 2, 1)\n", "plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[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.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"plt.subplot(2, 2, 2)\n", "plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[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", "# plt.xlabel('time')\n",
"\n", "\n",
"\n", "\n",
@ -419,13 +415,13 @@
"plt.subplot(2, 2, 1)\n", "plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[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.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"plt.subplot(2, 2, 2)\n", "plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2, :]))\n", "plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel('theta(t)')\n", "plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"\n", "\n",
@ -470,7 +466,12 @@
" delta = step # [m]\n", " delta = step # [m]\n",
"\n", "\n",
" for idx in range(len(start_xp) - 1):\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", "\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n", " interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n", "\n",
@ -482,6 +483,7 @@
"\n", "\n",
" return np.vstack((final_xp, final_yp))\n", " return np.vstack((final_xp, final_yp))\n",
"\n", "\n",
"\n",
"def get_nn_idx(state, path):\n", "def get_nn_idx(state, path):\n",
"\n", "\n",
" dx = state[0] - path[0, :]\n", " dx = state[0] - path[0, :]\n",
@ -490,12 +492,13 @@
" nn_idx = np.argmin(dist)\n", " nn_idx = np.argmin(dist)\n",
"\n", "\n",
" try:\n", " try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n", " v = [\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \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", " v /= np.linalg.norm(v)\n",
"\n", "\n",
" d = [path[0,nn_idx] - state[0],\n", " d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
" path[1,nn_idx] - state[1]]\n",
"\n", "\n",
" if np.dot(d, v) > 0:\n", " if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
@ -507,6 +510,7 @@
"\n", "\n",
" return target_idx\n", " return target_idx\n",
"\n", "\n",
"\n",
"def road_curve(state, track):\n", "def road_curve(state, track):\n",
"\n", "\n",
" # given vehicle pos find lookahead waypoints\n", " # given vehicle pos find lookahead waypoints\n",
@ -518,20 +522,39 @@
" dx = lk_wp[0, :] - state[0]\n", " dx = lk_wp[0, :] - state[0]\n",
" dy = lk_wp[1, :] - state[1]\n", " dy = lk_wp[1, :] - state[1]\n",
"\n", "\n",
" wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n", " wp_vehicle_frame = np.vstack(\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\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", "\n",
" # fit poly\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", "\n",
"def f(x, coeff):\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", "\n",
"# def f(x,coeff):\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", "# 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",
"\n",
"def df(x, coeff):\n", "def df(x, coeff):\n",
" return round(3 * coeff[0] * x**2 + 2 * coeff[1] * x**1 + coeff[2] * x**0, 6)\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", "# 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)" "# 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(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_traj[0, :], x_traj[1, :])\n", "plt.plot(x_traj[0, :], x_traj[1, :])\n",
"plt.axis(\"equal\")\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"# plot v(t)\n", "# plot v(t)\n",
"plt.subplot(2, 2, 2)\n", "plt.subplot(2, 2, 2)\n",
"plt.plot(v_mpc)\n", "plt.plot(v_mpc)\n",
"plt.ylabel('v(t)')\n", "plt.ylabel(\"v(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"# plot w(t)\n", "# plot w(t)\n",
@ -834,8 +857,7 @@
} }
], ],
"source": [ "source": [
"track = compute_path_from_wp([0,3,4,6,10,13],\n", "track = compute_path_from_wp([0, 3, 4, 6, 10, 13], [0, 0, 2, 4, 3, 3], 0.25)\n",
" [0,0,2,4,3,3],0.25)\n",
"\n", "\n",
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\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", "# [0,0,2.5,2.5,0,0,5,10],0.5)\n",
@ -889,7 +911,9 @@
" for t in range(T):\n", " for t in range(T):\n",
"\n", "\n",
" # cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\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", " cost += 20 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
"\n", "\n",
" # Actuation rate of change\n", " # Actuation rate of change\n",
@ -914,8 +938,9 @@
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n", " solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n", "\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\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", " u_bar = np.vstack(\n",
" (np.array(u.value[1, :]).flatten())))\n", " (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n", "\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n", " u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n", "\n",
@ -924,14 +949,15 @@
"\n", "\n",
" # move simulation to t+1\n", " # move simulation to t+1\n",
" tspan = [0, dt]\n", " tspan = [0, dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n", " x_sim[:, sim_time + 1] = odeint(\n",
" x_sim[:,sim_time],\n", " kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" tspan,\n", " )[1]\n",
" args=(u_bar[:,0],))[1]\n",
"\n", "\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", "print(\n",
" np.max(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.min(opt_time))) " " 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(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n", "plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"plt.subplot(grid[0, 2])\n", "plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0, :])\n", "plt.plot(u_sim[0, :])\n",
"plt.ylabel('v(t) [m/s]')\n", "plt.ylabel(\"v(t) [m/s]\")\n",
"\n", "\n",
"plt.subplot(grid[1, 2])\n", "plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1, :]))\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", "plt.tight_layout()\n",
"plt.show()" "plt.show()"
@ -1020,18 +1046,22 @@
], ],
"source": [ "source": [
"import dccp\n", "import dccp\n",
"track = compute_path_from_wp([0,3,4,6,10,13],\n", "\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", "\n",
"obstacles = np.array([[4, 6], [2, 4]])\n", "obstacles = np.array([[4, 6], [2, 4]])\n",
"obstacle_radius = 0.5\n", "obstacle_radius = 0.5\n",
"\n", "\n",
"\n",
"def to_vehic_frame(pt, pos_x, pos_y, theta):\n", "def to_vehic_frame(pt, pos_x, pos_y, theta):\n",
" dx = pt[0] - pos_x\n", " dx = pt[0] - pos_x\n",
" dy = pt[1] - pos_y\n", " dy = pt[1] - pos_y\n",
"\n", "\n",
" return [dx * np.cos(-theta) - dy * np.sin(-theta),\n", " return [\n",
" dy * np.cos(-theta) + dx * np.sin(-theta)]\n", " dx * np.cos(-theta) - dy * np.sin(-theta),\n",
" dy * np.cos(-theta) + dx * np.sin(-theta),\n",
" ]\n",
"\n",
"\n", "\n",
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\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", "# [0,0,2.5,2.5,0,0,5,10],0.5)\n",
@ -1068,7 +1098,14 @@
" # compute opstacles in ref frame\n", " # compute opstacles in ref frame\n",
" o_ = []\n", " o_ = []\n",
" for j in range(2):\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", "\n",
" # dynamics starting state w.r.t vehicle frame\n", " # dynamics starting state w.r.t vehicle frame\n",
" x_bar = np.zeros((N, T + 1))\n", " x_bar = np.zeros((N, T + 1))\n",
@ -1091,7 +1128,9 @@
" for t in range(T):\n", " for t in range(T):\n",
"\n", "\n",
" # cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\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", " cost += 20 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
"\n", "\n",
" # Actuation rate of change\n", " # Actuation rate of change\n",
@ -1120,8 +1159,9 @@
" solution = prob.solve(method=\"dccp\", verbose=False)\n", " solution = prob.solve(method=\"dccp\", verbose=False)\n",
"\n", "\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\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", " u_bar = np.vstack(\n",
" (np.array(u.value[1, :]).flatten())))\n", " (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n", "\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n", " u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n", "\n",
@ -1130,14 +1170,15 @@
"\n", "\n",
" # move simulation to t+1\n", " # move simulation to t+1\n",
" tspan = [0, dt]\n", " tspan = [0, dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n", " x_sim[:, sim_time + 1] = odeint(\n",
" x_sim[:,sim_time],\n", " kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" tspan,\n", " )[1]\n",
" args=(u_bar[:,0],))[1]\n",
"\n", "\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", "print(\n",
" np.max(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.min(opt_time))) " " 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(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n", "plt.plot(x_sim[0, :], x_sim[1, :])\n",
"# obstacles\n", "# obstacles\n",
"circle1=plt.Circle((obstacles[0,0], obstacles[1,0]), 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", "circle2 = plt.Circle((obstacles[0, 1], obstacles[1, 1]), obstacle_radius, color=\"r\")\n",
"plt.axis(\"equal\")\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"ax.add_artist(circle1)\n", "ax.add_artist(circle1)\n",
"ax.add_artist(circle2)\n", "ax.add_artist(circle2)\n",
"\n", "\n",
"plt.subplot(grid[0, 2])\n", "plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0, :])\n", "plt.plot(u_sim[0, :])\n",
"plt.ylabel('v(t) [m/s]')\n", "plt.ylabel(\"v(t) [m/s]\")\n",
"\n", "\n",
"plt.subplot(grid[1, 2])\n", "plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1, :]))\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",
"\n", "\n",
"plt.tight_layout()\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", "import cvxpy as cp\n",
"\n", "\n",
"import matplotlib.pyplot as plt\n", "import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n", "plt.style.use(\"ggplot\")\n",
"\n", "\n",
"import time" "import time"
@ -233,8 +234,7 @@
"outputs": [], "outputs": [],
"source": [ "source": [
"def get_linear_model(x_bar, u_bar):\n", "def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n", " \"\"\" \"\"\"\n",
" \"\"\"\n",
"\n", "\n",
" x = x_bar[0]\n", " x = x_bar[0]\n",
" y = x_bar[1]\n", " y = x_bar[1]\n",
@ -259,8 +259,12 @@
" B[4, 0] = -np.sin(-psi)\n", " B[4, 0] = -np.sin(-psi)\n",
" B_lin = dt * B\n", " B_lin = dt * B\n",
"\n", "\n",
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)\n", " f_xu = np.array(\n",
" C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\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", "\n",
" return A_lin, B_lin, C_lin" " return A_lin, B_lin, C_lin"
] ]
@ -280,8 +284,7 @@
"source": [ "source": [
"# Define process model\n", "# Define process model\n",
"def kinematics_model(x, t, u):\n", "def kinematics_model(x, t, u):\n",
" \"\"\"\n", " \"\"\" \"\"\"\n",
" \"\"\"\n",
"\n", "\n",
" dxdt = u[0] * np.cos(x[2])\n", " dxdt = u[0] * np.cos(x[2])\n",
" dydt = u[0] * np.sin(x[2])\n", " dydt = u[0] * np.sin(x[2])\n",
@ -289,17 +292,13 @@
" dpsidt = -u[1]\n", " dpsidt = -u[1]\n",
" dctedt = u[0] * np.sin(-x[3])\n", " dctedt = u[0] * np.sin(-x[3])\n",
"\n", "\n",
" dqdt = [dxdt,\n", " dqdt = [dxdt, dydt, dthetadt, dpsidt, dctedt]\n",
" dydt,\n",
" dthetadt,\n",
" dpsidt,\n",
" dctedt]\n",
"\n", "\n",
" return dqdt\n", " return dqdt\n",
"\n", "\n",
"\n",
"def predict(x0, u):\n", "def predict(x0, u):\n",
" \"\"\"\n", " \"\"\" \"\"\"\n",
" \"\"\"\n",
"\n", "\n",
" x_bar = np.zeros((N, T + 1))\n", " x_bar = np.zeros((N, T + 1))\n",
"\n", "\n",
@ -309,10 +308,7 @@
" for t in range(1, T + 1):\n", " for t in range(1, T + 1):\n",
"\n", "\n",
" tspan = [0, dt]\n", " tspan = [0, dt]\n",
" x_next = odeint(kinematics_model,\n", " x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"\n", "\n",
" x0 = x_next[1]\n", " x0 = x_next[1]\n",
" x_bar[:, t] = x_next[1]\n", " x_bar[:, t] = x_next[1]\n",
@ -386,23 +382,23 @@
"plt.subplot(2, 2, 1)\n", "plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[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.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"plt.subplot(2, 2, 2)\n", "plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[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", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 3)\n", "plt.subplot(2, 2, 3)\n",
"plt.plot(np.degrees(x_bar[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", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 4)\n", "plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4, :])\n", "plt.plot(x_bar[4, :])\n",
"plt.ylabel('cte(t)')\n", "plt.ylabel(\"cte(t)\")\n",
"\n", "\n",
"plt.tight_layout()\n", "plt.tight_layout()\n",
"plt.show()" "plt.show()"
@ -487,23 +483,23 @@
"plt.subplot(2, 2, 1)\n", "plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[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.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis('equal')\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"plt.subplot(2, 2, 2)\n", "plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2, :])\n", "plt.plot(x_bar[2, :])\n",
"plt.ylabel('theta(t)')\n", "plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 3)\n", "plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3, :])\n", "plt.plot(x_bar[3, :])\n",
"plt.ylabel('psi(t)')\n", "plt.ylabel(\"psi(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 4)\n", "plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4, :])\n", "plt.plot(x_bar[4, :])\n",
"plt.ylabel('cte(t)')\n", "plt.ylabel(\"cte(t)\")\n",
"\n", "\n",
"plt.tight_layout()\n", "plt.tight_layout()\n",
"plt.show()" "plt.show()"
@ -548,12 +544,13 @@
" nn_idx = np.argmin(dist)\n", " nn_idx = np.argmin(dist)\n",
"\n", "\n",
" try:\n", " try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n", " v = [\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \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", " v /= np.linalg.norm(v)\n",
"\n", "\n",
" d = [path[0,nn_idx] - state[0],\n", " d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
" path[1,nn_idx] - state[1]]\n",
"\n", "\n",
" if np.dot(d, v) > 0:\n", " if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
@ -565,8 +562,10 @@
"\n", "\n",
" # front_axle_vect = [np.cos(state[2] - np.pi / 2),\n", " # front_axle_vect = [np.cos(state[2] - np.pi / 2),\n",
" # np.sin(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", " path_ref_vect = [\n",
" np.sin(path[2,target_idx] + np.pi / 2)]\n", " np.cos(path[2, target_idx] + np.pi / 2),\n",
" np.sin(path[2, target_idx] + np.pi / 2),\n",
" ]\n",
"\n", "\n",
" # heading error w.r.t path frame\n", " # heading error w.r.t path frame\n",
" psi = path[2, target_idx] - state[2]\n", " psi = path[2, target_idx] - state[2]\n",
@ -578,6 +577,7 @@
"\n", "\n",
" return target_idx, psi, cte\n", " return target_idx, psi, cte\n",
"\n", "\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n", "def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" \"\"\"\n", " \"\"\"\n",
" Interpolation range is computed to assure one point every fixed distance step [m].\n", " Interpolation range is computed to assure one point every fixed distance step [m].\n",
@ -593,7 +593,12 @@
" delta = step # [m]\n", " delta = step # [m]\n",
"\n", "\n",
" for idx in range(len(start_xp) - 1):\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", "\n",
" interp_range = np.linspace(0, 1, int(section_len / delta))\n", " interp_range = np.linspace(0, 1, int(section_len / delta))\n",
"\n", "\n",
@ -676,23 +681,23 @@
"plt.subplot(2, 2, 1)\n", "plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n", "plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(track[0, :], track[1, :], \"b-\")\n", "plt.plot(track[0, :], track[1, :], \"b-\")\n",
"plt.axis('equal')\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"plt.subplot(2, 2, 2)\n", "plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2, :])\n", "plt.plot(x_bar[2, :])\n",
"plt.ylabel('theta(t)')\n", "plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 3)\n", "plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3, :])\n", "plt.plot(x_bar[3, :])\n",
"plt.ylabel('psi(t)')\n", "plt.ylabel(\"psi(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 4)\n", "plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4, :])\n", "plt.plot(x_bar[4, :])\n",
"plt.ylabel('cte(t)')\n", "plt.ylabel(\"cte(t)\")\n",
"\n", "\n",
"plt.tight_layout()\n", "plt.tight_layout()\n",
"plt.show()" "plt.show()"
@ -757,23 +762,23 @@
"plt.subplot(2, 2, 1)\n", "plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n", "plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(track[0, :], track[1, :], \"b-\")\n", "plt.plot(track[0, :], track[1, :], \"b-\")\n",
"plt.axis('equal')\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"plt.subplot(2, 2, 2)\n", "plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2, :])\n", "plt.plot(x_bar[2, :])\n",
"plt.ylabel('theta(t)')\n", "plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 3)\n", "plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3, :])\n", "plt.plot(x_bar[3, :])\n",
"plt.ylabel('psi(t)')\n", "plt.ylabel(\"psi(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 4)\n", "plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4, :])\n", "plt.plot(x_bar[4, :])\n",
"plt.ylabel('cte(t)')\n", "plt.ylabel(\"cte(t)\")\n",
"\n", "\n",
"plt.tight_layout()\n", "plt.tight_layout()\n",
"plt.show()" "plt.show()"
@ -1003,25 +1008,25 @@
"plt.plot(track[0, :], track[1, :], \"b*\")\n", "plt.plot(track[0, :], track[1, :], \"b*\")\n",
"plt.plot(x_traj[0, :], x_traj[1, :])\n", "plt.plot(x_traj[0, :], x_traj[1, :])\n",
"plt.axis(\"equal\")\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"# plot v(t)\n", "# plot v(t)\n",
"plt.subplot(2, 2, 2)\n", "plt.subplot(2, 2, 2)\n",
"plt.plot(v_mpc)\n", "plt.plot(v_mpc)\n",
"plt.ylabel('v(t)')\n", "plt.ylabel(\"v(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"# plot w(t)\n", "# plot w(t)\n",
"plt.subplot(2, 2, 3)\n", "plt.subplot(2, 2, 3)\n",
"plt.plot(w_mpc)\n", "plt.plot(w_mpc)\n",
"plt.ylabel('w(t)')\n", "plt.ylabel(\"w(t)\")\n",
"# plt.xlabel('time')\n", "# plt.xlabel('time')\n",
"\n", "\n",
"plt.subplot(2, 2, 4)\n", "plt.subplot(2, 2, 4)\n",
"plt.plot(cte_mpc)\n", "plt.plot(cte_mpc)\n",
"plt.ylabel('cte(t)')\n", "plt.ylabel(\"cte(t)\")\n",
"plt.legend(loc='best')\n", "plt.legend(loc=\"best\")\n",
"\n", "\n",
"plt.tight_layout()\n", "plt.tight_layout()\n",
"plt.show()" "plt.show()"
@ -1056,9 +1061,9 @@
} }
], ],
"source": [ "source": [
"\n", "track = compute_path_from_wp(\n",
"track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", " [0, 5, 7.5, 10, 12, 13, 13, 10], [0, 0, 2.5, 2.5, 0, 0, 5, 10]\n",
" [0,0,2.5,2.5,0,0,5,10])\n", ")\n",
"\n", "\n",
"sim_duration = 100\n", "sim_duration = 100\n",
"opt_time = []\n", "opt_time = []\n",
@ -1108,7 +1113,6 @@
"\n", "\n",
" x_bar[:, t] = xt_plus_one\n", " x_bar[:, t] = xt_plus_one\n",
"\n", "\n",
"\n",
" # CVXPY Linear MPC problem statement\n", " # CVXPY Linear MPC problem statement\n",
" cost = 0\n", " cost = 0\n",
" constr = []\n", " constr = []\n",
@ -1149,8 +1153,9 @@
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n", " solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n", "\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\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", " u_bar = np.vstack(\n",
" (np.array(u.value[1, :]).flatten())))\n", " (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n", "\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n", " u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n", "\n",
@ -1159,14 +1164,15 @@
"\n", "\n",
" # move simulation to t+1\n", " # move simulation to t+1\n",
" tspan = [0, dt]\n", " tspan = [0, dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n", " x_sim[:, sim_time + 1] = odeint(\n",
" x_sim[:,sim_time],\n", " kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" tspan,\n", " )[1]\n",
" args=(u_bar[:,0],))[1]\n",
"\n", "\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", "print(\n",
" np.max(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.min(opt_time))) " " 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(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n", "plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n", "plt.axis(\"equal\")\n",
"plt.ylabel('y')\n", "plt.ylabel(\"y\")\n",
"plt.xlabel('x')\n", "plt.xlabel(\"x\")\n",
"\n", "\n",
"plt.subplot(grid[0, 2])\n", "plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0, :])\n", "plt.plot(u_sim[0, :])\n",
"plt.ylabel('v(t) [m/s]')\n", "plt.ylabel(\"v(t) [m/s]\")\n",
"\n", "\n",
"plt.subplot(grid[1, 2])\n", "plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1, :]))\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", "plt.tight_layout()\n",
"plt.show()" "plt.show()"