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
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
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
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 = random.randint(0,1) track = RandomTrack() track.generateTrack(seed=seed, reversed=reversed) print('Using seed :: {}'.format(seed)) # -------------------- # Create controller(s) # -------------------- mpc_controller = MPCController() # Get initial pose (position and orientation) # TODO: Replace within some utilities file initLoc, initRot = calcPose([track.center.x[0],track.center.y[0]], [track.center.x[1],track.center.y[1]]) # Create the chrono simulator wrapper object chrono = ChronoSim( step_size=ch_step_size, track=track, initLoc=initLoc, initRot=initRot, irrlicht=irrlicht, ) mpc_controller.UpdateState(chrono) # Create matplotlib simulator mat = MatSim(mat_step_size) ch_time = mat_time = 0 while True: # Update controller throttle, steering, braking = mpc_controller.Advance(ch_step_size, chrono) chrono.driver.SetTargetSteering(steering) chrono.driver.SetTargetThrottle(throttle) chrono.driver.SetTargetBraking(braking) if chrono.Advance(ch_step_size) == -1: chrono.Close() break if matplotlib and ch_time >= mat_time: if mat.plot(track, chrono) == -1: print("Quit message received.") mat.close() break mat_time += mat_step_size ch_time += ch_step_size if ch_time > 50: break print("Exited") pass
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
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