def generate_target_course(x, y):
"""
Return the global course and frenet path defined by a list of waypoints.
Args:
x (:list(float):): list of x positions [m]
y (:list(float):): list of y positions [m]
Returns:
rx (:list(float):): list of x positions [m]
ry (:list(float):): list of y positions [m]
ryaw (:list(float):): list of yaws [rads]
rk (:list(float):): list of curvatures [1/m]
csp (:py:class:`~pylot.control.mpc.utils.CubicSpline2D`): Cubic spline
defined by input x, y
"""
csp = CubicSpline2D(x, y)
s = np.arange(0, csp.s[-1], 0.1)
rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = csp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(csp.calc_yaw(i_s))
rk.append(csp.calc_curvature(i_s))
return rx, ry, ryaw, rk, csp
def setup_mpc(self, waypoints, target_speeds):
path = waypoints.as_numpy_array_2D()
# convert target waypoints into spline
spline = CubicSpline2D(path[0, :], path[1, :])
ss = []
vs = []
xs = []
ys = []
yaws = []
ks = []
for i, s in enumerate(spline.s[:-1]):
x, y = spline.calc_position(s)
yaw = np.abs(spline.calc_yaw(s))
k = spline.calc_curvature(s)
xs.append(x)
ys.append(y)
yaws.append(yaw)
ks.append(k)
ss.append(s)
vs.append(target_speeds[i])
self._mpc_config["reference"] = {
't_list': [], # Time [s]
's_list': ss, # Arc distance [m]
'x_list': xs, # Desired X coordinates [m]
'y_list': ys, # Desired Y coordinates [m]
'k_list': ks, # Curvatures [1/m]
'vel_list': vs, # Desired tangential velocities [m/s]
'yaw_list': yaws, # Yaws [rad]
}
# initialize mpc controller
self._mpc = ModelPredictiveController(config=self._mpc_config)
def setup_mpc(self, waypoints):
path = np.array([[wp.location.x, wp.location.y] for wp in waypoints])
# convert target waypoints into spline
spline = CubicSpline2D(path[:, 0], path[:, 1], self._logger)
ss = []
vels = []
xs = []
ys = []
yaws = []
ks = []
for s in spline.s[:-2]:
x, y = spline.calc_position(s)
yaw = np.abs(spline.calc_yaw(s))
k = spline.calc_curvature(s)
xs.append(x)
ys.append(y)
yaws.append(yaw)
ks.append(k)
ss.append(s)
vels.append(18)
self._config["reference"] = {
't_list': [], # Time [s]
's_list': ss, # Arc distance [m]
'x_list': xs, # Desired X coordinates [m]
'y_list': ys, # Desired Y coordinates [m]
'k_list': ks, # Curvatures [1/m]
'vel_list': vels, # Desired tangential velocities [m/s]
'yaw_list': yaws, # Yaws [rad]
}
# initialize mpc controller
self.mpc = ModelPredictiveController(config=self._config)