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]'

2
mpc_pybullet_demo/mpcpy/__init__.py
View File

@ -1,2 +1,2 @@
from .cvxpy_mpc import * from .cvxpy_mpc import *
from .mpc_config import Params from .mpc_config import Params

327
mpc_pybullet_demo/racecar/racecar.py
View File

@ -8,59 +8,131 @@ p.connect(p.GUI)
p.resetSimulation() 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))
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
p.getJointInfo(car,wheel) p.getJointInfo(car, wheel)
wheels = [8,15] 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(
p.changeConstraint(c,gearRatio=1, maxForce=10000) car,
9,
car,
11,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(
p.changeConstraint(c,gearRatio=-1, maxForce=10000) car,
10,
car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(
p.changeConstraint(c,gearRatio=-1, maxForce=10000) car,
9,
car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(
p.changeConstraint(c,gearRatio=1, maxForce=10000) car,
16,
car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(
p.changeConstraint(c,gearRatio=-1, maxForce=10000) car,
16,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(
p.changeConstraint(c,gearRatio=-1, maxForce=10000) car,
17,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(
p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000) car,
c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) 1,
p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000) car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
c = p.createConstraint(
car,
3,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
steering = [0,2] steering = [0, 2]
hokuyo_joint=4 hokuyo_joint = 4
zed_camera_joint = 5 zed_camera_joint = 5
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-2,2,0) targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -2, 2, 0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20) maxForceSlider = p.addUserDebugParameter("maxForce", 0, 50, 20)
steeringSlider = p.addUserDebugParameter("steering",-1,1,0) steeringSlider = p.addUserDebugParameter("steering", -1, 1, 0)
# replaceLines=True # replaceLines=True
# numRays=100 # numRays=100
@ -81,115 +153,124 @@ steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
# rayIds.append(-1) # rayIds.append(-1)
# def getCarYaw(car):
# carPos,carOrn = p.getBasePositionAndOrientation(car)
# carEuler = p.getEulerFromQuaternion(carOrn)
# carYaw = carEuler[2]*360/(2.*math.pi)-90
# return carYaw
#def getCarYaw(car): # prevCarYaw = getCarYaw(car)
# carPos,carOrn = p.getBasePositionAndOrientation(car)
# carEuler = p.getEulerFromQuaternion(carOrn)
# carYaw = carEuler[2]*360/(2.*math.pi)-90
# return carYaw
#prevCarYaw = getCarYaw(car)
frame = 0 frame = 0
lineId = p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0]) lineId = p.addUserDebugLine([0, 0, 0], [0, 0, 1], [1, 0, 0])
lineId2 = p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0]) lineId2 = p.addUserDebugLine([0, 0, 0], [0, 0, 1], [1, 0, 0])
lineId3= p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0]) lineId3 = p.addUserDebugLine([0, 0, 0], [0, 0, 1], [1, 0, 0])
print("lineId=",lineId) print("lineId=", lineId)
#camInfo = p.getDebugVisualizerCamera() # camInfo = p.getDebugVisualizerCamera()
lastTime = time.time() 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
while (True):
#print (getCarVel(car))
nowTime = time.time()
#render Camera at 10Hertz
# if (nowTime-lastTime>.1):
# ls = p.getLinkState(car,zed_camera_joint, computeForwardKinematics=True)
# camPos = ls[0]
# camOrn = ls[1]
# camMat = p.getMatrixFromQuaternion(camOrn)
# upVector = [0,0,1]
# forwardVec = [camMat[0],camMat[3],camMat[6]]
# #sideVec = [camMat[1],camMat[4],camMat[7]]
# camUpVec = [camMat[2],camMat[5],camMat[8]]
# camTarget = [camPos[0]+forwardVec[0]*10,camPos[1]+forwardVec[1]*10,camPos[2]+forwardVec[2]*10]
# camUpTarget = [camPos[0]+camUpVec[0],camPos[1]+camUpVec[1],camPos[2]+camUpVec[2]]
# viewMat = p.computeViewMatrix(camPos, camTarget, camUpVec)
# projMat = camInfo[3]
# #p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, flags=p.ER_NO_SEGMENTATION_MASK, renderer=p.ER_BULLET_HARDWARE_OPENGL)
# p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
# lastTime=nowTime
nowControlTime = time.time() # frame=0
while True:
# print (getCarVel(car))
nowTime = time.time()
# render Camera at 10Hertz
# if (nowTime-lastTime>.1):
# ls = p.getLinkState(car,zed_camera_joint, computeForwardKinematics=True)
# camPos = ls[0]
# camOrn = ls[1]
# camMat = p.getMatrixFromQuaternion(camOrn)
# upVector = [0,0,1]
# forwardVec = [camMat[0],camMat[3],camMat[6]]
# #sideVec = [camMat[1],camMat[4],camMat[7]]
# camUpVec = [camMat[2],camMat[5],camMat[8]]
# camTarget = [camPos[0]+forwardVec[0]*10,camPos[1]+forwardVec[1]*10,camPos[2]+forwardVec[2]*10]
# camUpTarget = [camPos[0]+camUpVec[0],camPos[1]+camUpVec[1],camPos[2]+camUpVec[2]]
# viewMat = p.computeViewMatrix(camPos, camTarget, camUpVec)
# projMat = camInfo[3]
# #p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, flags=p.ER_NO_SEGMENTATION_MASK, renderer=p.ER_BULLET_HARDWARE_OPENGL)
# p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
# lastTime=nowTime
nowLidarTime = time.time() nowControlTime = time.time()
#lidar at 20Hz
# if (nowLidarTime-lastLidarTime>.3):
# #print("Lidar!")
# numThreads=0
# results = p.rayTestBatch(rayFrom,rayTo,numThreads, parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
# for i in range (numRays):
# hitObjectUid=results[i][0]
# hitFraction = results[i][2]
# hitPosition = results[i][3]
# if (hitFraction==1.):
# p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor,replaceItemUniqueId=rayIds[i],parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
# else:
# localHitTo = [rayFrom[i][0]+hitFraction*(rayTo[i][0]-rayFrom[i][0]),
# rayFrom[i][1]+hitFraction*(rayTo[i][1]-rayFrom[i][1]),
# rayFrom[i][2]+hitFraction*(rayTo[i][2]-rayFrom[i][2])]
# p.addUserDebugLine(rayFrom[i],localHitTo, rayHitColor,replaceItemUniqueId=rayIds[i],parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
# lastLidarTime = nowLidarTime
#control at 100Hz nowLidarTime = time.time()
if (nowControlTime-lastControlTime>.01): # lidar at 20Hz
carPos,carOrn = p.getBasePositionAndOrientation(car) # if (nowLidarTime-lastLidarTime>.3):
# #print("Lidar!")
# numThreads=0
# results = p.rayTestBatch(rayFrom,rayTo,numThreads, parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
# for i in range (numRays):
# hitObjectUid=results[i][0]
# hitFraction = results[i][2]
# hitPosition = results[i][3]
# if (hitFraction==1.):
# p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor,replaceItemUniqueId=rayIds[i],parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
# else:
# localHitTo = [rayFrom[i][0]+hitFraction*(rayTo[i][0]-rayFrom[i][0]),
# rayFrom[i][1]+hitFraction*(rayTo[i][1]-rayFrom[i][1]),
# rayFrom[i][2]+hitFraction*(rayTo[i][2]-rayFrom[i][2])]
# p.addUserDebugLine(rayFrom[i],localHitTo, rayHitColor,replaceItemUniqueId=rayIds[i],parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
# lastLidarTime = nowLidarTime
# Keep the previous orientation of the camera set by the user. # control at 100Hz
if nowControlTime - lastControlTime > 0.01:
carPos, carOrn = p.getBasePositionAndOrientation(car)
# yaw = camInfo[8] # Keep the previous orientation of the camera set by the user.
# pitch = camInfo[9]
# distance = camInfo[10]
# targetPos = camInfo[11]
# camFwd = camInfo[5]
# carYaw = getCarYaw(car)
#
# #the car yaw is clamped between -90 and 270, make sure to deal with angles that wrap around
# if (carYaw-prevCarYaw>45):
# yaw+=360
# if (carYaw-prevCarYaw<-45):
# yaw-=360
# prevCarYaw = carYaw
#print("carYaw=", carYaw) # yaw = camInfo[8]
#print("camYaw=", yaw) # pitch = camInfo[9]
# distance = camInfo[10]
# targetPos = camInfo[11]
# camFwd = camInfo[5]
# carYaw = getCarYaw(car)
#
# #the car yaw is clamped between -90 and 270, make sure to deal with angles that wrap around
# if (carYaw-prevCarYaw>45):
# yaw+=360
# if (carYaw-prevCarYaw<-45):
# yaw-=360
# prevCarYaw = carYaw
#slowly rotate the camera behind the car # print("carYaw=", carYaw)
#diffYaw = (carYaw-yaw)*0.03 # print("camYaw=", yaw)
#track the position of the car as target # slowly rotate the camera behind the car
#p.resetDebugVisualizerCamera(distance, yaw+diffYaw, pitch, carPos) # diffYaw = (carYaw-yaw)*0.03
maxForce = p.readUserDebugParameter(maxForceSlider) # track the position of the car as target
targetVelocity = p.readUserDebugParameter(targetVelocitySlider) # p.resetDebugVisualizerCamera(distance, yaw+diffYaw, pitch, carPos)
steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity)
gearRatio=1./21
for wheel in wheels: maxForce = p.readUserDebugParameter(maxForceSlider)
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity/gearRatio,force=maxForce) targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
# print(targetVelocity)
gearRatio = 1.0 / 21
for steer in steering: for wheel in wheels:
p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=steeringAngle) p.setJointMotorControl2(
car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity / gearRatio,
force=maxForce,
)
if (useRealTimeSim==0): for steer in steering:
frame+=1 p.setJointMotorControl2(
p.stepSimulation() car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle
lastControlTime=nowControlTime )
if useRealTimeSim == 0:
frame += 1
p.stepSimulation()
lastControlTime = nowControlTime

183
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"
@ -181,45 +182,39 @@
"outputs": [], "outputs": [],
"source": [ "source": [
"# 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",
" Returns the set of ODE of the vehicle model.\n", " Returns the set of ODE of the vehicle model.\n",
" \"\"\"\n", " \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" dxdt = x[2]*np.cos(x[3])\n",
" dydt = x[2]*np.sin(x[3])\n",
" dvdt = u[0]\n",
" dthetadt = x[2]*np.tan(u[1])/L\n",
"\n", "\n",
" dqdt = [dxdt,\n", " L = 0.3 # vehicle wheelbase\n",
" dydt,\n", " dxdt = x[2] * np.cos(x[3])\n",
" dvdt,\n", " dydt = x[2] * np.sin(x[3])\n",
" dthetadt]\n", " dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n", "\n",
" return dqdt\n", " return dqdt\n",
"\n", "\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x_ = np.zeros((N,T+1))\n",
" \n",
" x_[:,0] = x0\n",
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n", "\n",
" tspan = [0,DT]\n", "def predict(x0, u):\n",
" x_next = odeint(kinematics_model,\n", " \"\"\" \"\"\"\n",
" x0,\n", "\n",
" tspan,\n", " x_ = np.zeros((N, T + 1))\n",
" args=(u[:,t-1],))\n", "\n",
" x_[:, 0] = x0\n",
"\n",
" # solve ODE\n",
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, 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_" " return x_"
] ]
}, },
@ -240,9 +235,9 @@
"source": [ "source": [
"%%time\n", "%%time\n",
"\n", "\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M, T))\n",
"u_bar[0,:] = 0.2 #m/ss\n", "u_bar[0, :] = 0.2 # m/ss\n",
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n", "u_bar[1, :] = np.radians(-np.pi / 4) # rad\n",
"\n", "\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0\n", "x0[0] = 0\n",
@ -250,7 +245,7 @@
"x0[2] = 0\n", "x0[2] = 0\n",
"x0[3] = np.radians(0)\n", "x0[3] = np.radians(0)\n",
"\n", "\n",
"x_bar=predict(x0,u_bar)" "x_bar = predict(x0, u_bar)"
] ]
}, },
{ {
@ -270,18 +265,18 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"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",
"plt.tight_layout()\n", "plt.tight_layout()\n",
@ -305,10 +300,10 @@
"Control problem statement.\n", "Control problem statement.\n",
"\"\"\"\n", "\"\"\"\n",
"\n", "\n",
"N = 4 #number of state variables\n", "N = 4 # number of state variables\n",
"M = 2 #number of control variables\n", "M = 2 # number of control variables\n",
"T = 20 #Prediction Horizon\n", "T = 20 # Prediction Horizon\n",
"DT = 0.2 #discretization step" "DT = 0.2 # discretization step"
] ]
}, },
{ {
@ -317,38 +312,42 @@
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"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",
" \"\"\"\n", " \"\"\"\n",
" \n", "\n",
" L=0.3 #vehicle wheelbase\n", " L = 0.3 # vehicle wheelbase\n",
" \n", "\n",
" x = x_bar[0]\n", " x = x_bar[0]\n",
" y = x_bar[1]\n", " y = x_bar[1]\n",
" v = x_bar[2]\n", " v = x_bar[2]\n",
" theta = x_bar[3]\n", " theta = x_bar[3]\n",
" \n", "\n",
" a = u_bar[0]\n", " a = u_bar[0]\n",
" delta = u_bar[1]\n", " delta = u_bar[1]\n",
" \n", "\n",
" A = np.zeros((N,N))\n", " A = np.zeros((N, N))\n",
" A[0,2]=np.cos(theta)\n", " A[0, 2] = np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n", " A[0, 3] = -v * np.sin(theta)\n",
" A[1,2]=np.sin(theta)\n", " A[1, 2] = np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n", " A[1, 3] = v * np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n", " A[3, 2] = v * np.tan(delta) / L\n",
" A_lin=np.eye(N)+DT*A\n", " A_lin = np.eye(N) + DT * A\n",
" \n", "\n",
" B = np.zeros((N,M))\n", " B = np.zeros((N, M))\n",
" B[2,0]=1\n", " B[2, 0] = 1\n",
" 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",
" \n", " ).reshape(N, 1)\n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)" " C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)"
] ]
}, },
{ {
@ -368,9 +367,9 @@
"source": [ "source": [
"%%time\n", "%%time\n",
"\n", "\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M, T))\n",
"u_bar[0,:] = 0.2 #m/s\n", "u_bar[0, :] = 0.2 # m/s\n",
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n", "u_bar[1, :] = np.radians(-np.pi / 4) # rad\n",
"\n", "\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0\n", "x0[0] = 0\n",
@ -378,18 +377,18 @@
"x0[2] = 0\n", "x0[2] = 0\n",
"x0[3] = np.radians(0)\n", "x0[3] = np.radians(0)\n",
"\n", "\n",
"x_bar=np.zeros((N,T+1))\n", "x_bar = np.zeros((N, T + 1))\n",
"x_bar[:,0]=x0\n", "x_bar[:, 0] = x0\n",
"\n", "\n",
"for t in range (1,T+1):\n", "for t in range(1, T + 1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n", " xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n", " ut = u_bar[:, t - 1].reshape(M, 1)\n",
" \n", "\n",
" A,B,C=get_linear_model(xt,ut)\n", " A, B, C = get_linear_model(xt, ut)\n",
" \n", "\n",
" xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n", " xt_plus_one = np.dot(A, xt) + np.dot(B, ut) + C\n",
" \n", "\n",
" x_bar[:,t]= np.squeeze(xt_plus_one)" " x_bar[:, t] = np.squeeze(xt_plus_one)"
] ]
}, },
{ {
@ -409,18 +408,18 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"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",
"plt.tight_layout()\n", "plt.tight_layout()\n",

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

@ -23,43 +23,51 @@
"import numpy as np\n", "import numpy as np\n",
"from scipy.interpolate import interp1d\n", "from scipy.interpolate import interp1d\n",
"\n", "\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n", "\n",
" for idx in range(len(start_xp)-1):\n", "def compute_path_from_wp(start_xp, start_yp, step=0.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", " final_xp = []\n",
" final_yp = []\n",
" delta = step # [m]\n",
"\n", "\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n", " for idx in range(len(start_xp) - 1):\n",
" \n", " section_len = np.sum(\n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n", " np.sqrt(\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n", " np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" \n", " + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" final_xp=np.append(final_xp,fx(interp_range))\n", " )\n",
" final_yp=np.append(final_yp,fy(interp_range))\n", " )\n",
"\n", "\n",
" return np.vstack((final_xp,final_yp))\n", " interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n", "\n",
"def get_nn_idx(state,path):\n", " fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
" fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
"\n", "\n",
" dx = state[0]-path[0,:]\n", " final_xp = np.append(final_xp, fx(interp_range))\n",
" dy = state[1]-path[1,:]\n", " final_yp = np.append(final_yp, fy(interp_range))\n",
"\n",
" return np.vstack((final_xp, final_yp))\n",
"\n",
"\n",
"def get_nn_idx(state, path):\n",
"\n",
" dx = state[0] - path[0, :]\n",
" dy = state[1] - path[1, :]\n",
" dist = np.sqrt(dx**2 + dy**2)\n", " dist = np.sqrt(dx**2 + dy**2)\n",
" 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",
" else:\n", " else:\n",
" target_idx = nn_idx+1\n", " target_idx = nn_idx + 1\n",
"\n", "\n",
" except IndexError as e:\n", " except IndexError as e:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
@ -82,47 +90,70 @@
} }
], ],
"source": [ "source": [
"#define track\n", "# define track\n",
"wp=np.array([0,5,6,10,11,15, 0,0,2,2,0,4]).reshape(2,-1)\n", "wp = np.array([0, 5, 6, 10, 11, 15, 0, 0, 2, 2, 0, 4]).reshape(2, -1)\n",
"track = compute_path_from_wp(wp[0,:],wp[1,:],step=0.5)\n", "track = compute_path_from_wp(wp[0, :], wp[1, :], step=0.5)\n",
"\n", "\n",
"#vehicle state\n", "# vehicle state\n",
"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",
"LOOKAHED=6\n", " get_nn_idx(state, track) - 1\n",
"lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n", ") # index ox closest wp, take the previous to have a straighter line\n",
"LOOKAHED = 6\n",
"lk_wp = track[:, nn_idx : nn_idx + LOOKAHED]\n",
"\n", "\n",
"#trasform lookahead waypoints to vehicle ref frame\n", "# trasform lookahead waypoints to vehicle ref frame\n",
"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",
"def f(x,coeff):\n", "\n",
" y=0\n", "def f(x, coeff):\n",
" j=len(coeff)\n", " y = 0\n",
" j = len(coeff)\n",
" for k in range(j):\n", " for k in range(j):\n",
" 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",
" y=0\n", " y = 0\n",
" j=len(coeff)\n", " j = len(coeff)\n",
" for k in range(j-1):\n", " for k in range(j - 1):\n",
" y += (j-k-1)*coeff[k]*x**(j-k-2)\n", " y += (j - k - 1) * coeff[k] * x ** (j - k - 2)\n",
" return y" " return y"
] ]
}, },
@ -165,29 +196,30 @@
], ],
"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",
"#plt.savefig(\"fitted_poly\")" "# plt.savefig(\"fitted_poly\")"
] ]
}, },
{ {
@ -249,20 +281,24 @@
"outputs": [], "outputs": [],
"source": [ "source": [
"def spline_planning(qs, qf, ts, tf, dqs=0.0, dqf=0.0, ddqs=0.0, ddqf=0.0):\n", "def spline_planning(qs, qf, ts, tf, dqs=0.0, dqf=0.0, ddqs=0.0, ddqf=0.0):\n",
" \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",
" [0, 1, 2*xs**1, 3*xs**2, 4*xs**3, 5**xs^4], #df(xs)=dys\n", " [\n",
" [0, 0, 1, 6*xs**1, 12*xs**2, 20**xs^3], #ddf(xs)=ddys\n", " [1, xs, xs**2, xs**3, xs**4, xs**5], # f(xs)=ys\n",
" [1, xf, xf**2, xf**3, xf**4, xf**5], #f(xf)=yf\n", " [0, 1, 2 * xs**1, 3 * xs**2, 4 * xs**3, 5**xs ^ 4], # df(xs)=dys\n",
" [0, 1, 2*xf**1, 3*xf**2, 4*xf**3, 5**xf^4], #df(xf)=dyf\n", " [0, 0, 1, 6 * xs**1, 12 * xs**2, 20**xs ^ 3], # ddf(xs)=ddys\n",
" [0, 0, 1, 6*xf**1, 12*xf**2, 20**xf^3]]) #ddf(xf)=ddyf\n", " [1, xf, xf**2, xf**3, xf**4, xf**5], # f(xf)=yf\n",
" \n", " [0, 1, 2 * xf**1, 3 * xf**2, 4 * xf**3, 5**xf ^ 4], # df(xf)=dyf\n",
" #To compute the polynomial coefficients we solve:\n", " [0, 0, 1, 6 * xf**1, 12 * xf**2, 20**xf ^ 3],\n",
" #Ax = B. \n", " ]\n",
" #Matrices A and B must have the same number of rows\n", " ) # ddf(xf)=ddyf\n",
" a = np.linalg.lstsq(C,bc)[0]\n", "\n",
" # To compute the polynomial coefficients we solve:\n",
" # Ax = B.\n",
" # Matrices A and B must have the same number of rows\n",
" a = np.linalg.lstsq(C, bc)[0]\n",
" return a" " return a"
] ]
} }

529
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"
@ -140,180 +141,190 @@
"Control problem statement.\n", "Control problem statement.\n",
"\"\"\"\n", "\"\"\"\n",
"\n", "\n",
"N = 4 #number of state variables\n", "N = 4 # number of state variables\n",
"M = 2 #number of control variables\n", "M = 2 # number of control variables\n",
"T = 20 #Prediction Horizon\n", "T = 20 # Prediction Horizon\n",
"DT = 0.2 #discretization step\n", "DT = 0.2 # discretization step\n",
"\n", "\n",
"def get_linear_model(x_bar,u_bar):\n", "\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",
" \"\"\"\n", " \"\"\"\n",
" \n", "\n",
" L=0.3 #vehicle wheelbase\n", " L = 0.3 # vehicle wheelbase\n",
" \n", "\n",
" x = x_bar[0]\n", " x = x_bar[0]\n",
" y = x_bar[1]\n", " y = x_bar[1]\n",
" v = x_bar[2]\n", " v = x_bar[2]\n",
" theta = x_bar[3]\n", " theta = x_bar[3]\n",
" \n", "\n",
" a = u_bar[0]\n", " a = u_bar[0]\n",
" delta = u_bar[1]\n", " delta = u_bar[1]\n",
" \n", "\n",
" A = np.zeros((N,N))\n", " A = np.zeros((N, N))\n",
" A[0,2]=np.cos(theta)\n", " A[0, 2] = np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n", " A[0, 3] = -v * np.sin(theta)\n",
" A[1,2]=np.sin(theta)\n", " A[1, 2] = np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n", " A[1, 3] = v * np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n", " A[3, 2] = v * np.tan(delta) / L\n",
" A_lin=np.eye(N)+DT*A\n", " A_lin = np.eye(N) + DT * A\n",
" \n", "\n",
" B = np.zeros((N,M))\n", " B = np.zeros((N, M))\n",
" B[2,0]=1\n", " B[2, 0] = 1\n",
" 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",
" \n", " ).reshape(N, 1)\n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n", " C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n", "\n",
"\"\"\"\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",
"def kinematics_model(x,t,u):\n", "\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" dxdt = x[2]*np.cos(x[3])\n",
" dydt = x[2]*np.sin(x[3])\n",
" dvdt = u[0]\n",
" dthetadt = x[2]*np.tan(u[1])/L\n",
"\n", "\n",
" dqdt = [dxdt,\n", " L = 0.3 # vehicle wheelbase\n",
" dydt,\n", " dxdt = x[2] * np.cos(x[3])\n",
" dvdt,\n", " dydt = x[2] * np.sin(x[3])\n",
" dthetadt]\n", " dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n", "\n",
" return dqdt\n", " return dqdt\n",
"\n", "\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x_ = np.zeros((N,T+1))\n",
" \n",
" x_[:,0] = x0\n",
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n", "\n",
" tspan = [0,DT]\n", "def predict(x0, u):\n",
" x_next = odeint(kinematics_model,\n", " \"\"\" \"\"\"\n",
" x0,\n", "\n",
" tspan,\n", " x_ = np.zeros((N, T + 1))\n",
" args=(u[:,t-1],))\n", "\n",
" x_[:, 0] = x0\n",
"\n",
" # solve ODE\n",
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, 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",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", "\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n", "\n",
" for idx in range(len(start_xp)-1):\n", " final_xp = []\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", " final_yp = []\n",
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
" fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
" fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
"\n",
" final_xp = np.append(final_xp, fx(interp_range))\n",
" final_yp = np.append(final_yp, fy(interp_range))\n",
"\n", "\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
" \n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" final_xp=np.append(final_xp,fx(interp_range))\n",
" final_yp=np.append(final_yp,fy(interp_range))\n",
" \n",
" dx = np.append(0, np.diff(final_xp))\n", " dx = np.append(0, np.diff(final_xp))\n",
" 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))\n",
"\n", "\n",
"\n", "\n",
"def get_nn_idx(state,path):\n", "def get_nn_idx(state, path):\n",
" \"\"\"\n", " \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n", " Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n", " \"\"\"\n",
"\n", "\n",
" dx = state[0]-path[0,:]\n", " dx = state[0] - path[0, :]\n",
" dy = state[1]-path[1,:]\n", " dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx,dy)\n", " dist = np.hypot(dx, dy)\n",
" 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",
" else:\n", " else:\n",
" target_idx = nn_idx+1\n", " target_idx = nn_idx + 1\n",
"\n", "\n",
" except IndexError as e:\n", " except IndexError as e:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" xref = np.zeros((N, T + 1))\n", " xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n", " dref = np.zeros((1, T + 1))\n",
" \n", "\n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n", " # sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n", "\n",
" ncourse = path.shape[1]\n", " ncourse = path.shape[1]\n",
"\n", "\n",
" ind = get_nn_idx(state, path)\n", " ind = get_nn_idx(state, path)\n",
"\n", "\n",
" xref[0, 0] = path[0,ind] #X\n", " xref[0, 0] = path[0, ind] # X\n",
" xref[1, 0] = path[1,ind] #Y\n", " xref[1, 0] = path[1, ind] # Y\n",
" xref[2, 0] = target_v #sp[ind] #V\n", " xref[2, 0] = target_v # sp[ind] #V\n",
" xref[3, 0] = path[2,ind] #Theta\n", " xref[3, 0] = path[2, ind] # Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n", " dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n", "\n",
" dl = 0.05 # Waypoints spacing [m]\n", " dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n", " travel = 0.0\n",
"\n", "\n",
" for i in range(T + 1):\n", " for i in range(T + 1):\n",
" travel += abs(target_v) * DT #current V or target V?\n", " travel += abs(target_v) * DT # current V or target V?\n",
" dind = int(round(travel / dl))\n", " dind = int(round(travel / dl))\n",
"\n", "\n",
" if (ind + dind) < ncourse:\n", " if (ind + dind) < ncourse:\n",
" xref[0, i] = path[0,ind + dind]\n", " xref[0, i] = path[0, ind + dind]\n",
" xref[1, i] = path[1,ind + dind]\n", " xref[1, i] = path[1, ind + dind]\n",
" xref[2, i] = target_v #sp[ind + dind]\n", " xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = path[2,ind + dind]\n", " xref[3, i] = path[2, ind + dind]\n",
" dref[0, i] = 0.0\n", " dref[0, i] = 0.0\n",
" else:\n", " else:\n",
" xref[0, i] = path[0,ncourse - 1]\n", " xref[0, i] = path[0, ncourse - 1]\n",
" xref[1, i] = path[1,ncourse - 1]\n", " xref[1, i] = path[1, ncourse - 1]\n",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n", " xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = path[2,ncourse - 1]\n", " xref[3, i] = path[2, ncourse - 1]\n",
" dref[0, i] = 0.0\n", " dref[0, i] = 0.0\n",
"\n", "\n",
" return xref, dref" " return xref, dref"
@ -372,79 +383,78 @@
"source": [ "source": [
"%%time\n", "%%time\n",
"\n", "\n",
"MAX_SPEED = 1.5 #m/s\n", "MAX_SPEED = 1.5 # m/s\n",
"MAX_STEER = np.radians(30) #rad\n", "MAX_STEER = np.radians(30) # rad\n",
"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",
"x0[0] = 0 #x\n", "x0[0] = 0 # x\n",
"x0[1] = -0.25 #y\n", "x0[1] = -0.25 # y\n",
"x0[2] = 0.0 #v\n", "x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) #yaw\n", "x0[3] = np.radians(-0) # yaw\n",
" \n", "\n",
"#starting guess\n", "# starting guess\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M, T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n", "u_bar[0, :] = MAX_ACC / 2 # a\n",
"u_bar[1,:] = 0.1 #delta\n", "u_bar[1, :] = 0.1 # delta\n",
"\n", "\n",
"# dynamics starting state w.r.t world frame\n", "# dynamics starting state w.r.t world frame\n",
"x_bar = np.zeros((N,T+1))\n", "x_bar = np.zeros((N, T + 1))\n",
"x_bar[:,0] = x0\n", "x_bar[:, 0] = x0\n",
"\n", "\n",
"#prediction for linearization of costrains\n", "# prediction for linearization of costrains\n",
"for t in range (1,T+1):\n", "for t in range(1, T + 1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n", " xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n", " ut = u_bar[:, t - 1].reshape(M, 1)\n",
" A, B, C = get_linear_model(xt,ut)\n", " A, B, C = get_linear_model(xt, ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
" x_bar[:,t] = xt_plus_one\n", " x_bar[:, t] = xt_plus_one\n",
"\n", "\n",
"#CVXPY Linear MPC problem statement\n", "# CVXPY Linear MPC problem statement\n",
"x = cp.Variable((N, T+1))\n", "x = cp.Variable((N, T + 1))\n",
"u = cp.Variable((M, T))\n", "u = cp.Variable((M, T))\n",
"cost = 0\n", "cost = 0\n",
"constr = []\n", "constr = []\n",
"\n", "\n",
"# Cost Matrices\n", "# Cost Matrices\n",
"Q = np.diag([10,10,10,10]) #state error cost\n", "Q = np.diag([10, 10, 10, 10]) # state error cost\n",
"Qf = np.diag([10,10,10,10]) #state final error cost\n", "Qf = np.diag([10, 10, 10, 10]) # state final error cost\n",
"R = np.diag([10,10]) #input cost\n", "R = np.diag([10, 10]) # input cost\n",
"R_ = np.diag([10,10]) #input rate of change cost\n", "R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n", "\n",
"#Get Reference_traj\n", "# Get Reference_traj\n",
"x_ref, d_ref = get_ref_trajectory(x_bar[:,0], track, REF_VEL)\n", "x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
"\n", "\n",
"#Prediction Horizon\n", "# Prediction Horizon\n",
"for t in range(T):\n", "for t in range(T):\n",
" \n", "\n",
" # Tracking Error\n", " # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n", " cost += cp.quad_form(x[:, t] - x_ref[:, t], Q)\n",
"\n", "\n",
" # Actuation effort\n", " # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n", " cost += cp.quad_form(u[:, t], R)\n",
" \n", "\n",
" # 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",
"\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",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
"\n", "\n",
"#Final Point tracking\n", "# Final Point tracking\n",
"cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n", "cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n", "\n",
"# sums problem objectives and concatenates constraints.\n", "# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x_bar[:,0]] #starting condition\n", "constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
"constr += [x[2,:] <= MAX_SPEED] #max speed\n", "constr += [x[2, :] <= MAX_SPEED] # max speed\n",
"constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n", "constr += [x[2, :] >= 0.0] # min_speed (not really needed)\n",
"constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n", "constr += [cp.abs(u[0, :]) <= MAX_ACC] # max acc\n",
"constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n", "constr += [cp.abs(u[1, :]) <= MAX_STEER] # max steer\n",
"# for t in range(T):\n", "# for t in range(T):\n",
"# if t < (T - 1):\n", "# if t < (T - 1):\n",
"# constr += [cp.abs(u[0,t] - u[0,t-1])/DT <= MAX_ACC] #max acc\n", "# constr += [cp.abs(u[0,t] - u[0,t-1])/DT <= MAX_ACC] #max acc\n",
@ -471,40 +481,40 @@
} }
], ],
"source": [ "source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n", "x_mpc = np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n", "y_mpc = np.array(x.value[1, :]).flatten()\n",
"v_mpc=np.array(x.value[2, :]).flatten()\n", "v_mpc = np.array(x.value[2, :]).flatten()\n",
"theta_mpc=np.array(x.value[3, :]).flatten()\n", "theta_mpc = np.array(x.value[3, :]).flatten()\n",
"a_mpc=np.array(u.value[0, :]).flatten()\n", "a_mpc = np.array(u.value[0, :]).flatten()\n",
"delta_mpc=np.array(u.value[1, :]).flatten()\n", "delta_mpc = np.array(u.value[1, :]).flatten()\n",
"\n", "\n",
"#simulate robot state trajectory for optimized U\n", "# simulate robot state trajectory for optimized U\n",
"x_traj=predict(x0, np.vstack((a_mpc,delta_mpc)))\n", "x_traj = predict(x0, np.vstack((a_mpc, delta_mpc)))\n",
"\n", "\n",
"#plt.figure(figsize=(15,10))\n", "# plt.figure(figsize=(15,10))\n",
"#plot trajectory\n", "# plot trajectory\n",
"plt.subplot(2, 2, 1)\n", "plt.subplot(2, 2, 1)\n",
"plt.plot(track[0,:],track[1,:],\"b\")\n", "plt.plot(track[0, :], track[1, :], \"b\")\n",
"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,121 +549,128 @@
} }
], ],
"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",
"\n", "\n",
"sim_duration = 200 #time steps\n", "sim_duration = 200 # time steps\n",
"opt_time=[]\n", "opt_time = []\n",
"\n", "\n",
"x_sim = np.zeros((N,sim_duration))\n", "x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n", "u_sim = np.zeros((M, sim_duration - 1))\n",
"\n", "\n",
"MAX_SPEED = 1.5 #m/s\n", "MAX_SPEED = 1.5 # m/s\n",
"MAX_ACC = 1.0 #m/ss\n", "MAX_ACC = 1.0 # m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n", "MAX_D_ACC = 1.0 # m/sss\n",
"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",
"REF_VEL = 1.0 #m/s\n", "REF_VEL = 1.0 # m/s\n",
"\n", "\n",
"# Starting Condition\n", "# Starting Condition\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n", "x0[0] = 0 # x\n",
"x0[1] = -0.25 #y\n", "x0[1] = -0.25 # y\n",
"x0[2] = 0.0 #v\n", "x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) #yaw\n", "x0[3] = np.radians(-0) # yaw\n",
" \n", "\n",
"#starting guess\n", "# starting guess\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M, T))\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",
"for sim_time in range(sim_duration - 1):\n",
"\n", "\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n", " iter_start = time.time()\n",
" \n", "\n",
" # dynamics starting state\n", " # dynamics starting state\n",
" x_bar = np.zeros((N,T+1))\n", " x_bar = np.zeros((N, T + 1))\n",
" x_bar[:,0] = x_sim[:,sim_time]\n", " x_bar[:, 0] = x_sim[:, sim_time]\n",
" \n", "\n",
" #prediction for linearization of costrains\n", " # prediction for linearization of costrains\n",
" for t in range (1,T+1):\n", " for t in range(1, T + 1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n", " xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n", " ut = u_bar[:, t - 1].reshape(M, 1)\n",
" A,B,C = get_linear_model(xt,ut)\n", " A, B, C = get_linear_model(xt, ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
" x_bar[:,t] = xt_plus_one\n", " x_bar[:, t] = xt_plus_one\n",
" \n", "\n",
" #CVXPY Linear MPC problem statement\n", " # CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n", " x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n", " u = cp.Variable((M, T))\n",
" cost = 0\n", " cost = 0\n",
" constr = []\n", " constr = []\n",
"\n", "\n",
" # Cost Matrices\n", " # Cost Matrices\n",
" Q = np.diag([20,20,10,0]) #state error cost\n", " Q = np.diag([20, 20, 10, 0]) # state error cost\n",
" Qf = np.diag([30,30,30,0]) #state final error cost\n", " Qf = np.diag([30, 30, 30, 0]) # state final error cost\n",
" R = np.diag([10,10]) #input cost\n", " R = np.diag([10, 10]) # input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n", " R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n", "\n",
" #Get Reference_traj\n", " # Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n", " x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
" \n", "\n",
" #Prediction Horizon\n", " # Prediction Horizon\n",
" for t in range(T):\n", " for t in range(T):\n",
"\n", "\n",
" # Tracking Error\n", " # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n", " cost += cp.quad_form(x[:, t] - x_ref[:, t], Q)\n",
"\n", "\n",
" # Actuation effort\n", " # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n", " cost += cp.quad_form(u[:, t], R)\n",
"\n", "\n",
" # 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",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
" \n", "\n",
" #Final Point tracking\n", " # Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n", " cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n", "\n",
" # sums problem objectives and concatenates constraints.\n", " # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n", " constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n", " constr += [x[2, :] <= MAX_SPEED] # max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n", " constr += [x[2, :] >= 0.0] # min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n", " constr += [cp.abs(u[0, :]) <= MAX_ACC] # max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER] # max steer\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.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",
" u_sim[:,sim_time] = u_bar[:,0]\n", "\n",
" \n", " u_sim[:, sim_time] = u_bar[:, 0]\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", "print(\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.max(opt_time),\n", " np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" np.min(opt_time))) " " )\n",
")"
] ]
}, },
{ {
@ -673,33 +690,33 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n", "grid = plt.GridSpec(4, 5)\n",
"\n", "\n",
"plt.figure(figsize=(15,10))\n", "plt.figure(figsize=(15, 10))\n",
"\n", "\n",
"plt.subplot(grid[0:4, 0:4])\n", "plt.subplot(grid[0:4, 0:4])\n",
"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()"

414
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"
@ -45,180 +46,190 @@
"Control problem statement.\n", "Control problem statement.\n",
"\"\"\"\n", "\"\"\"\n",
"\n", "\n",
"N = 4 #number of state variables\n", "N = 4 # number of state variables\n",
"M = 2 #number of control variables\n", "M = 2 # number of control variables\n",
"T = 20 #Prediction Horizon\n", "T = 20 # Prediction Horizon\n",
"DT = 0.2 #discretization step\n", "DT = 0.2 # discretization step\n",
"\n", "\n",
"def get_linear_model(x_bar,u_bar):\n", "\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",
" \"\"\"\n", " \"\"\"\n",
" \n", "\n",
" L=0.3 #vehicle wheelbase\n", " L = 0.3 # vehicle wheelbase\n",
" \n", "\n",
" x = x_bar[0]\n", " x = x_bar[0]\n",
" y = x_bar[1]\n", " y = x_bar[1]\n",
" v = x_bar[2]\n", " v = x_bar[2]\n",
" theta = x_bar[3]\n", " theta = x_bar[3]\n",
" \n", "\n",
" a = u_bar[0]\n", " a = u_bar[0]\n",
" delta = u_bar[1]\n", " delta = u_bar[1]\n",
" \n", "\n",
" A = np.zeros((N,N))\n", " A = np.zeros((N, N))\n",
" A[0,2]=np.cos(theta)\n", " A[0, 2] = np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n", " A[0, 3] = -v * np.sin(theta)\n",
" A[1,2]=np.sin(theta)\n", " A[1, 2] = np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n", " A[1, 3] = v * np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n", " A[3, 2] = v * np.tan(delta) / L\n",
" A_lin=np.eye(N)+DT*A\n", " A_lin = np.eye(N) + DT * A\n",
" \n", "\n",
" B = np.zeros((N,M))\n", " B = np.zeros((N, M))\n",
" B[2,0]=1\n", " B[2, 0] = 1\n",
" 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",
" \n", " ).reshape(N, 1)\n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n", " C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n", "\n",
"\"\"\"\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",
"def kinematics_model(x,t,u):\n", "\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" dxdt = x[2]*np.cos(x[3])\n",
" dydt = x[2]*np.sin(x[3])\n",
" dvdt = u[0]\n",
" dthetadt = x[2]*np.tan(u[1])/L\n",
"\n", "\n",
" dqdt = [dxdt,\n", " L = 0.3 # vehicle wheelbase\n",
" dydt,\n", " dxdt = x[2] * np.cos(x[3])\n",
" dvdt,\n", " dydt = x[2] * np.sin(x[3])\n",
" dthetadt]\n", " dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n", "\n",
" return dqdt\n", " return dqdt\n",
"\n", "\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x_ = np.zeros((N,T+1))\n",
" \n",
" x_[:,0] = x0\n",
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n", "\n",
" tspan = [0,DT]\n", "def predict(x0, u):\n",
" x_next = odeint(kinematics_model,\n", " \"\"\" \"\"\"\n",
" x0,\n", "\n",
" tspan,\n", " x_ = np.zeros((N, T + 1))\n",
" args=(u[:,t-1],))\n", "\n",
" x_[:, 0] = x0\n",
"\n",
" # solve ODE\n",
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, 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",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", "\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n", "\n",
" for idx in range(len(start_xp)-1):\n", " final_xp = []\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", " final_yp = []\n",
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
" fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
" fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
"\n",
" final_xp = np.append(final_xp, fx(interp_range))\n",
" final_yp = np.append(final_yp, fy(interp_range))\n",
"\n", "\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
" \n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" final_xp=np.append(final_xp,fx(interp_range))\n",
" final_yp=np.append(final_yp,fy(interp_range))\n",
" \n",
" dx = np.append(0, np.diff(final_xp))\n", " dx = np.append(0, np.diff(final_xp))\n",
" 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))\n",
"\n", "\n",
"\n", "\n",
"def get_nn_idx(state,path):\n", "def get_nn_idx(state, path):\n",
" \"\"\"\n", " \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n", " Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n", " \"\"\"\n",
"\n", "\n",
" dx = state[0]-path[0,:]\n", " dx = state[0] - path[0, :]\n",
" dy = state[1]-path[1,:]\n", " dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx,dy)\n", " dist = np.hypot(dx, dy)\n",
" 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",
" else:\n", " else:\n",
" target_idx = nn_idx+1\n", " target_idx = nn_idx + 1\n",
"\n", "\n",
" except IndexError as e:\n", " except IndexError as e:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" xref = np.zeros((N, T + 1))\n", " xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n", " dref = np.zeros((1, T + 1))\n",
" \n", "\n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n", " # sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n", "\n",
" ncourse = path.shape[1]\n", " ncourse = path.shape[1]\n",
"\n", "\n",
" ind = get_nn_idx(state, path)\n", " ind = get_nn_idx(state, path)\n",
"\n", "\n",
" xref[0, 0] = path[0,ind] #X\n", " xref[0, 0] = path[0, ind] # X\n",
" xref[1, 0] = path[1,ind] #Y\n", " xref[1, 0] = path[1, ind] # Y\n",
" xref[2, 0] = target_v #sp[ind] #V\n", " xref[2, 0] = target_v # sp[ind] #V\n",
" xref[3, 0] = path[2,ind] #Theta\n", " xref[3, 0] = path[2, ind] # Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n", " dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n", "\n",
" dl = 0.05 # Waypoints spacing [m]\n", " dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n", " travel = 0.0\n",
"\n", "\n",
" for i in range(T + 1):\n", " for i in range(T + 1):\n",
" travel += abs(target_v) * DT #current V or target V?\n", " travel += abs(target_v) * DT # current V or target V?\n",
" dind = int(round(travel / dl))\n", " dind = int(round(travel / dl))\n",
"\n", "\n",
" if (ind + dind) < ncourse:\n", " if (ind + dind) < ncourse:\n",
" xref[0, i] = path[0,ind + dind]\n", " xref[0, i] = path[0, ind + dind]\n",
" xref[1, i] = path[1,ind + dind]\n", " xref[1, i] = path[1, ind + dind]\n",
" xref[2, i] = target_v #sp[ind + dind]\n", " xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = path[2,ind + dind]\n", " xref[3, i] = path[2, ind + dind]\n",
" dref[0, i] = 0.0\n", " dref[0, i] = 0.0\n",
" else:\n", " else:\n",
" xref[0, i] = path[0,ncourse - 1]\n", " xref[0, i] = path[0, ncourse - 1]\n",
" xref[1, i] = path[1,ncourse - 1]\n", " xref[1, i] = path[1, ncourse - 1]\n",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n", " xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = path[2,ncourse - 1]\n", " xref[3, i] = path[2, ncourse - 1]\n",
" dref[0, i] = 0.0\n", " dref[0, i] = 0.0\n",
"\n", "\n",
" return xref, dref" " return xref, dref"
@ -246,138 +257,143 @@
} }
], ],
"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",
"\n", "\n",
"sim_duration = 200 #time steps\n", "sim_duration = 200 # time steps\n",
"opt_time=[]\n", "opt_time = []\n",
"\n", "\n",
"x_sim = np.zeros((N,sim_duration))\n", "x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n", "u_sim = np.zeros((M, sim_duration - 1))\n",
"\n", "\n",
"MAX_SPEED = 1.5 #m/s\n", "MAX_SPEED = 1.5 # m/s\n",
"MAX_ACC = 1.0 #m/ss\n", "MAX_ACC = 1.0 # m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n", "MAX_D_ACC = 1.0 # m/sss\n",
"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",
"REF_VEL = 1.0 #m/s\n", "REF_VEL = 1.0 # m/s\n",
"\n", "\n",
"# Starting Condition\n", "# Starting Condition\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n", "x0[0] = 0 # x\n",
"x0[1] = -0.25 #y\n", "x0[1] = -0.25 # y\n",
"x0[2] = 0.0 #v\n", "x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) #yaw\n", "x0[3] = np.radians(-0) # yaw\n",
"\n",
"for sim_time in range(sim_duration - 1):\n",
"\n", "\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n", " iter_start = time.time()\n",
" \n", "\n",
" #starting guess for ctrl\n", " # starting guess for ctrl\n",
" u_bar = np.zeros((M,T))\n", " u_bar = np.zeros((M, T))\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",
" for _ in range(5):\n", " for _ in range(5):\n",
" u_prev = u_bar\n", " u_prev = u_bar\n",
" \n", "\n",
" # dynamics starting state\n", " # dynamics starting state\n",
" x_bar = np.zeros((N,T+1))\n", " x_bar = np.zeros((N, T + 1))\n",
" x_bar[:,0] = x_sim[:,sim_time]\n", " x_bar[:, 0] = x_sim[:, sim_time]\n",
"\n", "\n",
" #prediction for linearization of costrains\n", " # prediction for linearization of costrains\n",
" for t in range (1,T+1):\n", " for t in range(1, T + 1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n", " xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n", " ut = u_bar[:, t - 1].reshape(M, 1)\n",
" A,B,C = get_linear_model(xt,ut)\n", " A, B, C = get_linear_model(xt, ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
" x_bar[:,t] = xt_plus_one\n", " x_bar[:, t] = xt_plus_one\n",
"\n", "\n",
" #CVXPY Linear MPC problem statement\n", " # CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n", " x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n", " u = cp.Variable((M, T))\n",
" cost = 0\n", " cost = 0\n",
" constr = []\n", " constr = []\n",
"\n", "\n",
" # Cost Matrices\n", " # Cost Matrices\n",
" Q = np.diag([20,20,10,0]) #state error cost\n", " Q = np.diag([20, 20, 10, 0]) # state error cost\n",
" Qf = np.diag([30,30,30,0]) #state final error cost\n", " Qf = np.diag([30, 30, 30, 0]) # state final error cost\n",
" R = np.diag([10,10]) #input cost\n", " R = np.diag([10, 10]) # input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n", " R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n", "\n",
" #Get Reference_traj\n", " # Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n", " x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
"\n", "\n",
" #Prediction Horizon\n", " # Prediction Horizon\n",
" for t in range(T):\n", " for t in range(T):\n",
"\n", "\n",
" # Tracking Error\n", " # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n", " cost += cp.quad_form(x[:, t] - x_ref[:, t], Q)\n",
"\n", "\n",
" # Actuation effort\n", " # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n", " cost += cp.quad_form(u[:, t], R)\n",
"\n", "\n",
" # 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",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
"\n", "\n",
" #Final Point tracking\n", " # Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n", " cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n", "\n",
" # sums problem objectives and concatenates constraints.\n", " # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n", " constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n", " constr += [x[2, :] <= MAX_SPEED] # max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n", " constr += [x[2, :] >= 0.0] # min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n", " constr += [cp.abs(u[0, :]) <= MAX_ACC] # max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER] # max steer\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.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",
" #check how this solution differs from previous\n", "\n",
" #if the solutions are very\n", " # check how this solution differs from previous\n",
" delta_u = np.sum(np.sum(np.abs(u_bar - u_prev),axis=0),axis=0)\n", " # if the solutions are very\n",
" if delta_u < 0.05:\n", " delta_u = np.sum(np.sum(np.abs(u_bar - u_prev), axis=0), axis=0)\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(\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.max(opt_time),\n", " np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" np.min(opt_time))) " " )\n",
")"
] ]
}, },
{ {
@ -397,33 +413,33 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n", "grid = plt.GridSpec(4, 5)\n",
"\n", "\n",
"plt.figure(figsize=(15,10))\n", "plt.figure(figsize=(15, 10))\n",
"\n", "\n",
"plt.subplot(grid[0:4, 0:4])\n", "plt.subplot(grid[0:4, 0:4])\n",
"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()"

424
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"
@ -40,148 +41,159 @@
"Control problem statement.\n", "Control problem statement.\n",
"\"\"\"\n", "\"\"\"\n",
"\n", "\n",
"N = 4 #number of state variables\n", "N = 4 # number of state variables\n",
"M = 2 #number of control variables\n", "M = 2 # number of control variables\n",
"T = 20 #Prediction Horizon\n", "T = 20 # Prediction Horizon\n",
"DT = 0.2 #discretization step\n", "DT = 0.2 # discretization step\n",
"\n", "\n",
"def get_linear_model(x_bar,u_bar):\n", "\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",
" \"\"\"\n", " \"\"\"\n",
" \n", "\n",
" L=0.3 #vehicle wheelbase\n", " L = 0.3 # vehicle wheelbase\n",
" \n", "\n",
" x = x_bar[0]\n", " x = x_bar[0]\n",
" y = x_bar[1]\n", " y = x_bar[1]\n",
" v = x_bar[2]\n", " v = x_bar[2]\n",
" theta = x_bar[3]\n", " theta = x_bar[3]\n",
" \n", "\n",
" a = u_bar[0]\n", " a = u_bar[0]\n",
" delta = u_bar[1]\n", " delta = u_bar[1]\n",
" \n", "\n",
" A = np.zeros((N,N))\n", " A = np.zeros((N, N))\n",
" A[0,2]=np.cos(theta)\n", " A[0, 2] = np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n", " A[0, 3] = -v * np.sin(theta)\n",
" A[1,2]=np.sin(theta)\n", " A[1, 2] = np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n", " A[1, 3] = v * np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n", " A[3, 2] = v * np.tan(delta) / L\n",
" A_lin=np.eye(N)+DT*A\n", " A_lin = np.eye(N) + DT * A\n",
" \n", "\n",
" B = np.zeros((N,M))\n", " B = np.zeros((N, M))\n",
" B[2,0]=1\n", " B[2, 0] = 1\n",
" 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",
" \n", " ).reshape(N, 1)\n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n", " C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n", "\n",
"\"\"\"\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",
"def kinematics_model(x,t,u):\n", "\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" dxdt = x[2]*np.cos(x[3])\n",
" dydt = x[2]*np.sin(x[3])\n",
" dvdt = u[0]\n",
" dthetadt = x[2]*np.tan(u[1])/L\n",
"\n", "\n",
" dqdt = [dxdt,\n", " L = 0.3 # vehicle wheelbase\n",
" dydt,\n", " dxdt = x[2] * np.cos(x[3])\n",
" dvdt,\n", " dydt = x[2] * np.sin(x[3])\n",
" dthetadt]\n", " dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n", "\n",
" return dqdt\n", " return dqdt\n",
"\n", "\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x_ = np.zeros((N,T+1))\n",
" \n",
" x_[:,0] = x0\n",
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n", "\n",
" tspan = [0,DT]\n", "def predict(x0, u):\n",
" x_next = odeint(kinematics_model,\n", " \"\"\" \"\"\"\n",
" x0,\n", "\n",
" tspan,\n", " x_ = np.zeros((N, T + 1))\n",
" args=(u[:,t-1],))\n", "\n",
" x_[:, 0] = x0\n",
"\n",
" # solve ODE\n",
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, 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", "\n",
"\"\"\"\n", "\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\n", "MODIFIED TO INCLUDE FRAME TRANSFORMATION\n",
"\"\"\"\n", "\"\"\"\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", "\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n", "\n",
" for idx in range(len(start_xp)-1):\n", " final_xp = []\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", " final_yp = []\n",
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
" fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
" fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
"\n", "\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
" \n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" # watch out to duplicate points!\n", " # watch out to duplicate points!\n",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n", " final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n", " final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
" \n", "\n",
" dx = np.append(0, np.diff(final_xp))\n", " dx = np.append(0, np.diff(final_xp))\n",
" 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))\n",
"\n", "\n",
"\n", "\n",
"def get_nn_idx(state,path):\n", "def get_nn_idx(state, path):\n",
" \"\"\"\n", " \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n", " Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n", " \"\"\"\n",
"\n", "\n",
" dx = state[0]-path[0,:]\n", " dx = state[0] - path[0, :]\n",
" dy = state[1]-path[1,:]\n", " dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx,dy)\n", " dist = np.hypot(dx, dy)\n",
" 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",
" else:\n", " else:\n",
" target_idx = nn_idx+1\n", " target_idx = nn_idx + 1\n",
"\n", "\n",
" except IndexError as e:\n", " except IndexError as e:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
"\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,51 +206,52 @@
"\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",
" \"\"\"\n", " \"\"\"\n",
" xref = np.zeros((N, T + 1))\n", " xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n", " dref = np.zeros((1, T + 1))\n",
" \n", "\n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n", " # sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n", "\n",
" ncourse = path.shape[1]\n", " ncourse = path.shape[1]\n",
"\n", "\n",
" ind = get_nn_idx(state, path)\n", " ind = get_nn_idx(state, path)\n",
" 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",
" 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",
" xref[3, 0] = normalize_angle(path[2,ind]- state[3]) #Theta\n", " xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n", " dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n", "\n",
" dl = 0.05 # Waypoints spacing [m]\n", " dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n", " travel = 0.0\n",
" \n", "\n",
" for i in range(T + 1):\n", " for i in range(T + 1):\n",
" travel += abs(target_v) * DT #current V or target V?\n", " travel += abs(target_v) * DT # current V or target V?\n",
" dind = int(round(travel / dl))\n", " dind = int(round(travel / dl))\n",
" \n", "\n",
" if (ind + dind) < ncourse:\n", " if (ind + dind) < ncourse:\n",
" dx=path[0,ind + dind] - state[0]\n", " dx = path[0, ind + dind] - state[0]\n",
" dy=path[1,ind + dind] - state[1]\n", " dy = path[1, ind + dind] - state[1]\n",
" \n", "\n",
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n", " xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n", " xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
" xref[2, i] = target_v #sp[ind + dind]\n", " xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])\n", " xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])\n",
" dref[0, i] = 0.0\n", " dref[0, i] = 0.0\n",
" else:\n", " else:\n",
" dx=path[0,ncourse - 1] - state[0]\n", " dx = path[0, ncourse - 1] - state[0]\n",
" dy=path[1,ncourse - 1] - state[1]\n", " dy = path[1, ncourse - 1] - state[1]\n",
" \n", "\n",
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n", " xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n", " xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n", " xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])\n", " xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])\n",
" dref[0, i] = 0.0\n", " dref[0, i] = 0.0\n",
"\n", "\n",
" return xref, dref" " return xref, dref"
@ -258,122 +271,129 @@
} }
], ],
"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",
"\n", "\n",
"sim_duration = 200 #time steps\n", "sim_duration = 200 # time steps\n",
"opt_time=[]\n", "opt_time = []\n",
"\n", "\n",
"x_sim = np.zeros((N,sim_duration))\n", "x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n", "u_sim = np.zeros((M, sim_duration - 1))\n",
"\n", "\n",
"MAX_SPEED = 1.5 #m/s\n", "MAX_SPEED = 1.5 # m/s\n",
"MAX_ACC = 1.0 #m/ss\n", "MAX_ACC = 1.0 # m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n", "MAX_D_ACC = 1.0 # m/sss\n",
"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",
"REF_VEL = 1.0 #m/s\n", "REF_VEL = 1.0 # m/s\n",
"\n", "\n",
"# Starting Condition\n", "# Starting Condition\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n", "x0[0] = 0 # x\n",
"x0[1] = -0.25 #y\n", "x0[1] = -0.25 # y\n",
"x0[2] = 0.0 #v\n", "x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) #yaw\n", "x0[3] = np.radians(-0) # yaw\n",
" \n", "\n",
"#starting guess\n", "# starting guess\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M, T))\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",
"for sim_time in range(sim_duration-1):\n", "for sim_time in range(sim_duration - 1):\n",
" \n", "\n",
" iter_start = time.time()\n", " iter_start = time.time()\n",
" \n", "\n",
" # dynamics starting state w.r.t. robot are always null except vel \n", " # dynamics starting state w.r.t. robot are always null except vel\n",
" x_bar = np.zeros((N,T+1))\n", " x_bar = np.zeros((N, T + 1))\n",
" x_bar[2,0] = x_sim[2,sim_time]\n", " x_bar[2, 0] = x_sim[2, sim_time]\n",
" \n", "\n",
" #prediction for linearization of costrains\n", " # prediction for linearization of costrains\n",
" for t in range (1,T+1):\n", " for t in range(1, T + 1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n", " xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n", " ut = u_bar[:, t - 1].reshape(M, 1)\n",
" A,B,C = get_linear_model(xt,ut)\n", " A, B, C = get_linear_model(xt, ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
" x_bar[:,t] = xt_plus_one\n", " x_bar[:, t] = xt_plus_one\n",
" \n", "\n",
" #CVXPY Linear MPC problem statement\n", " # CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n", " x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n", " u = cp.Variable((M, T))\n",
" cost = 0\n", " cost = 0\n",
" constr = []\n", " constr = []\n",
"\n", "\n",
" # Cost Matrices\n", " # Cost Matrices\n",
" Q = np.diag([20,20,10,20]) #state error cost\n", " Q = np.diag([20, 20, 10, 20]) # state error cost\n",
" Qf = np.diag([30,30,30,30]) #state final error cost\n", " Qf = np.diag([30, 30, 30, 30]) # state final error cost\n",
" R = np.diag([10,10]) #input cost\n", " R = np.diag([10, 10]) # input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n", " R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n", "\n",
" #Get Reference_traj\n", " # Get Reference_traj\n",
" #dont use x0 in this case\n", " # dont use x0 in this case\n",
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n", " x_ref, d_ref = get_ref_trajectory(x_sim[:, sim_time], track, REF_VEL)\n",
" \n", "\n",
" #Prediction Horizon\n", " # Prediction Horizon\n",
" for t in range(T):\n", " for t in range(T):\n",
"\n", "\n",
" # Tracking Error\n", " # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n", " cost += cp.quad_form(x[:, t] - x_ref[:, t], Q)\n",
"\n", "\n",
" # Actuation effort\n", " # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n", " cost += cp.quad_form(u[:, t], R)\n",
"\n", "\n",
" # 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",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
" \n", "\n",
" #Final Point tracking\n", " # Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n", " cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n", "\n",
" # sums problem objectives and concatenates constraints.\n", " # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n", " constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n", " constr += [x[2, :] <= MAX_SPEED] # max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n", " constr += [x[2, :] >= 0.0] # min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n", " constr += [cp.abs(u[0, :]) <= MAX_ACC] # max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER] # max steer\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.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",
" u_sim[:,sim_time] = u_bar[:,0]\n", "\n",
" \n", " u_sim[:, sim_time] = u_bar[:, 0]\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", "print(\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.max(opt_time),\n", " np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" np.min(opt_time))) " " )\n",
")"
] ]
}, },
{ {
@ -393,33 +413,33 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n", "grid = plt.GridSpec(4, 5)\n",
"\n", "\n",
"plt.figure(figsize=(15,10))\n", "plt.figure(figsize=(15, 10))\n",
"\n", "\n",
"plt.subplot(grid[0:4, 0:4])\n", "plt.subplot(grid[0:4, 0:4])\n",
"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()"

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

@ -25,73 +25,87 @@
"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",
"M = 2 #number of control variables\n", "M = 2 # number of control variables\n",
"T = 10 #Prediction Horizon\n", "T = 10 # Prediction Horizon\n",
"DT = 0.2 #discretization step\n", "DT = 0.2 # discretization step\n",
"\n", "\n",
"L = 0.3 #vehicle wheelbase\n", "L = 0.3 # vehicle wheelbase\n",
"MAX_SPEED = 1.5 #m/s\n", "MAX_SPEED = 1.5 # m/s\n",
"MAX_ACC = 1.0 #m/ss\n", "MAX_ACC = 1.0 # m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n", "MAX_D_ACC = 1.0 # m/sss\n",
"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",
"def get_linear_model_matrices(x_bar,u_bar):\n", "\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",
" \"\"\"\n", " \"\"\"\n",
" \n", "\n",
" x = x_bar[0]\n", " x = x_bar[0]\n",
" y = x_bar[1]\n", " y = x_bar[1]\n",
" v = x_bar[2]\n", " v = x_bar[2]\n",
" theta = x_bar[3]\n", " theta = x_bar[3]\n",
" \n", "\n",
" a = u_bar[0]\n", " a = u_bar[0]\n",
" delta = u_bar[1]\n", " delta = u_bar[1]\n",
" \n", "\n",
" ct = np.cos(theta)\n", " ct = np.cos(theta)\n",
" st = np.sin(theta)\n", " st = np.sin(theta)\n",
" cd = np.cos(delta)\n", " cd = np.cos(delta)\n",
" td = np.tan(delta)\n", " td = np.tan(delta)\n",
" \n", "\n",
" A = np.zeros((N,N))\n", " A = np.zeros((N, N))\n",
" A[0,2] = ct\n", " A[0, 2] = ct\n",
" A[0,3] = -v*st\n", " A[0, 3] = -v * st\n",
" A[1,2] = st\n", " A[1, 2] = st\n",
" A[1,3] = v*ct\n", " A[1, 3] = v * ct\n",
" A[3,2] = v*td/L\n", " A[3, 2] = v * td / L\n",
" A_lin = np.eye(N)+DT*A\n", " A_lin = np.eye(N) + DT * A\n",
" \n", "\n",
" B = np.zeros((N,M))\n", " B = np.zeros((N, M))\n",
" B[2,0]=1\n", " B[2, 0] = 1\n",
" B[3,1]=v/(L*cd**2)\n", " B[3, 1] = v / (L * cd**2)\n",
" 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",
" \n", " f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" #return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)\n", " ) # .flatten()\n",
"\n",
" # return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)\n",
" return A_lin, B_lin, C_lin\n", " 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",
" :param B:\n", " :param B:\n",
" :param C: \n", " :param C:\n",
" :param initial_state:\n", " :param initial_state:\n",
" :param Q:\n", " :param Q:\n",
" :param R:\n", " :param R:\n",
@ -100,47 +114,53 @@
" :param verbose:\n", " :param verbose:\n",
" :return:\n", " :return:\n",
" \"\"\"\n", " \"\"\"\n",
" \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",
" _constraints = [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,\n", "\n",
" u[0, t] >= -MAX_ACC, u[0, t] <= MAX_ACC,\n", " _constraints = [\n",
" u[1, t] >= -MAX_STEER, u[1, t] <= MAX_STEER]\n", " x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,\n",
" #opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]\n", " u[0, t] >= -MAX_ACC,\n",
" \n", " u[0, t] <= MAX_ACC,\n",
" u[1, t] >= -MAX_STEER,\n",
" u[1, t] <= MAX_STEER,\n",
" ]\n",
" # opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]\n",
"\n",
" # Actuation rate of change\n", " # Actuation rate of change\n",
" if t < (time_horizon - 1):\n", " if t < (time_horizon - 1):\n",
" _cost += opt.quad_form(u[:,t + 1] - u[:,t], R * 1)\n", " _cost += opt.quad_form(u[:, t + 1] - u[:, t], R * 1)\n",
" _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",
" \n", "\n",
" # Minimize Problem\n", " # Minimize Problem\n",
" problem.solve(verbose=verbose, solver=opt.OSQP)\n", " problem.solve(verbose=verbose, solver=opt.OSQP)\n",
" return x, u" " return x, u"
@ -160,106 +180,111 @@
"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",
"def kinematics_model(x,t,u):\n", "\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" dxdt = x[2]*np.cos(x[3])\n",
" dydt = x[2]*np.sin(x[3])\n",
" dvdt = u[0]\n",
" dthetadt = x[2]*np.tan(u[1])/L\n",
"\n", "\n",
" dqdt = [dxdt,\n", " L = 0.3 # vehicle wheelbase\n",
" dydt,\n", " dxdt = x[2] * np.cos(x[3])\n",
" dvdt,\n", " dydt = x[2] * np.sin(x[3])\n",
" dthetadt]\n", " dvdt = u[0]\n",
" dthetadt = x[2] * np.tan(u[1]) / L\n",
"\n",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n", "\n",
" return dqdt\n", " return dqdt\n",
"\n", "\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x_ = np.zeros((N,T+1))\n",
" \n",
" x_[:,0] = x0\n",
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n", "\n",
" tspan = [0,DT]\n", "def predict(x0, u):\n",
" x_next = odeint(kinematics_model,\n", " \"\"\" \"\"\"\n",
" x0,\n", "\n",
" tspan,\n", " x_ = np.zeros((N, T + 1))\n",
" args=(u[:,t-1],))\n", "\n",
" x_[:, 0] = x0\n",
"\n",
" # solve ODE\n",
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, 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", "\n",
"\"\"\"\n", "\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\n", "MODIFIED TO INCLUDE FRAME TRANSFORMATION\n",
"\"\"\"\n", "\"\"\"\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", "\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n", "\n",
" for idx in range(len(start_xp)-1):\n", " final_xp = []\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", " final_yp = []\n",
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
" fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
" fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
"\n", "\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
" \n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" # watch out to duplicate points!\n", " # watch out to duplicate points!\n",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n", " final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n", " final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
" \n", "\n",
" dx = np.append(0, np.diff(final_xp))\n", " dx = np.append(0, np.diff(final_xp))\n",
" 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))\n",
"\n", "\n",
"\n", "\n",
"def get_nn_idx(state,path):\n", "def get_nn_idx(state, path):\n",
" \"\"\"\n", " \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n", " Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n", " \"\"\"\n",
"\n", "\n",
" dx = state[0]-path[0,:]\n", " dx = state[0] - path[0, :]\n",
" dy = state[1]-path[1,:]\n", " dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx,dy)\n", " dist = np.hypot(dx, dy)\n",
" 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",
" else:\n", " else:\n",
" target_idx = nn_idx+1\n", " target_idx = nn_idx + 1\n",
"\n", "\n",
" except IndexError as e:\n", " except IndexError as e:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
"\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,53 +297,53 @@
"\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",
" modified reference in robot frame\n", " modified reference in robot frame\n",
" \"\"\"\n", " \"\"\"\n",
" xref = np.zeros((N, T+1))\n", " xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T+1))\n", " dref = np.zeros((1, T + 1))\n",
" \n", "\n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n", " # sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n", "\n",
" ncourse = path.shape[1]\n", " ncourse = path.shape[1]\n",
"\n", "\n",
" ind = get_nn_idx(state, path)\n", " ind = get_nn_idx(state, path)\n",
" 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", " xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta\n",
" xref[3, 0] = normalize_angle(path[2,ind]- state[3]) #Theta\n", " dref[0, 0] = 0.0 # steer operational point should be 0\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n", "\n",
" \n", " dl = 0.05 # Waypoints spacing [m]\n",
" dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n", " travel = 0.0\n",
" \n", "\n",
" for i in range(1, T+1):\n", " for i in range(1, T + 1):\n",
" travel += abs(target_v) * DT #current V or target V?\n", " travel += abs(target_v) * DT # current V or target V?\n",
" dind = int(round(travel / dl))\n", " dind = int(round(travel / dl))\n",
" \n", "\n",
" if (ind + dind) < ncourse:\n", " if (ind + dind) < ncourse:\n",
" dx=path[0,ind + dind] - state[0]\n", " dx = path[0, ind + dind] - state[0]\n",
" dy=path[1,ind + dind] - state[1]\n", " dy = path[1, ind + dind] - state[1]\n",
" \n", "\n",
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n", " xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n", " xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
" xref[2, i] = target_v #sp[ind + dind]\n", " xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])\n", " xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])\n",
" dref[0, i] = 0.0\n", " dref[0, i] = 0.0\n",
" else:\n", " else:\n",
" dx=path[0,ncourse - 1] - state[0]\n", " dx = path[0, ncourse - 1] - state[0]\n",
" dy=path[1,ncourse - 1] - state[1]\n", " dy = path[1, ncourse - 1] - state[1]\n",
" \n", "\n",
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n", " xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n", " xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n", " xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])\n", " xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])\n",
" dref[0, i] = 0.0\n", " dref[0, i] = 0.0\n",
"\n", "\n",
" return xref, dref" " return xref, dref"
@ -338,78 +363,83 @@
} }
], ],
"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",
"\n", "\n",
"sim_duration = 200 #time steps\n", "sim_duration = 200 # time steps\n",
"opt_time=[]\n", "opt_time = []\n",
"x_sim = np.zeros((N,sim_duration))\n", "x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n", "u_sim = np.zeros((M, sim_duration - 1))\n",
"\n", "\n",
"# Starting Condition\n", "# Starting Condition\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n", "x0[0] = 0 # x\n",
"x0[1] = -0.25 #y\n", "x0[1] = -0.25 # y\n",
"x0[2] = 0.0 #v\n", "x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) #yaw\n", "x0[3] = np.radians(-0) # yaw\n",
"x_sim[:,0] = x0 #simulation_starting conditions\n", "x_sim[:, 0] = x0 # simulation_starting conditions\n",
"\n", "\n",
"#starting guess\n", "# starting guess\n",
"action = np.zeros(M)\n", "action = np.zeros(M)\n",
"action[0] = MAX_ACC/2 #a\n", "action[0] = MAX_ACC / 2 # a\n",
"action[1] = 0.0 #delta\n", "action[1] = 0.0 # delta\n",
"u_sim[:,0] = action\n", "u_sim[:, 0] = action\n",
"\n", "\n",
"# Cost Matrices\n", "# Cost Matrices\n",
"Q = np.diag([20,20,10,20]) #state error cost\n", "Q = np.diag([20, 20, 10, 20]) # state error cost\n",
"Qf = np.diag([30,30,30,30]) #state final error cost\n", "Qf = np.diag([30, 30, 30, 30]) # state final error cost\n",
"R = np.diag([10,10]) #input cost\n", "R = np.diag([10, 10]) # input cost\n",
"R_ = np.diag([10,10]) #input rate of change cost\n", "R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n", "\n",
"mpc = MPC(N,M,Q,R)\n", "mpc = MPC(N, M, Q, R)\n",
"REF_VEL = 1.0\n", "REF_VEL = 1.0\n",
"\n", "\n",
"for sim_time in range(sim_duration-1):\n", "for sim_time in range(sim_duration - 1):\n",
" \n", "\n",
" iter_start = time.time()\n", " iter_start = time.time()\n",
" \n", "\n",
" # dynamics starting state w.r.t. robot are always null except vel \n", " # dynamics starting state w.r.t. robot are always null except vel\n",
" start_state = np.array([0, 0, x_sim[2,sim_time], 0])\n", " start_state = np.array([0, 0, x_sim[2, sim_time], 0])\n",
" \n", "\n",
" # OPTIONAL: Add time delay to starting State- y\n", " # OPTIONAL: Add time delay to starting State- y\n",
" \n", "\n",
" current_action = np.array([u_sim[0,sim_time], u_sim[1,sim_time]])\n", " current_action = np.array([u_sim[0, sim_time], u_sim[1, sim_time]])\n",
" \n", "\n",
" # State Matrices\n", " # State Matrices\n",
" A,B,C = get_linear_model_matrices(start_state, current_action)\n", " A, B, C = get_linear_model_matrices(start_state, current_action)\n",
" \n", "\n",
" #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",
" \n", " A, B, C, start_state, target, time_horizon=T, verbose=False\n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n", " )\n",
" u_bar = np.vstack((np.array(u_mpc.value[0,:]).flatten(),\n", "\n",
" (np.array(u_mpc.value[1,:]).flatten())))\n", " # retrieved optimized U and assign to u_bar to linearize in next step\n",
" \n", " u_bar = np.vstack(\n",
" u_sim[:,sim_time] = u_bar[:,0]\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",
"\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", "print(\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.max(opt_time),\n", " np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" np.min(opt_time))) " " )\n",
")"
] ]
}, },
{ {
@ -444,33 +474,33 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n", "grid = plt.GridSpec(4, 5)\n",
"\n", "\n",
"plt.figure(figsize=(15,10))\n", "plt.figure(figsize=(15, 10))\n",
"\n", "\n",
"plt.subplot(grid[0:4, 0:4])\n", "plt.subplot(grid[0:4, 0:4])\n",
"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()"

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

File diff suppressed because it is too large Load Diff

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

@ -36,22 +36,23 @@
"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",
"\n", "\n",
" def line_intersection(line1, line2):\n", " def line_intersection(line1, line2):\n",
" xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])\n", " xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])\n",
" ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1]) #Typo was here\n", " ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1]) # Typo was here\n",
"\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",
@ -59,13 +60,13 @@
" return x, y\n", " return x, y\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",
" #shrink_value_x = 2\n", " # shrink_value_x = 2\n",
" #shrink_value_y = 2\n", " # shrink_value_y = 2\n",
"\n", "\n",
" # coords must be clockwise\n", " # coords must be clockwise\n",
" #coords = [(0, 0), (0, 100), (20, 100), (30, 60), (40, 100), (60, 100), (60, 0), (40, 10), (40, 40), (20, 40), (20, 10)]\n", " # coords = [(0, 0), (0, 100), (20, 100), (30, 60), (40, 100), (60, 100), (60, 0), (40, 10), (40, 40), (20, 40), (20, 10)]\n",
" \n", "\n",
" lines = [[coords[i-1], coords[i]] for i in range(len(coords))]\n", " lines = [[coords[i - 1], coords[i]] for i in range(len(coords))]\n",
"\n", "\n",
" new_lines = []\n", " new_lines = []\n",
" for i in lines:\n", " for i in lines:\n",
@ -73,46 +74,63 @@
" dy = i[1][1] - i[0][1]\n", " dy = i[1][1] - i[0][1]\n",
"\n", "\n",
" # this is to take into account slopes\n", " # this is to take into account slopes\n",
" factor = 1 / (dx*dx + dy*dy)**0.5\n", " factor = 1 / (dx * dx + dy * dy) ** 0.5\n",
" 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",
" for i in range(len(new_lines)):\n", " for i in range(len(new_lines)):\n",
" new_coords.append((line_intersection(new_lines[i-1], new_lines[i])))\n", " new_coords.append((line_intersection(new_lines[i - 1], new_lines[i])))\n",
" \n", "\n",
" return new_coords\n", " return new_coords\n",
"\n", "\n",
"#must be clockwise!\n", "\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", "# must be clockwise!\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",
"\n", "\n",
"up = shrink_polygon(coords, -SHIFT, -SHIFT)\n", "up = shrink_polygon(coords, -SHIFT, -SHIFT)\n",
"lo = shrink_polygon(coords, SHIFT, SHIFT)\n", "lo = shrink_polygon(coords, SHIFT, SHIFT)\n",
"\n", "\n",
"\"\"\"\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",
"up = rotate(up,1)\n", "\n",
"lo = rotate(lo,1)\n", "up = rotate(up, 1)\n",
"lo = rotate(lo, 1)\n",
"\n", "\n",
"a = np.array(up)\n", "a = np.array(up)\n",
"b = np.array(coords)\n", "b = np.array(coords)\n",
"c = np.array(lo)\n", "c = np.array(lo)\n",
"\n", "\n",
"plt.figure(figsize=(15,10))\n", "plt.figure(figsize=(15, 10))\n",
"plt.plot(a[:,0],a[:,1],\"b-\")\n", "plt.plot(a[:, 0], a[:, 1], \"b-\")\n",
"plt.plot(b[:,0],b[:,1],\"r-\")\n", "plt.plot(b[:, 0], b[:, 1], \"r-\")\n",
"plt.plot(c[:,0],c[:,1],\"g-\")\n", "plt.plot(c[:, 0], c[:, 1], \"g-\")\n",
"plt.axis(\"equal\")" "plt.axis(\"equal\")"
] ]
}, },
@ -131,37 +149,44 @@
"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",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1, smooth_factor=7):\n", "\n",
"\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",
" \"\"\"\n", " \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n", "\n",
" for idx in range(len(start_xp)-1):\n", " final_xp = []\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", " final_yp = []\n",
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp) - 1):\n",
" section_len = np.sum(\n",
" np.sqrt(\n",
" np.power(np.diff(start_xp[idx : idx + 2]), 2)\n",
" + np.power(np.diff(start_yp[idx : idx + 2]), 2)\n",
" )\n",
" )\n",
"\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\n",
"\n",
" fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
" fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
"\n", "\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
" \n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" # watch out to duplicate points!\n", " # watch out to duplicate points!\n",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n", " final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n", " final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
" \n", "\n",
" \"\"\"this smoothens up corners\"\"\"\n", " \"\"\"this smoothens up corners\"\"\"\n",
" window_size = smooth_factor # Smoothening filter window\n", " window_size = smooth_factor # Smoothening filter window\n",
" final_xp = savgol_filter(final_xp, window_size, 1)\n", " final_xp = savgol_filter(final_xp, window_size, 1)\n",
" final_yp = savgol_filter(final_yp, window_size, 1)\n", " final_yp = savgol_filter(final_yp, window_size, 1)\n",
" \n", "\n",
" dx = np.append(0, np.diff(final_xp))\n", " dx = np.append(0, np.diff(final_xp))\n",
" 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))" " return np.vstack((final_xp, final_yp, theta))"
] ]
}, },
{ {
@ -191,18 +216,18 @@
} }
], ],
"source": [ "source": [
"track = compute_path_from_wp(b[:,0],b[:,1])\n", "track = compute_path_from_wp(b[:, 0], b[:, 1])\n",
"track_lower = compute_path_from_wp(c[:,0],c[:,1])\n", "track_lower = compute_path_from_wp(c[:, 0], c[:, 1])\n",
"track_upper = compute_path_from_wp(a[:,0],a[:,1])\n", "track_upper = compute_path_from_wp(a[:, 0], a[:, 1])\n",
"\n", "\n",
"plt.figure(figsize=(15,10))\n", "plt.figure(figsize=(15, 10))\n",
"\n", "\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n", "plt.plot(track[0, :], track[1, :], \"b+\")\n",
"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\")"
] ]
}, },
{ {
@ -232,9 +257,9 @@
} }
], ],
"source": [ "source": [
"plt.plot(np.degrees(track[2,:]),\"b-\")\n", "plt.plot(np.degrees(track[2, :]), \"b-\")\n",
"plt.plot(np.degrees(track_lower[2,:]),\"g-\")\n", "plt.plot(np.degrees(track_lower[2, :]), \"g-\")\n",
"plt.plot(np.degrees(track_upper[2,:]),\"r-\")" "plt.plot(np.degrees(track_upper[2, :]), \"r-\")"
] ]
}, },
{ {
@ -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)"
] ]
}, },
{ {
@ -267,10 +292,10 @@
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"#matches perfectly!\n", "# matches perfectly!\n",
"#plt.plot(np.degrees(a[2,:]),\"b-\")\n", "# plt.plot(np.degrees(a[2,:]),\"b-\")\n",
"#plt.plot(np.degrees(b[2,:]),\"g-\")\n", "# plt.plot(np.degrees(b[2,:]),\"g-\")\n",
"#plt.plot(np.degrees(c[2,:]),\"r-\")" "# plt.plot(np.degrees(c[2,:]),\"r-\")"
] ]
}, },
{ {

25
notebooks/models/ackerman_model.ipynb
View File

@ -48,22 +48,19 @@
} }
], ],
"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",
"#A\n", "# A\n",
"A=gs.jacobian(X)\n", "A = gs.jacobian(X)\n",
"\n", "\n",
"U = sp.Matrix([a,delta])\n", "U = sp.Matrix([a, delta])\n",
"\n", "\n",
"#B\n", "# B\n",
"B=gs.jacobian(U)\n", "B = gs.jacobian(U)\n",
"display(A)\n", "display(A)\n",
"display(B)" "display(B)"
] ]
@ -132,9 +129,9 @@
"source": [ "source": [
"DT = sp.symbols(\"dt\")\n", "DT = sp.symbols(\"dt\")\n",
"\n", "\n",
"display(sp.eye(4)+A*DT)\n", "display(sp.eye(4) + A * DT)\n",
"display(B*DT)\n", "display(B * DT)\n",
"display(DT*(gs - A*X - B*U))" "display(DT * (gs - A * X - B * U))"
] ]
}, },
{ {

61
notebooks/models/differential_model.ipynb
View File

@ -36,17 +36,13 @@
"source": [ "source": [
"import sympy as sp\n", "import sympy as sp\n",
"\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",
"#A\n", "# A\n",
"gs.jacobian(state)" "gs.jacobian(state)"
] ]
}, },
@ -75,9 +71,9 @@
} }
], ],
"source": [ "source": [
"state = sp.Matrix([v,w])\n", "state = sp.Matrix([v, w])\n",
"\n", "\n",
"#B\n", "# B\n",
"gs.jacobian(state)" "gs.jacobian(state)"
] ]
}, },
@ -106,16 +102,16 @@
"source": [ "source": [
"import sympy as sp\n", "import sympy as sp\n",
"\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",
"#A\n", "# A\n",
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})" "gs.jacobian(state) # .subs({x:0,y:0,theta:0})"
] ]
}, },
{ {
@ -141,10 +137,10 @@
} }
], ],
"source": [ "source": [
"state = sp.Matrix([v,w])\n", "state = sp.Matrix([v, w])\n",
"\n", "\n",
"#B\n", "# B\n",
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})" "gs.jacobian(state) # .subs({x:0,y:0,theta:0})"
] ]
}, },
{ {
@ -195,22 +191,19 @@
} }
], ],
"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",
"#A\n", "# A\n",
"A=gs.jacobian(X)\n", "A = gs.jacobian(X)\n",
"\n", "\n",
"U = sp.Matrix([a,delta])\n", "U = sp.Matrix([a, delta])\n",
"\n", "\n",
"#B\n", "# B\n",
"B=gs.jacobian(U)\n", "B = gs.jacobian(U)\n",
"display(A)\n", "display(A)\n",
"display(B)" "display(B)"
] ]
@ -279,9 +272,9 @@
"source": [ "source": [
"DT = sp.symbols(\"dt\")\n", "DT = sp.symbols(\"dt\")\n",
"\n", "\n",
"display(sp.eye(4)+A*DT)\n", "display(sp.eye(4) + A * DT)\n",
"display(B*DT)\n", "display(B * DT)\n",
"display(DT*(gs - A*X - B*U))" "display(DT * (gs - A * X - B * U))"
] ]
}, },
{ {

90
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,35 +718,45 @@
"\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",
"print(\"Jacobian of the next state with respect to the previous state with 0 turn rate:\")\n", "print(\"Jacobian of the next state with respect to the previous state with 0 turn rate:\")\n",
"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,34 +1169,45 @@
"\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",
"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",
"\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))"
] ]
} }

102
notebooks/models/numerical_jacobian.ipynb
View File

@ -31,12 +31,12 @@
"# xy = x[1]\n", "# xy = x[1]\n",
"# v = x[2]\n", "# v = x[2]\n",
"# theta =x[3]\n", "# theta =x[3]\n",
" \n", "\n",
"# a = u[0]\n", "# a = u[0]\n",
"# delta = u[1]\n", "# delta = u[1]\n",
" \n", "\n",
"# 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",
"# np.cos(theta)*v,\n", "# np.cos(theta)*v,\n",
@ -45,7 +45,7 @@
"# v*np.arctan(delta)/L\n", "# v*np.arctan(delta)/L\n",
"# ])\n", "# ])\n",
"\n", "\n",
"#DISCRETE\n", "# DISCRETE\n",
"def f(x, u, dt=0.1):\n", "def f(x, u, dt=0.1):\n",
" \"\"\"\n", " \"\"\"\n",
" :param x:\n", " :param x:\n",
@ -53,55 +53,61 @@
" \"\"\"\n", " \"\"\"\n",
" xx = x[0]\n", " xx = x[0]\n",
" xy = x[1]\n", " xy = x[1]\n",
" v = x[2]\n", " v = x[2]\n",
" theta =x[3]\n", " theta = x[3]\n",
" \n", "\n",
" a = u[0]\n", " a = u[0]\n",
" delta = u[1]\n", " delta = u[1]\n",
" \n",
" L=0.3\n",
" \n",
" #vector of ackerman equations\n",
" return np.array([\n",
" xx + np.cos(theta)*v*dt,\n",
" xy + np.sin(theta)*v*dt,\n",
" v + a*dt,\n",
" theta + v*np.arctan(delta)/L*dt\n",
" ])\n",
"\n", "\n",
"def Jacobians(f,x,u,epsilon=1e-4):\n", " L = 0.3\n",
"\n",
" # vector of ackerman equations\n",
" return np.array(\n",
" [\n",
" xx + np.cos(theta) * v * dt,\n",
" xy + np.sin(theta) * v * dt,\n",
" v + a * dt,\n",
" theta + v * np.arctan(delta) / L * dt,\n",
" ]\n",
" )\n",
"\n",
"\n",
"def Jacobians(f, x, u, epsilon=1e-4):\n",
" \"\"\"\n", " \"\"\"\n",
" :param f:\n", " :param f:\n",
" :param x:\n", " :param x:\n",
" :param u:\n", " :param u:\n",
" \"\"\"\n", " \"\"\"\n",
" \n",
" jac_x = np.zeros((4,4))\n",
" jac_u = np.zeros((4,2))\n",
" \n",
" perturb_x = np.eye(4)*epsilon\n",
" perturb_u = np.eye(2)*epsilon\n",
" \n",
" #each row is state vector where one variable has been perturbed\n",
" x_perturbed_plus = np.tile(x,(4,1))+perturb_x\n",
" x_perturbed_minus = np.tile(x,(4,1))-perturb_x\n",
" \n",
" #each row is state vector where one variable has been perturbed\n",
" u_perturbed_plus = np.tile(u,(2,1))+perturb_u\n",
" u_perturbed_minus = np.tile(u,(2,1))-perturb_u\n",
" \n",
" for i in range(x.shape[0]):\n",
" \n",
" #each coloumn of the jac is given by perturbing a variable\n",
" jac_x[:,i]= (f(x+perturb_x[i,:], u)-f(x-perturb_x[i,:], u))/2*epsilon\n",
" \n",
" for i in range(u.shape[0]):\n",
" \n",
" #each coloumn of the jac is given by perturbing a variable\n",
" jac_u[:,i]= (f(x, u+perturb_u[i,:])-f(x, u-perturb_u[i,:]))/2*epsilon\n",
"\n", "\n",
" return jac_x, jac_u\n", " jac_x = np.zeros((4, 4))\n",
" " " jac_u = np.zeros((4, 2))\n",
"\n",
" perturb_x = np.eye(4) * epsilon\n",
" perturb_u = np.eye(2) * epsilon\n",
"\n",
" # each row is state vector where one variable has been perturbed\n",
" x_perturbed_plus = np.tile(x, (4, 1)) + perturb_x\n",
" x_perturbed_minus = np.tile(x, (4, 1)) - perturb_x\n",
"\n",
" # each row is state vector where one variable has been perturbed\n",
" u_perturbed_plus = np.tile(u, (2, 1)) + perturb_u\n",
" u_perturbed_minus = np.tile(u, (2, 1)) - perturb_u\n",
"\n",
" for i in range(x.shape[0]):\n",
"\n",
" # each coloumn of the jac is given by perturbing a variable\n",
" jac_x[:, i] = (\n",
" (f(x + perturb_x[i, :], u) - f(x - perturb_x[i, :], u)) / 2 * epsilon\n",
" )\n",
"\n",
" for i in range(u.shape[0]):\n",
"\n",
" # each coloumn of the jac is given by perturbing a variable\n",
" jac_u[:, i] = (\n",
" (f(x, u + perturb_u[i, :]) - f(x, u - perturb_u[i, :])) / 2 * epsilon\n",
" )\n",
"\n",
" return jac_x, jac_u"
] ]
}, },
{ {
@ -128,11 +134,11 @@
} }
], ],
"source": [ "source": [
"#starting condition\n", "# starting condition\n",
"x=np.array([0,0,1,0])\n", "x = np.array([0, 0, 1, 0])\n",
"u=np.array([1,0.2])\n", "u = np.array([1, 0.2])\n",
"\n", "\n",
"Jacobians(f,x,u)" "Jacobians(f, x, u)"
] ]
} }
], ],

681
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"
@ -181,10 +182,10 @@
"source": [ "source": [
"# Control problem statement.\n", "# Control problem statement.\n",
"\n", "\n",
"N = 3 #number of state variables\n", "N = 3 # number of state variables\n",
"M = 2 #number of control variables\n", "M = 2 # number of control variables\n",
"T = 20 #Prediction Horizon\n", "T = 20 # Prediction Horizon\n",
"dt = 0.25 #discretization step" "dt = 0.25 # discretization step"
] ]
}, },
{ {
@ -193,32 +194,33 @@
"metadata": {}, "metadata": {},
"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",
" theta = x_bar[2]\n", " theta = x_bar[2]\n",
" \n", "\n",
" v = u_bar[0]\n", " v = u_bar[0]\n",
" w = u_bar[1]\n", " w = u_bar[1]\n",
" \n", "\n",
" A = np.zeros((N,N))\n", " A = np.zeros((N, N))\n",
" A[0,2]=-v*np.sin(theta)\n", " A[0, 2] = -v * np.sin(theta)\n",
" A[1,2]=v*np.cos(theta)\n", " A[1, 2] = v * np.cos(theta)\n",
" A_lin=np.eye(N)+dt*A\n", " A_lin = np.eye(N) + dt * A\n",
" \n", "\n",
" B = np.zeros((N,M))\n", " B = np.zeros((N, M))\n",
" B[0,0]=np.cos(theta)\n", " B[0, 0] = np.cos(theta)\n",
" B[1,0]=np.sin(theta)\n", " B[1, 0] = np.sin(theta)\n",
" B[2,1]=1\n", " B[2, 1] = 1\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]).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",
" \n", " f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" return A_lin,B_lin,C_lin" " )\n",
"\n",
" return A_lin, B_lin, C_lin"
] ]
}, },
{ {
@ -235,41 +237,35 @@
"outputs": [], "outputs": [],
"source": [ "source": [
"# 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",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x_bar = np.zeros((N,T+1))\n",
" \n",
" x_bar[:,0] = x0\n",
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n", "\n",
" tspan = [0,dt]\n", "def predict(x0, u):\n",
" x_next = odeint(kinematics_model,\n", " \"\"\" \"\"\"\n",
" x0,\n", "\n",
" tspan,\n", " x_bar = np.zeros((N, T + 1))\n",
" args=(u[:,t-1],))\n", "\n",
" x_bar[:, 0] = x0\n",
"\n",
" # solve ODE\n",
" for t in range(1, T + 1):\n",
"\n",
" tspan = [0, dt]\n",
" x_next = odeint(kinematics_model, x0, tspan, 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",
" \n", "\n",
" return x_bar" " return x_bar"
] ]
}, },
@ -297,16 +293,16 @@
"source": [ "source": [
"%%time\n", "%%time\n",
"\n", "\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M, T))\n",
"u_bar[0,:] = 1 #m/s\n", "u_bar[0, :] = 1 # m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n", "u_bar[1, :] = np.radians(-10) # rad/s\n",
"\n", "\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0\n", "x0[0] = 0\n",
"x0[1] = 1\n", "x0[1] = 1\n",
"x0[2] = np.radians(0)\n", "x0[2] = np.radians(0)\n",
"\n", "\n",
"x_bar=predict(x0,u_bar)" "x_bar = predict(x0, u_bar)"
] ]
}, },
{ {
@ -333,18 +329,18 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"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",
"plt.tight_layout()\n", "plt.tight_layout()\n",
@ -375,27 +371,27 @@
"source": [ "source": [
"%%time\n", "%%time\n",
"\n", "\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M, T))\n",
"u_bar[0,:] = 1 #m/s\n", "u_bar[0, :] = 1 # m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n", "u_bar[1, :] = np.radians(-10) # rad/s\n",
"\n", "\n",
"x0 = np.zeros(N)\n", "x0 = np.zeros(N)\n",
"x0[0] = 0\n", "x0[0] = 0\n",
"x0[1] = 1\n", "x0[1] = 1\n",
"x0[2] = np.radians(0)\n", "x0[2] = np.radians(0)\n",
"\n", "\n",
"x_bar=np.zeros((N,T+1))\n", "x_bar = np.zeros((N, T + 1))\n",
"x_bar[:,0]=x0\n", "x_bar[:, 0] = x0\n",
"\n", "\n",
"for t in range (1,T+1):\n", "for t in range(1, T + 1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n", " xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n", " ut = u_bar[:, t - 1].reshape(M, 1)\n",
" \n", "\n",
" A,B,C=get_linear_model(xt,ut)\n", " A, B, C = get_linear_model(xt, ut)\n",
" \n", "\n",
" xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n", " xt_plus_one = np.dot(A, xt) + np.dot(B, ut) + C\n",
" \n", "\n",
" x_bar[:,t]= np.squeeze(xt_plus_one)" " x_bar[:, t] = np.squeeze(xt_plus_one)"
] ]
}, },
{ {
@ -415,18 +411,18 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"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",
"plt.tight_layout()\n", "plt.tight_layout()\n",
@ -464,74 +460,101 @@
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"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",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" final_xp=np.append(final_xp,fx(interp_range))\n",
" final_yp=np.append(final_yp,fy(interp_range))\n",
"\n", "\n",
" return np.vstack((final_xp,final_yp))\n", " fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)\n",
" fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)\n",
"\n", "\n",
"def get_nn_idx(state,path):\n", " final_xp = np.append(final_xp, fx(interp_range))\n",
" final_yp = np.append(final_yp, fy(interp_range))\n",
"\n", "\n",
" dx = state[0]-path[0,:]\n", " return np.vstack((final_xp, final_yp))\n",
" dy = state[1]-path[1,:]\n", "\n",
"\n",
"def get_nn_idx(state, path):\n",
"\n",
" dx = state[0] - path[0, :]\n",
" dy = state[1] - path[1, :]\n",
" dist = np.sqrt(dx**2 + dy**2)\n", " dist = np.sqrt(dx**2 + dy**2)\n",
" 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",
" else:\n", " else:\n",
" target_idx = nn_idx+1\n", " target_idx = nn_idx + 1\n",
"\n", "\n",
" except IndexError as e:\n", " except IndexError as e:\n",
" target_idx = nn_idx\n", " target_idx = nn_idx\n",
"\n", "\n",
" return target_idx\n", " return target_idx\n",
"\n", "\n",
"def road_curve(state,track):\n",
" \n",
" #given vehicle pos find lookahead waypoints\n",
" nn_idx=get_nn_idx(state,track)-1\n",
" LOOKAHED=6\n",
" lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n",
"\n", "\n",
" #trasform lookahead waypoints to vehicle ref frame\n", "def road_curve(state, track):\n",
" dx = lk_wp[0,:] - state[0]\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", " # given vehicle pos find lookahead waypoints\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n", " nn_idx = get_nn_idx(state, track) - 1\n",
" LOOKAHED = 6\n",
" lk_wp = track[:, nn_idx : nn_idx + LOOKAHED]\n",
"\n", "\n",
" #fit poly\n", " # trasform lookahead waypoints to vehicle ref frame\n",
" return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n", " dx = lk_wp[0, :] - state[0]\n",
" dy = lk_wp[1, :] - state[1]\n",
"\n",
" wp_vehicle_frame = np.vstack(\n",
" (\n",
" dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]),\n",
" )\n",
" )\n",
"\n",
" # fit poly\n",
" return np.polyfit(\n",
" wp_vehicle_frame[0, :],\n",
" wp_vehicle_frame[1, :],\n",
" 3,\n",
" rcond=None,\n",
" full=False,\n",
" w=None,\n",
" cov=False,\n",
" )\n",
"\n",
"\n",
"def f(x, coeff):\n",
" return round(\n",
" coeff[0] * x**3 + coeff[1] * x**2 + coeff[2] * x**1 + coeff[3] * x**0, 6\n",
" )\n",
"\n", "\n",
"def f(x,coeff):\n",
" return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\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",
"def df(x,coeff):\n", "\n",
" return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n", "def df(x, coeff):\n",
" return round(3 * coeff[0] * x**2 + 2 * coeff[1] * x**1 + coeff[2] * x**0, 6)\n",
"\n",
"\n",
"# def df(x,coeff):\n", "# 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)"
] ]
@ -698,34 +721,34 @@
"source": [ "source": [
"%%time\n", "%%time\n",
"\n", "\n",
"x = cp.Variable((N, T+1))\n", "x = cp.Variable((N, T + 1))\n",
"u = cp.Variable((M, T))\n", "u = cp.Variable((M, T))\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",
"\n", "\n",
"for t in range(T):\n", "for t in range(T):\n",
" \n", "\n",
" # Cost function\n", " # Cost function\n",
" #cost += 10*cp.sum_squares( x[3, t]) # psi\n", " # cost += 10*cp.sum_squares( x[3, t]) # psi\n",
" #cost += 500*cp.sum_squares( x[4, t]) # cte\n", " # cost += 500*cp.sum_squares( x[4, t]) # cte\n",
" \n", "\n",
" cost += 10*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", " cost += 10 * cp.sum_squares(x[2, t] - np.arctan(df(x_bar[0, t], K))) # psi\n",
" cost += 500*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", " cost += 500 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
" # Actuation effort\n", " # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n", " cost += cp.quad_form(u[:, t], 1 * np.eye(M))\n",
" \n", "\n",
" # 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], 100*np.eye(M))\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 100 * np.eye(M))\n",
" \n", "\n",
" # KINEMATICS constrains\n", " # KINEMATICS constrains\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",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
" \n", "\n",
"# sums problem objectives and concatenates constraints.\n", "# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", "constr += [x[:, 0] == x_bar[:, 0]] # <--watch out the start condition\n",
"constr += [u[0, :] <= MAX_SPEED]\n", "constr += [u[0, :] <= MAX_SPEED]\n",
"constr += [u[0, :] >= MIN_SPEED]\n", "constr += [u[0, :] >= MIN_SPEED]\n",
"constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", "constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
@ -751,35 +774,35 @@
} }
], ],
"source": [ "source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n", "x_mpc = np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n", "y_mpc = np.array(x.value[1, :]).flatten()\n",
"theta_mpc=np.array(x.value[2, :]).flatten()\n", "theta_mpc = np.array(x.value[2, :]).flatten()\n",
"v_mpc=np.array(u.value[0, :]).flatten()\n", "v_mpc = np.array(u.value[0, :]).flatten()\n",
"w_mpc=np.array(u.value[1, :]).flatten()\n", "w_mpc = np.array(u.value[1, :]).flatten()\n",
"\n", "\n",
"#simulate robot state trajectory for optimized U\n", "# simulate robot state trajectory for optimized U\n",
"x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n", "x_traj = predict(x0, np.vstack((v_mpc, w_mpc)))\n",
"\n", "\n",
"#plt.figure(figsize=(15,10))\n", "# plt.figure(figsize=(15,10))\n",
"#plot trajectory\n", "# plot trajectory\n",
"plt.subplot(2, 2, 1)\n", "plt.subplot(2, 2, 1)\n",
"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, 3)\n", "# plt.subplot(2, 2, 3)\n",
"# plt.plot(psi_mpc)\n", "# plt.plot(psi_mpc)\n",
@ -834,17 +857,16 @@
} }
], ],
"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",
"\n", "\n",
"sim_duration = 80 #time steps\n", "sim_duration = 80 # time steps\n",
"opt_time=[]\n", "opt_time = []\n",
"\n", "\n",
"x_sim = np.zeros((N,sim_duration))\n", "x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n", "u_sim = np.zeros((M, sim_duration - 1))\n",
"\n", "\n",
"MAX_SPEED = 1.25\n", "MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n", "MIN_SPEED = 0.75\n",
@ -855,83 +877,87 @@
"x0[0] = 0\n", "x0[0] = 0\n",
"x0[1] = -0.25\n", "x0[1] = -0.25\n",
"x0[2] = np.radians(-0)\n", "x0[2] = np.radians(-0)\n",
"x_sim[:,0]=x0\n", "x_sim[:, 0] = x0\n",
" \n", "\n",
"#starting guess\n", "# starting guess\n",
"u_bar = np.zeros((M,T))\n", "u_bar = np.zeros((M, T))\n",
"u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n", "u_bar[0, :] = 0.5 * (MAX_SPEED + MIN_SPEED)\n",
"u_bar[1,:]=0.00\n", "u_bar[1, :] = 0.00\n",
"\n",
"for sim_time in range(sim_duration - 1):\n",
"\n",
" iter_start = time.time()\n",
"\n",
" K = road_curve(x_sim[:, sim_time], track)\n",
"\n", "\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start=time.time()\n",
" \n",
" K=road_curve(x_sim[:,sim_time],track)\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",
" \n", "\n",
" #prediction for linearization of costrains\n", " # prediction for linearization of costrains\n",
" for t in range (1,T+1):\n", " for t in range(1, T + 1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n", " xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n", " ut = u_bar[:, t - 1].reshape(M, 1)\n",
" A,B,C=get_linear_model(xt,ut)\n", " A, B, C = get_linear_model(xt, ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
" x_bar[:,t]= xt_plus_one\n", " x_bar[:, t] = xt_plus_one\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",
" x = cp.Variable((N, T+1))\n", " x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n", " u = cp.Variable((M, T))\n",
" \n", "\n",
" #Prediction Horizon\n", " # Prediction Horizon\n",
" 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",
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", " x[2, t] - np.arctan2(df(x_bar[0, t], K), x_bar[0, t])\n",
" ) # psi\n",
" cost += 20 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
"\n", "\n",
" # 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], 100*np.eye(M))\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 100 * np.eye(M))\n",
" \n", "\n",
" # Actuation effort\n", " # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n", " cost += cp.quad_form(u[:, t], 1 * np.eye(M))\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",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
"\n", "\n",
" # sums problem objectives and concatenates constraints.\n", " # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", " constr += [x[:, 0] == x_bar[:, 0]] # <--watch out the start condition\n",
" constr += [u[0, :] <= MAX_SPEED]\n", " constr += [u[0, :] <= MAX_SPEED]\n",
" constr += [u[0, :] >= MIN_SPEED]\n", " constr += [u[0, :] >= MIN_SPEED]\n",
" constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\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.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",
" u_sim[:,sim_time] = u_bar[:,0]\n", "\n",
" \n", " u_sim[:, sim_time] = u_bar[:, 0]\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", "print(\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.max(opt_time),\n", " np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" np.min(opt_time))) " " )\n",
")"
] ]
}, },
{ {
@ -951,25 +977,25 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"grid = plt.GridSpec(2, 3)\n", "grid = plt.GridSpec(2, 3)\n",
"\n", "\n",
"plt.figure(figsize=(15,10))\n", "plt.figure(figsize=(15, 10))\n",
"\n", "\n",
"plt.subplot(grid[0:2, 0:2])\n", "plt.subplot(grid[0:2, 0:2])\n",
"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,27 +1046,31 @@
], ],
"source": [ "source": [
"import dccp\n", "import dccp\n",
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
" [0,0,2,4,3,3],0.25)\n",
"\n", "\n",
"obstacles=np.array([[4,6],[2,4]])\n", "track = compute_path_from_wp([0, 3, 4, 6, 10, 13], [0, 0, 2, 4, 3, 3], 0.25)\n",
"obstacle_radius=0.5\n",
"\n", "\n",
"def to_vehic_frame(pt,pos_x,pos_y,theta):\n", "obstacles = np.array([[4, 6], [2, 4]])\n",
"obstacle_radius = 0.5\n",
"\n",
"\n",
"def to_vehic_frame(pt, pos_x, pos_y, theta):\n",
" dx = pt[0] - pos_x\n", " 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",
" \n", " dy * np.cos(-theta) + dx * np.sin(-theta),\n",
" ]\n",
"\n",
"\n",
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", "# 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",
"\n", "\n",
"sim_duration = 80 #time steps\n", "sim_duration = 80 # time steps\n",
"opt_time=[]\n", "opt_time = []\n",
"\n", "\n",
"x_sim = np.zeros((N,sim_duration))\n", "x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n", "u_sim = np.zeros((M, sim_duration - 1))\n",
"\n", "\n",
"MAX_SPEED = 1.25\n", "MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n", "MIN_SPEED = 0.75\n",
@ -1051,93 +1081,104 @@
"x0[0] = 0\n", "x0[0] = 0\n",
"x0[1] = -0.25\n", "x0[1] = -0.25\n",
"x0[2] = np.radians(-0)\n", "x0[2] = np.radians(-0)\n",
"x_sim[:,0]=x0\n", "x_sim[:, 0] = x0\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n",
"u_bar[1,:]=0.00\n",
"\n", "\n",
"for sim_time in range(sim_duration-1):\n", "# starting guess\n",
" \n", "u_bar = np.zeros((M, T))\n",
" iter_start=time.time()\n", "u_bar[0, :] = 0.5 * (MAX_SPEED + MIN_SPEED)\n",
" \n", "u_bar[1, :] = 0.00\n",
" #compute coefficients\n", "\n",
" K=road_curve(x_sim[:,sim_time],track)\n", "for sim_time in range(sim_duration - 1):\n",
" \n", "\n",
" #compute opstacles in ref frame\n", " iter_start = time.time()\n",
" o_=[]\n", "\n",
" # compute coefficients\n",
" K = road_curve(x_sim[:, sim_time], track)\n",
"\n",
" # compute opstacles in ref frame\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",
" \n", " to_vehic_frame(\n",
" obstacles[:, j],\n",
" x_sim[0, sim_time],\n",
" x_sim[1, sim_time],\n",
" x_sim[2, sim_time],\n",
" )\n",
" )\n",
"\n",
" # dynamics starting state w.r.t vehicle frame\n", " # 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",
" \n", "\n",
" #prediction for linearization of costrains\n", " # prediction for linearization of costrains\n",
" for t in range (1,T+1):\n", " for t in range(1, T + 1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n", " xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n", " ut = u_bar[:, t - 1].reshape(M, 1)\n",
" A,B,C=get_linear_model(xt,ut)\n", " A, B, C = get_linear_model(xt, ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n",
" x_bar[:,t]= xt_plus_one\n", " x_bar[:, t] = xt_plus_one\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",
" x = cp.Variable((N, T+1))\n", " x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n", " u = cp.Variable((M, T))\n",
" \n", "\n",
" #Prediction Horizon\n", " # Prediction Horizon\n",
" 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",
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", " x[2, t] - np.arctan2(df(x_bar[0, t], K), x_bar[0, t])\n",
" ) # psi\n",
" cost += 20 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
"\n", "\n",
" # 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], 100*np.eye(M))\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 100 * np.eye(M))\n",
" \n", "\n",
" # Actuation effort\n", " # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n", " cost += cp.quad_form(u[:, t], 1 * np.eye(M))\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",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " constr += [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C.flatten()]\n",
" \n", "\n",
" # Obstacle Avoidance Contrains\n", " # Obstacle Avoidance Contrains\n",
" for j in range(2):\n", " for j in range(2):\n",
" constr += [ cp.norm(x[0:2,t]-o_[j],2) >= obstacle_radius ]\n", " constr += [cp.norm(x[0:2, t] - o_[j], 2) >= obstacle_radius]\n",
"\n", "\n",
" # sums problem objectives and concatenates constraints.\n", " # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", " constr += [x[:, 0] == x_bar[:, 0]] # <--watch out the start condition\n",
" constr += [u[0, :] <= MAX_SPEED]\n", " constr += [u[0, :] <= MAX_SPEED]\n",
" constr += [u[0, :] >= MIN_SPEED]\n", " constr += [u[0, :] >= MIN_SPEED]\n",
" constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\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(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",
" u_sim[:,sim_time] = u_bar[:,0]\n", "\n",
" \n", " u_sim[:, sim_time] = u_bar[:, 0]\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", "print(\n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.max(opt_time),\n", " np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" np.min(opt_time))) " " )\n",
")"
] ]
}, },
{ {
@ -1157,31 +1198,31 @@
} }
], ],
"source": [ "source": [
"#plot trajectory\n", "# plot trajectory\n",
"grid = plt.GridSpec(2, 3)\n", "grid = plt.GridSpec(2, 3)\n",
"\n", "\n",
"plt.figure(figsize=(15,10))\n", "plt.figure(figsize=(15, 10))\n",
"\n", "\n",
"ax=plt.subplot(grid[0:2, 0:2])\n", "ax = plt.subplot(grid[0:2, 0:2])\n",
"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",

706
notebooks/old_scipy_implementation/MPC_tracking_cvxpy.ipynb
View File

File diff suppressed because it is too large Load Diff