Skip to content

Commit

Permalink
feat: add multitrack training
Browse files Browse the repository at this point in the history
  • Loading branch information
@retinfai
retinfai committed Aug 27, 2023
1 parent 68d5939 commit c56195d
Show file tree
Hide file tree
Showing 6 changed files with 226 additions and 25 deletions.
2 changes: 1 addition & 1 deletion src/controllers/controllers/ftg_policy.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,7 +214,7 @@ def select_action(self,state,goal_pos):
dmin = min(border_ranges)
alpha = 1
goal_angle = self.constrain_angle(goal_angle)
final_heading_angle = ((alpha/dmin)*gap_centre_angle+goal_angle)/((alpha/dmin)+1)
final_heading_angle = gap_centre_angle #((alpha/dmin)*gap_centre_angle+goal_angle)/((alpha/dmin)+1)
# Convert to angular velocity
ang = self.angle_to_ang_vel(final_heading_angle, lin)
action = np.asarray([lin, ang])
Expand Down
48 changes: 45 additions & 3 deletions src/environments/environments/CarTrackEnvironment.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,16 +39,53 @@ def __init__(self,
self.REWARD_RANGE = reward_range

self.observation_mode = observation_mode
self.track = track

# Reset Client -----------------------------------------------
self.all_goals = goal_positions[track]

self.goals_reached = 0
self.start_goal_index = 0
self.car_waypoints = waypoints[track]

self.steps_since_last_goal = 0

if track != 'multi_track':
self.all_goals = goal_positions[track]
self.car_waypoints = waypoints[track]
else:
austin_gp = goal_positions['austin_track']
budapest_gp = goal_positions['budapest_track']
budapest_gp = [[x + 200, y] for x, y in budapest_gp]
hockenheim_gp = goal_positions['hockenheim_track']
hockenheim_gp = [[x + 300, y] for x, y in hockenheim_gp]

self.all_car_goals = {
'austin_track': austin_gp,
'budapest_track': budapest_gp,
'hockenheim_track': hockenheim_gp
}

austin_wp = waypoints['austin_track']

budapest_wp = []
for x, y, yaw, index in waypoints['budapest_track']:
x += 200
budapest_wp.append((x, y, yaw, index))

hockenheim_wp = []
for x, y, yaw, index in waypoints['hockenheim_track']:
x += 300
hockenheim_wp.append((x, y, yaw, index))

self.all_car_waypoints = {
'austin_track': austin_wp,
'budapest_track': budapest_wp,
'hockenheim_track': hockenheim_wp
}

self.current_track = 'austin_track'

self.all_goals = self.all_car_goals[self.current_track]
self.car_waypoints = self.all_car_waypoints[self.current_track]

self.get_logger().info('Environment Setup Complete')

def reset(self):
Expand All @@ -58,6 +95,11 @@ def reset(self):

self.set_velocity(0, 0)

if self.track == 'multi_track':
self.current_track = random.choice(list(self.all_car_goals.keys()))
self.all_goals = self.all_car_goals[self.current_track]
self.car_waypoints = self.all_car_waypoints[self.current_track]

# New random starting point for car
car_x, car_y, car_yaw, index = random.choice(self.car_waypoints)

Expand Down
23 changes: 3 additions & 20 deletions src/environments/environments/waypoints.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,26 +4,6 @@


waypoints = {
# 'track_1': [
# Waypoint(-14.11, 3.71, -1.81),
# Waypoint(-15.75, -1.71, -1.96),
# Waypoint(-16.71, -5.48, -1.35),
# Waypoint(-7.53, -5.43, -0.27),
# Waypoint(-0.22, -8.55, -0.87),
# Waypoint(1.9, -12.05, -0.66),
# Waypoint(5.5, -12.38, 0.79),
# Waypoint(7.32, -7.3, 0.97),
# Waypoint(9.62, -1.45, 1.23),
# Waypoint(10.76, 4.08, 1.64),
# Waypoint(8.48, 9.77, 2.07),
# Waypoint(4.93, 13.27, -3.06),
# Waypoint(1.5, 9.96, -1.9),
# Waypoint(-0.89, 5.67, -2.94),
# Waypoint(-5.52, 10.83, 2.02),
# Waypoint(-6.67, 15.67, 2.38),
# Waypoint(-10.84, 13.99, -1.93),
# Waypoint(12.81, 8.14, -1.93),
# ],
'austin_track': [
Waypoint(0, 0, -0.64, 0),
Waypoint(7.29, -5.57, -0.64, 8),
Expand Down Expand Up @@ -64,6 +44,9 @@
Waypoint(9.98, 8.54, -2.53, 254),
Waypoint(4.92, -10.3, 2.03, 296),
],
'multi_track': [
Waypoint(4.92, -10.3, 2.03, 0),
]



Expand Down
175 changes: 175 additions & 0 deletions src/environments/worlds/multi_track.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,175 @@
<?xml version='1.0'?>
<sdf version="1.6">
<world name="empty">

<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>10.0</real_time_factor>
</physics>

<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>

<light type
="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="austin_ground_plane">
<pose>100 0 0 0 0 0</pose>
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>1000 1000</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>50</mu>
</ode>
<bullet>
<friction>1</friction>
<rolling_friction>0.1</rolling_friction>
</bullet>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>1000 1000</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name="austin_track">
<static>true</static>
<!-- <pose>0 0 -9 0 0 0</pose> -->
<!-- <pose>0 0 -8.9 0 0 0</pose> -->
<link name="track">
<visual name='track_vis'>
<geometry>
<!-- <mesh filename="package://environments/meshes/track.dae" scale="0.001 0.001 0.001"/> -->
<mesh>
<uri>model://src/environments/meshes/austin_track.stl</uri>
<scale>0.001 0.001 0.001</scale>
</mesh>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<collision name='track_col'>
<geometry>
<!-- <mesh filename="package://environments/meshes/track.dae" scale="0.001 0.001 0.001" /> -->
<mesh>
<uri>model://src/environments/meshes/austin_track.stl</uri>
<scale>0.001 0.001 0.001</scale>
</mesh>
</geometry>
</collision>
</link>
</model>

<model name="budapest_track">
<pose>200 0 0 0 0 0</pose>
<static>true</static>
<!-- <pose>0 0 -9 0 0 0</pose> -->
<!-- <pose>0 0 -8.9 0 0 0</pose> -->
<link name="track">
<visual name='track_vis'>
<geometry>
<!-- <mesh filename="package://environments/meshes/track.dae" scale="0.001 0.001 0.001"/> -->
<mesh>
<uri>model://src/environments/meshes/budapest_track.stl</uri>
<scale>0.001 0.001 0.001</scale>
</mesh>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<collision name='track_col'>
<geometry>
<!-- <mesh filename="package://environments/meshes/track.dae" scale="0.001 0.001 0.001" /> -->
<mesh>
<uri>model://src/environments/meshes/budapest_track.stl</uri>
<scale>0.001 0.001 0.001</scale>
</mesh>
</geometry>
</collision>
</link>
</model>
<model name="hockenheim_track">
<static>true</static>
<pose>300 0 0 0 0 0</pose>
<!-- <pose>0 0 -9 0 0 0</pose> -->
<!-- <pose>0 0 -8.9 0 0 0</pose> -->
<link name="track">
<visual name='track_vis'>
<geometry>
<!-- <mesh filename="package://environments/meshes/track.dae" scale="0.001 0.001 0.001"/> -->
<mesh>
<uri>model://src/environments/meshes/hockenheim_track.stl</uri>
<scale>0.001 0.001 0.001</scale>
</mesh>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
<collision name='track_col'>
<geometry>
<!-- <mesh filename="package://environments/meshes/track.dae" scale="0.001 0.001 0.001" /> -->
<mesh>
<uri>model://src/environments/meshes/hockenheim_track.stl</uri>
<scale>0.001 0.001 0.001</scale>
</mesh>
</geometry>
</collision>
</link>
</model>
</world>
</sdf>
2 changes: 1 addition & 1 deletion src/reinforcement_learning/config/train.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
train:
ros__parameters:
environment: 'CarTrack' # CarGoal, CarWall, CarBlock, CarTrack, CarBeat
track: 'hockenheim_track' # track_1, track_2, track_3 -> only applies for CarTrack
track: 'multi_track' # track_1, track_2, track_3 -> only applies for CarTrack
algorithm: 'TD3'
max_steps_exploration: 1000
max_steps_training: 1000000
Expand Down
1 change: 1 addition & 0 deletions src/reinforcement_learning/reinforcement_learning/train.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -290,6 +290,7 @@ def evaluate_policy(env, agent, num_episodes):
while not truncated and not terminated:

action = agent.select_action_from_policy(state)
action = hlp.denormalize(action, env.MAX_ACTIONS, env.MIN_ACTIONS)
next_state, reward, terminated, truncated, _ = env.step(action)

episode_reward += reward
Expand Down

0 comments on commit c56195d

Please sign in to comment.