def expand_wpts(self):
n = 5 # number of pts per orig pt
dz = 1 / n
o_line = self.waypoints[:, 0:2]
# o_ss = self.ss
o_vs = self.waypoints[:, 2]
new_line = []
# new_ss = []
new_vs = []
for i in range(len(self.waypoints)-1):
dd = lib.sub_locations(o_line[i+1], o_line[i])
for j in range(n):
pt = lib.add_locations(o_line[i], dd, dz*j)
new_line.append(pt)
# ds = o_ss[i+1] - o_ss[i]
# new_ss.append(o_ss[i] + dz*j*ds)
dv = o_vs[i+1] - o_vs[i]
new_vs.append(o_vs[i] + dv * j * dz)
wpts = np.array(new_line)
# self.ss = np.array(new_ss)
vs = np.array(new_vs)
self.waypoints = np.concatenate([wpts, vs[:, None]], axis=-1)
def print_update(self, plot_reward=True):
if self.reward_ptr < 5:
return
mean = np.mean(self.ep_rewards[max(0, self.reward_ptr-101):self.reward_ptr-1])
print(f"Run: {self.step_counter} --> 100 ep Mean: {mean:.2f} ")
if plot_reward:
lib.plot(self.ep_rewards[0:self.reward_ptr], 20, figure_n=2)
def convert_pts_s_th(pts):
N = len(pts)
s_i = np.zeros(N - 1)
th_i = np.zeros(N - 1)
for i in range(N - 1):
s_i[i] = lib.get_distance(pts[i], pts[i + 1])
th_i[i] = lib.get_bearing(pts[i], pts[i + 1])
return s_i, th_i
def print_update(self, plot_reward=True):
if self.ptr < 5:
return
mean = np.mean(self.rewards[0:self.ptr])
score = self.rewards[-1]
print(
f"Run: {self.t_counter} --> Score: {score:.2f} --> Mean: {mean:.2f} "
)
if plot_reward:
lib.plot(self.rewards[0:self.ptr], figure_n=2)
def expand_wpts(self):
n = 5 # number of pts per orig pt
dz = 1 / n
o_line = self.wpts
new_line = []
for i in range(len(self.wpts) - 1):
dd = lib.sub_locations(o_line[i + 1], o_line[i])
for j in range(n):
pt = lib.add_locations(o_line[i], dd, dz * j)
new_line.append(pt)
self.wpts = np.array(new_line)
def get_target_obs(self):
target = self.env_map.end_goal
pos = np.array([self.car.x, self.car.y])
base_angle = lib.get_bearing(pos, target)
# angle = base_angle - self.car.theta
angle = lib.sub_angles_complex(base_angle, self.car.theta)
# angle = lib.add_angles_complex(base_angle, self.car.theta)
# distance = lib.get_distance(pos, target)
em = self.env_map
s = calculate_progress(pos, em.ref_pts, em.diffs, em.l2s, em.ss_normal)
return [angle, s]
def check_done_reward_track_train(self):
"""
Checks if the race lap is complete
Returns
Done flag
"""
self.reward = 0 # normal
if self.env_map.check_scan_location([self.car.x, self.car.y]):
self.done = True
self.colission = True
self.reward = -1
self.done_reason = f"Crash obstacle: [{self.car.x:.2f}, {self.car.y:.2f}]"
# horizontal_force = self.car.mass * self.car.th_dot * self.car.velocity
# self.y_forces.append(horizontal_force)
# if horizontal_force > self.car.max_friction_force:
# self.done = True
# self.reward = -1
# self.done_reason = f"Friction limit reached: {horizontal_force} > {self.car.max_friction_force}"
if self.steps > self.max_steps:
self.done = True
self.done_reason = f"Max steps"
car = [self.car.x, self.car.y]
cur_end_dis = lib.get_distance(car, self.env_map.start_pose[0:2])
if cur_end_dis < self.end_distance and self.steps > 800:
self.done = True
self.reward = 1
self.done_reason = f"Lap complete, d: {cur_end_dis}"
return self.done
def update_kinematic_state(self, a, d_dot, dt):
"""
Updates the internal state of the vehicle according to the kinematic equations for a bicycle model
Args:
a: acceleration
d_dot: rate of change of steering angle
dt: timestep in seconds
"""
self.x = self.x + self.velocity * np.sin(self.theta) * dt
self.y = self.y + self.velocity * np.cos(self.theta) * dt
theta_dot = self.velocity / self.wheelbase * np.tan(self.steering)
self.th_dot = theta_dot
dth = theta_dot * dt
self.theta = lib.add_angles_complex(self.theta, dth)
a = np.clip(a, -self.max_a, self.max_a)
d_dot = np.clip(d_dot, -self.max_d_dot, self.max_d_dot)
self.steering = self.steering + d_dot * dt
self.velocity = self.velocity + a * dt
self.steering = np.clip(self.steering, -self.max_steer, self.max_steer)
self.velocity = np.clip(self.velocity, -self.max_v, self.max_v)
def expand_wpts(self):
n = 5 # number of pts per orig pt
dz = 1 / n
o_line = self.waypoints
o_vs = self.vs
new_line = []
new_vs = []
for i in range(len(o_line) - 1):
dd = lib.sub_locations(o_line[i + 1], o_line[i])
for j in range(n):
pt = lib.add_locations(o_line[i], dd, dz * j)
new_line.append(pt)
dv = o_vs[i + 1] - o_vs[i]
new_vs.append(o_vs[i] + dv * j * dz)
self.waypoints = np.array(new_line)
self.vs = np.array(new_vs)
def transform_obs(self, obs):
observation = obs['state']
cur_v = [observation[3]/self.max_v]
cur_d = [observation[4]/self.max_steer]
angle = [lib.get_bearing(observation[0:2], [1, 21])/self.max_steer]
scan = np.array(obs['scan']) / self.range_finder_scale
nn_obs = np.concatenate([cur_v, cur_d, angle, scan])
return nn_obs
def add_obstacles(self):
obs_img = np.zeros_like(self.obs_img)
obs_size_m = np.array([self.obs_size, self.obs_size])
obs_size_px = obs_size_m / self.resolution
rands = np.random.uniform(size=(self.n_obs, 2))
idx_rands = rands[:, 0] * len(self.ws)
w_rands = (rands[:, 1] * 2 - np.ones_like(rands[:, 1]))
w_rands = w_rands * np.mean(
self.ws
) * 0.3 # x average length, adjusted to be both sides of track
# magic 0.8 is to keep the obstacles closer to the center of the track
obs_locations = []
for i in range(self.n_obs):
idx = idx_rands[i]
w = w_rands[i]
int_idx = int(idx) # note that int always rounds down
# start with using just int_idx
n = self.nvecs[i]
offset = np.array([n[0] * w, n[1] * w])
location = lib.add_locations(self.t_pts[int_idx], offset)
if lib.get_distance(location, self.start_pose[0:2]) < 1:
continue
# location = np.flip(location)
# location = self.t_pts[int_idx]
rc_location = self.xy_to_row_column(location)
location = np.array(location, dtype=int)
obs_locations.append(rc_location)
obs_locations = np.array(obs_locations)
for location in obs_locations:
x, y = location[0], location[1]
for i in range(0, int(obs_size_px[0])):
for j in range(0, int(obs_size_px[1])):
if x + i < self.map_width and y + j < self.map_height:
# obs_img[x+i, y+j] = 255
obs_img[y + j, x + i] = 255
self.obs_img = obs_img
def __init__(self, map_name, sim_conf=None):
"""
Init function
Args:
map_name: name of forest map to use.
sim_conf: config file for simulation
"""
if sim_conf is None:
path = os.path.dirname(__file__)
sim_conf = lib.load_conf(path, "std_config")
env_map = ForestMap(map_name)
BaseSim.__init__(self, env_map, self.check_done_forest, sim_conf)
def __init__(self, map_name, sim_conf=None):
"""
Init function
Args:
map_name: name of map to use.
sim_conf: config file for simulation
"""
if sim_conf is None:
path = os.path.dirname(__file__)
sim_conf = lib.load_conf(path, "std_config")
env_map = TrackMap(map_name)
BaseSim.__init__(self, env_map, self.check_done_reward_track_train,
sim_conf)
self.end_distance = sim_conf.end_distance
def find_true_widths(pts, nvecs, ws, check_scan_location):
tx = pts[:, 0]
ty = pts[:, 1]
onws = ws[:, 0]
opws = ws[:, 1]
stp_sze = 0.1
sf = 0.5 # safety factor
N = len(pts)
nws, pws = [], []
for i in range(N):
pt = [tx[i], ty[i]]
nvec = nvecs[i]
if not check_scan_location(pt):
j = stp_sze
s_pt = lib.add_locations(pt, nvec, j)
while not check_scan_location(s_pt) and j < opws[i]:
j += stp_sze
s_pt = lib.add_locations(pt, nvec, j)
pws.append(j * sf)
j = stp_sze
s_pt = lib.sub_locations(pt, nvec, j)
while not check_scan_location(s_pt) and j < onws[i]:
j += stp_sze
s_pt = lib.sub_locations(pt, nvec, j)
nws.append(j * sf)
# print(f"Pt added without being adjusted")
else:
print(f"Obs in way of pt: {i}")
for j in np.linspace(0, onws[i], 10):
p_pt = lib.add_locations(pt, nvec, j)
n_pt = lib.sub_locations(pt, nvec, j)
if not check_scan_location(p_pt):
nws.append(-j * (1 + sf))
pws.append(opws[i])
print(f"PosPt NewW: [{-j*(1+sf)}, {opws[i]}]")
break
elif not check_scan_location(n_pt):
pws.append(-j * (1 + sf))
nws.append(onws[i])
print(f"PosPt NewW: [{-j*(1+sf)}, {onws[i]}]")
break
if j == onws[i]:
print(f"Problem - no space found")
nws, pws = np.array(nws), np.array(pws)
ws = np.concatenate([nws[:, None], pws[:, None]], axis=-1)
return ws
def transform_obs(self, obs, pp_action):
"""
Transforms the observation received from the environment into a vector which can be used with a neural network.
Args:
obs: observation from env
pp_action: [steer, speed] from pure pursuit controller
Returns:
nn_obs: observation vector for neural network
"""
state = obs['state']
cur_v = [state[3] / self.max_v]
cur_d = [state[4] / self.max_steer]
# vr_scale = [(pp_action[1])/self.max_v]
angle = [lib.get_bearing(state[0:2], [1, 21]) / self.max_steer]
dr_scale = [pp_action[0] / self.max_steer]
scan = np.array(obs['scan']) / self.range_finder_scale
nn_obs = np.concatenate([cur_v, cur_d, dr_scale, angle, scan])
return nn_obs
def min_render(self, wait=False):
"""
TODO: deprecate
"""
fig = plt.figure(4)
plt.clf()
ret_map = self.env_map.scan_map
plt.imshow(ret_map)
# plt.xlim([0, self.env_map.width])
# plt.ylim([0, self.env_map.height])
s_x, s_y = self.env_map.convert_to_plot(self.env_map.start)
plt.plot(s_x, s_y, '*', markersize=12)
c_x, c_y = self.env_map.convert_to_plot([self.car.x, self.car.y])
plt.plot(c_x, c_y, '+', markersize=16)
for i in range(self.scan_sim.number_of_beams):
angle = i * self.scan_sim.dth + self.car.theta - self.scan_sim.fov / 2
fs = self.scan_sim.scan_output[
i] * self.scan_sim.n_searches * self.scan_sim.step_size
dx = [np.sin(angle) * fs, np.cos(angle) * fs]
range_val = lib.add_locations([self.car.x, self.car.y], dx)
r_x, r_y = self.env_map.convert_to_plot(range_val)
x = [c_x, r_x]
y = [c_y, r_y]
plt.plot(x, y)
for pos in self.action_memory:
p_x, p_y = self.env_map.convert_to_plot(pos)
plt.plot(p_x, p_y, 'x', markersize=6)
text_start = self.env_map.width + 10
spacing = int(self.env_map.height / 10)
s = f"Reward: [{self.reward:.1f}]"
plt.text(text_start, spacing * 1, s)
s = f"Action: [{self.action[0]:.2f}, {self.action[1]:.2f}]"
plt.text(text_start, spacing * 2, s)
s = f"Done: {self.done}"
plt.text(text_start, spacing * 3, s)
s = f"Pos: [{self.car.x:.2f}, {self.car.y:.2f}]"
plt.text(text_start, spacing * 4, s)
s = f"Vel: [{self.car.velocity:.2f}]"
plt.text(text_start, spacing * 5, s)
s = f"Theta: [{(self.car.theta * 180 / np.pi):.2f}]"
plt.text(text_start, spacing * 6, s)
s = f"Delta x100: [{(self.car.steering*100):.2f}]"
plt.text(text_start, spacing * 7, s)
s = f"Theta Dot: [{(self.car.th_dot):.2f}]"
plt.text(text_start, spacing * 8, s)
s = f"Steps: {self.steps}"
plt.text(100, spacing * 9, s)
plt.pause(0.0001)
if wait:
plt.show()
def MinCurvatureTrajectory(pts, nvecs, ws):
"""
This function uses optimisation to minimise the curvature of the path
"""
# w_min = ws[:, 0]
w_min = -ws[:, 0]
w_max = ws[:, 1]
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
N = len(pts)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N - 1)
th_f = ca.MX.sym('n_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
th1_f = ca.MX.sym('y1_f', N - 1)
th2_f = ca.MX.sym('y1_f', N - 1)
th1_f1 = ca.MX.sym('y1_f', N - 2)
th2_f1 = ca.MX.sym('y1_f', N - 2)
o_x_s = ca.Function('o_x', [n_f], [pts[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [pts[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [pts[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [pts[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
o_y_e(n_f_a[1:]))
])
real = ca.Function(
'real', [th1_f, th2_f],
[ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
im = ca.Function(
'im', [th1_f, th2_f],
[-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
[ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))])
get_th_n = ca.Function(
'gth', [th_f],
[sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
# objective
real1 = ca.Function(
'real1', [th1_f1, th2_f1],
[ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
im1 = ca.Function(
'im1', [th1_f1, th2_f1],
[-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)])
sub_cmplx1 = ca.Function(
'a_cpx1', [th1_f1, th2_f1],
[ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))])
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N - 1)
nlp = {\
'x': ca.vertcat(n, th),
'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])),
# 'f': ca.sumsqr(track_length(n)),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th)),
# boundary constraints
n[0], #th[0],
n[-1], #th[-1],
) \
}
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}})
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 0}})
ones = np.ones(N)
n0 = ones * 0
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(pts[i, 0:2], pts[i + 1, 0:2])
th0.append(th_00)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
lbx = list(w_min) + [-np.pi] * (N - 1)
ubx = list(w_max) + [np.pi] * (N - 1)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
x_opt = r['x']
print(f"Sol Status (Optimal found?): {S.stats()['success']}")
n_set = np.array(x_opt[:N])
# thetas = np.array(x_opt[1*N:2*(N-1)])
return n_set
