def objective(self, u):
N = self.N
T = self.T
t0 = self.t0
x0 = self.x0
path = self.path
obstacle = self.obstacle
posIdx = self.posIdx
V_cmd = self.V_cmd
fHandleCost = self.fHandleCost
x = prob.computeOpenloopSolution(u, N, T, t0, x0)
costvec = np.zeros([3 * N + 2, 1])
for k in range(N):
uk = np.array([u[k], u[k + N]])
costout = prob.runningCosts(uk, x[k], t0 + k * T, path, obstacle,
posIdx, V_cmd)
costvec[k] = costout[0] # V
costvec[k + N] = costout[1] # Vdot or Vddot
costvec[k + 2 * N] = costout[2] # Chidot or Chiddot
cost_goalDist, cost_goalDelChi = prob.goalCost(x0, t0) # goalcost_opt1
#cost_goalDist, cost_goalDelChi = prob.goalCost(x[-1,:], t0) # goalcost_opt2
costvec[3 * N] = cost_goalDist # goal dist
costvec[3 * N + 1] = cost_goalDelChi # goal delta chi
cost = np.sum(costvec)
return cost
def constraints(self, u):
N = self.N
T = self.T
t0 = self.t0
x0 = self.x0
lanes = self.lanes
obstacle = self.obstacle
x = prob.computeOpenloopSolution(u, N, T, t0, x0)
# running constraints
consR1_R = np.zeros(N)
consR1_L = np.zeros(N)
for k in range(N):
consR1_R[k], consR1_L[k] = prob.runningCons(
u, x[k], t0, lanes, obstacle)
consR1 = np.concatenate([consR1_R, consR1_L])
if nstates == 6:
consR2 = np.array([
x[0, idx_V] * x[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR3 = np.array([x[0, idx_V]]) # velocity
constmp = np.concatenate([consR1, consR2])
consR = np.concatenate([constmp, consR3])
# terminal constraint
consT1, consT2 = prob.terminalCons(u, x[N - 1], t0, lanes,
obstacle)
consT = np.concatenate([consT1, consT2])
elif nstates == 4:
consR2 = np.array([x[0, idx_V] * u[idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint
consT1, consT2 = prob.terminalCons(u, x[N - 1], t0, lanes,
obstacle) # ydist, VEnd
consT = consT1
# total constraints
cons = np.concatenate([consR, consT])
return cons
def objective(self, u):
N = self.N
T = self.T
t0 = self.t0
x0 = self.x0
lanes = self.lanes
obstacle = self.obstacle
posIdx = self.posIdx
x = prob.computeOpenloopSolution(u, N, T, t0, x0)
cost = 0.0
costvec = np.zeros([N, 1])
for k in range(N):
uk = np.array([u[k], u[k + N]])
costvec[k] = prob.runningCosts(uk, x[k], t0 + k * T, lanes,
obstacle, posIdx)
cost = cost + costvec[k]
return cost
def objective(self, u):
N = self.N
T = self.T
t0 = self.t0
x0 = self.x0
path = self.path
obstacle = self.obstacle
posIdx = self.posIdx
V_cmd = self.V_cmd
fHandleCost = self.fHandleCost
x00_23 = self.x00_23
x = prob.computeOpenloopSolution(u, N, T, t0, x0, x00_23)
costvec = np.zeros([3 * N + 2, 1])
for k in range(N):
uk = np.array([u[k], u[k + N]])
costout = prob.runningCosts(uk, x[k], t0 + k * T, path, obstacle,
posIdx, V_cmd)
costvec[k] = costout[0] # V
costvec[k + N] = costout[1] # Vdot or Vddot
costvec[k + 2 * N] = costout[2] # Chidot or Chiddot
cost_goalDist, cost_goalDelChi = prob.goalCost(x0, t0)
#cost_goalDist, cost_goalDelChi = prob.goalCost(x[-1,:], t0)
costvec[3 * N] = cost_goalDist # goal dist
costvec[3 * N + 1] = cost_goalDelChi # goal delta chi
cost = np.sum(costvec)
# write data once for analysis later using a global variable. other mentioned can be developed to not use the
# global variable - but this was the least intrusive way of adding the functionality
if globalVars.writeToFileCost == True:
for k in range(3 * N):
fHandleCost.write('%.2f ' % (costvec[k]))
fHandleCost.write('%.2f ' % (costvec[3 * N]))
fHandleCost.write('%.2f ' % (costvec[3 * N + 1]))
fHandleCost.write('\n')
globalVars.writeToFileCost = False
return cost
def getCostVec(u, N, T, t0, x0, path, obstacle, posIdx, V_cmd):
import probInfo
import numpy as np
x = probInfo.computeOpenloopSolution(u, N, T, t0, x0)
costvec = np.zeros([3 * N + 2, 1])
for k in range(N):
uk = np.array([u[k], u[k + N]])
costout = probInfo.runningCosts(uk, x[k], t0 + k * T, path, obstacle, posIdx, V_cmd)
costvec[k] = costout[0] # V
costvec[k + N] = costout[1] # Vdot or Vddot
costvec[k + 2 * N] = costout[2] # Chidot or Chiddot
cost_goalDist, cost_goalDelChi = probInfo.goalCost(x0, t0) # goalcost_opt1
# cost_goalDist, cost_goalDelChi = probInfo.goalCost(x[-1,:], t0) # goalcost_opt2
costvec[3 * N] = cost_goalDist # goal dist
costvec[3 * N + 1] = cost_goalDelChi # goal delta chi
return costvec
def constraints(self, u):
N = self.N
T = self.T
t0 = self.t0
x0 = self.x0
lanes = self.lanes
obstacle = self.obstacle
posIdx = self.posIdx
ns_option = self.ns_option
x = prob.computeOpenloopSolution(u, N, T, t0, x0)
# running constraints
consR1_R = np.zeros(N)
consR1_L = np.zeros(N)
for k in range(N):
consR1_R[k], consR1_L[k] = prob.runningCons(
u, x[k], t0, lanes, obstacle, posIdx)
consR1 = np.concatenate([consR1_R, consR1_L])
if ns == 6:
if ns_option == 1: # Additional Current velocity + Terminal velocity constraint
consR2 = np.array([
x[0, idx_V] * x[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR3 = np.array([x[0, idx_V]]) # current velocity
constmp = np.concatenate([consR1, consR2])
consR = np.concatenate([constmp, consR3])
# terminal constraint (dy, V)
consT1, consT2 = prob.terminalCons(u, x[N - 1], t0, lanes,
obstacle, posIdx)
consT = np.concatenate([consT1, consT2])
elif ns_option == 2:
# No terminal velocity constraint
consR2 = np.array([
x[0, idx_V] * x[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint (dy, dV)
consT1, consT2 = prob.terminalCons(u, x[N - 1], t0, lanes,
obstacle, posIdx)
consT = np.concatenate([consT1, consT2])
elif ns_option == 3:
# No terminal velocity constraint
consR2 = np.array([
x[0, idx_V] * x[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint (dy)
consT1, consT2 = prob.terminalCons(u, x[N - 1], t0, lanes,
obstacle, posIdx)
consT = consT1
elif ns == 4:
if ns_option == 1:
u_mat = u.reshape(2, -1).T
consR2 = np.array([
x[0, idx_V] * u_mat[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR3 = np.array([x[0, idx_V]]) # current velocity
constmp = np.concatenate([consR1, consR2])
consR = np.concatenate([constmp, consR3])
# terminal constraint (dy, V)
consT1, consT2 = prob.terminalCons(u, x[N - 1], t0, lanes,
obstacle, posIdx)
consT = np.concatenate([consT1, consT2])
elif ns_option == 2:
u_mat = u.reshape(2, -1).T
consR2 = np.array([
x[0, idx_V] * u_mat[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint
consT1, consT2 = prob.terminalCons(u, x[N - 1], t0, lanes,
obstacle,
posIdx) # ydist, VEnd
#consT1 = []
#consT2 = []
consT = np.concatenate([consT1, consT2])
elif ns_option == 3:
u_mat = u.reshape(2, -1).T
consR2 = np.array([
x[0, idx_V] * u_mat[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint
consT1, consT2 = prob.terminalCons(u, x[N - 1], t0, lanes,
obstacle,
posIdx) # ydist, VEnd
consT = consT1
# total constraints
cons = np.concatenate([consR, consT])
return cons
def nmpcPlotSol(u_new,path,drawLPPath,x0,obstacle,pathType):
u_mpciter = u_new.flatten(1)
x_mpciter = probInfo.computeOpenloopSolution(u_mpciter, pdata.N, pdata.T, pdata.t0, x0)
East = x_mpciter[:,0]
North = x_mpciter[:,1]
V_terminal = x_mpciter[-1,2]
# figure 1
plt.figure(1,figsize=(5, 7), dpi=100)
plt.ylabel('N [ft]')
plt.xlabel('E [ft]')
#plt.axis('equal')
if drawLPPath == True:
# Detailed Path
plt.plot(path.pathData.E, path.pathData.N, linestyle='--', color='c')
plt.plot(path.pathData.PathStartPoint[0], path.pathData.PathStartPoint[1], marker='o', markersize=8, color='r')
plt.plot(path.pathData.PathEndPoint[0], path.pathData.PathEndPoint[1], marker='o', markersize=8, color='g')
if True:
plt.plot(path.pathData.PathRightEndPointsE, path.pathData.PathRightEndPointsN,'m+')
plt.plot(path.pathData.PathLeftEndPointsE, path.pathData.PathLeftEndPointsN,'m+')
x1 = path.pathData.PathRightEndPointsE
x2 = path.pathData.PathLeftEndPointsE
y1 = path.pathData.PathRightEndPointsN
y2 = path.pathData.PathLeftEndPointsN
if pathType == 'default':
plt.plot(x1, y1, 'm', x2, y2, 'm')
else:
#plt.plot(x1, y1, 'g', x2, y2, 'g--')
plt.plot(x1, y1, 'm', x2, y2, 'm')
x3 = path.pathData.PathCenterEndPointsE + pdata.delta_yRoad*np.sin(path.pathData.Theta_endpoints)
x4 = path.pathData.PathCenterEndPointsE - pdata.delta_yRoad*np.sin(path.pathData.Theta_endpoints)
y3 = path.pathData.PathCenterEndPointsN - pdata.delta_yRoad*np.cos(path.pathData.Theta_endpoints)
y4 = path.pathData.PathCenterEndPointsN + pdata.delta_yRoad*np.cos(path.pathData.Theta_endpoints)
if pathType == 'default':
plt.plot(x3, y3, 'r', x4, y4, 'r')
else:
#plt.plot(x3, y3, 'k', x4, y4, 'k--')
plt.plot(x3, y3, 'r', x4, y4, 'r')
plt.grid(True)
if True: # obstacle is present:
nObs = obstacle.E.size
if nObs > 0:
for k in range(nObs):
REPEAT_MIDTERM = False
if REPEAT_MIDTERM:
Efc = obstacle.E[k] + pdata.pathWidth/2
Nfc = obstacle.N[k]
W = obstacle.w[k] - pdata.pathWidth
L = obstacle.l[k]
Theta = obstacle.Chi[k]
fc = "red"
polygon_obstacle = getPatch(Efc, Nfc, W, L, Theta, fc)
else:
Elb = obstacle.E[k] + obstacle.SM
Nlb = obstacle.N[k] + obstacle.SM
W = obstacle.w[k] - 2.0 * obstacle.SM
L = obstacle.l[k] - 2.0 * obstacle.SM
Theta = obstacle.Chi[k]
fc = "red"
polygon_obstacle = getPatch(Elb, Nlb, W, L, Theta, fc)
Elb = obstacle.E[k]
Nlb = obstacle.N[k]
W = obstacle.w[k]
L = obstacle.l[k]
Theta = obstacle.Chi[k]
fc = "green"
polygon_safezone = getPatch(Elb, Nlb, W, L, Theta, fc)
ax = plt.gca()
ax.add_patch(polygon_safezone)
ax.add_patch(polygon_obstacle)
nEN = len(East)
plt.plot(East[0:nEN], North[0:nEN], marker='x', markersize=4, color='b')
plt.plot(East[0], North[0], marker='o', markersize=4, color='r')
#plt.ylim([0, 240])
plt.xlim([-25, 75])
#plt.axis('equal')
#plt.draw()
plt.pause(0.01)
#if mpciter == mpciterations-1:
# ax1 = f1.gca()
# del ax1.lines[7:12]
#dummy = raw_input('Press Enter to continue: ')
return V_terminal
def nmpcPlotSol(u_new, path, x0, obstacle, pathType, mpciter,
detectionWindowParam):
u_mpciter = u_new.flatten(1)
x_mpciter = probInfo.computeOpenloopSolution(u_mpciter, pdata.N, pdata.T,
pdata.t0, x0)
East = x_mpciter[:, 0]
North = x_mpciter[:, 1]
V_terminal = x_mpciter[-1, 2]
# figure 1
plt.figure(1, figsize=(5, 7), dpi=100)
plt.ylabel('N [ft]')
plt.xlabel('E [ft]')
#plt.axis('equal')
# Detailed Path
plt.plot(path.pathData.E, path.pathData.N, linestyle='--', color='c')
plt.plot(path.pathData.PathStartPoint[0],
path.pathData.PathStartPoint[1],
marker='o',
markersize=8,
color='r')
plt.plot(path.pathData.PathEndPoint[0],
path.pathData.PathEndPoint[1],
marker='o',
markersize=8,
color='g')
# draw all lines
if False:
plt.plot(path.pathData.PathRightEndPointsE,
path.pathData.PathRightEndPointsN, 'm+')
plt.plot(path.pathData.PathLeftEndPointsE,
path.pathData.PathLeftEndPointsN, 'm+')
x1 = path.pathData.PathRightEndPointsE
x2 = path.pathData.PathLeftEndPointsE
y1 = path.pathData.PathRightEndPointsN
y2 = path.pathData.PathLeftEndPointsN
if pathType == 'default':
plt.plot(x1, y1, 'm', x2, y2, 'm')
else:
#plt.plot(x1, y1, 'g', x2, y2, 'g--')
plt.plot(x1, y1, 'm', x2, y2, 'm')
x3 = path.pathData.PathCenterEndPointsE + pdata.delta_yRoad * np.sin(
path.pathData.Theta_endpoints)
x4 = path.pathData.PathCenterEndPointsE - pdata.delta_yRoad * np.sin(
path.pathData.Theta_endpoints)
y3 = path.pathData.PathCenterEndPointsN - pdata.delta_yRoad * np.cos(
path.pathData.Theta_endpoints)
y4 = path.pathData.PathCenterEndPointsN + pdata.delta_yRoad * np.cos(
path.pathData.Theta_endpoints)
if pathType == 'default':
plt.plot(x3, y3, 'r', x4, y4, 'r')
else:
#plt.plot(x3, y3, 'k', x4, y4, 'k--')
plt.plot(x3, y3, 'r', x4, y4, 'r')
plt.grid(True)
# draw obstacles
if True:
nObs = obstacle.E.size
if nObs > 0:
for k in range(nObs):
Ec = obstacle.E[k]
Nc = obstacle.N[k]
W = obstacle.w[k]
L = obstacle.l[k]
Theta = -obstacle.Chi[k]
fc = "red"
polygon_obstacle = getPatch(Ec, Nc, W, L, Theta, fc)
Ec = obstacle.E[k]
Nc = obstacle.N[k]
W = 2 * obstacle.sr[k]
L = 2 * obstacle.sr[k]
Theta = -obstacle.Chi[k]
fc = "green"
#polygon_safezone = getPatch(Ec, Nc, W, L, Theta, fc)
#ellipse_safezone = patches.Ellipse((Ec,Nc), W, L, angle=Theta, fc=fc)
ellipse_safezone = patches.Ellipse((Ec, Nc),
W,
L,
angle=Theta,
fc=fc)
ax = plt.gca()
ax.add_patch(ellipse_safezone)
ax.add_patch(polygon_obstacle)
# draw detection window
if False: # and ( (mpciter== 0) or (mpciter==5) ):
p1Win, p2Win, p3Win, p4Win = odata.window(x0, detectionWindowParam)
p1Obs = [obstacle.E_corners[0, 0], obstacle.N_corners[0, 0]]
p2Obs = [obstacle.E_corners[0, 1], obstacle.N_corners[0, 1]]
p3Obs = [obstacle.E_corners[0, 2], obstacle.N_corners[0, 2]]
p4Obs = [obstacle.E_corners[0, 3], obstacle.N_corners[0, 3]]
L1 = plt.plot([p1Win[0], p2Win[0]], [p1Win[1], p2Win[1]], 'c')
L2 = plt.plot([p2Win[0], p3Win[0]], [p2Win[1], p3Win[1]], 'c')
L3 = plt.plot([p3Win[0], p4Win[0]], [p3Win[1], p4Win[1]], 'c')
L4 = plt.plot([p4Win[0], p1Win[0]], [p4Win[1], p1Win[1]], 'c')
if mpciter == 1:
None
nEN = len(East)
# plt.plot(East[0:nEN], North[0:nEN], marker='x', markersize=4, color='b')
# plt.plot(East[0], North[0], marker='o', markersize=4, color='r')
plt.plot(East[1:nEN], North[1:nEN], marker='x', markersize=4, color='b')
plt.plot(East[1], North[1], marker='o', markersize=4, color='r')
#plt.ylim([0, 240])
#plt.xlim([-10, 40])
plt.axis('equal')
#plt.draw()
plt.pause(0.01)
#if mpciter == mpciterations-1:
# ax1 = f1.gca()
# del ax1.lines[7:12]
#dummy = raw_input('Press Enter to continue: ')
Chi_N = x_mpciter[-1, 3] * 180 / np.pi
p1 = x_mpciter[-1, 0:2]
p2 = pdata.endPoint
dx = p2[0] - p1[0] # dE
dy = p2[1] - p1[1] # dN
Chi_goal = np.arctan2(dx, dy) * 180 / np.pi # w.r.t. +ve y axis
delChi = Chi_goal - Chi_N
# print(Chi_goal, Chi_N, Chi_goal - Chi_N)
# plt.figure(100)
# plt.plot(mpciter * pdata.T, Chi_goal, 'bo', pdata.T * mpciter, Chi_N, 'ro')
# plt.xlabel('t [sec]')
# plt.ylabel('Chigoal, Chi_N [deg]')
# plt.grid('on')
if pdata.ns == 4:
latAccel = x_mpciter[0, 2] * u_new[0, 1] # V * Chidot
elif pdata.ns == 6:
latAccel = x_mpciter[0, 2] * x_mpciter[0, 5] # V * Chidot
else:
latAccel = []
if abs(East[-1]) < 5:
None
return latAccel / 32.2, V_terminal, delChi
def Main(isBatch, showPlot, kRun=None, fBatchRun=None):
# -------------------------------------------------------------------
# Main.py lets the user run different test cases for Model
# Predictive Control (MPC) based trajectory generation
#
# The user needs to select 3 items:
# 1. Test Case Type: Road shape and orientation
# 2. Experiment Number: Sets various design parameters for MPC
# 3. Number of States: Sets the vehicles states and control states
#
# Edit problemData.py to run different experiments
#
# 01/25/2018
# -------------------------------------------------------------------
# For saving data and figures
rundate = datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")
rundir = './run_' + rundate + '/'
distutils.dir_util.mkpath(rundir)
# Path data
pathClass = pathMain.pathInfo('default', pdata.startPoint, pdata.endPoint)
path = pathClass()
pathType = 'default'
# Obstacle data
obstaclePresent, nObstacle, obstacleE, obstacleN, obstacleChi, obstacleLength, obstacleWidth, \
obstacleSafeLength, obstacleSafeWidth, obstacleSafeRadius, safeDistance, detectionWindowParam = \
obstacleData.createObstacleData(pdata.no, pdata.scaleFactorE, pdata.scaleFactorN,
pdata.widthSpace, pdata.lengthSpace, pdata.horzDistance,
rundate, rundir)
obstacleClass = obstacleData.obstacleInfo(obstaclePresent, obstacleE,
obstacleN, obstacleChi,
obstacleWidth, obstacleLength,
obstacleSafeWidth,
obstacleSafeLength,
obstacleSafeRadius)
obstacle = obstacleClass()
# Storage data
t = np.zeros([pdata.mpciterations, 1])
x = np.zeros([pdata.mpciterations, pdata.nx])
u = np.zeros([pdata.mpciterations, pdata.nu])
# Iteration data
pdata.x0[3] = np.pi / 2 - path.pathData.Theta[
0] # align vehicle heading with road heading
tmeasure = pdata.t0
xmeasure = pdata.x0
u_new = np.zeros([1, pdata.nu])
mpciter = 0
# Other parameters
t_slowDown = []
t_slowDown_detected = False
delChi_maxvec_obstacleInView = np.array([], dtype=float)
delChi_maxvec_obstacleNotInView = np.array([], dtype=float)
Dist_stop = 0.0
T_stop = 0.0
fVbnd = False
# Print to File
writeToFile = True
if writeToFile == True:
fileName = 'logFile.txt'
fHandle = open(fileName, 'w')
else:
fHandle = -1
fileName = ''
debug = True
if debug == True:
globalVars.writeToFileCost = True
fileNameCost = 'logFileCost.txt'
if os.path.isfile(fileNameCost) == True:
os.remove('logFileCost.txt') # remove previous file
fHandleCost = open(
fileNameCost,
'a') # file to append cost (see nlp.py > objective function)
else:
fHandleCost = -1
fileNameCost = ''
# Initialize storage arrays
tElapsed = np.zeros(pdata.mpciterations)
VTerminal = np.zeros(pdata.mpciterations)
latAccel = np.zeros(pdata.mpciterations)
delChi = np.zeros(pdata.mpciterations)
breakLoop1 = False
# Speficy initial position index
posIdx = obstacleData.getPosIdx(pdata.x0[0], pdata.x0[1], path,
pdata.posIdx0)
# Specify Booleans
saveData = True
plotData = True
trimVals = True
# Create array of paths
pathObj = obstacleData.makePathObj(pdata, path, obstacle)
pathObjArray = [pathObj]
# Main loop
while mpciter < pdata.mpciterations:
# start time keeping
tStart = time.time()
# get new initial value
t0, x0 = nmpc.measureInitialValue(tmeasure, xmeasure)
# search for obstacle
detected, obstacleID = obstacleData.detectObstacle(
x0, detectionWindowParam, obstacle)
if detected == True:
delChi_max = pdata.delChi_max_InView
delChi_maxvec_obstacleInView = \
obstacleData.np.concatenate([delChi_maxvec_obstacleInView, obstacleData.np.array([pdata.delChi_max_InView])])
delChi_maxvec_obstacleNotInView = \
obstacleData.np.concatenate([delChi_maxvec_obstacleNotInView, obstacleData.np.array([0])])
#print('Obstacle(s) detected at mpciter = ' + str(mpciter))
else:
delChi_max = pdata.delChi_max_NotInView
delChi_maxvec_obstacleNotInView = \
obstacleData.np.concatenate([delChi_maxvec_obstacleNotInView, obstacleData.np.array([pdata.delChi_max_NotInView])])
delChi_maxvec_obstacleInView = \
obstacleData.np.concatenate([delChi_maxvec_obstacleInView, obstacleData.np.array([0])])
#print('No obstacle detected at mpciter = ' + str(mpciter))
# solve optimal control problem
u_new, info = nmpc.solveOptimalControlProblem(
pdata.N, t0, x0, pdata.u0, pdata.T, pdata.ncons, pdata.nu, path,
obstacle, posIdx, pdata.ncons_option, pdata.V_cmd, pdata.lb_VTerm,
pdata.lb_VdotVal, delChi_max, obstacleID, safeDistance,
fHandleCost)
tElapsed[mpciter] = (time.time() - tStart)
# stop iteration if solution is not "solved" for "acceptable"
if info['status'] > 1 or info['status'] < -1:
breakLoop1 = True
# write the batch run number where solution was not obtained
if isBatch:
fBatchRun.write("%d %d\n" % (kBatchRun, info['status']))
# mpc future path plot
latAccel[mpciter], VTerminal[mpciter], delChi[
mpciter] = printPlots.nmpcPlotSol(u_new, path, x0, obstacle,
pathType, mpciter,
detectionWindowParam)
# solution information
printPlots.nmpcPrint(mpciter, info, pdata.N, x0, u_new, writeToFile,
fHandle, tElapsed[mpciter], latAccel[mpciter],
VTerminal[mpciter], delChi[mpciter])
# store closed loop data
t[mpciter] = tmeasure
for k in range(pdata.nx):
x[mpciter, k] = xmeasure[k]
for j in range(pdata.nu):
u[mpciter, j] = u_new[0, j]
# change flag (global variable) to write cost breakdown in nlp.py
if debug is True:
writeToFileCost = True
# apply control
tmeasure, xmeasure = nmpc.applyControl(pdata.T, t0, x0, u_new)
# prepare restart
u0 = nmpc.shiftHorizon(pdata.N, u_new)
posIdx = obstacleData.getPosIdx(xmeasure[0], xmeasure[1], path, posIdx)
# reset global variable to write cost breakdown in nlp.py
globalVars.writeToFileCost = True
x_mpciter = probInfo.computeOpenloopSolution(u0.flatten(1), pdata.N,
pdata.T, t0, x0)
current_point = x_mpciter[0, 0:2]
terminal_point = x_mpciter[-1, 0:2]
# stop vehicle if required
breakLoop2, V_cmd, t_slowDown, t_slowDown_detected, lb_VTerm, lb_VdotVal = \
utils.vehicleStop(pdata.T, x, mpciter, pdata.decelType, terminal_point, pdata.endPoint,
pdata.lb_reachedGoal, pdata.lb_reachedNearGoal, pdata.zeroDistanceChange,
t_slowDown_detected, tmeasure, pdata.V_cmd, pdata.lb_VTermSlowDown, pdata.lb_VdotValSlowDown, pdata.decel,
t_slowDown, pdata.lb_VTerm, pdata.lb_VdotVal)
# break loop is solution not obtained or vehicle stops
if (breakLoop1 == True) or (breakLoop2 == True):
break
# next iteration
mpciter = mpciter + 1
if trimVals is True:
mpciterations = mpciter
tElapsed = tElapsed[0:mpciterations]
VTerminal = VTerminal[0:mpciterations]
latAccel = latAccel[0:mpciterations]
delChi = delChi[0:mpciterations]
t = t[0:mpciterations, :]
x = x[0:mpciterations, :]
u = u[0:mpciterations, :]
# close log file
if writeToFile == True:
fHandle.close()
if debug == True:
fHandleCost.close()
# start preparing for generating plots
#rundate = datetime.datetime.now().strftime("%Y-%m-%d")
#rundir = './run_' + rundate + '/'
if pdata.N < 10:
suffix = '_N0' + str(pdata.N) + '_Tp' + str(int(
10 * pdata.T)) + '_ns' + str(pdata.ns) + '_no' + str(pdata.no)
else:
suffix = '_N' + str(pdata.N) + '_Tp' + str(int(
10 * pdata.T)) + '_ns' + str(pdata.ns) + '_no' + str(pdata.no)
#suffix = suffix + pdata.suffix_Popup_NoPopup # suffix_Popup_NoPopup = '_NoPopup' set in problemData.py
# start preparation for saving plots if required
if saveData == True:
distutils.dir_util.mkpath(rundir)
dst_file = rundir + 'logFile' + suffix + '.txt'
shutil.copyfile('logFile.txt', dst_file)
# figure 1: path
dst_fig = rundir + 'path' + suffix + '_Before.png'
fig = plt.figure(1)
plt.pause(0.01)
fig.savefig(dst_fig)
obstacleDict = obstacleData.obstacleDict_from_ClassInstance(obstacle)
file_pkl = rundir + 'pathDict_no' + str(pdata.no) + '.pkl'
obstacleData.savepkl((pathObjArray, obstacleDict), file_pkl)
file_pkl2 = rundir + 'pathDict_no' + str(pdata.no) + '_b.pkl'
obstacleData.savepkl(obstacle, file_pkl2)
print('saved data and figure')
# create plots
oldpwd = os.getcwd()
os.chdir(rundir)
settingsFile = 'settings' + suffix + '.txt'
figno = printPlots.nmpcPlot(t, x, u, path, obstacle, tElapsed, VTerminal,
latAccel, delChi, settingsFile, pathObjArray,
t_slowDown, delChi_maxvec_obstacleInView,
delChi_maxvec_obstacleNotInView)
os.chdir(oldpwd)
if saveData == True:
# figure 2: E, N
dst_fig = rundir + 'E-N' + suffix + '.png'
fig = plt.figure(2)
plt.pause(0.01)
fig.savefig(dst_fig)
# figure 3: V, Vdot
dst_fig = rundir + 'V-Vdot' + suffix + '.png'
fig = plt.figure(3)
plt.pause(0.01)
fig.savefig(dst_fig)
# figure 4: Chi, Chidot
dst_fig = rundir + 'Chi-Chidot' + suffix + '.png'
fig = plt.figure(4)
plt.pause(0.01)
fig.savefig(dst_fig)
# figure 5: LatAccel, dy
dst_fig = rundir + 'LatAccel-dy' + suffix + '.png'
fig = plt.figure(6)
plt.pause(0.01)
fig.savefig(dst_fig)
if pdata.ns == 6:
# figure 6: V, Vdot
dst_fig = rundir + 'Vddot-Chiddot' + suffix + '.png'
fig = plt.figure(5)
plt.pause(0.01)
fig.savefig(dst_fig)
# figure 7: CPU time
dst_fig = rundir + 'CPUtime' + suffix + '.png'
fig = plt.figure(7)
plt.pause(0.01)
fig.savefig(dst_fig)
# figure 8: V-terminal
dst_fig = rundir + 'V-terminal' + suffix + '.png'
fig = plt.figure(8)
plt.pause(0.01)
fig.savefig(dst_fig)
# figure 9: path
dst_fig = rundir + 'path' + suffix + '_After.png'
fig = plt.figure(9)
plt.pause(0.01)
fig.savefig(dst_fig)
print('done!')
# show plots for single run, close plots for batch run
if showPlot:
plt.show()
else:
plt.pause(2)
plt.close("all")
def setup(self, u0):
N = self.N
T = self.T
t0 = self.t0
x0 = self.x0
nu = self.nu
lanes = self.lanes
obstacle = self.obstacle
if nstates == 6:
lb_Vddot = np.ones([N, 1]) * lb_VddotVal
lb_Chiddot = np.ones([N, 1]) * lb_ChiddotVal
ub_Vddot = np.ones([N, 1]) * ub_VddotVal
ub_Chiddot = np.ones([N, 1]) * ub_ChiddotVal
lb = np.concatenate([lb_Vddot, lb_Chiddot])
ub = np.concatenate([ub_Vddot, ub_Chiddot])
elif nstates == 4:
lb_Vdot = np.ones([N, 1]) * lb_VdotVal
lb_Chidot = np.ones([N, 1]) * lb_ChidotVal
ub_Vdot = np.ones([N, 1]) * ub_VdotVal
ub_Chidot = np.ones([N, 1]) * ub_ChidotVal
lb = np.concatenate([lb_Vdot, lb_Chidot])
ub = np.concatenate([ub_Vdot, ub_Chidot])
lataccel_max = lataccel_maxVal
# Running Constraints
u = u0.flatten(1)
x = prob.computeOpenloopSolution(u, N, T, t0, x0)
if obstacle.obstaclePresent == True:
lane1Lines = lanes.lane1Lines
lane2Lines = lanes.lane2Lines
acrossLines = lanes.acrossLines
idx_Vehicle, laneNo = lanes.insideRoadSegment(
x0[0], x0[1], lane1Lines, lane2Lines, acrossLines)
if (idx_Vehicle < obstacle.idx_StartSafeZone):
dyRoadL = delta_yRoad
dyRoadR = delta_yRoad
elif (idx_Vehicle >= obstacle.idx_StartSafeZone) and (
idx_Vehicle < obstacle.idx_EndSafeZone):
dyRoadL = delta_yRoad
dyRoadR = delta_yRoadRelaxed
elif (idx_Vehicle >= obstacle.idx_StartObstacle) and (
idx_Vehicle < obstacle.idx_EndObstacle):
dyRoadL = delta_yRoad
dyRoadR = delta_yRoad
else:
dyRoadL = delta_yRoad
dyRoadR = delta_yRoad
else:
dyRoadL = delta_yRoad
dyRoadR = delta_yRoad
# Running Constraint
#cl_running = np.concatenate([-1*np.ones(N), 0*np.ones(N)])
#cu_running = np.concatenate([ 0*np.ones(N), 1*np.ones(N)])
cl_running = np.concatenate([-100 * np.ones(N), 0 * np.ones(N)])
cu_running = np.concatenate([0 * np.ones(N), 100 * np.ones(N)])
cl_tmp1 = np.concatenate([cl_running, [-lataccel_max]])
cu_tmp1 = np.concatenate([cu_running, [+lataccel_max]])
if nstates == 6:
# Speed Constraint
cl_tmp2 = np.concatenate([cl_tmp1, [lb_V]])
cu_tmp2 = np.concatenate([cu_tmp1, [ub_V]])
# Terminal Constraint
cl_tmp3 = np.concatenate([cl_tmp2, [-dyRoadL]])
cu_tmp3 = np.concatenate([cu_tmp2, [dyRoadR]])
cl = np.concatenate([cl_tmp3, [-delta_V + V_cmd]])
cu = np.concatenate([cu_tmp3, [delta_V + V_cmd]])
elif nstates == 4:
# Terminal Constraint
cl = np.concatenate([cl_tmp1, [-dyRoadL]])
cu = np.concatenate([cu_tmp1, [dyRoadR]])
if ncons != len(cl) and ncons != len(cu):
print('Error: resolve number of constraints')
nlp = ipopt.problem(n=nu * N,
m=len(cl),
problem_obj=nlpProb(N, T, t0, x0, ncons, nu, lanes,
obstacle),
lb=lb,
ub=ub,
cl=cl,
cu=cu)
nlp.addOption('print_level', nlpPrintLevel)
nlp.addOption('max_iter', nlpMaxIter)
#nlp.addOption('dual_inf_tol',10.0) # defaut = 1
nlp.addOption('constr_viol_tol', 0.1) # default = 1e-4
nlp.addOption('compl_inf_tol', 0.1) # default = 1e-4
nlp.addOption('acceptable_tol', 0.1) # default = 0.01
nlp.addOption('acceptable_constr_viol_tol', 0.1) # default = 0.01
return nlp
def constraints(self, u):
try:
N = self.N
T = self.T
t0 = self.t0
x0 = self.x0
path = self.path
obstacle = self.obstacle
posIdx = self.posIdx
ns_option = self.ns_option
x = prob.computeOpenloopSolution(u, N, T, t0, x0)
consR1 = np.array([], dtype=float)
if ns == 6:
if ns_option == 1: # Additional Current velocity + Terminal velocity constraint
consR2 = np.array([
x[0, idx_V] * x[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR3 = np.array([x[0, idx_V]]) # current velocity
constmp = np.concatenate([consR1, consR2])
consR = np.concatenate([constmp, consR3])
# terminal constraint (dy, V, delChi)
consT1, consT2, consT3 = prob.terminalCons(
u, x[N - 1], t0, path, obstacle, posIdx)
consT = np.concatenate([consT2, consT3])
elif ns_option == 2:
# No terminal velocity constraint
consR2 = np.array([
x[0, idx_V] * x[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint (dy, dV, delChi)
consT1, consT2, consT3 = prob.terminalCons(
u, x[N - 1], t0, path, obstacle, posIdx)
consT = np.concatenate([consT2, consT3])
elif ns_option == 3:
# No terminal velocity constraint
consR2 = np.array([
x[0, idx_V] * x[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint (dy, V, delChi)
consT1, consT2, consT3 = prob.terminalCons(
u, x[N - 1], t0, path, obstacle, posIdx)
consT = consT3
elif ns == 4:
if ns_option == 1:
u_mat = u.reshape(2, -1).T
consR2 = np.array([
x[0, idx_V] * u_mat[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR3 = np.array([x[0, idx_V]]) # current velocity
constmp = np.concatenate([consR1, consR2])
consR = np.concatenate([constmp, consR3])
# terminal constraint (dy, dV, delChi)
consT1, consT2, consT3 = prob.terminalCons(
u, x[N - 1], t0, path, obstacle, posIdx)
consT = np.concatenate([consT2, consT3])
elif ns_option == 2:
u_mat = u.reshape(2, -1).T
consR2 = np.array([
x[0, idx_V] * u_mat[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint (dy, V, delChi)
consT1, consT2, consT3 = prob.terminalCons(
u, x[N - 1], t0, path, obstacle, posIdx) # ydist, VEnd
consT = np.concatenate([consT2, consT3])
elif ns_option == 3:
u_mat = u.reshape(2, -1).T
consR2 = np.array([
x[0, idx_V] * u_mat[0, idx_Chidot] * useLatAccelCons
]) # lateral acceleration
consR = np.concatenate([consR1, consR2])
# terminal constraint (dy, dV, delChi)
consT1, consT2, consT3 = prob.terminalCons(
u, x[N - 1], t0, path, obstacle, posIdx) # ydist, VEnd
consT = consT3
# total constraints without obstacles
cons = np.concatenate([consR, consT])
# total constraints with obstacles
if self.addObstacleConstraints == True:
for j in self.obstacleNumber:
for k in range(N):
position = x[k][0:2]
obstacleDistance = np.sqrt([
(obstacle.E[j] - position[0])**2 +
(obstacle.N[j] - position[1])**2
])
cons = np.concatenate([cons, obstacleDistance])
return cons
except:
print('Error in constraints')
# change flag (global variable) to write cost breakdown in nlp.py
if debug is True:
writeToFileCost = True
# apply control
tmeasure, xmeasure = applyControl(T, t0, x0, u_new)
# prepare restart
u0 = shiftHorizon(N, u_new)
posIdx = getPosIdx(xmeasure[0], xmeasure[1], path, posIdx)
# reset global variable to write cost breakdown in nlp.py
globalVars.writeToFileCost = True
x_mpciter = probInfo.computeOpenloopSolution(u0.flatten(1), N, T, t0, x0, x00_23)
current_point = x_mpciter[0, 0:2]
terminal_point = x_mpciter[-1, 0:2]
# stop vehicle if required
breakLoop, V_cmd, t_slowDown, t_slowDown_detected, lb_VTerm, lb_VdotVal = \
vehicleStop(T, x, mpciter, decelType, terminal_point, endPoint,
lb_reachedGoal, lb_reachedNearGoal, zeroDistanceChange,
t_slowDown_detected, tmeasure, V_cmd, lb_VTermSlowDown, lb_VdotValSlowDown, decel,
t_slowDown, lb_VTerm, lb_VdotVal)
if breakLoop == True:
break
if mpciter > 30:
None
currentObstacle = currentObstacleClass()
if mpciter == 0:
startPoint = np.array([x0[0], x0[1]])
startPoint0 = startPoint
pathClass = pathInfo('newpath', startPoint, endPoint, currentObstacle, startPoint0)
print('need to debug ...')
path = pathClass()
else:
chi = np.array([x0[3]])
dN = dNewPathAdjust * np.cos(chi)
dE = dNewPathAdjust * np.sin(chi)
startPoint = np.array([x0[0]-dE, x0[1]-dN])
u_newpath = u0.flatten(1)
x_newpath = probInfo.computeOpenloopSolution(u_newpath, pdata.N, pdata.T, pdata.t0, x0)
E_newpath = x_newpath[-1, 0]
N_newpath = x_newpath[-1, 1]
startPoint_newpath = np.array([E_newpath, N_newpath])
pathClass = pathInfo('newpath', startPoint_newpath, endPoint, currentObstacle, startPoint)
path = pathClass()
#path = addCurrentPointToPath(path, startPoint, chi)
pathType = 'newpath'
posIdx = getPosIdx(x0[0], x0[1], path, posIdx0)
# laplacian path computation time
tLP[mpciter] = (time.time() - tStart)
print("Computation time for LP = " + str(tLP[mpciter]))