def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eig_val, eig_vec = np.linalg.eig(Pxy)
if eig_val[0] >= eig_val[1]:
big_ind = 0
small_ind = 1
else:
big_ind = 1
small_ind = 0
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
# eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
# numbers extremely close to 0 (~10^-20), catch these cases and set
# the respective variable to 0
try:
a = math.sqrt(eig_val[big_ind])
except ValueError:
a = 0
try:
b = math.sqrt(eig_val[small_ind])
except ValueError:
b = 0
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
fx = np.stack([x, y]).T @ rot_mat_2d(angle)
px = np.array(fx[:, 0] + xEst[0, 0]).flatten()
py = np.array(fx[:, 1] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_position):
tx = [ix - sweep_start_position[0] for ix in ox]
ty = [iy - sweep_start_position[1] for iy in oy]
th = math.atan2(sweep_vec[1], sweep_vec[0])
converted_xy = np.stack([tx, ty]).T @ rot_mat_2d(th)
return converted_xy[:, 0], converted_xy[:, 1]
def rectangle_check(x, y, yaw, ox, oy):
# transform obstacles to base link frame
rot = rot_mat_2d(yaw)
for iox, ioy in zip(ox, oy):
tx = iox - x
ty = ioy - y
converted_xy = np.stack([tx, ty]).T @ rot
rx, ry = converted_xy[0], converted_xy[1]
if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0):
return False # no collision
return True # collision
def plot_ellipse(xCenter, cBest, cMin, e_theta): # pragma: no cover
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
b = cBest / 2.0
angle = math.pi / 2.0 - e_theta
cx = xCenter[0]
cy = xCenter[1]
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
fx = rot_mat_2d(-angle) @ np.array([x, y])
px = np.array(fx[0, :] + cx).flatten()
py = np.array(fx[1, :] + cy).flatten()
plt.plot(cx, cy, "xc")
plt.plot(px, py, "--c")
def _rectangle_search(self, x, y):
X = np.array([x, y]).T
d_theta = np.deg2rad(self.d_theta_deg_for_search)
min_cost = (-float('inf'), None)
for theta in np.arange(0.0, np.pi / 2.0 - d_theta, d_theta):
c = X @ rot_mat_2d(theta)
c1 = c[:, 0]
c2 = c[:, 1]
# Select criteria
cost = 0.0
if self.criteria == self.Criteria.AREA:
cost = self._calc_area_criterion(c1, c2)
elif self.criteria == self.Criteria.CLOSENESS:
cost = self._calc_closeness_criterion(c1, c2)
elif self.criteria == self.Criteria.VARIANCE:
cost = self._calc_variance_criterion(c1, c2)
if min_cost[0] < cost:
min_cost = (cost, theta)
# calc best rectangle
sin_s = np.sin(min_cost[1])
cos_s = np.cos(min_cost[1])
c1_s = X @ np.array([cos_s, sin_s]).T
c2_s = X @ np.array([-sin_s, cos_s]).T
rect = RectangleData()
rect.a[0] = cos_s
rect.b[0] = sin_s
rect.c[0] = min(c1_s)
rect.a[1] = -sin_s
rect.b[1] = cos_s
rect.c[1] = min(c2_s)
rect.a[2] = cos_s
rect.b[2] = sin_s
rect.c[2] = max(c1_s)
rect.a[3] = -sin_s
rect.b[3] = cos_s
rect.c[3] = max(c2_s)
return rect
def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy)
if eigval[0] >= eigval[1]:
bigind = 0
smallind = 1
else:
bigind = 1
smallind = 0
t = np.arange(0, 2 * math.pi + 0.1, 0.1)
a = math.sqrt(eigval[bigind])
b = math.sqrt(eigval[smallind])
x = [a * math.cos(it) for it in t]
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
fx = rot_mat_2d(angle) @ (np.array([x, y]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()
plt.plot(px, py, "--r")
def convert_global_coordinate(x, y, sweep_vec, sweep_start_position):
th = math.atan2(sweep_vec[1], sweep_vec[0])
converted_xy = np.stack([x, y]).T @ rot_mat_2d(-th)
rx = [ix + sweep_start_position[0] for ix in converted_xy[:, 0]]
ry = [iy + sweep_start_position[1] for iy in converted_xy[:, 1]]
return rx, ry
def plan_dubins_path(s_x,
s_y,
s_yaw,
g_x,
g_y,
g_yaw,
curvature,
step_size=0.1,
selected_types=None):
"""
Plan dubins path
Parameters
----------
s_x : float
x position of the start point [m]
s_y : float
y position of the start point [m]
s_yaw : float
yaw angle of the start point [rad]
g_x : float
x position of the goal point [m]
g_y : float
y position of the end point [m]
g_yaw : float
yaw angle of the end point [rad]
curvature : float
curvature for curve [1/m]
step_size : float (optional)
step size between two path points [m]. Default is 0.1
selected_types : a list of string or None
selected path planning types. If None, all types are used for
path planning, and minimum path length result is returned.
You can select used path plannings types by a string list.
e.g.: ["RSL", "RSR"]
Returns
-------
x_list: array
x positions of the path
y_list: array
y positions of the path
yaw_list: array
yaw angles of the path
modes: array
mode list of the path
lengths: array
arrow_length list of the path segments.
Examples
--------
You can generate a dubins path.
>>> start_x = 1.0 # [m]
>>> start_y = 1.0 # [m]
>>> start_yaw = np.deg2rad(45.0) # [rad]
>>> end_x = -3.0 # [m]
>>> end_y = -3.0 # [m]
>>> end_yaw = np.deg2rad(-45.0) # [rad]
>>> curvature = 1.0
>>> path_x, path_y, path_yaw, mode, _ = plan_dubins_path(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
>>> plt.plot(path_x, path_y, label="final course " + "".join(mode))
>>> plot_arrow(start_x, start_y, start_yaw)
>>> plot_arrow(end_x, end_y, end_yaw)
>>> plt.legend()
>>> plt.grid(True)
>>> plt.axis("equal")
>>> plt.show()
.. image:: dubins_path.jpg
"""
if selected_types is None:
planning_funcs = _PATH_TYPE_MAP.values()
else:
planning_funcs = [_PATH_TYPE_MAP[ptype] for ptype in selected_types]
# calculate local goal x, y, yaw
l_rot = rot_mat_2d(s_yaw)
le_xy = np.stack([g_x - s_x, g_y - s_y]).T @ l_rot
local_goal_x = le_xy[0]
local_goal_y = le_xy[1]
local_goal_yaw = g_yaw - s_yaw
lp_x, lp_y, lp_yaw, modes, lengths = _dubins_path_planning_from_origin(
local_goal_x, local_goal_y, local_goal_yaw, curvature, step_size,
planning_funcs)
# Convert a local coordinate path to the global coordinate
rot = rot_mat_2d(-s_yaw)
converted_xy = np.stack([lp_x, lp_y]).T @ rot
x_list = converted_xy[:, 0] + s_x
y_list = converted_xy[:, 1] + s_y
yaw_list = angle_mod(np.array(lp_yaw) + s_yaw)
return x_list, y_list, yaw_list, modes, lengths
def calc_global_contour(self):
gxy = np.stack([self.vc_x, self.vc_y]).T @ rot_mat_2d(self.yaw)
gx = gxy[:, 0] + self.x
gy = gxy[:, 1] + self.y
return gx, gy
def test_rot_mat_2d():
assert_allclose(angle.rot_mat_2d(0.0), np.array([[1., 0.], [0., 1.]]))
