From 7f7cdf31b4ae57fd7c9428b4fec762d90e6368a7 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Tue, 2 Aug 2022 09:33:49 +0100 Subject: [PATCH] reformatted notebooks with black --- env.yml | 1 + mpc_pybullet_demo/mpcpy/__init__.py | 2 +- mpc_pybullet_demo/racecar/racecar.py | 327 ++++--- notebooks/1.0-lti-system-modelling.ipynb | 183 ++-- notebooks/1.1-parametrized-path-curves.ipynb | 194 ++-- notebooks/2.0-MPC-base.ipynb | 529 ++++++----- ...2.1-MPC-with-iterative-linearization.ipynb | 414 ++++---- .../2.2-MPC-v2-car-reference-frame.ipynb | 424 +++++---- notebooks/2.3-MPC-simplified.ipynb | 458 ++++----- notebooks/3.0-MPC-v3-track-constrains.ipynb | 886 +++++++++--------- notebooks/3.1-better-track.ipynb | 157 ++-- notebooks/models/ackerman_model.ipynb | 25 +- notebooks/models/differential_model.ipynb | 61 +- notebooks/models/motion_model.ipynb | 90 +- notebooks/models/numerical_jacobian.ipynb | 102 +- .../MPC_cte_cvxpy.ipynb | 681 +++++++------- .../MPC_tracking_cvxpy.ipynb | 706 +++++++------- 17 files changed, 2794 insertions(+), 2446 deletions(-) diff --git a/env.yml b/env.yml index 3f264df..1fc7cb6 100644 --- a/env.yml +++ b/env.yml @@ -20,3 +20,4 @@ dependencies: - pip: - pybullet - black==22.3.0 + - 'black[jupyter]' diff --git a/mpc_pybullet_demo/mpcpy/__init__.py b/mpc_pybullet_demo/mpcpy/__init__.py index ac2dcc8..b10be34 100644 --- a/mpc_pybullet_demo/mpcpy/__init__.py +++ b/mpc_pybullet_demo/mpcpy/__init__.py @@ -1,2 +1,2 @@ -from .cvxpy_mpc import * +from .cvxpy_mpc import * from .mpc_config import Params diff --git a/mpc_pybullet_demo/racecar/racecar.py b/mpc_pybullet_demo/racecar/racecar.py index c18c03a..40a3161 100644 --- a/mpc_pybullet_demo/racecar/racecar.py +++ b/mpc_pybullet_demo/racecar/racecar.py @@ -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 diff --git a/notebooks/1.0-lti-system-modelling.ipynb b/notebooks/1.0-lti-system-modelling.ipynb index 49a503a..e1f28c7 100644 --- a/notebooks/1.0-lti-system-modelling.ipynb +++ b/notebooks/1.0-lti-system-modelling.ipynb @@ -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", diff --git a/notebooks/1.1-parametrized-path-curves.ipynb b/notebooks/1.1-parametrized-path-curves.ipynb index 4f6a193..b99eb86 100644 --- a/notebooks/1.1-parametrized-path-curves.ipynb +++ b/notebooks/1.1-parametrized-path-curves.ipynb @@ -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" ] } diff --git a/notebooks/2.0-MPC-base.ipynb b/notebooks/2.0-MPC-base.ipynb index 780f67d..5d13437 100644 --- a/notebooks/2.0-MPC-base.ipynb +++ b/notebooks/2.0-MPC-base.ipynb @@ -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()" diff --git a/notebooks/2.1-MPC-with-iterative-linearization.ipynb b/notebooks/2.1-MPC-with-iterative-linearization.ipynb index 6267a48..2c0057e 100644 --- a/notebooks/2.1-MPC-with-iterative-linearization.ipynb +++ b/notebooks/2.1-MPC-with-iterative-linearization.ipynb @@ -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()" diff --git a/notebooks/2.2-MPC-v2-car-reference-frame.ipynb b/notebooks/2.2-MPC-v2-car-reference-frame.ipynb index b418f8f..4c27c50 100644 --- a/notebooks/2.2-MPC-v2-car-reference-frame.ipynb +++ b/notebooks/2.2-MPC-v2-car-reference-frame.ipynb @@ -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()" diff --git a/notebooks/2.3-MPC-simplified.ipynb b/notebooks/2.3-MPC-simplified.ipynb index afe8eb8..6954a60 100644 --- a/notebooks/2.3-MPC-simplified.ipynb +++ b/notebooks/2.3-MPC-simplified.ipynb @@ -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()" diff --git a/notebooks/3.0-MPC-v3-track-constrains.ipynb b/notebooks/3.0-MPC-v3-track-constrains.ipynb index eb9f922..aee8644 100644 --- a/notebooks/3.0-MPC-v3-track-constrains.ipynb +++ b/notebooks/3.0-MPC-v3-track-constrains.ipynb @@ -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" @@ -27,148 +28,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", @@ -181,51 +193,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" @@ -247,37 +260,44 @@ "outputs": [], "source": [ "from scipy.signal import savgol_filter\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", " \"\"\"this smoothens up corners\"\"\"\n", - " window_size = 7 # Smoothening filter window\n", + " window_size = 7 # 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))\n" + " return np.vstack((final_xp, final_yp, theta))" ] }, { @@ -320,39 +340,41 @@ } ], "source": [ - "def generate_track_bounds(track,width=0.5):\n", + "def generate_track_bounds(track, width=0.5):\n", " \"\"\"\n", " in world frame\n", " \"\"\"\n", - " bounds_low=np.zeros((2, track.shape[1]))\n", - " bounds_upp=np.zeros((2, track.shape[1]))\n", - " \n", + " bounds_low = np.zeros((2, track.shape[1]))\n", + " bounds_upp = np.zeros((2, track.shape[1]))\n", + "\n", " for idx in range(track.shape[1]):\n", - " x = track[0,idx]\n", - " y = track [1,idx]\n", - " th = track [2,idx]\n", - " \n", + " x = track[0, idx]\n", + " y = track[1, idx]\n", + " th = track[2, idx]\n", + "\n", " \"\"\"\n", " trasform the points\n", " \"\"\"\n", - " bounds_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n", - " bounds_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n", - " \n", - " bounds_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n", - " bounds_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n", - " \n", + " bounds_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x # X\n", + " bounds_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y # Y\n", + "\n", + " bounds_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x # X\n", + " bounds_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y # Y\n", + "\n", " return bounds_low, bounds_upp\n", "\n", - "track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n", - " [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n", + "\n", + "track = compute_path_from_wp(\n", + " [0, 3, 4, 6, 10, 12, 14, 6, 1, 0], [0, 0, 2, 4, 3, 3, -2, -6, -2, -2], 0.05\n", + ")\n", "\n", "lower, upper = generate_track_bounds(track)\n", "\n", - "plt.figure(figsize=(15,10))\n", + "plt.figure(figsize=(15, 10))\n", "\n", - "plt.plot(track[0,:],track[1,:],\"b-\")\n", - "plt.plot(lower[0,:],lower[1,:],\"g-\")\n", - "plt.plot(upper[0,:],upper[1,:],\"r-\")" + "plt.plot(track[0, :], track[1, :], \"b-\")\n", + "plt.plot(lower[0, :], lower[1, :], \"g-\")\n", + "plt.plot(upper[0, :], upper[1, :], \"r-\")" ] }, { @@ -432,23 +454,26 @@ "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", + "\n", "plt.style.use(\"ggplot\")\n", "\n", - "def get_coeff(x,y,theta):\n", - " m = np.sin(theta)/np.cos(theta)\n", - " return(-m,1,y-m*x)\n", "\n", - "#test -> assume point 10,1,pi/6\n", - "#coeff = get_coeff(1,-1, np.pi/2)\n", - "coeff = get_coeff(1,-1, np.pi/4)\n", + "def get_coeff(x, y, theta):\n", + " m = np.sin(theta) / np.cos(theta)\n", + " return (-m, 1, y - m * x)\n", + "\n", + "\n", + "# test -> assume point 10,1,pi/6\n", + "# coeff = get_coeff(1,-1, np.pi/2)\n", + "coeff = get_coeff(1, -1, np.pi / 4)\n", "y = []\n", - "pts = np.linspace(0,20,100)\n", + "pts = np.linspace(0, 20, 100)\n", "\n", "for x in pts:\n", - " y.append((-coeff[0]*x+coeff[2])/coeff[1])\n", - " \n", - "plt.figure(figsize=(5,5))\n", - "plt.plot(pts,y,\"b-\")\n", + " y.append((-coeff[0] * x + coeff[2]) / coeff[1])\n", + "\n", + "plt.figure(figsize=(5, 5))\n", + "plt.plot(pts, y, \"b-\")\n", "\n", "plt.xlim((-2, 2))\n", "plt.ylim((-2, 2))" @@ -490,7 +515,7 @@ ], "source": [ "# def get_coeff(x,y,theta):\n", - " \n", + "\n", "# if (theta - np.pi/2) < 0.01:\n", "# #print (\"WARN -> theta is 90, tan is: \" + str(theta))\n", "# # eq is x = val\n", @@ -499,18 +524,18 @@ "# else:\n", "# m = np.sin(theta)/np.cos(theta)\n", "# return(-m,1,y-m*x)\n", - " \n", - "#test -> assume point 10,1,pi/6\n", - "coeff = get_coeff(1,-1, np.pi/2)\n", + "\n", + "# test -> assume point 10,1,pi/6\n", + "coeff = get_coeff(1, -1, np.pi / 2)\n", "y = []\n", - "pts = np.linspace(0,20,100)\n", + "pts = np.linspace(0, 20, 100)\n", "\n", "for x in pts:\n", - " y.append((-coeff[0]*x+coeff[2])/coeff[1])\n", - " \n", - "plt.figure(figsize=(5,5))\n", + " y.append((-coeff[0] * x + coeff[2]) / coeff[1])\n", "\n", - "plt.plot(pts,y,\"b-\")\n", + "plt.figure(figsize=(5, 5))\n", + "\n", + "plt.plot(pts, y, \"b-\")\n", "plt.axis(\"equal\")\n", "plt.xlim((-2, 2))\n", "plt.ylim((-2, 2))" @@ -533,46 +558,45 @@ "source": [ "def get_track_constrains(x_ref, width=0.5):\n", " \"\"\"\n", - " x_ref has hape (4,T) -> [x,y,v,theta]_ref \n", + " x_ref has hape (4,T) -> [x,y,v,theta]_ref\n", " \"\"\"\n", - " \n", - " #1-> get the upper and lower points\n", - " pts_low=np.zeros((3, x_ref.shape[1]))\n", - " pts_upp=np.zeros((3, x_ref.shape[1]))\n", - " \n", + "\n", + " # 1-> get the upper and lower points\n", + " pts_low = np.zeros((3, x_ref.shape[1]))\n", + " pts_upp = np.zeros((3, x_ref.shape[1]))\n", + "\n", " for idx in range(x_ref.shape[1]):\n", - " x = x_ref [0, idx]\n", - " y = x_ref [1, idx]\n", - " th = x_ref [3, idx]\n", - " \n", + " x = x_ref[0, idx]\n", + " y = x_ref[1, idx]\n", + " th = x_ref[3, idx]\n", + "\n", " \"\"\"\n", " trasform the points\n", " \"\"\"\n", - " pts_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n", - " pts_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n", - " pts_upp[2, idx] = th #heading\n", - " \n", - " pts_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n", - " pts_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n", - " pts_low[2, idx] = th #heading\n", - " \n", - " #get coefficients ->(a,b,c)\n", - " coeff_low=np.zeros((3, x_ref.shape[1]))\n", - " coeff_upp=np.zeros((3, x_ref.shape[1]))\n", - " \n", + " pts_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x # X\n", + " pts_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y # Y\n", + " pts_upp[2, idx] = th # heading\n", + "\n", + " pts_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x # X\n", + " pts_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y # Y\n", + " pts_low[2, idx] = th # heading\n", + "\n", + " # get coefficients ->(a,b,c)\n", + " coeff_low = np.zeros((3, x_ref.shape[1]))\n", + " coeff_upp = np.zeros((3, x_ref.shape[1]))\n", + "\n", " for idx in range(pts_upp.shape[1]):\n", - " f = get_coeff(pts_low[0,idx],pts_low[1,idx],pts_low[2,idx])\n", - " coeff_low[0,idx]=f[0]\n", - " coeff_low[1,idx]=f[1]\n", - " coeff_low[2,idx]=f[2]\n", - " \n", - " f = get_coeff(pts_upp[0,idx],pts_upp[1,idx],pts_upp[2,idx])\n", - " coeff_upp[0,idx]=f[0]\n", - " coeff_upp[1,idx]=f[1]\n", - " coeff_upp[2,idx]=f[2]\n", - " \n", - " return coeff_low, coeff_upp\n", - " " + " f = get_coeff(pts_low[0, idx], pts_low[1, idx], pts_low[2, idx])\n", + " coeff_low[0, idx] = f[0]\n", + " coeff_low[1, idx] = f[1]\n", + " coeff_low[2, idx] = f[2]\n", + "\n", + " f = get_coeff(pts_upp[0, idx], pts_upp[1, idx], pts_upp[2, idx])\n", + " coeff_upp[0, idx] = f[0]\n", + " coeff_upp[1, idx] = f[1]\n", + " coeff_upp[2, idx] = f[2]\n", + "\n", + " return coeff_low, coeff_upp" ] }, { @@ -607,122 +631,127 @@ } ], "source": [ - "track = compute_path_from_wp([0,3,3,0],\n", - " [0,0,1,1],0.05)\n", + "track = compute_path_from_wp([0, 3, 3, 0], [0, 0, 1, 1], 0.05)\n", "\n", - "track_lower, track_upper = generate_track_bounds(track,0.12)\n", + "track_lower, track_upper = generate_track_bounds(track, 0.12)\n", "\n", - "sim_duration = 50 #time steps\n", - "opt_time=[]\n", + "sim_duration = 50 # 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 = 0.5 #m/s\n", + "REF_VEL = 0.5 # m/s\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", - "x0[0] = 0 #x\n", - "x0[1] = -0.05 #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", - "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.05 # 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", + "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", + ")" ] }, { @@ -742,35 +771,35 @@ } ], "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(track_lower[0,:],track_lower[1,:],\"g-\")\n", - "plt.plot(track_upper[0,:],track_upper[1,:],\"r-\")\n", - "plt.plot(x_sim[0,:],x_sim[1,:])\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(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()" @@ -799,133 +828,140 @@ } ], "source": [ - "WIDTH=0.18\n", - "REF_VEL = 0.4 #m/s\n", + "WIDTH = 0.18\n", + "REF_VEL = 0.4 # m/s\n", "\n", "computed_coeff = []\n", "\n", - "track = compute_path_from_wp([0,3,3,0],\n", - " [0,0,-1,-1],0.05)\n", + "track = compute_path_from_wp([0, 3, 3, 0], [0, 0, -1, -1], 0.05)\n", "\n", - "track_lower, track_upper = generate_track_bounds(track,WIDTH)\n", + "track_lower, track_upper = generate_track_bounds(track, WIDTH)\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", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", - "x0[0] = 0 #x\n", - "x0[1] = WIDTH/2 #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", - "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] = WIDTH / 2 # 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", + "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", - " #Track constrains\n", - " low,upp = get_track_constrains(x_ref,WIDTH)\n", - " computed_coeff.append((low,upp))\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", + " # Track constrains\n", + " low, upp = get_track_constrains(x_ref, WIDTH)\n", + " computed_coeff.append((low, upp))\n", " for ii in range(low.shape[1]):\n", - " #constr += [low[0,ii]*x[0,ii] + x[1,ii] >= low[2,ii]]\n", - " constr += [upp[0,ii]*x[0,ii] + x[1,ii] <= upp[2,ii]] #<-- CAUSES ISSUES Y?\n", - " \n", + " # constr += [low[0,ii]*x[0,ii] + x[1,ii] >= low[2,ii]]\n", + " constr += [\n", + " upp[0, ii] * x[0, ii] + x[1, ii] <= upp[2, ii]\n", + " ] # <-- CAUSES ISSUES Y?\n", + "\n", " # Solve\n", " prob = cp.Problem(cp.Minimize(cost), constr)\n", " solution = prob.solve(solver=cp.ECOS, verbose=False)\n", - " \n", - " #retrieved optimized U and assign to u_bar to linearize in next step\n", - " u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n", - " (np.array(u.value[1,:]).flatten())))\n", - " \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", + ")" ] }, { @@ -948,36 +984,36 @@ ], "source": [ "%matplotlib inline\n", - "#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(track_lower[0,:],track_lower[1,:],\"g.\")\n", - "plt.plot(track_upper[0,:],track_upper[1,:],\"r.\")\n", - "plt.plot(x_sim[0,:],x_sim[1,:])\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(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", "\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()" @@ -1009,6 +1045,7 @@ "source": [ "import numpy as np\n", "import matplotlib.pyplot as plt\n", + "\n", "plt.style.use(\"ggplot\")\n", "\n", "\n", @@ -1018,50 +1055,51 @@ " each column represent the couefficient at each\n", " step of the optimization\n", " \"\"\"\n", - " \n", - " #for low,up in zip(low, up):\n", + "\n", + " # for low,up in zip(low, up):\n", " for idx in range(low.shape[1]):\n", - " \n", - " ll = low[:,idx]\n", - " uu = up[:,idx]\n", - " \n", - " x = np.linspace(-2,2,100)\n", "\n", - " #low\n", + " ll = low[:, idx]\n", + " uu = up[:, idx]\n", + "\n", + " x = np.linspace(-2, 2, 100)\n", + "\n", + " # low\n", " y = []\n", " for xx in x:\n", - " y.append((-ll[0]*xx+ll[2])/ll[1])\n", + " y.append((-ll[0] * xx + ll[2]) / ll[1])\n", "\n", - " plt.plot(x,y,\"b-\")\n", + " plt.plot(x, y, \"b-\")\n", "\n", - " #high\n", + " # high\n", " y = []\n", " for xx in x:\n", - " y.append((-uu[0]*xx+uu[2])/uu[1])\n", + " y.append((-uu[0] * xx + uu[2]) / uu[1])\n", "\n", - " plt.plot(x,y,\"r-\")\n", + " plt.plot(x, y, \"r-\")\n", "\n", " plt.xlim((-2, 2))\n", " plt.ylim((-2, 2))\n", "\n", + "\n", "def plot_lines():\n", " \"\"\"\n", " sample randomly from computed coeff\n", " and plot grid\n", " \"\"\"\n", - " \n", - " plt.figure(figsize=(15,15))\n", + "\n", + " plt.figure(figsize=(15, 15))\n", " indices = np.random.choice(len(computed_coeff), 9)\n", "\n", " for i in range(len(indices)):\n", - " plt.subplot(3,3,i+1)\n", - " plt.title(\"t = \"+str(indices[i]))\n", - " plot_low_upp(computed_coeff[indices[i]][0]\n", - " ,computed_coeff[indices[i]][1])\n", - " \n", + " plt.subplot(3, 3, i + 1)\n", + " plt.title(\"t = \" + str(indices[i]))\n", + " plot_low_upp(computed_coeff[indices[i]][0], computed_coeff[indices[i]][1])\n", + "\n", " plt.tight_layout()\n", " plt.show()\n", "\n", + "\n", "plot_lines()" ] }, diff --git a/notebooks/3.1-better-track.ipynb b/notebooks/3.1-better-track.ipynb index eb2dc13..7f998a9 100644 --- a/notebooks/3.1-better-track.ipynb +++ b/notebooks/3.1-better-track.ipynb @@ -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-\")" ] }, { diff --git a/notebooks/models/ackerman_model.ipynb b/notebooks/models/ackerman_model.ipynb index e3afc87..2dd31a2 100644 --- a/notebooks/models/ackerman_model.ipynb +++ b/notebooks/models/ackerman_model.ipynb @@ -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))" ] }, { diff --git a/notebooks/models/differential_model.ipynb b/notebooks/models/differential_model.ipynb index 80239e8..2a247e4 100644 --- a/notebooks/models/differential_model.ipynb +++ b/notebooks/models/differential_model.ipynb @@ -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))" ] }, { diff --git a/notebooks/models/motion_model.ipynb b/notebooks/models/motion_model.ipynb index 87ee09e..4bf05df 100644 --- a/notebooks/models/motion_model.ipynb +++ b/notebooks/models/motion_model.ipynb @@ -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))" ] } diff --git a/notebooks/models/numerical_jacobian.ipynb b/notebooks/models/numerical_jacobian.ipynb index 72813ae..0e88e1f 100644 --- a/notebooks/models/numerical_jacobian.ipynb +++ b/notebooks/models/numerical_jacobian.ipynb @@ -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)" ] } ], diff --git a/notebooks/old_scipy_implementation/MPC_cte_cvxpy.ipynb b/notebooks/old_scipy_implementation/MPC_cte_cvxpy.ipynb index 16a75c6..a8c6160 100644 --- a/notebooks/old_scipy_implementation/MPC_cte_cvxpy.ipynb +++ b/notebooks/old_scipy_implementation/MPC_cte_cvxpy.ipynb @@ -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", diff --git a/notebooks/old_scipy_implementation/MPC_tracking_cvxpy.ipynb b/notebooks/old_scipy_implementation/MPC_tracking_cvxpy.ipynb index 7edb2c3..0b09e36 100644 --- a/notebooks/old_scipy_implementation/MPC_tracking_cvxpy.ipynb +++ b/notebooks/old_scipy_implementation/MPC_tracking_cvxpy.ipynb @@ -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" @@ -217,12 +218,12 @@ "source": [ "# Control problem statement.\n", "\n", - "N = 5 #number of state variables\n", - "M = 2 #number of control variables\n", - "T = 20 #Prediction Horizon\n", - "dt = 0.25 #discretization step\n", + "N = 5 # number of state variables\n", + "M = 2 # number of control variables\n", + "T = 20 # Prediction Horizon\n", + "dt = 0.25 # discretization step\n", "\n", - "x = cp.Variable((N, T+1))\n", + "x = cp.Variable((N, T + 1))\n", "u = cp.Variable((M, T))" ] }, @@ -232,37 +233,40 @@ "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", " psi = x_bar[3]\n", " cte = x_bar[4]\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[4,3]=-v*np.cos(-psi)\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[3,1]=-1\n", - " B[4,0]=-np.sin(-psi)\n", - " B_lin=dt*B\n", - " \n", - " f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)\n", - " C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n", - " \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[4, 3] = -v * np.cos(-psi)\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[3, 1] = -1\n", + " B[4, 0] = -np.sin(-psi)\n", + " B_lin = dt * B\n", + "\n", + " f_xu = np.array(\n", + " [v * np.cos(theta), v * np.sin(theta), w, -w, v * np.sin(-psi)]\n", + " ).reshape(N, 1)\n", + " C_lin = dt * (\n", + " f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n", + " )\n", + "\n", + " return A_lin, B_lin, C_lin" ] }, { @@ -279,44 +283,36 @@ "outputs": [], "source": [ "# Define process model\n", - "def kinematics_model(x,t,u):\n", - " \"\"\"\n", - " \"\"\"\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", " dpsidt = -u[1]\n", - " dctedt = u[0]*np.sin(-x[3])\n", + " dctedt = u[0] * np.sin(-x[3])\n", "\n", - " dqdt = [dxdt,\n", - " dydt,\n", - " dthetadt,\n", - " dpsidt,\n", - " dctedt]\n", + " dqdt = [dxdt, dydt, dthetadt, dpsidt, dctedt]\n", "\n", " return dqdt\n", "\n", - "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" ] }, @@ -344,9 +340,9 @@ "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", @@ -355,7 +351,7 @@ "x0[3] = 0\n", "x0[4] = 1\n", "\n", - "x_bar=predict(x0,u_bar)" + "x_bar = predict(x0, u_bar)" ] }, { @@ -382,27 +378,27 @@ } ], "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", "plt.subplot(2, 2, 3)\n", - "plt.plot(np.degrees(x_bar[3,:]))\n", - "plt.ylabel('psi(t) [deg]')\n", - "#plt.xlabel('time')\n", + "plt.plot(np.degrees(x_bar[3, :]))\n", + "plt.ylabel(\"psi(t) [deg]\")\n", + "# plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 4)\n", - "plt.plot(x_bar[4,:])\n", - "plt.ylabel('cte(t)')\n", + "plt.plot(x_bar[4, :])\n", + "plt.ylabel(\"cte(t)\")\n", "\n", "plt.tight_layout()\n", "plt.show()" @@ -441,9 +437,9 @@ "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", @@ -452,18 +448,18 @@ "x0[3] = 0\n", "x0[4] = 1\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(5,1)\n", - " ut=u_bar[:,t-1].reshape(2,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(5, 1)\n", + " ut = u_bar[:, t - 1].reshape(2, 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)" ] }, { @@ -483,27 +479,27 @@ } ], "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(x_bar[2,:])\n", - "plt.ylabel('theta(t)')\n", - "#plt.xlabel('time')\n", + "plt.plot(x_bar[2, :])\n", + "plt.ylabel(\"theta(t)\")\n", + "# plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 3)\n", - "plt.plot(x_bar[3,:])\n", - "plt.ylabel('psi(t)')\n", - "#plt.xlabel('time')\n", + "plt.plot(x_bar[3, :])\n", + "plt.ylabel(\"psi(t)\")\n", + "# plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 4)\n", - "plt.plot(x_bar[4,:])\n", - "plt.ylabel('cte(t)')\n", + "plt.plot(x_bar[4, :])\n", + "plt.ylabel(\"cte(t)\")\n", "\n", "plt.tight_layout()\n", "plt.show()" @@ -533,7 +529,7 @@ "metadata": {}, "outputs": [], "source": [ - "def calc_err(state,path):\n", + "def calc_err(state, path):\n", " \"\"\"\n", " Finds psi and cte w.r.t. the closest waypoint.\n", "\n", @@ -542,72 +538,81 @@ " :returns: (float,float)\n", " \"\"\"\n", "\n", - " dx = state[0]-path[0,:]\n", - " dy = state[1]-path[1,:]\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", - "# front_axle_vect = [np.cos(state[2] - np.pi / 2),\n", - "# np.sin(state[2] - np.pi / 2)]\n", - " path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2),\n", - " np.sin(path[2,target_idx] + np.pi / 2)]\n", - " \n", - " #heading error w.r.t path frame\n", - " psi = path[2,target_idx] - state[2]\n", - " #psi = -path[2,target_idx] + state[2]\n", - " \n", + " # front_axle_vect = [np.cos(state[2] - np.pi / 2),\n", + " # np.sin(state[2] - np.pi / 2)]\n", + " path_ref_vect = [\n", + " np.cos(path[2, target_idx] + np.pi / 2),\n", + " np.sin(path[2, target_idx] + np.pi / 2),\n", + " ]\n", + "\n", + " # heading error w.r.t path frame\n", + " psi = path[2, target_idx] - state[2]\n", + " # psi = -path[2,target_idx] + state[2]\n", + "\n", " # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor\n", - " #cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)\n", - " cte = np.dot([dx[target_idx], dy[target_idx]],path_ref_vect)\n", + " # cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect)\n", + " cte = np.dot([dx[target_idx], dy[target_idx]], path_ref_vect)\n", "\n", - " return target_idx,psi,cte\n", + " return target_idx, psi, cte\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", " Interpolation range is computed to assure one point every fixed distance step [m].\n", - " \n", + "\n", " :param start_xp: array_like, list of starting x coordinates\n", " :param start_yp: array_like, list of starting y coordinates\n", " :param step: float, interpolation distance [m] between consecutive waypoints\n", " :returns: array_like, of shape (3,N)\n", " \"\"\"\n", "\n", - " final_xp=[]\n", - " final_yp=[]\n", - " delta = step #[m]\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, int(section_len / delta))\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,int(section_len/delta))\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))" + " return np.vstack((final_xp, final_yp, theta))" ] }, { @@ -623,36 +628,36 @@ "metadata": {}, "outputs": [], "source": [ - "track = compute_path_from_wp([0,5],[0,0])\n", + "track = compute_path_from_wp([0, 5], [0, 0])\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", - "_,psi,cte = calc_err(x0,track)\n", - "x0[3]=psi\n", - "x0[4]=cte\n", + "_, psi, cte = calc_err(x0, track)\n", + "x0[3] = psi\n", + "x0[4] = cte\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(5,1)\n", - " ut=u_bar[:,t-1].reshape(2,1)\n", - " \n", - " A,B,C=get_linear_model(xt,ut)\n", - " \n", - " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", - " \n", - " _,psi,cte = calc_err(xt_plus_one,track)\n", - " xt_plus_one[3]=psi\n", - " xt_plus_one[4]=cte\n", - " \n", - " x_bar[:,t]= xt_plus_one" + "for t in range(1, T + 1):\n", + " xt = x_bar[:, t - 1].reshape(5, 1)\n", + " ut = u_bar[:, t - 1].reshape(2, 1)\n", + "\n", + " A, B, C = get_linear_model(xt, ut)\n", + "\n", + " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n", + "\n", + " _, psi, cte = calc_err(xt_plus_one, track)\n", + " xt_plus_one[3] = psi\n", + " xt_plus_one[4] = cte\n", + "\n", + " x_bar[:, t] = xt_plus_one" ] }, { @@ -672,27 +677,27 @@ } ], "source": [ - "#plot trajectory\n", + "# plot trajectory\n", "plt.subplot(2, 2, 1)\n", - "plt.plot(x_bar[0,:],x_bar[1,:])\n", - "plt.plot(track[0,:],track[1,:],\"b-\")\n", - "plt.axis('equal')\n", - "plt.ylabel('y')\n", - "plt.xlabel('x')\n", + "plt.plot(x_bar[0, :], x_bar[1, :])\n", + "plt.plot(track[0, :], track[1, :], \"b-\")\n", + "plt.axis(\"equal\")\n", + "plt.ylabel(\"y\")\n", + "plt.xlabel(\"x\")\n", "\n", "plt.subplot(2, 2, 2)\n", - "plt.plot(x_bar[2,:])\n", - "plt.ylabel('theta(t)')\n", - "#plt.xlabel('time')\n", + "plt.plot(x_bar[2, :])\n", + "plt.ylabel(\"theta(t)\")\n", + "# plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 3)\n", - "plt.plot(x_bar[3,:])\n", - "plt.ylabel('psi(t)')\n", - "#plt.xlabel('time')\n", + "plt.plot(x_bar[3, :])\n", + "plt.ylabel(\"psi(t)\")\n", + "# plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 4)\n", - "plt.plot(x_bar[4,:])\n", - "plt.ylabel('cte(t)')\n", + "plt.plot(x_bar[4, :])\n", + "plt.ylabel(\"cte(t)\")\n", "\n", "plt.tight_layout()\n", "plt.show()" @@ -704,36 +709,36 @@ "metadata": {}, "outputs": [], "source": [ - "track = compute_path_from_wp([0,2,4,5,10],[0,0,3,1,1])\n", + "track = compute_path_from_wp([0, 2, 4, 5, 10], [0, 0, 3, 1, 1])\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] = 2\n", "x0[1] = 2\n", "x0[2] = np.radians(0)\n", - "_,psi,cte = calc_err(x0,track)\n", - "x0[3]=psi\n", - "x0[4]=cte\n", + "_, psi, cte = calc_err(x0, track)\n", + "x0[3] = psi\n", + "x0[4] = cte\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(5,1)\n", - " ut=u_bar[:,t-1].reshape(2,1)\n", - " \n", - " A,B,C=get_linear_model(xt,ut)\n", - " \n", - " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", - " \n", - " _,psi,cte = calc_err(xt_plus_one,track)\n", - " xt_plus_one[3]=psi\n", - " xt_plus_one[4]=cte\n", - " \n", - " x_bar[:,t]= xt_plus_one" + "for t in range(1, T + 1):\n", + " xt = x_bar[:, t - 1].reshape(5, 1)\n", + " ut = u_bar[:, t - 1].reshape(2, 1)\n", + "\n", + " A, B, C = get_linear_model(xt, ut)\n", + "\n", + " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n", + "\n", + " _, psi, cte = calc_err(xt_plus_one, track)\n", + " xt_plus_one[3] = psi\n", + " xt_plus_one[4] = cte\n", + "\n", + " x_bar[:, t] = xt_plus_one" ] }, { @@ -753,27 +758,27 @@ } ], "source": [ - "#plot trajectory\n", + "# plot trajectory\n", "plt.subplot(2, 2, 1)\n", - "plt.plot(x_bar[0,:],x_bar[1,:])\n", - "plt.plot(track[0,:],track[1,:],\"b-\")\n", - "plt.axis('equal')\n", - "plt.ylabel('y')\n", - "plt.xlabel('x')\n", + "plt.plot(x_bar[0, :], x_bar[1, :])\n", + "plt.plot(track[0, :], track[1, :], \"b-\")\n", + "plt.axis(\"equal\")\n", + "plt.ylabel(\"y\")\n", + "plt.xlabel(\"x\")\n", "\n", "plt.subplot(2, 2, 2)\n", - "plt.plot(x_bar[2,:])\n", - "plt.ylabel('theta(t)')\n", - "#plt.xlabel('time')\n", + "plt.plot(x_bar[2, :])\n", + "plt.ylabel(\"theta(t)\")\n", + "# plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 3)\n", - "plt.plot(x_bar[3,:])\n", - "plt.ylabel('psi(t)')\n", - "#plt.xlabel('time')\n", + "plt.plot(x_bar[3, :])\n", + "plt.ylabel(\"psi(t)\")\n", + "# plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 4)\n", - "plt.plot(x_bar[4,:])\n", - "plt.ylabel('cte(t)')\n", + "plt.plot(x_bar[4, :])\n", + "plt.ylabel(\"cte(t)\")\n", "\n", "plt.tight_layout()\n", "plt.show()" @@ -881,72 +886,72 @@ "source": [ "%%time\n", "\n", - "track = compute_path_from_wp([0,10],[0,0])\n", + "track = compute_path_from_wp([0, 10], [0, 0])\n", "\n", "MAX_SPEED = 2.5\n", "MIN_SPEED = 0.5\n", - "MAX_STEER_SPEED = 1.57/2\n", + "MAX_STEER_SPEED = 1.57 / 2\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.01\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.01\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = 1\n", "x0[2] = np.radians(-0)\n", - "_,psi,cte = calc_err(x0,track)\n", - "x0[3]=psi\n", - "x0[4]=cte\n", + "_, psi, cte = calc_err(x0, track)\n", + "x0[3] = psi\n", + "x0[4] = cte\n", "\n", "# Linearized Model Prediction\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].reshape(5,1)\n", - " ut=u_bar[:,t-1].reshape(2,1)\n", - " \n", - " A,B,C=get_linear_model(xt,ut)\n", - " \n", - " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", - " \n", - " _,psi,cte = calc_err(xt_plus_one,track)\n", - " xt_plus_one[3]=psi\n", - " xt_plus_one[4]=cte\n", - " x_bar[:,t]= xt_plus_one\n", + "for t in range(1, T + 1):\n", + " xt = x_bar[:, t].reshape(5, 1)\n", + " ut = u_bar[:, t - 1].reshape(2, 1)\n", + "\n", + " A, B, C = get_linear_model(xt, ut)\n", + "\n", + " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n", + "\n", + " _, psi, cte = calc_err(xt_plus_one, track)\n", + " xt_plus_one[3] = psi\n", + " xt_plus_one[4] = cte\n", + " x_bar[:, t] = xt_plus_one\n", "\n", "\n", - "#CVXPY Linear MPC problem statement\n", + "# CVXPY Linear MPC problem statement\n", "cost = 0\n", "constr = []\n", "\n", "for t in range(T):\n", - " \n", + "\n", " # Tracking\n", " if t > 0:\n", - " idx,_,_ = calc_err(x_bar[:,t],track)\n", - " delta_x = track[:,idx]-x[0:3,t]\n", - " cost+= cp.quad_form(delta_x,10*np.eye(3))\n", + " idx, _, _ = calc_err(x_bar[:, t], track)\n", + " delta_x = track[:, idx] - x[0:3, t]\n", + " cost += cp.quad_form(delta_x, 10 * np.eye(3))\n", "\n", " # Tracking last time step\n", " if t == T:\n", - " idx,_,_ = calc_err(x_bar[:,t],track)\n", - " delta_x = track[:,idx]-x[0:3,t]\n", - " cost+= cp.quad_form(delta_x,100*np.eye(3))\n", + " idx, _, _ = calc_err(x_bar[:, t], track)\n", + " delta_x = track[:, idx] - x[0:3, t]\n", + " cost += cp.quad_form(delta_x, 100 * np.eye(3))\n", "\n", " # Actuation rate of change\n", " if t < (T - 1):\n", - " cost += cp.quad_form(u[:, t + 1] - u[:, t], 25*np.eye(M))\n", - " \n", - " #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", + " cost += cp.quad_form(u[:, t + 1] - u[:, t], 25 * np.eye(M))\n", + "\n", + " # 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", "# sums problem objectives and concatenates constraints.\n", - "constr += [x[:,0] == x0]\n", + "constr += [x[:, 0] == x0]\n", "constr += [u[0, :] <= MAX_SPEED]\n", "constr += [u[0, :] >= MIN_SPEED]\n", "constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", @@ -987,41 +992,41 @@ } ], "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", - "psi_mpc=np.array(x.value[3, :]).flatten()\n", - "cte_mpc=np.array(x.value[4, :]).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", + "psi_mpc = np.array(x.value[3, :]).flatten()\n", + "cte_mpc = np.array(x.value[4, :]).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", - "plt.figure(figsize=(15,10))\n", - "#plot trajectory\n", + "# simulate robot state trajectory for optimized U\n", + "x_traj = predict(x0, np.vstack((v_mpc, w_mpc)))\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.ylabel(\"w(t)\")\n", + "# plt.xlabel('time')\n", "\n", "plt.subplot(2, 2, 4)\n", "plt.plot(cte_mpc)\n", - "plt.ylabel('cte(t)')\n", - "plt.legend(loc='best')\n", + "plt.ylabel(\"cte(t)\")\n", + "plt.legend(loc=\"best\")\n", "\n", "plt.tight_layout()\n", "plt.show()" @@ -1056,60 +1061,59 @@ } ], "source": [ + "track = compute_path_from_wp(\n", + " [0, 5, 7.5, 10, 12, 13, 13, 10], [0, 0, 2.5, 2.5, 0, 0, 5, 10]\n", + ")\n", "\n", - "track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", - " [0,0,2.5,2.5,0,0,5,10])\n", + "sim_duration = 100\n", + "opt_time = []\n", "\n", - "sim_duration =100 \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", - "MAX_STEER_SPEED = 1.57/2\n", + "MAX_STEER_SPEED = 1.57 / 2\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = -0.5\n", "x0[2] = np.radians(-0)\n", - "_,psi,cte = calc_err(x0,track)\n", - "x0[3]=psi\n", - "x0[4]=cte\n", + "_, psi, cte = calc_err(x0, track)\n", + "x0[3] = psi\n", + "x0[4] = cte\n", "\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.01\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.01\n", + "\n", + "for sim_time in range(sim_duration - 1):\n", + "\n", + " iter_start = time.time()\n", "\n", - "for sim_time in range(sim_duration-1):\n", - " \n", - " iter_start=time.time()\n", - " \n", " # Prediction\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", - " for t in range (1,T+1):\n", - " xt=x_bar[:,t-1].reshape(5,1)\n", - " ut=u_bar[:,t-1].reshape(2,1)\n", + " for t in range(1, T + 1):\n", + " xt = x_bar[:, t - 1].reshape(5, 1)\n", + " ut = u_bar[:, t - 1].reshape(2, 1)\n", "\n", - " A,B,C=get_linear_model(xt,ut)\n", + " A, B, C = get_linear_model(xt, ut)\n", "\n", - " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", + " xt_plus_one = np.squeeze(np.dot(A, xt) + np.dot(B, ut) + C)\n", "\n", - " _,psi,cte = calc_err(xt_plus_one,track)\n", - " xt_plus_one[3]=psi\n", - " xt_plus_one[4]=cte\n", + " _, psi, cte = calc_err(xt_plus_one, track)\n", + " xt_plus_one[3] = psi\n", + " xt_plus_one[4] = cte\n", "\n", - " x_bar[:,t]= xt_plus_one\n", + " x_bar[:, t] = xt_plus_one\n", "\n", - "\n", - " #CVXPY Linear MPC problem statement\n", + " # CVXPY Linear MPC problem statement\n", " cost = 0\n", " constr = []\n", "\n", @@ -1117,56 +1121,58 @@ "\n", " # Tracking\n", " if t > 0:\n", - " idx,_,_ = calc_err(x_bar[:,t],track)\n", - " delta_x = track[:,idx]-x[0:3,t]\n", - " cost+= cp.quad_form(delta_x,10*np.eye(3))\n", - " \n", + " idx, _, _ = calc_err(x_bar[:, t], track)\n", + " delta_x = track[:, idx] - x[0:3, t]\n", + " cost += cp.quad_form(delta_x, 10 * np.eye(3))\n", + "\n", " # Tracking last time step\n", " if t == T:\n", - " idx,_,_ = calc_err(x_bar[:,t],track)\n", - " delta_x = track[:,idx]-x[0:3,t]\n", - " cost+= cp.quad_form(delta_x,100*np.eye(3))\n", + " idx, _, _ = calc_err(x_bar[:, t], track)\n", + " delta_x = track[:, idx] - x[0:3, t]\n", + " cost += cp.quad_form(delta_x, 100 * np.eye(3))\n", "\n", " # Actuation rate of change\n", " if t < (T - 1):\n", - " cost += cp.quad_form(u[:, t + 1] - u[:, t], 25*np.eye(M))\n", - " \n", + " cost += cp.quad_form(u[:, t + 1] - u[:, t], 25 * 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", " # 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", + " 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_sim[:,sim_time]] # starting condition\n", + " constr += [x[:, 0] == x_sim[:, sim_time]] # starting 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", + ")" ] }, { @@ -1186,25 +1192,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()"