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 __call__(self, s, a, s_p, r, dev):
if r == -1:
return r
else:
pt_i, pt_ii, d_i, d_ii = find_closest_pt(s_p[0:2], self.wpts)
d = lib.get_distance(pt_i, pt_ii)
d_c = get_tiangle_h(d_i, d_ii, d) / self.dis_scale
th_ref = lib.get_bearing(pt_i, pt_ii)
th = s_p[2]
d_th = abs(lib.sub_angles_complex(th_ref, th))
v_scale = s_p[3] / self.max_v
new_r = self.mh * np.cos(d_th) * v_scale - self.md * d_c
return new_r + r
def MinCurvatureTrajectory(pts, nvecs, ws):
"""
This function uses optimisation to minimise the curvature of the path
"""
w_min = -ws[:, 0] * 0.9
w_max = ws[:, 1] * 0.9
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']
n_set = np.array(x_opt[:N])
# thetas = np.array(x_opt[1*N:2*(N-1)])
return n_set