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

reformatted notebooks with black

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

1
env.yml
View File

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

327
mpc_pybullet_demo/racecar/racecar.py
View File

@ -8,59 +8,131 @@ p.connect(p.GUI)
p.resetSimulation()
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
useRealTimeSim = 0
p.setTimeStep(1./120.)
p.setRealTimeSimulation(useRealTimeSim) # either this
p.setTimeStep(1.0 / 120.0)
p.setRealTimeSimulation(useRealTimeSim) # either this
track = p.loadURDF("plane.urdf")
#track = p.loadSDF("f10_racecar/meshes/barca_track.sdf", globalScaling=1)
#otherCar = p.loadURDF("f10_racecar/racecar_differential.urdf", [0,1,.3])
car = p.loadURDF("f10_racecar/racecar_differential.urdf", [0,0,.3])
# track = p.loadSDF("f10_racecar/meshes/barca_track.sdf", globalScaling=1)
# otherCar = p.loadURDF("f10_racecar/racecar_differential.urdf", [0,1,.3])
car = p.loadURDF("f10_racecar/racecar_differential.urdf", [0, 0, 0.3])
for wheel in range(p.getNumJoints(car)):
print("joint[",wheel,"]=", p.getJointInfo(car,wheel))
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(car,wheel)
print("joint[", wheel, "]=", p.getJointInfo(car, wheel))
p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
p.getJointInfo(car, wheel)
wheels = [8,15]
wheels = [8, 15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
# 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],
)
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])
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],
)
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])
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],
)
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])
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],
)
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])
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],
)
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])
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],
)
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])
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)
c = p.createConstraint(
car,
1,
car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
c = p.createConstraint(
car,
3,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
)
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
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-2,2,0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -2, 2, 0)
maxForceSlider = p.addUserDebugParameter("maxForce", 0, 50, 20)
steeringSlider = p.addUserDebugParameter("steering", -1, 1, 0)
# replaceLines=True
# numRays=100
@ -81,115 +153,124 @@ steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
# 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):
# carPos,carOrn = p.getBasePositionAndOrientation(car)
# carEuler = p.getEulerFromQuaternion(carOrn)
# carYaw = carEuler[2]*360/(2.*math.pi)-90
# return carYaw
#prevCarYaw = getCarYaw(car)
# prevCarYaw = getCarYaw(car)
frame = 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])
lineId3= p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0])
print("lineId=",lineId)
lineId = 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])
print("lineId=", lineId)
#camInfo = p.getDebugVisualizerCamera()
# camInfo = p.getDebugVisualizerCamera()
lastTime = time.time()
lastControlTime = time.time()
#lastLidarTime = time.time()
# lastLidarTime = time.time()
def getCarVel(car):
linVel,angVel = p.getBaseVelocity(car)
return linVel[0]
linVel, angVel = p.getBaseVelocity(car)
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()
#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
nowControlTime = time.time()
#control at 100Hz
if (nowControlTime-lastControlTime>.01):
carPos,carOrn = p.getBasePositionAndOrientation(car)
nowLidarTime = 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
# 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]
# 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
# Keep the previous orientation of the camera set by the user.
#print("carYaw=", carYaw)
#print("camYaw=", yaw)
# yaw = camInfo[8]
# 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
#diffYaw = (carYaw-yaw)*0.03
# print("carYaw=", carYaw)
# print("camYaw=", yaw)
#track the position of the car as target
#p.resetDebugVisualizerCamera(distance, yaw+diffYaw, pitch, carPos)
# slowly rotate the camera behind the car
# diffYaw = (carYaw-yaw)*0.03
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity)
gearRatio=1./21
# track the position of the car as target
# p.resetDebugVisualizerCamera(distance, yaw+diffYaw, pitch, carPos)
for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity/gearRatio,force=maxForce)
maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider)
# print(targetVelocity)
gearRatio = 1.0 / 21
for steer in steering:
p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=steeringAngle)
for wheel in wheels:
p.setJointMotorControl2(
car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity / gearRatio,
force=maxForce,
)
if (useRealTimeSim==0):
frame+=1
p.stepSimulation()
lastControlTime=nowControlTime
for steer in steering:
p.setJointMotorControl2(
car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle
)
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",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -181,45 +182,39 @@
"outputs": [],
"source": [
"# Define process model\n",
"# This uses the continuous model \n",
"def kinematics_model(x,t,u):\n",
"# This uses the continuous model\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\n",
" \"\"\"\n",
" \n",
" 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",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\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",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\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",
" tspan = [0,DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"def predict(x0, u):\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",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:,t]=x_next[1]\n",
" \n",
" x_[:, t] = x_next[1]\n",
"\n",
" return x_"
]
},
@ -240,9 +235,9 @@
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 0.2 #m/ss\n",
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n",
"u_bar = np.zeros((M, T))\n",
"u_bar[0, :] = 0.2 # m/ss\n",
"u_bar[1, :] = np.radians(-np.pi / 4) # rad\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
@ -250,7 +245,7 @@
"x0[2] = 0\n",
"x0[3] = np.radians(0)\n",
"\n",
"x_bar=predict(x0,u_bar)"
"x_bar = predict(x0, u_bar)"
]
},
{
@ -270,18 +265,18 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2,:]))\n",
"plt.ylabel('theta(t) [deg]')\n",
"#plt.xlabel('time')\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel(\"theta(t) [deg]\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
"plt.tight_layout()\n",
@ -305,10 +300,10 @@
"Control problem statement.\n",
"\"\"\"\n",
"\n",
"N = 4 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"DT = 0.2 #discretization step"
"N = 4 # number of state variables\n",
"M = 2 # number of control variables\n",
"T = 20 # Prediction Horizon\n",
"DT = 0.2 # discretization step"
]
},
{
@ -317,38 +312,42 @@
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
" \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" \n",
"\n",
" L = 0.3 # vehicle wheelbase\n",
"\n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" v = x_bar[2]\n",
" theta = x_bar[3]\n",
" \n",
"\n",
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n",
" A[1,2]=np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n",
" A_lin=np.eye(N)+DT*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
" B[3,1]=v/(L*np.cos(delta)**2)\n",
" B_lin=DT*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" \n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)"
"\n",
" A = np.zeros((N, N))\n",
" A[0, 2] = np.cos(theta)\n",
" A[0, 3] = -v * np.sin(theta)\n",
" A[1, 2] = np.sin(theta)\n",
" A[1, 3] = v * np.cos(theta)\n",
" A[3, 2] = v * np.tan(delta) / L\n",
" A_lin = np.eye(N) + DT * A\n",
"\n",
" B = np.zeros((N, M))\n",
" B[2, 0] = 1\n",
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)"
]
},
{
@ -368,9 +367,9 @@
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 0.2 #m/s\n",
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n",
"u_bar = np.zeros((M, T))\n",
"u_bar[0, :] = 0.2 # m/s\n",
"u_bar[1, :] = np.radians(-np.pi / 4) # rad\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
@ -378,18 +377,18 @@
"x0[2] = 0\n",
"x0[3] = np.radians(0)\n",
"\n",
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"x_bar = np.zeros((N, T + 1))\n",
"x_bar[:, 0] = x0\n",
"\n",
"for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n",
" \n",
" x_bar[:,t]= np.squeeze(xt_plus_one)"
"for t in range(1, T + 1):\n",
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:, t - 1].reshape(M, 1)\n",
"\n",
" A, B, C = get_linear_model(xt, ut)\n",
"\n",
" xt_plus_one = np.dot(A, xt) + np.dot(B, ut) + C\n",
"\n",
" x_bar[:, t] = np.squeeze(xt_plus_one)"
]
},
{
@ -409,18 +408,18 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2,:]))\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
"plt.tight_layout()\n",

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

@ -23,43 +23,51 @@
"import numpy as np\n",
"from scipy.interpolate import interp1d\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" final_xp = []\n",
" final_yp = []\n",
" delta = step # [m]\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",
" 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",
" return np.vstack((final_xp,final_yp))\n",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\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",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" final_xp = np.append(final_xp, fx(interp_range))\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",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d,v) > 0:\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
" else:\n",
" target_idx = nn_idx+1\n",
" target_idx = nn_idx + 1\n",
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
@ -82,47 +90,70 @@
}
],
"source": [
"#define track\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",
"# define track\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",
"\n",
"#vehicle state\n",
"state=[3.5,0.5,np.radians(30)]\n",
"# vehicle state\n",
"state = [3.5, 0.5, np.radians(30)]\n",
"\n",
"#given vehicle pos find lookahead waypoints\n",
"nn_idx=get_nn_idx(state,track)-1 #index ox closest wp, take the previous to have a straighter line\n",
"LOOKAHED=6\n",
"lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n",
"# given vehicle pos find lookahead waypoints\n",
"nn_idx = (\n",
" get_nn_idx(state, track) - 1\n",
") # index ox closest wp, take the previous to have a straighter line\n",
"LOOKAHED = 6\n",
"lk_wp = track[:, nn_idx : nn_idx + LOOKAHED]\n",
"\n",
"#trasform lookahead waypoints to vehicle ref frame\n",
"dx = lk_wp[0,:] - state[0]\n",
"dy = lk_wp[1,:] - state[1]\n",
"# trasform lookahead waypoints to vehicle ref frame\n",
"dx = lk_wp[0, :] - state[0]\n",
"dy = lk_wp[1, :] - state[1]\n",
"\n",
"wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
"wp_vehicle_frame = np.vstack(\n",
" (\n",
" dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]),\n",
" )\n",
")\n",
"\n",
"#fit poly\n",
"coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
"# fit poly\n",
"coeff = np.polyfit(\n",
" wp_vehicle_frame[0, :],\n",
" wp_vehicle_frame[1, :],\n",
" 5,\n",
" rcond=None,\n",
" full=False,\n",
" w=None,\n",
" cov=False,\n",
")\n",
"\n",
"#def f(x,coeff):\n",
"# def f(x,coeff):\n",
"# return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n",
"def f(x,coeff):\n",
" return coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0\n",
"def f(x, coeff):\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",
"def f(x,coeff):\n",
" y=0\n",
" j=len(coeff)\n",
"\n",
"def f(x, coeff):\n",
" y = 0\n",
" j = len(coeff)\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",
"\n",
"\n",
"# def df(x,coeff):\n",
"# return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
"def df(x,coeff):\n",
" y=0\n",
" j=len(coeff)\n",
" for k in range(j-1):\n",
" y += (j-k-1)*coeff[k]*x**(j-k-2)\n",
"def df(x, coeff):\n",
" y = 0\n",
" j = len(coeff)\n",
" for k in range(j - 1):\n",
" y += (j - k - 1) * coeff[k] * x ** (j - k - 2)\n",
" return y"
]
},
@ -165,29 +196,30 @@
],
"source": [
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"x=np.arange(-1,2,0.001) #interp range of curve \n",
"x = np.arange(-1, 2, 0.001) # interp range of curve\n",
"\n",
"# VEHICLE REF FRAME\n",
"plt.subplot(2,1,1)\n",
"plt.title('parametrized curve, vehicle ref frame')\n",
"plt.scatter(0,0)\n",
"plt.scatter(wp_vehicle_frame[0,:],wp_vehicle_frame[1,:])\n",
"plt.plot(x,[f(xs,coeff) for xs in x])\n",
"plt.axis('equal')\n",
"plt.subplot(2, 1, 1)\n",
"plt.title(\"parametrized curve, vehicle ref frame\")\n",
"plt.scatter(0, 0)\n",
"plt.scatter(wp_vehicle_frame[0, :], wp_vehicle_frame[1, :])\n",
"plt.plot(x, [f(xs, coeff) for xs in x])\n",
"plt.axis(\"equal\")\n",
"\n",
"# MAP REF FRAME\n",
"plt.subplot(2,1,2)\n",
"plt.title('waypoints, map ref frame')\n",
"plt.scatter(state[0],state[1])\n",
"plt.scatter(track[0,:],track[1,:])\n",
"plt.scatter(track[0,nn_idx:nn_idx+LOOKAHED],track[1,nn_idx:nn_idx+LOOKAHED])\n",
"plt.axis('equal')\n",
"plt.subplot(2, 1, 2)\n",
"plt.title(\"waypoints, map ref frame\")\n",
"plt.scatter(state[0], state[1])\n",
"plt.scatter(track[0, :], track[1, :])\n",
"plt.scatter(track[0, nn_idx : nn_idx + LOOKAHED], track[1, nn_idx : nn_idx + LOOKAHED])\n",
"plt.axis(\"equal\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()\n",
"#plt.savefig(\"fitted_poly\")"
"# plt.savefig(\"fitted_poly\")"
]
},
{
@ -249,20 +281,24 @@
"outputs": [],
"source": [
"def spline_planning(qs, qf, ts, tf, dqs=0.0, dqf=0.0, ddqs=0.0, ddqf=0.0):\n",
" \n",
" bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T \n",
" \n",
" C = np.array([[1, xs, xs**2, xs**3, xs**4, xs**5], #f(xs)=ys\n",
" [0, 1, 2*xs**1, 3*xs**2, 4*xs**3, 5**xs^4], #df(xs)=dys\n",
" [0, 0, 1, 6*xs**1, 12*xs**2, 20**xs^3], #ddf(xs)=ddys\n",
" [1, xf, xf**2, xf**3, xf**4, xf**5], #f(xf)=yf\n",
" [0, 1, 2*xf**1, 3*xf**2, 4*xf**3, 5**xf^4], #df(xf)=dyf\n",
" [0, 0, 1, 6*xf**1, 12*xf**2, 20**xf^3]]) #ddf(xf)=ddyf\n",
" \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",
"\n",
" bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T\n",
"\n",
" C = np.array(\n",
" [\n",
" [1, xs, xs**2, xs**3, xs**4, xs**5], # f(xs)=ys\n",
" [0, 1, 2 * xs**1, 3 * xs**2, 4 * xs**3, 5**xs ^ 4], # df(xs)=dys\n",
" [0, 0, 1, 6 * xs**1, 12 * xs**2, 20**xs ^ 3], # ddf(xs)=ddys\n",
" [1, xf, xf**2, xf**3, xf**4, xf**5], # f(xf)=yf\n",
" [0, 1, 2 * xf**1, 3 * xf**2, 4 * xf**3, 5**xf ^ 4], # df(xf)=dyf\n",
" [0, 0, 1, 6 * xf**1, 12 * xf**2, 20**xf ^ 3],\n",
" ]\n",
" ) # ddf(xf)=ddyf\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"
]
}

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

@ -125,6 +125,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -140,180 +141,190 @@
"Control problem statement.\n",
"\"\"\"\n",
"\n",
"N = 4 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"DT = 0.2 #discretization step\n",
"N = 4 # number of state variables\n",
"M = 2 # number of control variables\n",
"T = 20 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"def get_linear_model(x_bar,u_bar):\n",
"\n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
" \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" \n",
"\n",
" L = 0.3 # vehicle wheelbase\n",
"\n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" v = x_bar[2]\n",
" theta = x_bar[3]\n",
" \n",
"\n",
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n",
" A[1,2]=np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n",
" A_lin=np.eye(N)+DT*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
" B[3,1]=v/(L*np.cos(delta)**2)\n",
" B_lin=DT*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" \n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n",
"\n",
" A = np.zeros((N, N))\n",
" A[0, 2] = np.cos(theta)\n",
" A[0, 3] = -v * np.sin(theta)\n",
" A[1, 2] = np.sin(theta)\n",
" A[1, 3] = v * np.cos(theta)\n",
" A[3, 2] = v * np.tan(delta) / L\n",
" A_lin = np.eye(N) + DT * A\n",
"\n",
" B = np.zeros((N, M))\n",
" B[2, 0] = 1\n",
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"def kinematics_model(x,t,u):\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\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",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\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",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\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",
" tspan = [0,DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"def predict(x0, u):\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",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:,t]=x_next[1]\n",
" \n",
" x_[:, t] = x_next[1]\n",
"\n",
" return x_\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",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" final_xp = []\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",
" 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",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))\n",
" return np.vstack((final_xp, final_yp, theta))\n",
"\n",
"\n",
"def get_nn_idx(state,path):\n",
"def get_nn_idx(state, path):\n",
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.hypot(dx,dy)\n",
" dx = state[0] - path[0, :]\n",
" dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx, dy)\n",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d,v) > 0:\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
" else:\n",
" target_idx = nn_idx+1\n",
" target_idx = nn_idx + 1\n",
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
"\n",
" return target_idx\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" Adapted from pythonrobotics\n",
" \"\"\"\n",
" xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
"\n",
" # sp = np.ones((1,T +1))*target_v #speed profile\n",
"\n",
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_idx(state, path)\n",
"\n",
" xref[0, 0] = path[0,ind] #X\n",
" xref[1, 0] = path[1,ind] #Y\n",
" xref[2, 0] = target_v #sp[ind] #V\n",
" xref[3, 0] = path[2,ind] #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n",
" dl = 0.05 # Waypoints spacing [m]\n",
" xref[0, 0] = path[0, ind] # X\n",
" xref[1, 0] = path[1, ind] # Y\n",
" xref[2, 0] = target_v # sp[ind] #V\n",
" xref[3, 0] = path[2, ind] # Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
"\n",
" dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n",
"\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",
"\n",
" if (ind + dind) < ncourse:\n",
" xref[0, i] = path[0,ind + dind]\n",
" xref[1, i] = path[1,ind + dind]\n",
" xref[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = path[2,ind + dind]\n",
" xref[0, i] = path[0, ind + dind]\n",
" xref[1, i] = path[1, ind + dind]\n",
" xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = path[2, ind + dind]\n",
" dref[0, i] = 0.0\n",
" else:\n",
" xref[0, i] = path[0,ncourse - 1]\n",
" xref[1, i] = path[1,ncourse - 1]\n",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
" xref[3, i] = path[2,ncourse - 1]\n",
" xref[0, i] = path[0, ncourse - 1]\n",
" xref[1, i] = path[1, ncourse - 1]\n",
" xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = path[2, ncourse - 1]\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
@ -372,79 +383,78 @@
"source": [
"%%time\n",
"\n",
"MAX_SPEED = 1.5 #m/s\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_SPEED = 1.5 # m/s\n",
"MAX_STEER = np.radians(30) # rad\n",
"MAX_ACC = 1.0\n",
"REF_VEL=1.0\n",
"REF_VEL = 1.0\n",
"\n",
"track = compute_path_from_wp([0,3,6],\n",
" [0,0,0],0.05)\n",
"track = compute_path_from_wp([0, 3, 6], [0, 0, 0], 0.05)\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.1 #delta\n",
"x0[0] = 0 # x\n",
"x0[1] = -0.25 # y\n",
"x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) # yaw\n",
"\n",
"# starting guess\n",
"u_bar = np.zeros((M, T))\n",
"u_bar[0, :] = MAX_ACC / 2 # a\n",
"u_bar[1, :] = 0.1 # delta\n",
"\n",
"# dynamics starting state w.r.t world frame\n",
"x_bar = np.zeros((N,T+1))\n",
"x_bar[:,0] = x0\n",
"x_bar = np.zeros((N, T + 1))\n",
"x_bar[:, 0] = x0\n",
"\n",
"#prediction for linearization of costrains\n",
"for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\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",
" x_bar[:,t] = xt_plus_one\n",
"# prediction for linearization of costrains\n",
"for t in range(1, T + 1):\n",
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:, t - 1].reshape(M, 1)\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",
" x_bar[:, t] = xt_plus_one\n",
"\n",
"#CVXPY Linear MPC problem statement\n",
"x = cp.Variable((N, T+1))\n",
"# CVXPY Linear MPC problem statement\n",
"x = cp.Variable((N, T + 1))\n",
"u = cp.Variable((M, T))\n",
"cost = 0\n",
"constr = []\n",
"\n",
"# Cost Matrices\n",
"Q = np.diag([10,10,10,10]) #state 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 rate of change 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",
"R = np.diag([10, 10]) # input cost\n",
"R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n",
"#Get Reference_traj\n",
"x_ref, d_ref = get_ref_trajectory(x_bar[:,0], track, REF_VEL)\n",
"# Get Reference_traj\n",
"x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
"\n",
"#Prediction Horizon\n",
"# Prediction Horizon\n",
"for t in range(T):\n",
" \n",
"\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",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
" \n",
" cost += cp.quad_form(u[:, t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\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",
" 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",
"\n",
"#Final Point tracking\n",
"# Final Point tracking\n",
"cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
"# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
"constr += [x[2,:] <= MAX_SPEED] #max speed\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[1,:]) <= MAX_STEER] #max steer\n",
"constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
"constr += [x[2, :] <= MAX_SPEED] # max speed\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[1, :]) <= MAX_STEER] # max steer\n",
"# for t in range(T):\n",
"# if t < (T - 1):\n",
"# constr += [cp.abs(u[0,t] - u[0,t-1])/DT <= MAX_ACC] #max acc\n",
@ -471,40 +481,40 @@
}
],
"source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n",
"v_mpc=np.array(x.value[2, :]).flatten()\n",
"theta_mpc=np.array(x.value[3, :]).flatten()\n",
"a_mpc=np.array(u.value[0, :]).flatten()\n",
"delta_mpc=np.array(u.value[1, :]).flatten()\n",
"x_mpc = np.array(x.value[0, :]).flatten()\n",
"y_mpc = np.array(x.value[1, :]).flatten()\n",
"v_mpc = np.array(x.value[2, :]).flatten()\n",
"theta_mpc = np.array(x.value[3, :]).flatten()\n",
"a_mpc = np.array(u.value[0, :]).flatten()\n",
"delta_mpc = np.array(u.value[1, :]).flatten()\n",
"\n",
"#simulate robot state trajectory for optimized U\n",
"x_traj=predict(x0, np.vstack((a_mpc,delta_mpc)))\n",
"# simulate robot state trajectory for optimized U\n",
"x_traj = predict(x0, np.vstack((a_mpc, delta_mpc)))\n",
"\n",
"#plt.figure(figsize=(15,10))\n",
"#plot trajectory\n",
"# plt.figure(figsize=(15,10))\n",
"# plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(track[0,:],track[1,:],\"b\")\n",
"plt.plot(x_ref[0,:],x_ref[1,:],\"g+\")\n",
"plt.plot(x_traj[0,:],x_traj[1,:])\n",
"plt.plot(track[0, :], track[1, :], \"b\")\n",
"plt.plot(x_ref[0, :], x_ref[1, :], \"g+\")\n",
"plt.plot(x_traj[0, :], x_traj[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"#plot v(t)\n",
"# plot v(t)\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(a_mpc)\n",
"plt.ylabel('a_in(t)')\n",
"#plt.xlabel('time')\n",
"plt.ylabel(\"a_in(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(theta_mpc)\n",
"plt.ylabel('theta(t)')\n",
"plt.ylabel(\"theta(t)\")\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(delta_mpc)\n",
"plt.ylabel('d_in(t)')\n",
"plt.ylabel(\"d_in(t)\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -539,121 +549,128 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
"\n",
"sim_duration = 200 #time steps\n",
"opt_time=[]\n",
"sim_duration = 200 # time steps\n",
"opt_time = []\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M, sim_duration - 1))\n",
"\n",
"MAX_SPEED = 1.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"MAX_SPEED = 1.5 # m/s\n",
"MAX_ACC = 1.0 # m/ss\n",
"MAX_D_ACC = 1.0 # m/sss\n",
"MAX_STEER = np.radians(30) # rad\n",
"MAX_D_STEER = np.radians(30) # rad/s\n",
"\n",
"REF_VEL = 1.0 #m/s\n",
"REF_VEL = 1.0 # m/s\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.0 #delta\n",
"x0[0] = 0 # x\n",
"x0[1] = -0.25 # y\n",
"x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) # yaw\n",
"\n",
"# starting guess\n",
"u_bar = np.zeros((M, T))\n",
"u_bar[0, :] = MAX_ACC / 2 # a\n",
"u_bar[1, :] = 0.0 # delta\n",
"\n",
"for sim_time in range(sim_duration - 1):\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" \n",
"\n",
" # dynamics starting state\n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[:,0] = x_sim[:,sim_time]\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\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",
" x_bar[:,t] = xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" x_bar = np.zeros((N, T + 1))\n",
" x_bar[:, 0] = x_sim[:, sim_time]\n",
"\n",
" # prediction for linearization of costrains\n",
" for t in range(1, T + 1):\n",
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:, t - 1].reshape(M, 1)\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",
" x_bar[:, t] = xt_plus_one\n",
"\n",
" # CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,0]) #state 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 rate of change 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",
" R = np.diag([10, 10]) # input cost\n",
" R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n",
" \n",
" #Prediction Horizon\n",
" # Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
"\n",
" # Prediction Horizon\n",
" for t in range(T):\n",
"\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",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
" cost += cp.quad_form(u[:, t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
" \n",
" #Final Point tracking\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",
"\n",
" # Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\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[1,:]) <= MAX_STEER] #max steer\n",
" \n",
" constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
" constr += [x[2, :] <= MAX_SPEED] # max speed\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[1, :]) <= MAX_STEER] # max steer\n",
"\n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" opt_time.append(time.time() - iter_start)\n",
"\n",
" # move simulation to t+1\n",
" tspan = [0,DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
" tspan = [0, DT]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -673,33 +690,33 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"plt.figure(figsize=(15, 10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"

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

@ -30,6 +30,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -45,180 +46,190 @@
"Control problem statement.\n",
"\"\"\"\n",
"\n",
"N = 4 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"DT = 0.2 #discretization step\n",
"N = 4 # number of state variables\n",
"M = 2 # number of control variables\n",
"T = 20 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"def get_linear_model(x_bar,u_bar):\n",
"\n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
" \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" \n",
"\n",
" L = 0.3 # vehicle wheelbase\n",
"\n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" v = x_bar[2]\n",
" theta = x_bar[3]\n",
" \n",
"\n",
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n",
" A[1,2]=np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n",
" A_lin=np.eye(N)+DT*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
" B[3,1]=v/(L*np.cos(delta)**2)\n",
" B_lin=DT*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" \n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n",
"\n",
" A = np.zeros((N, N))\n",
" A[0, 2] = np.cos(theta)\n",
" A[0, 3] = -v * np.sin(theta)\n",
" A[1, 2] = np.sin(theta)\n",
" A[1, 3] = v * np.cos(theta)\n",
" A[3, 2] = v * np.tan(delta) / L\n",
" A_lin = np.eye(N) + DT * A\n",
"\n",
" B = np.zeros((N, M))\n",
" B[2, 0] = 1\n",
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"def kinematics_model(x,t,u):\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\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",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\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",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\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",
" tspan = [0,DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"def predict(x0, u):\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",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:,t]=x_next[1]\n",
" \n",
" x_[:, t] = x_next[1]\n",
"\n",
" return x_\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",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" final_xp = []\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",
" 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",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))\n",
" return np.vstack((final_xp, final_yp, theta))\n",
"\n",
"\n",
"def get_nn_idx(state,path):\n",
"def get_nn_idx(state, path):\n",
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.hypot(dx,dy)\n",
" dx = state[0] - path[0, :]\n",
" dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx, dy)\n",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d,v) > 0:\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
" else:\n",
" target_idx = nn_idx+1\n",
" target_idx = nn_idx + 1\n",
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
"\n",
" return target_idx\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" Adapted from pythonrobotics\n",
" \"\"\"\n",
" xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
"\n",
" # sp = np.ones((1,T +1))*target_v #speed profile\n",
"\n",
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_idx(state, path)\n",
"\n",
" xref[0, 0] = path[0,ind] #X\n",
" xref[1, 0] = path[1,ind] #Y\n",
" xref[2, 0] = target_v #sp[ind] #V\n",
" xref[3, 0] = path[2,ind] #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n",
" dl = 0.05 # Waypoints spacing [m]\n",
" xref[0, 0] = path[0, ind] # X\n",
" xref[1, 0] = path[1, ind] # Y\n",
" xref[2, 0] = target_v # sp[ind] #V\n",
" xref[3, 0] = path[2, ind] # Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
"\n",
" dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n",
"\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",
"\n",
" if (ind + dind) < ncourse:\n",
" xref[0, i] = path[0,ind + dind]\n",
" xref[1, i] = path[1,ind + dind]\n",
" xref[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = path[2,ind + dind]\n",
" xref[0, i] = path[0, ind + dind]\n",
" xref[1, i] = path[1, ind + dind]\n",
" xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = path[2, ind + dind]\n",
" dref[0, i] = 0.0\n",
" else:\n",
" xref[0, i] = path[0,ncourse - 1]\n",
" xref[1, i] = path[1,ncourse - 1]\n",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
" xref[3, i] = path[2,ncourse - 1]\n",
" xref[0, i] = path[0, ncourse - 1]\n",
" xref[1, i] = path[1, ncourse - 1]\n",
" xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = path[2, ncourse - 1]\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
@ -246,138 +257,143 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
"\n",
"sim_duration = 200 #time steps\n",
"opt_time=[]\n",
"sim_duration = 200 # time steps\n",
"opt_time = []\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M, sim_duration - 1))\n",
"\n",
"MAX_SPEED = 1.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"MAX_SPEED = 1.5 # m/s\n",
"MAX_ACC = 1.0 # m/ss\n",
"MAX_D_ACC = 1.0 # m/sss\n",
"MAX_STEER = np.radians(30) # rad\n",
"MAX_D_STEER = np.radians(30) # rad/s\n",
"\n",
"REF_VEL = 1.0 #m/s\n",
"REF_VEL = 1.0 # m/s\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
"x0[0] = 0 # x\n",
"x0[1] = -0.25 # y\n",
"x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) # yaw\n",
"\n",
"for sim_time in range(sim_duration - 1):\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" \n",
" #starting guess for ctrl\n",
" u_bar = np.zeros((M,T))\n",
" u_bar[0,:] = MAX_ACC/2 #a\n",
" u_bar[1,:] = 0.0 #delta \n",
" \n",
"\n",
" # starting guess for ctrl\n",
" u_bar = np.zeros((M, T))\n",
" u_bar[0, :] = MAX_ACC / 2 # a\n",
" u_bar[1, :] = 0.0 # delta\n",
"\n",
" for _ in range(5):\n",
" u_prev = u_bar\n",
" \n",
"\n",
" # dynamics starting state\n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[:,0] = x_sim[:,sim_time]\n",
" x_bar = np.zeros((N, T + 1))\n",
" x_bar[:, 0] = x_sim[:, sim_time]\n",
"\n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\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",
" x_bar[:,t] = xt_plus_one\n",
" # prediction for linearization of costrains\n",
" for t in range(1, T + 1):\n",
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:, t - 1].reshape(M, 1)\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",
" x_bar[:, t] = xt_plus_one\n",
"\n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" # CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,0]) #state 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 rate of change 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",
" R = np.diag([10, 10]) # input cost\n",
" R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n",
" # Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:, 0], track, REF_VEL)\n",
"\n",
" #Prediction Horizon\n",
" # Prediction Horizon\n",
" for t in range(T):\n",
"\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",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
" cost += cp.quad_form(u[:, t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\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",
"\n",
" #Final Point tracking\n",
" # Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\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[1,:]) <= MAX_STEER] #max steer\n",
" constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
" constr += [x[2, :] <= MAX_SPEED] # max speed\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[1, :]) <= MAX_STEER] # max steer\n",
"\n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
"\n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" \n",
" #check how this solution differs from previous\n",
" #if the solutions are very\n",
" delta_u = np.sum(np.sum(np.abs(u_bar - u_prev),axis=0),axis=0)\n",
" if delta_u < 0.05:\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" # check how this solution differs from previous\n",
" # if the solutions are very\n",
" 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",
" \n",
" \n",
"\n",
" # select u from best iteration\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",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" opt_time.append(time.time() - iter_start)\n",
"\n",
" # move simulation to t+1\n",
" tspan = [0,DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
" #reset u_bar? -> this simulates that we don use previous solution!\n",
" u_bar[0,:] = MAX_ACC/2 #a\n",
" u_bar[1,:] = 0.0 #delta\n",
" \n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
" tspan = [0, DT]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
" # reset u_bar? -> this simulates that we don use previous solution!\n",
" u_bar[0, :] = MAX_ACC / 2 # a\n",
" u_bar[1, :] = 0.0 # delta\n",
"\n",
"\n",
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -397,33 +413,33 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"plt.figure(figsize=(15, 10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"

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

@ -12,6 +12,7 @@
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -40,148 +41,159 @@
"Control problem statement.\n",
"\"\"\"\n",
"\n",
"N = 4 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"DT = 0.2 #discretization step\n",
"N = 4 # number of state variables\n",
"M = 2 # number of control variables\n",
"T = 20 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"def get_linear_model(x_bar,u_bar):\n",
"\n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
" \"\"\"\n",
" \n",
" L=0.3 #vehicle wheelbase\n",
" \n",
"\n",
" L = 0.3 # vehicle wheelbase\n",
"\n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" v = x_bar[2]\n",
" theta = x_bar[3]\n",
" \n",
"\n",
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n",
" A[1,2]=np.sin(theta)\n",
" A[1,3]=v*np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n",
" A_lin=np.eye(N)+DT*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
" B[3,1]=v/(L*np.cos(delta)**2)\n",
" B_lin=DT*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" \n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)\n",
"\n",
" A = np.zeros((N, N))\n",
" A[0, 2] = np.cos(theta)\n",
" A[0, 3] = -v * np.sin(theta)\n",
" A[1, 2] = np.sin(theta)\n",
" A[1, 3] = v * np.cos(theta)\n",
" A[3, 2] = v * np.tan(delta) / L\n",
" A_lin = np.eye(N) + DT * A\n",
"\n",
" B = np.zeros((N, M))\n",
" B[2, 0] = 1\n",
" B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu = np.array(\n",
" [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
" ).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)\n",
"\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"def kinematics_model(x,t,u):\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\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",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\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",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\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",
" tspan = [0,DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"def predict(x0, u):\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",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:,t]=x_next[1]\n",
" \n",
" x_[:, t] = x_next[1]\n",
"\n",
" return x_\n",
"\n",
"\n",
"\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\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",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" final_xp = []\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",
" 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",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n",
" \n",
" final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
" final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
"\n",
" dx = np.append(0, np.diff(final_xp))\n",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))\n",
" return np.vstack((final_xp, final_yp, theta))\n",
"\n",
"\n",
"def get_nn_idx(state,path):\n",
"def get_nn_idx(state, path):\n",
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.hypot(dx,dy)\n",
" dx = state[0] - path[0, :]\n",
" dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx, dy)\n",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d,v) > 0:\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
" else:\n",
" target_idx = nn_idx+1\n",
" target_idx = nn_idx + 1\n",
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
"\n",
" return target_idx\n",
"\n",
"\n",
"def normalize_angle(angle):\n",
" \"\"\"\n",
" Normalize an angle to [-pi, pi]\n",
@ -194,51 +206,52 @@
"\n",
" return angle\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" modified reference in robot frame\n",
" \"\"\"\n",
" xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
"\n",
" # sp = np.ones((1,T +1))*target_v #speed profile\n",
"\n",
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_idx(state, path)\n",
" dx=path[0,ind] - state[0]\n",
" dy=path[1,ind] - state[1]\n",
" \n",
" xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X\n",
" xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y\n",
" xref[2, 0] = target_v #V\n",
" xref[3, 0] = normalize_angle(path[2,ind]- state[3]) #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n",
" dl = 0.05 # Waypoints spacing [m]\n",
" dx = path[0, ind] - state[0]\n",
" dy = path[1, ind] - state[1]\n",
"\n",
" xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X\n",
" xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y\n",
" xref[2, 0] = target_v # V\n",
" xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
"\n",
" dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n",
" \n",
"\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",
" \n",
"\n",
" if (ind + dind) < ncourse:\n",
" dx=path[0,ind + dind] - state[0]\n",
" dy=path[1,ind + dind] - state[1]\n",
" \n",
" dx = path[0, ind + dind] - state[0]\n",
" dy = path[1, ind + dind] - state[1]\n",
"\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[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])\n",
" xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])\n",
" dref[0, i] = 0.0\n",
" else:\n",
" dx=path[0,ncourse - 1] - state[0]\n",
" dy=path[1,ncourse - 1] - state[1]\n",
" \n",
" dx = path[0, ncourse - 1] - state[0]\n",
" dy = path[1, ncourse - 1] - state[1]\n",
"\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[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])\n",
" xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
@ -258,122 +271,129 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
"\n",
"sim_duration = 200 #time steps\n",
"opt_time=[]\n",
"sim_duration = 200 # time steps\n",
"opt_time = []\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M, sim_duration - 1))\n",
"\n",
"MAX_SPEED = 1.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"MAX_SPEED = 1.5 # m/s\n",
"MAX_ACC = 1.0 # m/ss\n",
"MAX_D_ACC = 1.0 # m/sss\n",
"MAX_STEER = np.radians(30) # rad\n",
"MAX_D_STEER = np.radians(30) # rad/s\n",
"\n",
"REF_VEL = 1.0 #m/s\n",
"REF_VEL = 1.0 # m/s\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.0 #delta\n",
" \n",
"for sim_time in range(sim_duration-1):\n",
" \n",
"x0[0] = 0 # x\n",
"x0[1] = -0.25 # y\n",
"x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) # yaw\n",
"\n",
"# starting guess\n",
"u_bar = np.zeros((M, T))\n",
"u_bar[0, :] = MAX_ACC / 2 # a\n",
"u_bar[1, :] = 0.0 # delta\n",
"\n",
"for sim_time in range(sim_duration - 1):\n",
"\n",
" iter_start = time.time()\n",
" \n",
" # dynamics starting state w.r.t. robot are always null except vel \n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[2,0] = x_sim[2,sim_time]\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\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",
" x_bar[:,t] = xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
"\n",
" # dynamics starting state w.r.t. robot are always null except vel\n",
" x_bar = np.zeros((N, T + 1))\n",
" x_bar[2, 0] = x_sim[2, sim_time]\n",
"\n",
" # prediction for linearization of costrains\n",
" for t in range(1, T + 1):\n",
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:, t - 1].reshape(M, 1)\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",
" x_bar[:, t] = xt_plus_one\n",
"\n",
" # CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,20]) #state 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 rate of change 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",
" R = np.diag([10, 10]) # input cost\n",
" R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" #dont use x0 in this case\n",
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
" \n",
" #Prediction Horizon\n",
" # Get Reference_traj\n",
" # dont use x0 in this case\n",
" x_ref, d_ref = get_ref_trajectory(x_sim[:, sim_time], track, REF_VEL)\n",
"\n",
" # Prediction Horizon\n",
" for t in range(T):\n",
"\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",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
" cost += cp.quad_form(u[:, t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
" constr += [\n",
" cp.abs(u[0, t + 1] - u[0, t]) / DT <= MAX_D_ACC\n",
" ] # max acc rate of change\n",
" constr += [\n",
" cp.abs(u[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER\n",
" ] # max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
" \n",
" #Final Point tracking\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",
"\n",
" # Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\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[1,:]) <= MAX_STEER] #max steer\n",
" \n",
" constr += [x[:, 0] == x_bar[:, 0]] # starting condition\n",
" constr += [x[2, :] <= MAX_SPEED] # max speed\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[1, :]) <= MAX_STEER] # max steer\n",
"\n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" opt_time.append(time.time() - iter_start)\n",
"\n",
" # move simulation to t+1\n",
" tspan = [0,DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
" tspan = [0, DT]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -393,33 +413,33 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"plt.figure(figsize=(15, 10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"

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

@ -25,73 +25,87 @@
"from scipy.integrate import odeint\n",
"from scipy.interpolate import interp1d\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"N = 4 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 10 #Prediction Horizon\n",
"DT = 0.2 #discretization step\n",
"N = 4 # number of state variables\n",
"M = 2 # number of control variables\n",
"T = 10 # Prediction Horizon\n",
"DT = 0.2 # discretization step\n",
"\n",
"L = 0.3 #vehicle wheelbase\n",
"MAX_SPEED = 1.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"L = 0.3 # vehicle wheelbase\n",
"MAX_SPEED = 1.5 # m/s\n",
"MAX_ACC = 1.0 # m/ss\n",
"MAX_D_ACC = 1.0 # m/sss\n",
"MAX_STEER = np.radians(30) # rad\n",
"MAX_D_STEER = np.radians(30) # rad/s\n",
"\n",
"def get_linear_model_matrices(x_bar,u_bar):\n",
"\n",
"def get_linear_model_matrices(x_bar, u_bar):\n",
" \"\"\"\n",
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
" \"\"\"\n",
" \n",
"\n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" v = x_bar[2]\n",
" theta = x_bar[3]\n",
" \n",
"\n",
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
" \n",
"\n",
" ct = np.cos(theta)\n",
" st = np.sin(theta)\n",
" cd = np.cos(delta)\n",
" td = np.tan(delta)\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2] = ct\n",
" A[0,3] = -v*st\n",
" A[1,2] = st\n",
" A[1,3] = v*ct\n",
" A[3,2] = v*td/L\n",
" A_lin = np.eye(N)+DT*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
" B[3,1]=v/(L*cd**2)\n",
" B_lin=DT*B\n",
" \n",
" f_xu=np.array([v*ct, v*st, a, v*td/L]).reshape(N,1)\n",
" C_lin = DT*(f_xu - np.dot(A, x_bar.reshape(N,1)) - np.dot(B, u_bar.reshape(M,1)))#.flatten()\n",
" \n",
" #return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)\n",
"\n",
" A = np.zeros((N, N))\n",
" A[0, 2] = ct\n",
" A[0, 3] = -v * st\n",
" A[1, 2] = st\n",
" A[1, 3] = v * ct\n",
" A[3, 2] = v * td / L\n",
" A_lin = np.eye(N) + DT * A\n",
"\n",
" B = np.zeros((N, M))\n",
" B[2, 0] = 1\n",
" B[3, 1] = v / (L * cd**2)\n",
" B_lin = DT * B\n",
"\n",
" f_xu = np.array([v * ct, v * st, a, v * td / L]).reshape(N, 1)\n",
" C_lin = DT * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" ) # .flatten()\n",
"\n",
" # return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)\n",
" return A_lin, B_lin, C_lin\n",
"\n",
"class MPC():\n",
" \n",
"\n",
"class MPC:\n",
" def __init__(self, N, M, Q, R):\n",
" \"\"\"\n",
" \"\"\"\n",
" \"\"\" \"\"\"\n",
" self.state_len = N\n",
" self.action_len = M\n",
" self.state_cost = Q\n",
" self.action_cost = R\n",
" \n",
" def optimize_linearized_model(self, A, B, C, initial_state, target, time_horizon=10, Q=None, R=None, verbose=False):\n",
"\n",
" def optimize_linearized_model(\n",
" self,\n",
" A,\n",
" B,\n",
" C,\n",
" initial_state,\n",
" target,\n",
" time_horizon=10,\n",
" Q=None,\n",
" R=None,\n",
" verbose=False,\n",
" ):\n",
" \"\"\"\n",
" Optimisation problem defined for the linearised model,\n",
" :param A: \n",
" :param A:\n",
" :param B:\n",
" :param C: \n",
" :param C:\n",
" :param initial_state:\n",
" :param Q:\n",
" :param R:\n",
@ -100,47 +114,53 @@
" :param verbose:\n",
" :return:\n",
" \"\"\"\n",
" \n",
"\n",
" assert len(initial_state) == self.state_len\n",
" \n",
" if (Q == None or R==None):\n",
"\n",
" if Q == None or R == None:\n",
" Q = self.state_cost\n",
" R = self.action_cost\n",
" \n",
"\n",
" # Create variables\n",
" x = opt.Variable((self.state_len, time_horizon + 1), name='states')\n",
" u = opt.Variable((self.action_len, time_horizon), name='actions')\n",
" x = opt.Variable((self.state_len, time_horizon + 1), name=\"states\")\n",
" u = opt.Variable((self.action_len, time_horizon), name=\"actions\")\n",
"\n",
" # Loop through the entire time_horizon and append costs\n",
" cost_function = []\n",
"\n",
" for t in range(time_horizon):\n",
"\n",
" _cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) +\\\n",
" opt.quad_form(u[:, t], R)\n",
" \n",
" _constraints = [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,\n",
" u[0, t] >= -MAX_ACC, u[0, t] <= MAX_ACC,\n",
" u[1, t] >= -MAX_STEER, u[1, t] <= MAX_STEER]\n",
" #opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]\n",
" \n",
" _cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) + opt.quad_form(\n",
" u[:, t], R\n",
" )\n",
"\n",
" _constraints = [\n",
" x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,\n",
" u[0, t] >= -MAX_ACC,\n",
" u[0, t] <= MAX_ACC,\n",
" u[1, t] >= -MAX_STEER,\n",
" u[1, t] <= MAX_STEER,\n",
" ]\n",
" # opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]\n",
"\n",
" # Actuation rate of change\n",
" if t < (time_horizon - 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[1, t + 1] - u[1, t])/DT <= MAX_D_STEER]\n",
" \n",
" \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[1, t + 1] - u[1, t]) / DT <= MAX_D_STEER]\n",
"\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",
" _constraints += [x[:, 0] == initial_state]\n",
" \n",
" cost_function.append(opt.Problem(opt.Minimize(_cost), constraints=_constraints))\n",
"\n",
" cost_function.append(\n",
" opt.Problem(opt.Minimize(_cost), constraints=_constraints)\n",
" )\n",
"\n",
" # Add final cost\n",
" problem = sum(cost_function)\n",
" \n",
"\n",
" # Minimize Problem\n",
" problem.solve(verbose=verbose, solver=opt.OSQP)\n",
" return x, u"
@ -160,106 +180,111 @@
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\n",
"def kinematics_model(x,t,u):\n",
"\n",
"\n",
"def kinematics_model(x, t, u):\n",
" \"\"\"\n",
" Returns the set of ODE of the vehicle model.\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",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\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",
" dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
"\n",
" return dqdt\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",
" tspan = [0,DT]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"def predict(x0, u):\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",
" tspan = [0, DT]\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_[:,t]=x_next[1]\n",
" \n",
" x_[:, t] = x_next[1]\n",
"\n",
" return x_\n",
"\n",
"\n",
"\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\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",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" final_xp = []\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",
" 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",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n",
" \n",
" final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
" final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
"\n",
" dx = np.append(0, np.diff(final_xp))\n",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))\n",
" return np.vstack((final_xp, final_yp, theta))\n",
"\n",
"\n",
"def get_nn_idx(state,path):\n",
"def get_nn_idx(state, path):\n",
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.hypot(dx,dy)\n",
" dx = state[0] - path[0, :]\n",
" dy = state[1] - path[1, :]\n",
" dist = np.hypot(dx, dy)\n",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d,v) > 0:\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
" else:\n",
" target_idx = nn_idx+1\n",
" target_idx = nn_idx + 1\n",
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
"\n",
" return target_idx\n",
"\n",
"\n",
"def normalize_angle(angle):\n",
" \"\"\"\n",
" Normalize an angle to [-pi, pi]\n",
@ -272,53 +297,53 @@
"\n",
" return angle\n",
"\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" For each step in the time horizon\n",
" modified reference in robot frame\n",
" \"\"\"\n",
" xref = np.zeros((N, T+1))\n",
" dref = np.zeros((1, T+1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
" xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
"\n",
" # sp = np.ones((1,T +1))*target_v #speed profile\n",
"\n",
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_idx(state, path)\n",
" dx=path[0,ind] - state[0]\n",
" dy=path[1,ind] - state[1]\n",
" \n",
" \n",
" xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X\n",
" xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y\n",
" xref[2, 0] = target_v #V\n",
" xref[3, 0] = normalize_angle(path[2,ind]- state[3]) #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n",
" dl = 0.05 # Waypoints spacing [m]\n",
" dx = path[0, ind] - state[0]\n",
" dy = path[1, ind] - state[1]\n",
"\n",
" xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X\n",
" xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y\n",
" xref[2, 0] = target_v # V\n",
" xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
"\n",
" dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n",
" \n",
" for i in range(1, T+1):\n",
" travel += abs(target_v) * DT #current V or target V?\n",
"\n",
" for i in range(1, T + 1):\n",
" travel += abs(target_v) * DT # current V or target V?\n",
" dind = int(round(travel / dl))\n",
" \n",
"\n",
" if (ind + dind) < ncourse:\n",
" dx=path[0,ind + dind] - state[0]\n",
" dy=path[1,ind + dind] - state[1]\n",
" \n",
" dx = path[0, ind + dind] - state[0]\n",
" dy = path[1, ind + dind] - state[1]\n",
"\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[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])\n",
" xref[2, i] = target_v # sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])\n",
" dref[0, i] = 0.0\n",
" else:\n",
" dx=path[0,ncourse - 1] - state[0]\n",
" dy=path[1,ncourse - 1] - state[1]\n",
" \n",
" dx = path[0, ncourse - 1] - state[0]\n",
" dy = path[1, ncourse - 1] - state[1]\n",
"\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[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])\n",
" xref[2, i] = 0.0 # stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
@ -338,78 +363,83 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"track = compute_path_from_wp(\n",
" [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n",
")\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
"\n",
"sim_duration = 200 #time steps\n",
"opt_time=[]\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"sim_duration = 200 # time steps\n",
"opt_time = []\n",
"x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M, sim_duration - 1))\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
"x_sim[:,0] = x0 #simulation_starting conditions\n",
"x0[0] = 0 # x\n",
"x0[1] = -0.25 # y\n",
"x0[2] = 0.0 # v\n",
"x0[3] = np.radians(-0) # yaw\n",
"x_sim[:, 0] = x0 # simulation_starting conditions\n",
"\n",
"#starting guess\n",
"# starting guess\n",
"action = np.zeros(M)\n",
"action[0] = MAX_ACC/2 #a\n",
"action[1] = 0.0 #delta\n",
"u_sim[:,0] = action\n",
"action[0] = MAX_ACC / 2 # a\n",
"action[1] = 0.0 # delta\n",
"u_sim[:, 0] = action\n",
"\n",
"# Cost Matrices\n",
"Q = np.diag([20,20,10,20]) #state 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 rate of change 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",
"R = np.diag([10, 10]) # input cost\n",
"R_ = np.diag([10, 10]) # input rate of change cost\n",
"\n",
"mpc = MPC(N,M,Q,R)\n",
"mpc = MPC(N, M, Q, R)\n",
"REF_VEL = 1.0\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
"for sim_time in range(sim_duration - 1):\n",
"\n",
" iter_start = time.time()\n",
" \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",
" \n",
"\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",
"\n",
" # OPTIONAL: Add time delay to starting State- y\n",
" \n",
" current_action = np.array([u_sim[0,sim_time], u_sim[1,sim_time]])\n",
" \n",
"\n",
" current_action = np.array([u_sim[0, sim_time], u_sim[1, sim_time]])\n",
"\n",
" # State Matrices\n",
" A,B,C = get_linear_model_matrices(start_state, current_action)\n",
" \n",
" #Get Reference_traj -> inputs are in worldframe\n",
" target, _ = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
" \n",
" x_mpc, u_mpc = mpc.optimize_linearized_model(A, B, C, start_state, target, time_horizon=T, verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u_mpc.value[0,:]).flatten(),\n",
" (np.array(u_mpc.value[1,:]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
" A, B, C = get_linear_model_matrices(start_state, current_action)\n",
"\n",
" # Get Reference_traj -> inputs are in worldframe\n",
" target, _ = get_ref_trajectory(x_sim[:, sim_time], track, REF_VEL)\n",
"\n",
" x_mpc, u_mpc = mpc.optimize_linearized_model(\n",
" A, B, C, start_state, target, time_horizon=T, verbose=False\n",
" )\n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack(\n",
" (np.array(u_mpc.value[0, :]).flatten(), (np.array(u_mpc.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" opt_time.append(time.time() - iter_start)\n",
"\n",
" # move simulation to t+1\n",
" tspan = [0,DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
" tspan = [0, DT]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -444,33 +474,33 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"plt.figure(figsize=(15, 10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel(\"a(t) [m/ss]\")\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.plot(x_sim[2, :])\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel(\"delta(t) [rad]\")\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"plt.plot(x_sim[3, :])\n",
"plt.ylabel(\"theta(t) [rad]\")\n",
"\n",
"plt.tight_layout()\n",
"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": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"\n",
"def shrink_polygon(coords, shrink_value_x, shrink_value_y):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" \"\"\" \"\"\"\n",
"\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",
" def line_intersection(line1, line2):\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",
" div = det(xdiff, ydiff)\n",
" if div == 0:\n",
" raise Exception('lines do not intersect')\n",
" raise Exception(\"lines do not intersect\")\n",
"\n",
" d = (det(*line1), det(*line2))\n",
" x = det(d, xdiff) / div\n",
@ -59,13 +60,13 @@
" return x, y\n",
"\n",
" # how much the coordinates are moved as an absolute value\n",
" #shrink_value_x = 2\n",
" #shrink_value_y = 2\n",
" # shrink_value_x = 2\n",
" # shrink_value_y = 2\n",
"\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",
" \n",
" lines = [[coords[i-1], coords[i]] for i in range(len(coords))]\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",
" lines = [[coords[i - 1], coords[i]] for i in range(len(coords))]\n",
"\n",
" new_lines = []\n",
" for i in lines:\n",
@ -73,46 +74,63 @@
" dy = i[1][1] - i[0][1]\n",
"\n",
" # this is to take into account slopes\n",
" factor = 1 / (dx*dx + dy*dy)**0.5\n",
" new_dx = dy*shrink_value_x * factor\n",
" new_dy = dx*shrink_value_y * factor\n",
" factor = 1 / (dx * dx + dy * dy) ** 0.5\n",
" new_dx = dy * shrink_value_x * factor\n",
" new_dy = dx * shrink_value_y * factor\n",
"\n",
" new_lines.append([(i[0][0] + new_dx, i[0][1] - new_dy),\n",
" (i[1][0] + new_dx, i[1][1] - new_dy)])\n",
" new_lines.append(\n",
" [(i[0][0] + new_dx, i[0][1] - new_dy), (i[1][0] + new_dx, i[1][1] - new_dy)]\n",
" )\n",
"\n",
" # find position of intersection of all the lines\n",
" new_coords = []\n",
" for i in range(len(new_lines)):\n",
" new_coords.append((line_intersection(new_lines[i-1], new_lines[i])))\n",
" \n",
" new_coords.append((line_intersection(new_lines[i - 1], new_lines[i])))\n",
"\n",
" return new_coords\n",
"\n",
"#must be clockwise!\n",
"coords = [(0, 0), (0, 10), (2, 10), (3, 8), (4, 10), (6, 10), (6, 0), (4, 1), (4, 4), (2, 4), (2, 1)]\n",
"\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",
"# how much the coordinates are moved as an absolute value\n",
"SHIFT = 0.5 #[m]\n",
"SHIFT = 0.5 # [m]\n",
"\n",
"up = shrink_polygon(coords, -SHIFT, -SHIFT)\n",
"lo = shrink_polygon(coords, SHIFT, SHIFT)\n",
"up = shrink_polygon(coords, -SHIFT, -SHIFT)\n",
"lo = shrink_polygon(coords, SHIFT, SHIFT)\n",
"\n",
"\"\"\"\n",
"the last point is out of order for my needs\n",
"\"\"\"\n",
"\n",
"\n",
"def rotate(l, n):\n",
" return l[n:] + l[:n]\n",
"\n",
"up = rotate(up,1)\n",
"lo = rotate(lo,1)\n",
"\n",
"up = rotate(up, 1)\n",
"lo = rotate(lo, 1)\n",
"\n",
"a = np.array(up)\n",
"b = np.array(coords)\n",
"c = np.array(lo)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"plt.plot(a[:,0],a[:,1],\"b-\")\n",
"plt.plot(b[:,0],b[:,1],\"r-\")\n",
"plt.plot(c[:,0],c[:,1],\"g-\")\n",
"plt.figure(figsize=(15, 10))\n",
"plt.plot(a[:, 0], a[:, 1], \"b-\")\n",
"plt.plot(b[:, 0], b[:, 1], \"r-\")\n",
"plt.plot(c[:, 0], c[:, 1], \"g-\")\n",
"plt.axis(\"equal\")"
]
},
@ -131,37 +149,44 @@
"source": [
"from scipy.interpolate import interp1d\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",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" final_xp = []\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",
" 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",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n",
" \n",
" final_xp = np.append(final_xp, fx(interp_range)[1:])\n",
" final_yp = np.append(final_yp, fy(interp_range)[1:])\n",
"\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_yp = savgol_filter(final_yp, window_size, 1)\n",
" \n",
"\n",
" dx = np.append(0, np.diff(final_xp))\n",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))"
" return np.vstack((final_xp, final_yp, theta))"
]
},
{
@ -191,18 +216,18 @@
}
],
"source": [
"track = compute_path_from_wp(b[:,0],b[:,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 = compute_path_from_wp(b[:, 0], b[:, 1])\n",
"track_lower = compute_path_from_wp(c[:, 0], c[:, 1])\n",
"track_upper = compute_path_from_wp(a[:, 0], a[:, 1])\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"plt.figure(figsize=(15, 10))\n",
"\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(track_lower[0,:],track_lower[1,:],\"g-\")\n",
"plt.plot(track_upper[0,:],track_upper[1,:],\"r-\")\n",
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(track_lower[0, :], track_lower[1, :], \"g-\")\n",
"plt.plot(track_upper[0, :], track_upper[1, :], \"r-\")\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')"
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")"
]
},
{
@ -232,9 +257,9 @@
}
],
"source": [
"plt.plot(np.degrees(track[2,:]),\"b-\")\n",
"plt.plot(np.degrees(track_lower[2,:]),\"g-\")\n",
"plt.plot(np.degrees(track_upper[2,:]),\"r-\")"
"plt.plot(np.degrees(track[2, :]), \"b-\")\n",
"plt.plot(np.degrees(track_lower[2, :]), \"g-\")\n",
"plt.plot(np.degrees(track_upper[2, :]), \"r-\")"
]
},
{
@ -250,15 +275,15 @@
"metadata": {},
"outputs": [],
"source": [
"with open('tracks/test.npy', 'wb') as f:\n",
"with open(\"tracks/test.npy\", \"wb\") as f:\n",
" np.save(f, track)\n",
" np.save(f, track_lower)\n",
" np.save(f, track_upper)\n",
"\n",
"with open('tracks/test.npy', 'rb') as f:\n",
"with open(\"tracks/test.npy\", \"rb\") as f:\n",
" a = np.load(f)\n",
" b = np.load(f)\n",
" c = np.load(f)\n"
" c = np.load(f)"
]
},
{
@ -267,10 +292,10 @@
"metadata": {},
"outputs": [],
"source": [
"#matches perfectly!\n",
"#plt.plot(np.degrees(a[2,:]),\"b-\")\n",
"#plt.plot(np.degrees(b[2,:]),\"g-\")\n",
"#plt.plot(np.degrees(c[2,:]),\"r-\")"
"# matches perfectly!\n",
"# plt.plot(np.degrees(a[2,:]),\"b-\")\n",
"# plt.plot(np.degrees(b[2,:]),\"g-\")\n",
"# plt.plot(np.degrees(c[2,:]),\"r-\")"
]
},
{

25
notebooks/models/ackerman_model.ipynb
View File

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

61
notebooks/models/differential_model.ipynb
View File

@ -36,17 +36,13 @@
"source": [
"import sympy as sp\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",
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
" [ sp.sin(theta)*v],\n",
" [w],\n",
" [-w],\n",
" [ v*sp.sin(psi)]])\n",
"gs = sp.Matrix([[sp.cos(theta) * v], [sp.sin(theta) * v], [w], [-w], [v * sp.sin(psi)]])\n",
"\n",
"state = sp.Matrix([x,y,theta,psi,cte])\n",
"state = sp.Matrix([x, y, theta, psi, cte])\n",
"\n",
"#A\n",
"# A\n",
"gs.jacobian(state)"
]
},
@ -75,9 +71,9 @@
}
],
"source": [
"state = sp.Matrix([v,w])\n",
"state = sp.Matrix([v, w])\n",
"\n",
"#B\n",
"# B\n",
"gs.jacobian(state)"
]
},
@ -106,16 +102,16 @@
"source": [
"import sympy as sp\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",
"gs = sp.Matrix([[x + sp.cos(theta)*v*dt],\n",
" [y+ sp.sin(theta)*v*dt],\n",
" [theta + w*dt]])\n",
"gs = sp.Matrix(\n",
" [[x + sp.cos(theta) * v * dt], [y + sp.sin(theta) * v * dt], [theta + w * dt]]\n",
")\n",
"\n",
"state = sp.Matrix([x,y,theta])\n",
"state = sp.Matrix([x, y, theta])\n",
"\n",
"#A\n",
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
"# A\n",
"gs.jacobian(state) # .subs({x:0,y:0,theta:0})"
]
},
{
@ -141,10 +137,10 @@
}
],
"source": [
"state = sp.Matrix([v,w])\n",
"state = sp.Matrix([v, w])\n",
"\n",
"#B\n",
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
"# B\n",
"gs.jacobian(state) # .subs({x:0,y:0,theta:0})"
]
},
{
@ -195,22 +191,19 @@
}
],
"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",
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
" [ sp.sin(theta)*v],\n",
" [a],\n",
" [ v*sp.tan(delta)/L]])\n",
"gs = sp.Matrix([[sp.cos(theta) * v], [sp.sin(theta) * v], [a], [v * sp.tan(delta) / L]])\n",
"\n",
"X = sp.Matrix([x,y,v,theta])\n",
"X = sp.Matrix([x, y, v, theta])\n",
"\n",
"#A\n",
"A=gs.jacobian(X)\n",
"# A\n",
"A = gs.jacobian(X)\n",
"\n",
"U = sp.Matrix([a,delta])\n",
"U = sp.Matrix([a, delta])\n",
"\n",
"#B\n",
"B=gs.jacobian(U)\n",
"# B\n",
"B = gs.jacobian(U)\n",
"display(A)\n",
"display(B)"
]
@ -279,9 +272,9 @@
"source": [
"DT = sp.symbols(\"dt\")\n",
"\n",
"display(sp.eye(4)+A*DT)\n",
"display(B*DT)\n",
"display(DT*(gs - A*X - B*U))"
"display(sp.eye(4) + A * DT)\n",
"display(B * DT)\n",
"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 time\n",
"from scipy.stats import norm\n",
"from sympy import Symbol, symbols, Matrix, sin, cos, integrate, diff, Q, refine, simplify, factor, expand_trig, exp, latex, Integral\n",
"from sympy import (\n",
" Symbol,\n",
" symbols,\n",
" Matrix,\n",
" sin,\n",
" cos,\n",
" integrate,\n",
" diff,\n",
" Q,\n",
" refine,\n",
" simplify,\n",
" factor,\n",
" expand_trig,\n",
" exp,\n",
" latex,\n",
" Integral,\n",
")\n",
"from sympy import init_printing\n",
"from sympy.utilities.codegen import codegen\n",
"\n",
"init_printing(use_latex=True)"
]
},
@ -674,15 +691,18 @@
}
],
"source": [
"speed, yaw, dt, x, y, acceleration, t, d_sin, d_cos = symbols('v \\\\theta \\Delta{t} x y a t d_{sin} d_{cos}')\n",
"turn_rate_non_zero = Symbol('\\hat{\\omega}', nonzero=True, finite=True)\n",
"zero_turn_rate = Symbol('\\omega_0', zero=True)\n",
"turn_rate = Symbol('\\omega')\n",
"speed, yaw, dt, x, y, acceleration, t, d_sin, d_cos = symbols(\n",
" \"v \\\\theta \\Delta{t} x y a t d_{sin} d_{cos}\"\n",
")\n",
"turn_rate_non_zero = Symbol(\"\\hat{\\omega}\", nonzero=True, finite=True)\n",
"zero_turn_rate = Symbol(\"\\omega_0\", zero=True)\n",
"turn_rate = Symbol(\"\\omega\")\n",
"\n",
"state_catr = Matrix([x, y, yaw, speed, turn_rate, acceleration])\n",
"print(\"CART state is:\")\n",
"display(state_catr)\n",
"\n",
"\n",
"def get_next_state(turn_rate):\n",
" # Specify the functions for yaw, speed, x, and y.\n",
" yaw_func = yaw + turn_rate * t\n",
@ -698,35 +718,45 @@
"\n",
" return Matrix([next_x, next_y, next_yaw, next_speed, turn_rate, acceleration])\n",
"\n",
"\n",
"# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not\n",
"print(\"Assuming a non-zero turn rate, the next state is:\")\n",
"next_state = get_next_state(turn_rate_non_zero)\n",
"display(next_state)\n",
"\n",
"substitutes = {x:42, y:23, yaw:0.5, speed:2, acceleration:2, dt:0.1, turn_rate_non_zero:2, zero_turn_rate:0}\n",
"display('Plugging in the numbers:', substitutes)\n",
"substitutes = {\n",
" x: 42,\n",
" y: 23,\n",
" yaw: 0.5,\n",
" speed: 2,\n",
" acceleration: 2,\n",
" dt: 0.1,\n",
" turn_rate_non_zero: 2,\n",
" zero_turn_rate: 0,\n",
"}\n",
"display(\"Plugging in the numbers:\", substitutes)\n",
"display(next_state.evalf(subs=substitutes))\n",
"\n",
"state = Matrix([x,y,yaw,speed,turn_rate_non_zero,acceleration])\n",
"state = Matrix([x, y, yaw, speed, turn_rate_non_zero, acceleration])\n",
"print(\"Jacobian of the next state with respect to the previous state:\")\n",
"J = next_state.jacobian(state)\n",
"display(J)\n",
"display('Plugging in the numbers:', substitutes)\n",
"display(\"Plugging in the numbers:\", substitutes)\n",
"display(J.evalf(subs=substitutes))\n",
"\n",
"print(\"Assuming a zero turn rate, state is:\")\n",
"next_state = get_next_state(zero_turn_rate)\n",
"display(next_state)\n",
"display('Plugging in the numbers:', substitutes)\n",
"display(\"Plugging in the numbers:\", substitutes)\n",
"display(next_state.evalf(subs=substitutes))\n",
"\n",
"state = Matrix([x,y,yaw,speed,zero_turn_rate,acceleration])\n",
"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",
"J = next_state.jacobian(state)\n",
"display(J)\n",
"\n",
"display('Plugging in the numbers:', substitutes)\n",
"display(J.evalf(subs=substitutes))\n"
"display(\"Plugging in the numbers:\", substitutes)\n",
"display(J.evalf(subs=substitutes))"
]
},
{
@ -1117,14 +1147,15 @@
}
],
"source": [
"speed, yaw, dt, x, y, t = symbols('v \\\\theta \\Delta{t} x y t')\n",
"turn_rate_non_zero = Symbol('\\omega', nonzero=True, finite=True)\n",
"zero_turn_rate = Symbol('\\omega_0', zero=True)\n",
"speed, yaw, dt, x, y, t = symbols(\"v \\\\theta \\Delta{t} x y t\")\n",
"turn_rate_non_zero = Symbol(\"\\omega\", nonzero=True, finite=True)\n",
"zero_turn_rate = Symbol(\"\\omega_0\", zero=True)\n",
"\n",
"state_cvtr = Matrix([x, y, yaw, speed, turn_rate_non_zero])\n",
"print(\"CVRT state is:\")\n",
"display(state_cvtr)\n",
"\n",
"\n",
"def get_next_state(turn_rate):\n",
" # Specify the functions for yaw, x, and y.\n",
" yaw_func = yaw + turn_rate * t\n",
@ -1138,34 +1169,45 @@
"\n",
" return Matrix([next_x, next_y, next_yaw, speed, turn_rate])\n",
"\n",
"\n",
"# There is a difference in computation betwee the cases when the turn rate is allowed to be zero or not\n",
"print(\"Assuming a non-zero turn rate, next state is:\")\n",
"next_state = get_next_state(turn_rate_non_zero)\n",
"display(next_state)\n",
"substitutes = {x:42, y:23, yaw:0.5, speed:2, dt:0.1, turn_rate_non_zero:2, zero_turn_rate:0}\n",
"display(Markdown('Plugging in the numbers:'), substitutes)\n",
"substitutes = {\n",
" x: 42,\n",
" y: 23,\n",
" yaw: 0.5,\n",
" speed: 2,\n",
" dt: 0.1,\n",
" turn_rate_non_zero: 2,\n",
" zero_turn_rate: 0,\n",
"}\n",
"display(Markdown(\"Plugging in the numbers:\"), substitutes)\n",
"display(next_state.evalf(subs=substitutes))\n",
"\n",
"state = Matrix([x,y,yaw,speed,turn_rate_non_zero])\n",
"state = Matrix([x, y, yaw, speed, turn_rate_non_zero])\n",
"print(\"Jacobian of the next state with respect to the previous state:\")\n",
"J = next_state.jacobian(state)\n",
"display(J)\n",
"\n",
"display(Markdown('Plugging in the numbers:'), substitutes)\n",
"display(Markdown(\"Plugging in the numbers:\"), substitutes)\n",
"display(J.evalf(subs=substitutes))\n",
"\n",
"print(\"Assuming a zero turn rate, next state is:\")\n",
"next_state = get_next_state(zero_turn_rate)\n",
"display(next_state)\n",
"display(Markdown('Plugging in the numbers:'), substitutes)\n",
"display(Markdown(\"Plugging in the numbers:\"), substitutes)\n",
"display(next_state.evalf(subs=substitutes))\n",
"\n",
"state = Matrix([x,y,yaw,speed,zero_turn_rate])\n",
"print(\"Jacobian of the next state with respect to the previous one with zero turn rate:\")\n",
"state = Matrix([x, y, yaw, speed, zero_turn_rate])\n",
"print(\n",
" \"Jacobian of the next state with respect to the previous one with zero turn rate:\"\n",
")\n",
"J = next_state.jacobian(state)\n",
"display(J)\n",
"\n",
"display(Markdown('Plugging in the numbers:'), substitutes)\n",
"display(Markdown(\"Plugging in the numbers:\"), substitutes)\n",
"display(J.evalf(subs=substitutes))"
]
}

102
notebooks/models/numerical_jacobian.ipynb
View File

@ -31,12 +31,12 @@
"# xy = x[1]\n",
"# v = x[2]\n",
"# theta =x[3]\n",
" \n",
"\n",
"# a = u[0]\n",
"# delta = u[1]\n",
" \n",
"\n",
"# L=0.3\n",
" \n",
"\n",
"# #vector of ackerman equations\n",
"# return np.array([\n",
"# np.cos(theta)*v,\n",
@ -45,7 +45,7 @@
"# v*np.arctan(delta)/L\n",
"# ])\n",
"\n",
"#DISCRETE\n",
"# DISCRETE\n",
"def f(x, u, dt=0.1):\n",
" \"\"\"\n",
" :param x:\n",
@ -53,55 +53,61 @@
" \"\"\"\n",
" xx = x[0]\n",
" xy = x[1]\n",
" v = x[2]\n",
" theta =x[3]\n",
" \n",
" v = x[2]\n",
" theta = x[3]\n",
"\n",
" a = u[0]\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",
"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",
" :param f:\n",
" :param x:\n",
" :param u:\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",
" 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": [
"#starting condition\n",
"x=np.array([0,0,1,0])\n",
"u=np.array([1,0.2])\n",
"# starting condition\n",
"x = np.array([0, 0, 1, 0])\n",
"u = np.array([1, 0.2])\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",
"\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
@ -181,10 +182,10 @@
"source": [
"# Control problem statement.\n",
"\n",
"N = 3 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"dt = 0.25 #discretization step"
"N = 3 # number of state variables\n",
"M = 2 # number of control variables\n",
"T = 20 # Prediction Horizon\n",
"dt = 0.25 # discretization step"
]
},
{
@ -193,32 +194,33 @@
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
"def get_linear_model(x_bar, u_bar):\n",
" \"\"\" \"\"\"\n",
"\n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" theta = x_bar[2]\n",
" \n",
"\n",
" v = u_bar[0]\n",
" w = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=-v*np.sin(theta)\n",
" A[1,2]=v*np.cos(theta)\n",
" A_lin=np.eye(N)+dt*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[0,0]=np.cos(theta)\n",
" B[1,0]=np.sin(theta)\n",
" B[2,1]=1\n",
" B_lin=dt*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w]).reshape(N,1)\n",
" C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" \n",
" return A_lin,B_lin,C_lin"
"\n",
" A = np.zeros((N, N))\n",
" A[0, 2] = -v * np.sin(theta)\n",
" A[1, 2] = v * np.cos(theta)\n",
" A_lin = np.eye(N) + dt * A\n",
"\n",
" B = np.zeros((N, M))\n",
" B[0, 0] = np.cos(theta)\n",
" B[1, 0] = np.sin(theta)\n",
" B[2, 1] = 1\n",
" B_lin = dt * B\n",
"\n",
" f_xu = np.array([v * np.cos(theta), v * np.sin(theta), w]).reshape(N, 1)\n",
" C_lin = dt * (\n",
" f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
" )\n",
"\n",
" return A_lin, B_lin, C_lin"
]
},
{
@ -235,41 +237,35 @@
"outputs": [],
"source": [
"# Define process model\n",
"# This uses the continuous model \n",
"def kinematics_model(x,t,u):\n",
" \"\"\"\n",
" \"\"\"\n",
"# This uses the continuous model\n",
"def kinematics_model(x, t, u):\n",
" \"\"\" \"\"\"\n",
"\n",
" dxdt = u[0]*np.cos(x[2])\n",
" dydt = u[0]*np.sin(x[2])\n",
" dxdt = u[0] * np.cos(x[2])\n",
" dydt = u[0] * np.sin(x[2])\n",
" dthetadt = u[1]\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dthetadt]\n",
" dqdt = [dxdt, dydt, dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"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",
" tspan = [0,dt]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"def predict(x0, u):\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",
" tspan = [0, dt]\n",
" x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_bar[:,t]=x_next[1]\n",
" \n",
" x_bar[:, t] = x_next[1]\n",
"\n",
" return x_bar"
]
},
@ -297,16 +293,16 @@
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 1 #m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n",
"u_bar = np.zeros((M, T))\n",
"u_bar[0, :] = 1 # m/s\n",
"u_bar[1, :] = np.radians(-10) # rad/s\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(0)\n",
"\n",
"x_bar=predict(x0,u_bar)"
"x_bar = predict(x0, u_bar)"
]
},
{
@ -333,18 +329,18 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2,:]))\n",
"plt.ylabel('theta(t) [deg]')\n",
"#plt.xlabel('time')\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel(\"theta(t) [deg]\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
"plt.tight_layout()\n",
@ -375,27 +371,27 @@
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 1 #m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n",
"u_bar = np.zeros((M, T))\n",
"u_bar[0, :] = 1 # m/s\n",
"u_bar[1, :] = np.radians(-10) # rad/s\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(0)\n",
"\n",
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"x_bar = np.zeros((N, T + 1))\n",
"x_bar[:, 0] = x0\n",
"\n",
"for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n",
" \n",
" x_bar[:,t]= np.squeeze(xt_plus_one)"
"for t in range(1, T + 1):\n",
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:, t - 1].reshape(M, 1)\n",
"\n",
" A, B, C = get_linear_model(xt, ut)\n",
"\n",
" xt_plus_one = np.dot(A, xt) + np.dot(B, ut) + C\n",
"\n",
" x_bar[:, t] = np.squeeze(xt_plus_one)"
]
},
{
@ -415,18 +411,18 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x_bar[0,:],x_bar[1,:])\n",
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.plot(x_bar[0, :], x_bar[1, :])\n",
"plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
"plt.axis(\"equal\")\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2,:]))\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"plt.plot(np.degrees(x_bar[2, :]))\n",
"plt.ylabel(\"theta(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"\n",
"plt.tight_layout()\n",
@ -464,74 +460,101 @@
"metadata": {},
"outputs": [],
"source": [
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"def compute_path_from_wp(start_xp, start_yp, step=0.1):\n",
" final_xp = []\n",
" final_yp = []\n",
" delta = step # [m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
" 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",
" interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))\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",
"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",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\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",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v = [\n",
" path[0, nn_idx + 1] - path[0, nn_idx],\n",
" path[1, nn_idx + 1] - path[1, nn_idx],\n",
" ]\n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
" d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]\n",
"\n",
" if np.dot(d,v) > 0:\n",
" if np.dot(d, v) > 0:\n",
" target_idx = nn_idx\n",
" else:\n",
" target_idx = nn_idx+1\n",
" target_idx = nn_idx + 1\n",
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
"\n",
" return target_idx\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",
" #trasform lookahead waypoints to vehicle ref frame\n",
" dx = lk_wp[0,:] - state[0]\n",
" dy = lk_wp[1,:] - state[1]\n",
"def road_curve(state, track):\n",
"\n",
" wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
" # 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",
" #fit poly\n",
" return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n",
" # trasform lookahead waypoints to vehicle ref frame\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",
"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",
"# def f(x,coeff):\n",
"# return round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)\n",
"\n",
"def df(x,coeff):\n",
" return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
"\n",
"def df(x, coeff):\n",
" return round(3 * coeff[0] * x**2 + 2 * coeff[1] * x**1 + coeff[2] * x**0, 6)\n",
"\n",
"\n",
"# def df(x,coeff):\n",
"# return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)"
]
@ -698,34 +721,34 @@
"source": [
"%%time\n",
"\n",
"x = cp.Variable((N, T+1))\n",
"x = cp.Variable((N, T + 1))\n",
"u = cp.Variable((M, T))\n",
"\n",
"#CVXPY Linear MPC problem statement\n",
"# CVXPY Linear MPC problem statement\n",
"cost = 0\n",
"constr = []\n",
"\n",
"for t in range(T):\n",
" \n",
"\n",
" # Cost function\n",
" #cost += 10*cp.sum_squares( x[3, t]) # psi\n",
" #cost += 500*cp.sum_squares( x[4, t]) # cte\n",
" \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 += 10*cp.sum_squares( x[3, t]) # psi\n",
" # cost += 500*cp.sum_squares( x[4, t]) # cte\n",
"\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",
" # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
" \n",
" cost += cp.quad_form(u[:, t], 1 * np.eye(M))\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n",
" \n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100 * np.eye(M))\n",
"\n",
" # KINEMATICS constrains\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",
" \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",
"\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, :] >= MIN_SPEED]\n",
"constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
@ -751,35 +774,35 @@
}
],
"source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n",
"theta_mpc=np.array(x.value[2, :]).flatten()\n",
"v_mpc=np.array(u.value[0, :]).flatten()\n",
"w_mpc=np.array(u.value[1, :]).flatten()\n",
"x_mpc = np.array(x.value[0, :]).flatten()\n",
"y_mpc = np.array(x.value[1, :]).flatten()\n",
"theta_mpc = np.array(x.value[2, :]).flatten()\n",
"v_mpc = np.array(u.value[0, :]).flatten()\n",
"w_mpc = np.array(u.value[1, :]).flatten()\n",
"\n",
"#simulate robot state trajectory for optimized U\n",
"x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n",
"# simulate robot state trajectory for optimized U\n",
"x_traj = predict(x0, np.vstack((v_mpc, w_mpc)))\n",
"\n",
"#plt.figure(figsize=(15,10))\n",
"#plot trajectory\n",
"# plt.figure(figsize=(15,10))\n",
"# plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_traj[0,:],x_traj[1,:])\n",
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_traj[0, :], x_traj[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"#plot v(t)\n",
"# plot v(t)\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(v_mpc)\n",
"plt.ylabel('v(t)')\n",
"#plt.xlabel('time')\n",
"plt.ylabel(\"v(t)\")\n",
"# plt.xlabel('time')\n",
"\n",
"#plot w(t)\n",
"# plot w(t)\n",
"# plt.subplot(2, 2, 3)\n",
"# plt.plot(w_mpc)\n",
"# plt.ylabel('w(t)')\n",
"#plt.xlabel('time')\n",
"# plt.xlabel('time')\n",
"\n",
"# plt.subplot(2, 2, 3)\n",
"# plt.plot(psi_mpc)\n",
@ -834,17 +857,16 @@
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
" [0,0,2,4,3,3],0.25)\n",
"track = compute_path_from_wp([0, 3, 4, 6, 10, 13], [0, 0, 2, 4, 3, 3], 0.25)\n",
"\n",
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
"# [0,0,2.5,2.5,0,0,5,10],0.5)\n",
"\n",
"sim_duration = 80 #time steps\n",
"opt_time=[]\n",
"sim_duration = 80 # time steps\n",
"opt_time = []\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M, sim_duration - 1))\n",
"\n",
"MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n",
@ -855,83 +877,87 @@
"x0[0] = 0\n",
"x0[1] = -0.25\n",
"x0[2] = np.radians(-0)\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",
"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",
"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",
"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",
" x_bar=np.zeros((N,T+1))\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\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",
" x_bar[:,t]= xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x_bar = np.zeros((N, T + 1))\n",
"\n",
" # prediction for linearization of costrains\n",
" for t in range(1, T + 1):\n",
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:, t - 1].reshape(M, 1)\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",
" x_bar[:, t] = xt_plus_one\n",
"\n",
" # CVXPY Linear MPC problem statement\n",
" cost = 0\n",
" constr = []\n",
" x = cp.Variable((N, T+1))\n",
" x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n",
" \n",
" #Prediction Horizon\n",
"\n",
" # Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
" cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n",
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
" # cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
" cost += 50 * cp.sum_squares(\n",
" x[2, t] - np.arctan2(df(x_bar[0, t], K), x_bar[0, t])\n",
" ) # psi\n",
" cost += 20 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n",
" \n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100 * np.eye(M))\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
" \n",
" cost += cp.quad_form(u[:, t], 1 * np.eye(M))\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\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",
" 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",
"\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, :] >= MIN_SPEED]\n",
" constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
" \n",
"\n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n",
" (np.array(u.value[1, :]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" opt_time.append(time.time() - iter_start)\n",
"\n",
" # move simulation to t+1\n",
" tspan = [0,dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
" tspan = [0, dt]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -951,25 +977,25 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"grid = plt.GridSpec(2, 3)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"plt.figure(figsize=(15, 10))\n",
"\n",
"plt.subplot(grid[0:2, 0:2])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('w(t) [deg/s]')\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel(\"w(t) [deg/s]\")\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
@ -1020,27 +1046,31 @@
],
"source": [
"import dccp\n",
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
" [0,0,2,4,3,3],0.25)\n",
"\n",
"obstacles=np.array([[4,6],[2,4]])\n",
"obstacle_radius=0.5\n",
"track = compute_path_from_wp([0, 3, 4, 6, 10, 13], [0, 0, 2, 4, 3, 3], 0.25)\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",
" dy = pt[1] - pos_y\n",
"\n",
" return [dx * np.cos(-theta) - dy * np.sin(-theta),\n",
" dy * np.cos(-theta) + dx * np.sin(-theta)]\n",
" \n",
" return [\n",
" dx * np.cos(-theta) - dy * np.sin(-theta),\n",
" dy * np.cos(-theta) + dx * np.sin(-theta),\n",
" ]\n",
"\n",
"\n",
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
"# [0,0,2.5,2.5,0,0,5,10],0.5)\n",
"\n",
"sim_duration = 80 #time steps\n",
"opt_time=[]\n",
"sim_duration = 80 # time steps\n",
"opt_time = []\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"x_sim = np.zeros((N, sim_duration))\n",
"u_sim = np.zeros((M, sim_duration - 1))\n",
"\n",
"MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n",
@ -1051,93 +1081,104 @@
"x0[0] = 0\n",
"x0[1] = -0.25\n",
"x0[2] = np.radians(-0)\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",
"x_sim[:, 0] = x0\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start=time.time()\n",
" \n",
" #compute coefficients\n",
" K=road_curve(x_sim[:,sim_time],track)\n",
" \n",
" #compute opstacles in ref frame\n",
" o_=[]\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",
"for sim_time in range(sim_duration - 1):\n",
"\n",
" iter_start = time.time()\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",
" o_.append(to_vehic_frame(obstacles[:,j],x_sim[0,sim_time],x_sim[1,sim_time],x_sim[2,sim_time]) )\n",
" \n",
" o_.append(\n",
" to_vehic_frame(\n",
" obstacles[:, j],\n",
" x_sim[0, sim_time],\n",
" x_sim[1, sim_time],\n",
" x_sim[2, sim_time],\n",
" )\n",
" )\n",
"\n",
" # dynamics starting state w.r.t vehicle frame\n",
" x_bar=np.zeros((N,T+1))\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,1)\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",
" x_bar[:,t]= xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x_bar = np.zeros((N, T + 1))\n",
"\n",
" # prediction for linearization of costrains\n",
" for t in range(1, T + 1):\n",
" xt = x_bar[:, t - 1].reshape(N, 1)\n",
" ut = u_bar[:, t - 1].reshape(M, 1)\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",
" x_bar[:, t] = xt_plus_one\n",
"\n",
" # CVXPY Linear MPC problem statement\n",
" cost = 0\n",
" constr = []\n",
" x = cp.Variable((N, T+1))\n",
" x = cp.Variable((N, T + 1))\n",
" u = cp.Variable((M, T))\n",
" \n",
" #Prediction Horizon\n",
"\n",
" # Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
" cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n",
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
" # cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
" cost += 50 * cp.sum_squares(\n",
" x[2, t] - np.arctan2(df(x_bar[0, t], K), x_bar[0, t])\n",
" ) # psi\n",
" cost += 20 * cp.sum_squares(f(x_bar[0, t], K) - x[1, t]) # cte\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n",
" \n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100 * np.eye(M))\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
" \n",
" cost += cp.quad_form(u[:, t], 1 * np.eye(M))\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\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",
" \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",
"\n",
" # Obstacle Avoidance Contrains\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",
" # 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, :] >= MIN_SPEED]\n",
" constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
" \n",
"\n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(method=\"dccp\", verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n",
" (np.array(u.value[1, :]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
"\n",
" # retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack(\n",
" (np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))\n",
" )\n",
"\n",
" u_sim[:, sim_time] = u_bar[:, 0]\n",
"\n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" opt_time.append(time.time() - iter_start)\n",
"\n",
" # move simulation to t+1\n",
" tspan = [0,dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
" tspan = [0, dt]\n",
" x_sim[:, sim_time + 1] = odeint(\n",
" kinematics_model, x_sim[:, sim_time], tspan, args=(u_bar[:, 0],)\n",
" )[1]\n",
"\n",
"print(\n",
" \"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(\n",
" np.mean(opt_time), np.max(opt_time), np.min(opt_time)\n",
" )\n",
")"
]
},
{
@ -1157,31 +1198,31 @@
}
],
"source": [
"#plot trajectory\n",
"# plot trajectory\n",
"grid = plt.GridSpec(2, 3)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"plt.figure(figsize=(15, 10))\n",
"\n",
"ax=plt.subplot(grid[0:2, 0:2])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"#obstacles\n",
"circle1=plt.Circle((obstacles[0,0], obstacles[1,0]), obstacle_radius, color='r')\n",
"circle2=plt.Circle((obstacles[0,1], obstacles[1,1]), obstacle_radius, color='r')\n",
"ax = plt.subplot(grid[0:2, 0:2])\n",
"plt.plot(track[0, :], track[1, :], \"b+\")\n",
"plt.plot(x_sim[0, :], x_sim[1, :])\n",
"# obstacles\n",
"circle1 = plt.Circle((obstacles[0, 0], obstacles[1, 0]), obstacle_radius, color=\"r\")\n",
"circle2 = plt.Circle((obstacles[0, 1], obstacles[1, 1]), obstacle_radius, color=\"r\")\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"plt.ylabel(\"y\")\n",
"plt.xlabel(\"x\")\n",
"\n",
"ax.add_artist(circle1)\n",
"ax.add_artist(circle2)\n",
"\n",
"plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"plt.plot(u_sim[0, :])\n",
"plt.ylabel(\"v(t) [m/s]\")\n",
"\n",
"plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('w(t) [deg/s]')\n",
"plt.plot(np.degrees(u_sim[1, :]))\n",
"plt.ylabel(\"w(t) [deg/s]\")\n",
"\n",
"\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