コード例 #1
0
ファイル: nlp.py プロジェクト: suvoganguli/TrajGen
    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
コード例 #2
0
    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
コード例 #3
0
ファイル: nlp.py プロジェクト: suvoganguli/GIG-MPC-Laplacian
    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
コード例 #4
0
ファイル: nlp.py プロジェクト: suvoganguli/GIG-MPC-LMPC
    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
コード例 #5
0
ファイル: debugLogs.py プロジェクト: suvoganguli/TrajGen
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
コード例 #6
0
ファイル: nlp.py プロジェクト: suvoganguli/GIG-MPC-Laplacian
    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
コード例 #7
0
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
コード例 #8
0
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
コード例 #9
0
ファイル: Main.py プロジェクト: suvoganguli/GIG-MPC-NMPC
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")
コード例 #10
0
    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
コード例 #11
0
ファイル: nlp.py プロジェクト: suvoganguli/TrajGen
    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')
コード例 #12
0
    # 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
コード例 #13
0
ファイル: Main.py プロジェクト: suvoganguli/GIG-MPC-Laplacian
        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]))