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)