def _trajectory(self,
n_waypoints=25,
n_samples=100,
return_waypoints_only=True,
**kwargs):
""" center, inner and outer track lines
n_waypoints : no of points used to fit cubic splines
n_samples : no of points finally sampled after fitting cubic splines
"""
if "length" not in kwargs:
length = self.length
else:
length = kwargs["length"]
if "breadth" not in kwargs:
breadth = self.breadth
else:
breadth = kwargs["breadth"]
s = np.linspace(0, 2 * (length + breadth) - 1e-2, n_waypoints)
wx = np.empty([n_waypoints])
wy = np.empty([n_waypoints])
for ids, theta in enumerate(s):
wx[ids], wy[ids] = self.param_to_xy(theta, **kwargs)
if return_waypoints_only:
return wx, wy
sp = Spline2D(wx, wy)
s = np.arange(0, sp.s[-1], self.track_length / n_samples)
x, y = [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
x.append(ix)
y.append(iy)
return x, y
def fit_cubic_splines(self, wx, wy, n_samples):
""" fit cubic splines on the waypoints
"""
sp = Spline2D(wx, wy)
s = np.linspace(0, sp.s[-1] - 1e-3, n_samples)
x, y = [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
x.append(ix)
y.append(iy)
return x, y
def _fit_cubic_splines(self, wx, wy, n_samples):
""" fit cubic splines on waypoints
"""
sp = Spline2D(wx, wy)
self.spline = sp
# raceline = np.concatenate([[wx],[wy]])
# raceline_length = self._calc_raceline_length(raceline)
s = np.linspace(0, sp.s[-1] - 0.001, n_samples)
x, y = [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
x.append(ix)
y.append(iy)
return x, y, s
def gen_traj(x_all, idx, sim):
w_idx = x_all[sim][idx]
wx, wy = rand_traj.calculate_xy(
width=w_idx,
last_index=NODES[LASTIDX],
theta=theta,
)
sp = Spline2D(wx, wy)
s = np.linspace(0, sp.s[-1]-0.001, n_samples)
x, y = [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
x.append(ix)
y.append(iy)
return wx, wy, x, y
def _load_raceline(self, wx, wy, n_samples, v=None, t=None):
""" load raceline and fit cubic splines
"""
# x, y, theta = self._fit_cubic_splines(
# wx=wx,
# wy=wy,
# n_samples=n_samples
# )
# theta = np.cumsum(np.linalg.norm(np.diff(np.array([x,y])), 2, axis=0))
# theta = np.concatenate([np.array([0]), theta])
self.spline = Spline2D(wx, wy)
x, y = wx, wy
theta = self.spline.s
self.x_raceline = np.array(x)
self.y_raceline = np.array(y)
self.raceline = np.array([x, y])
if v is not None:
self.v_raceline = v
self.t_raceline = t
self.spline_v = Spline(theta, v)