def GenerateHallwayTrack(z=.15, width=1.1):
from control_utilities.track import Track
points = [[-8.713, -1.646], [-7.851, -1.589], [-6.847, -1.405],
[-6.048, -1.449], [-5.350, -1.658], [-4.628, -1.767],
[-3.807, -1.789], [-2.865, -1.778], [-1.823, -1.743],
[-0.724, -1.691], [0.373, -1.650], [1.411, -1.527],
[2.349, -1.453], [3.174, -1.439], [3.915,
-1.474], [4.652, -1.513],
[5.487, -1.694], [6.506, -1.756], [7.506,
-1.456], [7.732, -1.060],
[7.983, -0.617], [7.432, 1.112], [6.610, 1.143], [5.688, 1.206],
[4.950, 1.281], [4.331, 1.337], [3.754, 1.349], [3.152, 1.303],
[2.478, 1.207], [1.708, 1.077], [0.832, 0.940], [-0.143, 0.828],
[-1.201, 0.767], [-2.318, 0.781], [-3.463,
0.830], [-4.605, 0.838],
[-5.715, 0.864], [-6.765, 0.934], [-7.737, 1.121],
[-8.822, 1.318], [-10.024, 0.608], [-10.102, 0.437],
[-10.211, -0.569], [-9.522, -1.514], [-8.713, -1.646]]
track = Track(points, width=width, z=z)
track.generateTrack(z=z)
return track
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
import matplotlib.pyplot as plt
if __name__ == "__main__":
points = [[-9.013, -1.346], [-7.851, -1.289], [-6.847, -1.305],
[-6.048, -1.449], [-5.350, -1.658], [-4.628, -1.846],
[-3.807, -1.859], [-2.865, -1.878], [-1.823, -1.813],
[-0.724, -1.791], [0.373, -1.650], [1.411, -1.527],
[2.349, -1.453], [3.174, -1.439], [3.915,
-1.474], [4.652, -1.513],
[5.487, -1.494], [6.506, -1.356], [7.732,
-1.060], [9.083, -0.617],
[7.732, 1.112], [6.610, 1.143], [5.688, 1.206], [4.950, 1.281],
[4.331, 1.337], [3.754, 1.349], [3.152, 1.303], [2.478, 1.207],
[1.708, 1.077], [0.832, 0.940], [-0.143, 0.828], [-1.201, 0.767],
[-2.318, 0.781], [-3.463, 0.880], [-4.605,
1.060], [-5.715, 1.294],
[-6.765, 1.332], [-7.737, 1.291], [-8.622, 1.255],
[-9.424, 1.218], [-10.102, 0.437], [-10.411, -0.569],
[-10.022, -1.214], [-9.013, -1.346]]
import numpy as np
track = Track(points, width=.4, num_points=500)
track.generateTrack()
track.plot(show=False)
# t = range(len(track.center))[::10]
# for i in t:
# plt.plot([track.left.x[i], track.right.x[i]], [track.left.y[i], track.right.y[i]], "g-")
plt.show()
[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,width=1)
track.generateTrack()
l1 = track.left.points[0]
r1 = track.right.points[0]
v1 = track.right.points[1]-track.right.points[0]
v2 = chrono.ChVectorD(1,0,0)
ang = math.atan2((v1%v2).Length(), v1^v2)
if (chrono.ChVectorD(0,0,1)^(v1%v2) > 0.0): ang *= -1
q = chrono.Q_from_AngZ(ang)
initLoc = chrono.ChVectorD((l1 + r1) / 2) + chrono.ChVectorD(0,0,1.0)
initRot = q
def main():
if len(sys.argv) == 2:
seed = int(sys.argv[1])
else:
seed = random.randint(0, 100)
# Render preferences
matplotlib = 1
irrlicht = 0
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 = False
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()
solution_t = []
for _ in range(100):
stable_path = ga_search(segmentation)
solution_t.append(np.sum(stable_path.t))
print(np.array(solution_t))
print("Mean: " + str(np.mean(solution_t)))
print("Var: " + str(np.var(solution_t)))
print("Min: " + str(np.min(solution_t)))
print("Max: " + str(np.max(solution_t)))
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
[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()
track.plot()
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))
# print("FIRST 5 POINTS OF INITIAL PATH:")
# for i in range(5):
# print(track.center.getPoint(i))
# --------------------- ----
# Generate centerline array
# -------------------------
center = [] # array of centerline points
for i in range(track.num_points):
leftPt = track.left.getPoint(i)
rightPt = track.right.getPoint(i)
centerX = (leftPt.x + rightPt.x) / 2
centerY = (leftPt.y + rightPt.y) / 2
center.append([centerX, centerY])
# print("FIRST 5 POINTS OF CENTER PATH:")
# for i in range(5):
# print(track.center.getPoint(i))
# ------------------------------------
# Create new track based on centerline
# ------------------------------------
track = Track(center)
track.generateTrack()
# --------------------
# 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(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