Skip to content
Snippets Groups Projects
Commit 76a94216 authored by Chauchat Eymeric's avatar Chauchat Eymeric
Browse files

Merge branch 'main' of gitlab.ec-lyon.fr:epsa-personnal-git/epsa-pae-ai-self-driving

parents fb22b9ef 24d68aa6
Branches
No related tags found
No related merge requests found
Showing
with 4720 additions and 0 deletions
# 2D-car-dynamics-simulation
2D car simulator
import xml.etree.ElementTree as ET
from track_2_generator import create_track
from shapely.geometry import Polygon, Point, LineString
import numpy as np
from matplotlib.pyplot import plot, legend
import json
"""loading track"""
track_points = create_track(0.5,0,0, (0,0))[:-1]+[(1,0.5)]
track = Polygon(track_points)
#outside_l = create_double_circles(0.59375, (0.5,0.5),(1.5,0.5))
"""
x_shift, y_shift = 1433.0000000000002, -406.0866103896103*2#Tom: 1433.0000000000002, -406.0866103896103*2 / Damien: -540, 960
R_simu = 150#Tom: 150 / Damien: 350
R_reel = 0.5
x_shift, y_shift = x_shift/R_simu-4.77, y_shift/R_simu+2.7 #Tom:-4.77,2.7 / Damien: 1.88, -2.5
"""
def get_error(name, R_simu, center_x, center_y, start=-1, end=-1):
"""loading trajectory"""
with open(name) as json_file:
data = json.load(json_file)
x_shift, y_shift = center_x/R_simu, center_y/R_simu
time = data["time"]
x_l = [(i/R_simu-x_shift)/2+1 for i in data["x"]]
y_l = [(i/R_simu-y_shift)/2+0.5 for i in data["y"]]
e_l = []
if start>=0:
x_l = x_l[start:]
y_l = y_l[start:]
time = time[start:]
if end>=0:
x_l = x_l[0:end]
y_l = y_l[0:end]
time = time[0:end]
for x,y in zip(x_l,y_l):
front_center = Point(np.array([x,y]))
e = track.distance(front_center)
e_l.append(e)
return x_l, y_l, time, e_l
#plot([x for x,y in track_points], [y for x,y in track_points])
#plot([i[0] for i in outside_l], [i[1] for i in outside_l])
prefix = 'paths/'
suffix = '_normal.json'
Tom = get_error(prefix+'errorT'+suffix, 150, 1733, -666, 70, 450)
Damien = get_error(prefix+'errorD'+suffix, 350, 930, 510, 25)
plot(Damien[2],Damien[3])
plot(*Tom[2:])
import pygame
class CarDataDisplay:
def __init__(self, car, track):
self.car = car
self.track = track
pygame.font.init()
def display_data(self, screen, position=(0, 0), font='Verdana', size=20):
font_color = (255, 255, 255)
font = pygame.font.SysFont(font, size)
vel_long = font.render('Long velocity: ' + str(round(self.car.velocity.x * 3.6, 2)), False, font_color)
vel_lat = font.render("Lat velocity: " + str(round(self.car.velocity.y * 3.6, 2)), False, font_color)
rpm = font.render('Engine RPM: ' + str(round(self.car.wheel_rpm * self.car.diff_ratio * self.car.gears[self.car.gear], 2)),
False, font_color)
gear = font.render('Gear: ' + str(self.car.gear), False, font_color)
steering = font.render('Steering: ' + str(self.car.steering), False, font_color)
throttle = font.render('Throttle: ' + str(self.car.throttle), False, font_color)
brake = font.render('brake: ' + str(self.car.brakes), False, font_color)
velocity = font.render('Velocity: ' + str(round((self.car.velocity.x**2+self.car.velocity.y**2)**0.5 * 3.6, 2)), False, font_color)
pos_x = font.render('position.x: ' + str(self.track.car_pos[0]), False, font_color)
pos_y = font.render('position.y: ' + str(self.track.car_pos[1]), False, font_color)
track_phase = font.render('phase: ' + str(self.track.track_phase), False, font_color)
lap_number = font.render('lap: ' + str(self.track.lap_nb), False, font_color)
#screen.blit(vel_long, position)
#screen.blit(vel_lat, (position[0], position[1] + (size * 5 / 4)))
#screen.blit(rpm, (position[0], position[1] + 2 * (size * 5 / 4)))
#screen.blit(gear, (position[0], position[1] + 3 * (size * 5/4)))
screen.blit(velocity, (position[0], position[1] + 1 * (size * 5/4)))
screen.blit(steering, (position[0], position[1] + 2 * (size * 5/4)))
screen.blit(throttle, (position[0], position[1] + 3 * (size * 5/4)))
screen.blit(pos_x, (position[0], position[1] + 4 * (size * 5/4)))
screen.blit(pos_y, (position[0], position[1] + 5 * (size * 5/4)))
screen.blit(track_phase, (position[0], position[1] + 6 * (size * 5/4)))
screen.blit(lap_number, (position[0], position[1] + 7 * (size * 5/4)))
#screen.blit(brake, (position[0], position[1] + 6 * (size * 5/4)))
\ No newline at end of file
from shapely.geometry.polygon import Polygon
from shapely.affinity import rotate
import pygame
class CarDrawer:
def __init__(self, length=55, width=20, init_position=(1366/2, 768/2)):
self.length = length
self.width = width
self.trace = []
self.init_position = init_position
pos_x = self.init_position[0]
pos_y = self.init_position[1]
self.r_center = (pos_x, pos_y)
self.car_model = Polygon([(pos_x - self.length * 2 / 10, pos_y - self.width / 2),
(pos_x - self.length * 2 / 10, pos_y + self.width / 2),
(pos_x + self.length * 8 / 10, pos_y + self.width / 2),
(pos_x + self.length * 8 / 10, pos_y - self.width / 2)])
self.f_axle = Polygon([(pos_x + self.length * 6 / 10, pos_y + self.width * 5 / 6),
(pos_x + self.length * 6 / 10, pos_y - self.width * 5 / 6),
(pos_x + self.length * 6 / 10, pos_y + self.width * 5 / 6)])
self.f_tire_1 = Polygon([(pos_x + self.length * 4.5 / 10, pos_y + self.width * 5 / 6),
(pos_x + self.length * 4.5 / 10, pos_y + self.width * 7 / 6),
(pos_x + 7.5 / 10 * self.length, pos_y + self.width * 7 / 6),
(pos_x + 7.5 / 10 * self.length, pos_y + self.width * 5 / 6)])
self.f_tire_2 = Polygon([(pos_x + self.length * 4.5 / 10, pos_y - self.width * 5 / 6),
(pos_x + self.length * 4.5 / 10, pos_y - self.width * 7 / 6),
(pos_x + 7.5 / 10 * self.length, pos_y - self.width * 7 / 6),
(pos_x + 7.5 / 10 * self.length, pos_y - self.width * 5 / 6)])
self.r_axle = Polygon([(pos_x, pos_y + self.width * 5 / 6),
(pos_x, pos_y - self.width * 5 / 6),
(pos_x, pos_y + self.width * 5 / 6)])
self.r_tire_1 = Polygon([(pos_x + self.length * 1.5 / 10, pos_y + self.width * 5 / 6),
(pos_x + self.length * 1.5 / 10, pos_y + self.width * 7 / 6),
(pos_x - 1.5 / 10 * self.length, pos_y + self.width * 7 / 6),
(pos_x - 1.5 / 10 * self.length, pos_y + self.width * 5 / 6)])
self.r_tire_2 = Polygon([(pos_x + self.length * 1.5 / 10, pos_y - self.width * 5 / 6),
(pos_x + self.length * 1.5 / 10, pos_y - self.width * 7 / 6),
(pos_x - 1.5 / 10 * self.length, pos_y - self.width * 7 / 6),
(pos_x - 1.5 / 10 * self.length, pos_y - self.width * 5 / 6)])
def draw(self, screen, car):
car_color = (181, 25, 253)
angle = -car.angle
steering = car.steering
car_model = rotate(self.car_model, angle, self.r_center)
x, y = car_model.exterior.xy
pygame.draw.polygon(screen, (0, 0, 255), [(xx, yy) for xx, yy in zip(x, y)])
f_axle = rotate(self.f_axle, angle, self.r_center)
x_axle, y_axle = f_axle.exterior.xy
pygame.draw.polygon(screen, car_color, [(xx, yy) for xx, yy in zip(x_axle, y_axle)], 2)
f_tire_1 = rotate(self.f_tire_1, angle, self.r_center)
f_tire_1 = rotate(f_tire_1, - steering, (x_axle[0], y_axle[0]))
x, y = f_tire_1.exterior.xy
pygame.draw.polygon(screen, (0, 0, 0), [(xx, yy) for xx, yy in zip(x, y)])
f_tire_2 = rotate(self.f_tire_2, angle, self.r_center)
f_tire_2 = rotate(f_tire_2, - steering, (x_axle[1], y_axle[1]))
x, y = f_tire_2.exterior.xy
pygame.draw.polygon(screen, (0, 0, 0), [(xx, yy) for xx, yy in zip(x, y)])
r_axle = rotate(self.r_axle, angle, self.r_center)
x, y = r_axle.exterior.xy
pygame.draw.polygon(screen, car_color, [(xx, yy) for xx, yy in zip(x, y)], 2)
r_tire_1 = rotate(self.r_tire_1, angle, self.r_center)
x, y = r_tire_1.exterior.xy
pygame.draw.polygon(screen, (0, 0, 0), [(xx, yy) for xx, yy in zip(x, y)])
r_tire_2 = rotate(self.r_tire_2, angle, self.r_center)
x, y = r_tire_2.exterior.xy
pygame.draw.polygon(screen, (0, 0, 0), [(xx, yy) for xx, yy in zip(x, y)])
rect = pygame.Rect(self.r_center[0], self.r_center[1], 5, 5)
pygame.draw.rect(screen, (255, 0, 0), rect)
from pygame.math import Vector2
from math import tan, radians, degrees, pi, atan2, sin, cos
import numpy as np
import random
class Car:
def __init__(self, x, y, angle=0.0, length=5):
self.position = Vector2(x, y)
self.velocity = Vector2(0.0, 0.0)
self.angle = angle
self.length = length
self.width = 2
self.brake_deceleration = 30000
self.free_deceleration = 2
self.acceleration = Vector2(0, 0)
self.steering = 0.0
self.gear = 0
self.throttle = 0
self.brakes = 0
self.wheel_rpm = 0
self.rpm = 2000
self.angular_velocity = 0
self.force = Vector2(0, 0)
# Parts:
self.engine = Engine()
# Characteristics:
self.gears = {0: 0, 1: 3, 2: 2, 3: 1.5, 4: 1.25, 5: 1, 6: 0.75, -1: -2.9}
self.diff_ratio = 3.42
self.n = 0.8 # power transfer efficiency
self.wheel_radius = 0.35
self.mass = 1100
self.rear_wheels_mass = 100
self.c_drag = 0.4257
self.cornering_stiffness_f = -5.0
self.cornering_stiffness_r = -5.2
self.max_grip = 2.0
self.max_steering = 20
def get_driver_input(self, throttle, gear, brakes, steering_angle):
self.brakes = brakes
self.gear = gear
#on ajoute des vibrations
#self.steering+=random.randint(-5,5)
self.steering = steering_angle
self.throttle = abs(throttle)
def update(self, dt):
self.rpm = self.wheel_rpm * self.diff_ratio * self.gears[self.gear]
if self.rpm < 2000:
self.rpm = 2000
traction_force = self.engine.get_torque(self.rpm, self.throttle) * self.diff_ratio * \
self.gears[self.gear] * (self.n / self.wheel_radius) - \
self.brake_deceleration * self.brakes * np.sign(self.velocity.x)
resistance_force = Vector2(- self.c_drag * self.velocity.x * abs(self.velocity.x) - 12.8 * self.velocity.x,
- self.c_drag * self.velocity.y * abs(self.velocity.y) - 12.8 * self.velocity.y)
if self.velocity.x > 5.55:
yawspeed = 2 * self.angular_velocity
if self.velocity.x == 0:
rot_angle = 0
sideslip = 0
else:
rot_angle = atan2(yawspeed, self.velocity.x)
sideslip = atan2(self.velocity.y, self.velocity.x)
slipanglefront = sideslip + rot_angle - radians(self.steering)
slipanglerear = sideslip - rot_angle
flatf = Vector2(0, 0)
flatr = Vector2(0, 0)
flatf.x = 0
flatf.y = self.cornering_stiffness_f * slipanglefront
flatf.y = min(self.max_grip, flatf.y)
flatf.y = max(-self.max_grip, flatf.y)
flatf.y *= self.mass * 4.9
flatr.x = 0
flatr.y = self.cornering_stiffness_r * slipanglerear
flatr.y = min(self.max_grip, flatr.y)
flatr.y = max(- self.max_grip, flatr.y)
flatr.y *= self.mass * 4.9
self.force.x = traction_force + sin(radians(self.steering)) * flatf.x + flatr.x + resistance_force.x
self.force.y = cos(radians(self.steering)) * flatf.y + flatr.y + resistance_force.y
torque = 1.5 * (flatf.y - flatr.y)
self.acceleration = self.force / self.mass
self.wheel_rpm = self.velocity.x / self.wheel_radius * 30 / pi
self.velocity += self.acceleration * dt
self.position += self.velocity.rotate(-self.angle) * dt
angular_acceleration = torque / 1000
self.angular_velocity += angular_acceleration * dt
self.angle += degrees(self.angular_velocity) * dt
else:
self.force.x = traction_force + resistance_force.x
self.force.y = resistance_force.y
self.acceleration = self.force / self.mass
self.wheel_rpm = self.velocity.x / self.wheel_radius * 30 / pi
self.velocity += self.acceleration * dt
if self.steering:
turning_radius = self.length / tan(radians(self.steering))
angular_velocity = self.velocity.x / turning_radius
else:
angular_velocity = 0
self.position += self.velocity.rotate(-self.angle) * dt
self.angle += degrees(angular_velocity) * dt
class Engine:
def __init__(self):
self.rpm_lut = np.array([1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000, 9000])
self.torque_lut = np.array([200, 325, 475, 550, 550, 500, 375, 300, 0])
def get_torque(self, rpm, throttle):
if rpm < 1000:
rpm = 1000
torque = np.interp(rpm, self.rpm_lut, self.torque_lut)
return torque * throttle
File added
This diff is collapsed.
{
"time": [
0.2,
0.4,
0.6000000000000001,
0.8,
1.0,
1.2,
1.4,
1.5999999999999999,
1.7999999999999998,
1.9999999999999998,
2.1999999999999997,
2.4,
2.6,
2.8000000000000003,
3.0000000000000004,
3.2000000000000006,
3.400000000000001,
3.600000000000001,
3.800000000000001,
4.000000000000001,
4.200000000000001,
4.400000000000001,
4.600000000000001,
4.800000000000002,
5.000000000000002,
5.200000000000002,
5.400000000000002,
5.600000000000002,
5.8000000000000025,
6.000000000000003,
6.200000000000003,
6.400000000000003,
6.600000000000003,
6.800000000000003,
7.0000000000000036,
7.200000000000004,
7.400000000000004,
7.600000000000004,
7.800000000000004,
8.000000000000004,
8.200000000000003,
8.400000000000002,
8.600000000000001,
8.8,
9.0,
9.2,
9.399999999999999,
9.599999999999998,
9.799999999999997,
9.999999999999996,
10.199999999999996,
10.399999999999995,
10.599999999999994,
10.799999999999994,
10.999999999999993,
11.199999999999992,
11.399999999999991,
11.59999999999999,
11.79999999999999,
11.99999999999999,
12.199999999999989,
12.399999999999988,
12.599999999999987,
12.799999999999986,
12.999999999999986,
13.199999999999985,
13.399999999999984,
13.599999999999984,
13.799999999999983,
13.999999999999982,
14.199999999999982,
14.39999999999998,
14.59999999999998,
14.79999999999998,
14.999999999999979,
15.199999999999978,
15.399999999999977,
15.599999999999977,
15.799999999999976,
15.999999999999975,
16.199999999999974,
16.399999999999974,
16.599999999999973,
16.799999999999972,
16.99999999999997,
17.19999999999997,
17.39999999999997,
17.59999999999997,
17.79999999999997,
17.999999999999968,
18.199999999999967,
18.399999999999967,
18.599999999999966,
18.799999999999965,
18.999999999999964,
19.199999999999964,
19.399999999999963,
19.599999999999962,
19.79999999999996,
19.99999999999996,
20.19999999999996,
20.39999999999996,
20.59999999999996,
20.799999999999958,
20.999999999999957,
21.199999999999957,
21.399999999999956,
21.599999999999955,
21.799999999999955,
21.999999999999954,
22.199999999999953,
22.399999999999952,
22.59999999999995
],
"x": [
1732.961286387469,
1732.1526264091385,
1732.3471681903861,
1732.2408878911015,
1732.1597730980216,
1731.5970807639185,
1732.0396357136551,
1731.5831140336522,
1730.9454885844018,
1734.4604226971344,
1710.2313881477512,
1723.0853565578536,
1697.834704882184,
1704.9171165268822,
1678.4674650048732,
1678.64617358835,
1663.8681485158445,
1677.2253629120341,
1659.5429674319553,
1676.8234346383222,
1679.837523146973,
1707.3913389356733,
1727.7523903942556,
1757.421682804023,
1787.335885374785,
1815.5525552677468,
1836.0759445730455,
1856.0091221624198,
1884.9167934953487,
1912.049440527352,
1942.359393049133,
1973.4653245132022,
2000.2245521430734,
2027.3009316476896,
2042.565440411347,
2057.353972013345,
2056.2946467356514,
2054.046605574749,
2036.8897779452925,
2018.089178819594,
2008.0105180571222,
1999.375186658567,
2002.2475343666129,
1988.2369020733113,
1976.7995089720039,
1953.973011362798,
1929.7211597303321,
1899.5702402493023,
1867.9716946644241,
1839.0043039469551,
1810.0664341767936,
1791.2983035107504,
1774.558776123896,
1771.986904334932,
1773.061556216539,
1762.9958341974534,
1751.7130695655565,
1731.1877905775837,
1724.096636962187,
1716.5814105065717,
1693.232796996565,
1694.1444414947143,
1668.549851619035,
1649.768271430797,
1619.4100763464303,
1588.6620350135713,
1558.277848130564,
1525.8603036490567,
1504.192002462834,
1481.9658148770195,
1475.8984474527151,
1471.8503301267024,
1457.4885976228659,
1441.155359570068,
1416.9122898253477,
1404.0629565734348,
1392.2228641542615,
1393.364326420527,
1398.8904431646479,
1416.6246744636996,
1439.384930225874,
1469.012913293123,
1501.2736313171504,
1533.2213225710068,
1563.529372043896,
1587.0287888184832,
1604.5808193286337,
1632.1733382809073,
1660.4355375317052,
1675.1436077784408,
1707.8152494287776,
1737.6869471112495,
1758.376854127139,
1778.1625434851464,
1783.9417070337638,
1785.4613324113277,
1773.1570712657503,
1772.863193044739,
1776.013025666458,
1788.9183082003865,
1808.394795928098,
1835.5562009425853,
1866.9903319383652,
1900.2721855594268,
1932.6749979835827,
1960.4521524084503,
1981.9814693167236,
1994.1972335397857,
1996.896181862177,
1988.9794825792592,
1971.7830231287653,
1963.9812514158043,
1960.9902151298913
],
"y": [
-407.38574749946434,
-410.1450781792827,
-414.29637723334713,
-419.818127512097,
-426.71211840747475,
-434.9647822879525,
-444.59943904856783,
-455.58038002735066,
-467.91044947354226,
-481.442834320628,
-488.51871123137795,
-510.6809772390142,
-516.5700532944252,
-542.5218465339217,
-547.4461353722113,
-576.2700374912258,
-596.5567969953598,
-621.8927342720442,
-650.1798270897751,
-676.2700116023415,
-708.6531209885454,
-721.0133330771085,
-746.8476387905589,
-741.8178038711459,
-758.3850957433021,
-771.2982274243907,
-793.7167352819238,
-817.2932482633912,
-832.1131628686057,
-847.2890239621718,
-849.1416291299522,
-849.8532770923348,
-835.9452716692517,
-820.3526660927863,
-794.3060860117681,
-766.639332443908,
-736.3960864707866,
-704.9903533695303,
-679.9641880209497,
-654.5789391437684,
-623.1055836005652,
-592.6860125342031,
-561.8138535489933,
-530.9977671174613,
-501.20467805698325,
-480.07553076074544,
-459.4138034057579,
-452.9993001585988,
-448.5462631127082,
-459.2041936105157,
-472.7696039970146,
-497.3915321526512,
-524.6687568240793,
-555.626367429977,
-587.653089043164,
-619.9959728506324,
-650.3018007126493,
-674.4776916354847,
-708.1495163476111,
-739.854135446169,
-763.938113101683,
-798.3070504334978,
-821.5342579407381,
-849.0252828967887,
-858.0331750272223,
-870.9067994540502,
-863.1401210396791,
-855.9792400311505,
-833.1992064440471,
-808.7048856340194,
-777.7105267827612,
-744.9765338119622,
-713.6017626909212,
-684.8663918174944,
-663.3042435908567,
-630.735688199831,
-599.6288835560953,
-567.1823441927924,
-534.6369712723381,
-507.70916531776646,
-483.87948489298327,
-470.9629076346409,
-464.30916078072187,
-469.9462355403672,
-482.8678252494172,
-505.40692506178317,
-533.3151952232445,
-555.4528168518173,
-573.8224954223197,
-605.342527239041,
-618.2987266909263,
-635.0138534193854,
-662.7337669088829,
-690.2833230301019,
-722.6557520411346,
-756.3215540312655,
-786.717122358282,
-822.7297349496205,
-856.7275742126394,
-887.6215141972548,
-914.9837010446107,
-934.1482397721055,
-945.7574379704569,
-946.3635567071002,
-937.787339300482,
-919.289062000256,
-893.5545930254899,
-862.4046904032793,
-828.9163342544612,
-796.3228802290259,
-767.4021377154822,
-731.9563138296951,
-697.4870500671282
]
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment