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 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 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 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 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