Exemple #1
0
def main():
    if len(sys.argv) == 2:
        seed = int(sys.argv[1])
    else:
        seed = random.randint(0, 100)

    # Render preferences
    matplotlib = 0
    irrlicht = 1

    import matplotlib.pyplot as plt
    plt.figure()

    # Chrono simulation step size
    ch_step_size = 1e-2
    # Matplotlib visualization step size
    mat_step_size = 1e-2

    # ------------
    # Create track
    # ------------
    createRandomTrack = True
    track = None

    if createRandomTrack:
        reversed = random.randint(0, 1)
        track = RandomTrack(x_max=50, y_max=50)
        track.generateTrack(seed=seed, reversed=reversed)
    else:
        print('Using seed :: {}'.format(seed))
        points = [
            [49.8, 132.9],
            [60.3, 129.3],
            [75.6, 129.0],
            [87.9, 131.7],
            [96.9, 129.6],
            [111.0, 120.0],
            [115.2, 110.7],
            [120.6, 96.9],
            [127.8, 88.5],
            [135.9, 77.4],
            [135.9, 65.1],
            [133.2, 51.3],
            [128.4, 43.2],
            [119.7, 36.3],
            [105.0, 35.7],
            [90.0, 36.3],
            [82.5, 46.2],
            [82.5, 63.6],
            [83.4, 82.2],
            [77.1, 93.9],
            [61.2, 88.5],
            [55.5, 73.5],
            [57.9, 54.6],
            [66.6, 45.0],
            [75.9, 36.3],
            [79.2, 25.5],
            [78.0, 13.2],
            [65.1, 6.0],
            [50.7, 6.0],
            [36.6, 11.7],
            [29.1, 21.3],
            [24.0, 36.9],
            [24.0, 56.1],
            [29.1, 70.8],
            [24.9, 77.7],
            [13.5, 77.7],
            [6.3, 81.6],
            [5.7, 92.7],
            [6.3, 107.7],
            [8.7, 118.2],
            [15.3, 122.7],
            [24.3, 125.4],
            [31.2, 126.0],
            [40.8, 129.6],
            [49.8, 132.9],
        ]
        track = Track(points)
        track.generateTrack()

    segmentation = Segmentations(track)
    segmentation.create_segmentations()

    stable_path = ga_search(segmentation)
    stable_path.generate_final_path()

    plt.clf()
    stable_path.generate_final_path()
    path = stable_path.final_path
    path.update_vmax()
    path.update_profile()
    #path.plot_speed_profile()
    # plt.show()
    # path.v_max = v
    # plt.pause(1)

    # --------------------
    # Create controller(s)
    # --------------------

    # Lateral controller (steering)
    lat_controller = PIDLateralController(path)
    lat_controller.SetGains(Kp=1.9, Ki=0.000, Kd=0.40)
    lat_controller.SetLookAheadDistance(dist=5)

    # Longitudinal controller (throttle and braking)
    long_controller = PIDLongitudinalController(path)
    long_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
    long_controller.SetLookAheadDistance(dist=path.ps[0] * 2)

    # PID controller (wraps both lateral and longitudinal controllers)
    controller = PIDController(lat_controller, long_controller)

    # ------------------------
    # Create chrono components
    # ------------------------
    setDataDirectory()

    # Create chrono system
    system = createChronoSystem()

    # Calculate initial position and initial rotation of vehicle
    initLoc, initRot = calcPose(path.points[0], path.points[1])
    # Create chrono vehicle
    vehicle = ChronoVehicle(ch_step_size,
                            system,
                            controller,
                            irrlicht=irrlicht,
                            vehicle_type='json',
                            initLoc=initLoc,
                            initRot=initRot,
                            vis_balls=True)

    # Create chrono terrain
    terrain = ChronoTerrain(ch_step_size,
                            system,
                            irrlicht=irrlicht,
                            terrain_type='concrete')
    vehicle.SetTerrain(terrain)

    # Create chrono wrapper
    chrono_wrapper = ChronoWrapper(ch_step_size,
                                   system,
                                   track,
                                   vehicle,
                                   terrain,
                                   irrlicht=irrlicht,
                                   draw_barriers=True)

    if matplotlib:
        matplotlib_wrapper = MatplotlibWrapper(mat_step_size,
                                               vehicle,
                                               render_step_size=1.0 / 20)
        path.plot(color='-b', show=False)
        matplotlib_wrapper.plotTrack(track)

    ch_time = mat_time = 0
    updates = total_time = 0.0
    while True:
        # if vehicle.vehicle.GetVehicleSpeed() < 7:
        #     lat_controller.SetGains(Kp=0.2, Ki=0, Kd=0.6)
        # elif vehicle.vehicle.GetVehicleSpeed() < 8:
        #     lat_controller.SetGains(Kp=0.3, Ki=0, Kd=0.45)
        # else:
        #     lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3)

        if chrono_wrapper.Advance(ch_step_size) == -1:
            chrono_wrapper.Close()
            break

        # Update controller
        controller.Advance(ch_step_size, vehicle)

        if matplotlib and ch_time >= mat_time:
            if not matplotlib_wrapper.Advance(mat_step_size, save=False):
                print("Quit message received.")
                matplotlib_wrapper.close()
                break
            mat_time += mat_step_size

        ch_time += ch_step_size

        if ch_time > 300:
            break
        updates += 1.0
    # print('Update Summary:\n\tChrono Time :: {0:0.2f}\n\tTime per Update :: {1:0.5f}'.format(ch_time, total_time / updates))
    print("Exited")
    pass
Exemple #2
0
def main():
    if len(sys.argv) == 2:
        seed = int(sys.argv[1])
    else:
        seed = random.randint(0, 100)

    # Render preferences
    matplotlib = 0
    pyqtgraph = 1
    irrlicht = 0

    # Chrono simulation step size
    ch_step_size = 1e-2
    # Matplotlib visualization step size
    mat_step_size = 1e-2
    # PyQtGraph visualization step size
    pyqt_step_size = 1e-2

    # ------------
    # Create track
    # ------------
    reversed = random.randint(0, 1)
    track = RandomTrack()
    track.generateTrack(seed=seed, reversed=reversed)
    print('Using seed :: {}'.format(seed))

    # --------------------
    # Create controller(s)
    # --------------------

    # Lateral controller (steering)
    lat_controller = PIDLateralController(track.center)
    lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.25)
    lat_controller.SetLookAheadDistance(dist=5)

    # Longitudinal controller (throttle and braking)
    long_controller = PIDLongitudinalController()
    long_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
    long_controller.SetTargetSpeed(speed=10.0)

    # PID controller (wraps both lateral and longitudinal controllers)
    controller = PIDController(lat_controller, long_controller)

    # ------------------------
    # Create chrono components
    # ------------------------
    setDataDirectory()

    # Create chrono system
    system = createChronoSystem()

    # Calculate initial position and initial rotation of vehicle
    initLoc, initRot = calcPose(track.center.points[0], track.center.points[1])
    # Create chrono vehicle
    vehicle = ChronoVehicle(ch_step_size,
                            system,
                            controller,
                            irrlicht=irrlicht,
                            vehicle_type='json',
                            initLoc=initLoc,
                            initRot=initRot,
                            vis_balls=True)

    # Create chrono terrain
    terrain = ChronoTerrain(ch_step_size,
                            system,
                            irrlicht=irrlicht,
                            terrain_type='concrete')
    vehicle.SetTerrain(terrain)

    # Create chrono wrapper
    chrono_wrapper = ChronoWrapper(ch_step_size,
                                   system,
                                   track,
                                   vehicle,
                                   terrain,
                                   irrlicht=irrlicht,
                                   draw_barriers=True)

    if matplotlib:
        matplotlib_wrapper = MatplotlibWrapper(mat_step_size,
                                               vehicle,
                                               render_step_size=1.0 / 20)
        matplotlib_wrapper.plotTrack(track)
    elif pyqtgraph:
        pyqtgraph_wrapper = PyQtGraphWrapper(pyqt_step_size,
                                             vehicle,
                                             render_step_size=1.0 / 20)
        pyqtgraph_wrapper.plotTrack(track)
        pyqtgraph_wrapper.exec()

    ch_time = mat_time = pyqt_time = 0
    updates = total_time = 0.0
    while True:
        if chrono_wrapper.Advance(ch_step_size) == -1:
            chrono_wrapper.Close()
            break

        # Update controller
        controller.Advance(ch_step_size, vehicle)

        if matplotlib and ch_time >= mat_time:
            if not matplotlib_wrapper.Advance(mat_step_size):
                print("Quit message received.")
                matplotlib_wrapper.close()
                break
            mat_time += mat_step_size

        if pyqtgraph and ch_time >= pyqt_time:
            if not pyqtgraph_wrapper.Advance(pyqt_step_size):
                print("Quit message received.")
                pyqtgraph_wrapper.close()
                break
            pyqt_time += pyqt_step_size

        ch_time += ch_step_size

        if ch_time > 50:
            break
        updates += 1.0
    # print('Update Summary:\n\tChrono Time :: {0:0.2f}\n\tTime per Update :: {1:0.5f}'.format(ch_time, total_time / updates))
    print("Exited")
    pass
Exemple #3
0
def main():
    global first, time
    if len(sys.argv) == 2:
        seed = int(sys.argv[1])
    else:
        seed = random.randint(0, 100)

    # Render preferences
    matplotlib = 1
    irrlicht = 1

    # Chrono Simulation step size
    ch_step_size = 1e-2
    # Matplotlib Simulation step size
    mat_step_size = 1e-2

    # ------------
    # Create track
    # ------------
    reversed = random.randint(0, 1)

    track = RandomTrack(x_max=200, y_max=200, width=20)
    track.generateTrack(seed=seed, reversed=reversed)
    print('Using seed :: {}'.format(seed))

    # ----------------
    # Create obstacles
    # ----------------
    # Change n to add more obstacles

    obstacles = RandomObstacleGenerator.generateObstacles(
        track.center,
        i_min=50,
        i_max=60,
        n=1,
        seed=seed * random.randint(0, 90),
        reversed=reversed,
        movement_rate=5,
        movement_distance=1)

    print(obstacles)  # Is a python dictionary
    # To access: obstacles[<path_index>] = position_vector

    # --------------------
    # Create controller(s)
    # --------------------
    lat_controller = PIDLateralController(track, obstacles)
    lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3)
    lat_controller.SetLookAheadDistance(lookDist=5)
    lat_controller.SetObstacleDistance(obsDist=50)

    long_controller = PIDLongitudinalController(track.center)
    long_controller.SetGains(Kp=0.4, Ki=0, Kd=0.45)
    long_controller.SetLookAheadDistance(dist=5)
    long_controller.SetTargetSpeed(speed=6.0)

    # PID controller (wraps both lateral and longitudinal controllers)
    controller = PIDController(lat_controller, long_controller)

    # ------------------------
    # Create chrono components
    # ------------------------
    setDataDirectory()

    # Create chrono system
    system = createChronoSystem()

    # Calculate initial position and initial rotation of vehicle
    initLoc, initRot = calcPose(track.center.points[0], track.center.points[1])
    # Create chrono vehicle
    vehicle = ChronoVehicle(ch_step_size,
                            system,
                            controller,
                            irrlicht=irrlicht,
                            vehicle_type='json',
                            initLoc=initLoc,
                            initRot=initRot,
                            vis_balls=True)

    # Create chrono terrain
    terrain = ChronoTerrain(ch_step_size,
                            system,
                            irrlicht=irrlicht,
                            terrain_type='concrete')
    vehicle.SetTerrain(terrain)

    # Create chrono wrapper
    chrono_wrapper = ChronoWrapper(ch_step_size,
                                   system,
                                   track,
                                   vehicle,
                                   terrain,
                                   irrlicht=irrlicht,
                                   obstacles=obstacles,
                                   draw_barriers=True)

    if matplotlib:
        matplotlib_wrapper = MatplotlibWrapper(mat_step_size,
                                               vehicle,
                                               obstacles=obstacles,
                                               render_step_size=1.0 / 20)
        matplotlib_wrapper.plotTrack(track)

    ch_time = mat_time = 0
    while True:
        # Update controller
        controller.Advance(ch_step_size, vehicle)

        if vehicle.vehicle.GetVehicleSpeed() < 7:
            lat_controller.SetGains(Kp=0.2, Ki=0, Kd=0.6)
        elif vehicle.vehicle.GetVehicleSpeed() < 8:
            lat_controller.SetGains(Kp=0.3, Ki=0, Kd=0.45)
        else:
            lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3)

        if chrono_wrapper.Advance(ch_step_size) == -1:
            chrono_wrapper.Close()
            break

        if matplotlib and ch_time >= mat_time:
            if not matplotlib_wrapper.Advance(mat_step_size):
                print("Quit message received.")
                matplotlib_wrapper.close()
                break
            mat_time += mat_step_size

        ch_time += ch_step_size

        if ch_time > 50:
            break
    print("Exited")
    pass
Exemple #4
0
def main():
    global first, time
    if len(sys.argv) == 2:
        seed = int(sys.argv[1])
    else:
        seed = random.randint(0, 100)

    # Render preferences
    matplotlib = 0
    irrlicht = 1

    # Chrono Simulation step size
    ch_step_size = 1e-2
    # Matplotlib Simulation step size
    mat_step_size = 1e-2

    # ------------
    # Create track
    # ------------
    reversed = 1  # random.randint(0,1)
    track = RandomTrack(width=20)
    track.generateTrack(seed=seed, reversed=reversed)
    print('Using seed :: {}'.format(seed))

    # --------------------
    # Create controller(s)
    # --------------------
    lat_controller = PIDLateralController(track.center)
    lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.25)
    lat_controller.SetLookAheadDistance(dist=5)

    long_controller = PIDLongitudinalController()
    long_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
    long_controller.SetTargetSpeed(speed=10.0)

    # PID controller (wraps both lateral and longitudinal controllers)
    controller = PIDController(lat_controller, long_controller)

    # ------------------------
    # Create chrono components
    # ------------------------
    setDataDirectory()

    # Create chrono system
    system = createChronoSystem()

    # Calculate initial position and initial rotation of vehicle
    initLoc, initRot = calcPose(track.center.points[0], track.center.points[1])
    # Create chrono vehicle
    vehicle = ChronoVehicle(ch_step_size,
                            system,
                            controller,
                            irrlicht=irrlicht,
                            vehicle_type='json',
                            initLoc=initLoc,
                            initRot=initRot,
                            vis_balls=True)

    # Create chrono terrain
    terrain = ChronoTerrain(ch_step_size,
                            system,
                            irrlicht=irrlicht,
                            terrain_type='concrete')
    vehicle.SetTerrain(terrain)

    # ------------------
    # Create opponent(s)
    # ------------------
    opponents = []
    n = 1
    for i in range(n):
        opponent_track = Track(track.center.waypoints, width=track.width / 2)
        opponent_track.generateTrack()
        if i % 3 == 0:
            opponent_path = opponent_track.left
        elif i % 3 == 1:
            opponent_path = opponent_track.right
        else:
            opponent_path = opponent_track.center

        opponent_lat_controller = PIDLateralController(opponent_path)
        opponent_lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.25)
        opponent_lat_controller.SetLookAheadDistance(dist=5)

        opponent_long_controller = PIDLongitudinalController()
        opponent_long_controller.SetGains(Kp=0.4, Ki=0, Kd=0)
        opponent_long_controller.SetTargetSpeed(speed=5.0)

        opponent_controller = PIDController(opponent_lat_controller,
                                            opponent_long_controller)

        opponent_initLoc, opponent_initRot = calcRandomPose(
            opponent_path,
            s_min=opponent_path.s[100 + i * 100],
            s_max=opponent_path.s[100 + i * 100])
        opponent_vehicle = ChronoVehicle(ch_step_size,
                                         system,
                                         opponent_controller,
                                         irrlicht=irrlicht,
                                         vehicle_type='json',
                                         initLoc=opponent_initLoc,
                                         initRot=opponent_initRot)
        opponent_vehicle.SetTerrain(terrain)
        opponent = Opponent(opponent_vehicle, opponent_controller)

        opponents.append(opponent)

    controller.SetOpponents(opponents)

    # Create chrono wrapper
    chrono_wrapper = ChronoWrapper(ch_step_size,
                                   system,
                                   track,
                                   vehicle,
                                   terrain,
                                   irrlicht=irrlicht,
                                   opponents=opponents,
                                   draw_barriers=True)

    if matplotlib:
        matplotlib_wrapper = MatplotlibWrapper(mat_step_size,
                                               vehicle,
                                               opponents=opponents)
        matplotlib_wrapper.plotTrack(track)

    ch_time = mat_time = 0
    while True:
        if chrono_wrapper.Advance(ch_step_size) == -1:
            chrono_wrapper.Close()
            break

        controller.Advance(ch_step_size, vehicle, perception_distance=30)
        for opponent in opponents:
            opponent.Update(ch_time, ch_step_size)

        if matplotlib and ch_time >= mat_time:
            if not matplotlib_wrapper.Advance(mat_step_size):
                print("Quit message received.")
                matplotlib_wrapper.close()
                break
            mat_time += mat_step_size

        ch_time += ch_step_size

        if ch_time > 50:
            break
    print("Exited")
    pass
Exemple #5
0
def main():
    if len(sys.argv) == 2:
        seed = int(sys.argv[1])
    else:
        seed = random.randint(0, 100)

    # Render preferences
    matplotlib = 0
    irrlicht = 1

    # Chrono Simulation step size
    ch_step_size = 1e-2
    # Matplotlib Simulation step size
    mat_step_size = 1e-2

    # ------------
    # Create track
    # ------------
    reversed = random.randint(0, 1)
    track = RandomTrack()
    track.generateTrack(seed=seed, reversed=reversed)
    print('Using seed :: {}'.format(seed))

    # --------------------
    # Create controller(s)
    # --------------------

    # Lateral controller (steering)
    lat_controller = PIDLateralController(track.center)
    lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3)
    lat_controller.SetLookAheadDistance(dist=5)

    # Longitudinal controller (throttle and braking)
    long_controller = PIDLongitudinalController(track.center)
    long_controller.SetGains(Kp=0.4, Ki=0, Kd=0.45)
    long_controller.SetLookAheadDistance(dist=5)
    long_controller.SetTargetSpeed(speed=10.0)

    # PID controller (wraps both lateral and longitudinal controllers)
    controller = PIDController(lat_controller, long_controller)

    # ------------------------
    # Create chrono components
    # ------------------------
    setDataDirectory()

    # Create chrono system
    system = createChronoSystem()

    # Calculate initial position and initial rotation of vehicle
    initLoc, initRot = calcPose(track.center.points[0], track.center.points[1])
    # Create chrono vehicle
    vehicle = ChronoVehicle(ch_step_size,
                            system,
                            controller,
                            irrlicht=irrlicht,
                            vehicle_type='json',
                            initLoc=initLoc,
                            initRot=initRot,
                            vis_balls=True)

    # Create chrono terrain
    terrain = ChronoTerrain(ch_step_size,
                            system,
                            irrlicht=irrlicht,
                            terrain_type='concrete')
    vehicle.SetTerrain(terrain)

    # Create chrono wrapper
    chrono_wrapper = ChronoWrapper(ch_step_size,
                                   system,
                                   track,
                                   vehicle,
                                   terrain,
                                   irrlicht=irrlicht,
                                   draw_barriers=True)

    if matplotlib:
        matplotlib_wrapper = MatplotlibWrapper(mat_step_size,
                                               vehicle,
                                               render_step_size=1.0 / 20)
        matplotlib_wrapper.plotTrack(track)

    ch_time = mat_time = 0
    while True:
        # Update controllers
        if vehicle.vehicle.GetVehicleSpeed() < 7:
            lat_controller.SetGains(Kp=0.2, Ki=0, Kd=0.6)
        elif vehicle.vehicle.GetVehicleSpeed() < 8:
            lat_controller.SetGains(Kp=0.3, Ki=0, Kd=0.45)
        else:
            lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3)

        # Update controller
        controller.Advance(ch_step_size, vehicle)

        if chrono_wrapper.Advance(ch_step_size) == -1:
            chrono_wrapper.Close()
            break

        if matplotlib and ch_time >= mat_time:
            if not matplotlib_wrapper.Advance(mat_step_size):
                print("Quit message received.")
                matplotlib_wrapper.close()
                break
            mat_time += mat_step_size

        ch_time += ch_step_size

        if ch_time > 50:
            break
    print("Exited")
    pass