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