github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-12 15:24:57 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix code for coveralls

Browse Source
This commit is contained in:
Atsushi Sakai
2019-02-02 16:51:00 +09:00
parent 1119587b15
commit 4880986bb5
24 changed files with 122 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
AerialNavigation/drone_3d_trajectory_following/Quadrotor.py
View File

@@ -51,12 +51,12 @@ class Quadrotor():
yaw = self.yaw yaw = self.yaw
return np.array( return np.array(
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x], [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) * [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch)
sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y], * sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
[-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z] [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
]) ])
def plot(self): def plot(self): # pragma: no cover
T = self.transformation_matrix() T = self.transformation_matrix()
p1_t = np.matmul(T, self.p1) p1_t = np.matmul(T, self.p1)

86
AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
View File

@@ -129,12 +129,12 @@ class Rocket_Model_6DoF:
[vx], [vx],
[vy], [vy],
[vz], [vz],
[(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy * [(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy
(q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m], * (q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m],
[(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2 + [(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2
2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m], + 2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m],
[(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy * [(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy
(q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m], * (q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m],
[-0.5 * q1 * wx - 0.5 * q2 * wy - 0.5 * q3 * wz], [-0.5 * q1 * wx - 0.5 * q2 * wy - 0.5 * q3 * wz],
[0.5 * q0 * wx + 0.5 * q2 * wz - 0.5 * q3 * wy], [0.5 * q0 * wx + 0.5 * q2 * wz - 0.5 * q3 * wy],
[0.5 * q0 * wy - 0.5 * q1 * wz + 0.5 * q3 * wx], [0.5 * q0 * wy - 0.5 * q1 * wz + 0.5 * q3 * wx],
@@ -154,12 +154,12 @@ class Rocket_Model_6DoF:
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz - [(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz
q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0], - q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0],
[(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz + [(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz
q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0], + q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0],
[(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy - [(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy
q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0], - q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy, [0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy,
- 0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3], - 0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3],
[0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz, [0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz,
@@ -184,12 +184,12 @@ class Rocket_Model_6DoF:
[0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0],
[(-2 * q2**2 - 2 * q3**2 + 1) / m, 2 * [(-2 * q2**2 - 2 * q3**2 + 1) / m, 2
(-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m], * (-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m],
[2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2 * [2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2
q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m], * q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m],
[2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3) / [2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3)
m, (-2 * q1**2 - 2 * q2**2 + 1) / m], / m, (-2 * q1**2 - 2 * q2**2 + 1) / m],
[0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0],
[0, 0, 0], [0, 0, 0],
@@ -227,12 +227,12 @@ class Rocket_Model_6DoF:
def dir_cosine(self, q): def dir_cosine(self, q):
return np.matrix([ return np.matrix([
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] + [1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2]
q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])], + q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 * [2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2
(q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])], * (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
[2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] - [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3]
q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)] - q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
]) ])
def omega(self, w): def omega(self, w):
@@ -459,16 +459,16 @@ class SCProblem:
# Dynamics: # Dynamics:
# x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu # x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu
constraints += [ constraints += [
self.var['X'][:, k + 1] self.var['X'][:, k + 1] ==
== cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) *
* self.var['X'][:, k] self.var['X'][:, k] +
+ cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) *
* self.var['U'][:, k] self.var['U'][:, k] +
+ cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) *
* self.var['U'][:, k + 1] self.var['U'][:, k + 1] +
+ self.par['S_bar'][:, k] * self.var['sigma'] self.par['S_bar'][:, k] * self.var['sigma'] +
+ self.par['z_bar'][:, k] self.par['z_bar'][:, k] +
+ self.var['nu'][:, k] self.var['nu'][:, k]
for k in range(K - 1) for k in range(K - 1)
] ]
@@ -486,10 +486,10 @@ class SCProblem:
# Objective: # Objective:
sc_objective = cvxpy.Minimize( sc_objective = cvxpy.Minimize(
self.par['weight_sigma'] * self.var['sigma'] self.par['weight_sigma'] * self.var['sigma'] +
+ self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf') self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf') +
+ self.par['weight_delta'] * self.var['delta_norm'] self.par['weight_delta'] * self.var['delta_norm'] +
+ self.par['weight_delta_sigma'] * self.var['sigma_norm'] self.par['weight_delta_sigma'] * self.var['sigma_norm']
) )
objective = sc_objective objective = sc_objective
@@ -550,8 +550,8 @@ class SCProblem:
def axis3d_equal(X, Y, Z, ax): def axis3d_equal(X, Y, Z, ax):
max_range = np.array([X.max() - X.min(), Y.max() - max_range = np.array([X.max() - X.min(), Y.max()
Y.min(), Z.max() - Z.min()]).max() - Y.min(), Z.max() - Z.min()]).max()
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
- 1:2:2][0].flatten() + 0.5 * (X.max() + X.min()) - 1:2:2][0].flatten() + 0.5 * (X.max() + X.min())
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
@@ -563,7 +563,7 @@ def axis3d_equal(X, Y, Z, ax):
ax.plot([xb], [yb], [zb], 'w') ax.plot([xb], [yb], [zb], 'w')
def plot_animation(X, U): def plot_animation(X, U): # pragma: no cover
fig = plt.figure() fig = plt.figure()
ax = fig.gca(projection='3d') ax = fig.gca(projection='3d')
@@ -582,8 +582,8 @@ def plot_animation(X, U):
CBI = np.array([ CBI = np.array([
[1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz), [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz),
2 * (qx * qz - qw * qy)], 2 * (qx * qz - qw * qy)],
[2 * (qx * qy - qw * qz), 1 - 2 * [2 * (qx * qy - qw * qz), 1 - 2
(qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)], * (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
[2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx), [2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx),
1 - 2 * (qx ** 2 + qy ** 2)] 1 - 2 * (qx ** 2 + qy ** 2)]
]) ])

2
ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation.py
View File

@@ -241,7 +241,7 @@ class NLinkArm(object):
self.end_effector = np.array(self.points[self.n_links]).T self.end_effector = np.array(self.points[self.n_links]).T
def plot(self, obstacles=[]): def plot(self, obstacles=[]): # pragma: no cover
plt.cla() plt.cla()
for obstacle in obstacles: for obstacle in obstacles:

2
ArmNavigation/arm_obstacle_navigation/arm_obstacle_navigation_2.py
View File

@@ -272,7 +272,7 @@ class NLinkArm(object):
self.end_effector = np.array(self.points[self.n_links]).T self.end_effector = np.array(self.points[self.n_links]).T
def plot(self, myplt, obstacles=[]): def plot(self, myplt, obstacles=[]): # pragma: no cover
myplt.cla() myplt.cla()
for obstacle in obstacles: for obstacle in obstacles:

2
ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py
View File

@@ -49,7 +49,7 @@ class NLinkArm(object):
if self.show_animation: if self.show_animation:
self.plot() self.plot()
def plot(self): def plot(self): # pragma: no cover
plt.cla() plt.cla()
for i in range(self.n_links + 1): for i in range(self.n_links + 1):

4
ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py
View File

@@ -159,5 +159,5 @@ def ang_diff(theta1, theta2):
if __name__ == '__main__': if __name__ == '__main__':
main() # main()
# animation() animation()

6
ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py
View File

@@ -39,8 +39,8 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
try: try:
theta2_goal = np.arccos( theta2_goal = np.arccos(
(x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)) (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2))
theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 * theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2
np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal)))
if theta1_goal < 0: if theta1_goal < 0:
theta2_goal = -theta2_goal theta2_goal = -theta2_goal
@@ -61,7 +61,7 @@ def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0):
return theta1, theta2 return theta1, theta2
def plot_arm(theta1, theta2, x, y): def plot_arm(theta1, theta2, x, y): # pragma: no cover
shoulder = np.array([0, 0]) shoulder = np.array([0, 0])
elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)]) elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
wrist = elbow + \ wrist = elbow + \

2
Localization/extended_kalman_filter/extended_kalman_filter.py
View File

@@ -134,7 +134,7 @@ def ekf_estimation(xEst, PEst, z, u):
return xEst, PEst return xEst, PEst
def plot_covariance_ellipse(xEst, PEst): def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2] Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy) eigval, eigvec = np.linalg.eig(Pxy)

2
Localization/particle_filter/particle_filter.py
View File

@@ -156,7 +156,7 @@ def resampling(px, pw):
return px, pw return px, pw
def plot_covariance_ellipse(xEst, PEst): def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2] Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy) eigval, eigvec = np.linalg.eig(Pxy)

2
Localization/unscented_kalman_filter/unscented_kalman_filter.py
View File

@@ -161,7 +161,7 @@ def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):
return xEst, PEst return xEst, PEst
def plot_covariance_ellipse(xEst, PEst): def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
Pxy = PEst[0:2, 0:2] Pxy = PEst[0:2, 0:2]
eigval, eigvec = np.linalg.eig(Pxy) eigval, eigvec = np.linalg.eig(Pxy)

2
Mapping/circle_fitting/circle_fitting.py
View File

@@ -90,7 +90,7 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
return rx, ry return rx, ry
def plot_circle(x, y, size, color="-b"): def plot_circle(x, y, size, color="-b"): # pragma: no cover
deg = list(range(0, 360, 5)) deg = list(range(0, 360, 5))
deg.append(0) deg.append(0)
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]

14
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
View File

@@ -172,8 +172,8 @@ class BITStar(object):
cBest = self.g_scores[self.goalId] cBest = self.g_scores[self.goalId]
# Computing the sampling space # Computing the sampling space
cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2) + cMin = math.sqrt(pow(self.start[0] - self.goal[1], 2)
pow(self.start[0] - self.goal[1], 2)) / 1.5 + pow(self.start[0] - self.goal[1], 2)) / 1.5
xCenter = np.array([[(self.start[0] + self.goal[0]) / 2.0], xCenter = np.array([[(self.start[0] + self.goal[0]) / 2.0],
[(self.goal[1] - self.start[1]) / 2.0], [0]]) [(self.goal[1] - self.start[1]) / 2.0], [0]])
a1 = np.array([[(self.goal[0] - self.start[0]) / cMin], a1 = np.array([[(self.goal[0] - self.start[0]) / cMin],
@@ -413,8 +413,8 @@ class BITStar(object):
def bestVertexQueueValue(self): def bestVertexQueueValue(self):
if(len(self.vertex_queue) == 0): if(len(self.vertex_queue) == 0):
return float('inf') return float('inf')
values = [self.g_scores[v] + values = [self.g_scores[v]
self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue] + self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue]
values.sort() values.sort()
return values[0] return values[0]
@@ -422,8 +422,8 @@ class BITStar(object):
if(len(self.edge_queue) == 0): if(len(self.edge_queue) == 0):
return float('inf') return float('inf')
# return the best value in the queue by score g_tau[v] + c(v,x) + h(x) # return the best value in the queue by score g_tau[v] + c(v,x) + h(x)
values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1]) + values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1])
self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue] + self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue]
values.sort(reverse=True) values.sort(reverse=True)
return values[0] return values[0]
@@ -549,7 +549,7 @@ class BITStar(object):
plt.grid(True) plt.grid(True)
plt.pause(0.01) plt.pause(0.01)
def plot_ellipse(self, xCenter, cBest, cMin, etheta): def plot_ellipse(self, xCenter, cBest, cMin, etheta): # pragma: no cover
a = math.sqrt(cBest**2 - cMin**2) / 2.0 a = math.sqrt(cBest**2 - cMin**2) / 2.0
b = cBest / 2.0 b = cBest / 2.0

11
PathPlanning/BezierPath/bezier_path.py
View File

@@ -94,7 +94,8 @@ def bezier_derivatives_control_points(control_points, n_derivatives):
w = {0: control_points} w = {0: control_points}
for i in range(n_derivatives): for i in range(n_derivatives):
n = len(w[i]) n = len(w[i])
w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j]) for j in range(n - 1)]) w[i + 1] = np.array([(n - 1) * (w[i][j + 1] - w[i][j])
for j in range(n - 1)])
return w return w
@@ -111,7 +112,7 @@ def curvature(dx, dy, ddx, ddy):
return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2) return (dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** (3 / 2)
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
"""Plot arrow.""" """Plot arrow."""
if not isinstance(x, float): if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw): for (ix, iy, iyaw) in zip(x, y, yaw):
@@ -155,7 +156,8 @@ def main():
tangent = np.array([point, point + dt]) tangent = np.array([point, point + dt])
normal = np.array([point, point + [- dt[1], dt[0]]]) normal = np.array([point, point + [- dt[1], dt[0]]])
curvature_center = point + np.array([- dt[1], dt[0]]) * radius curvature_center = point + np.array([- dt[1], dt[0]]) * radius
circle = plt.Circle(tuple(curvature_center), radius, color=(0, 0.8, 0.8), fill=False, linewidth=1) circle = plt.Circle(tuple(curvature_center), radius,
color=(0, 0.8, 0.8), fill=False, linewidth=1)
assert path.T[0][0] == start_x, "path is invalid" assert path.T[0][0] == start_x, "path is invalid"
assert path.T[1][0] == start_y, "path is invalid" assert path.T[1][0] == start_y, "path is invalid"
@@ -165,7 +167,8 @@ def main():
if show_animation: if show_animation:
fig, ax = plt.subplots() fig, ax = plt.subplots()
ax.plot(path.T[0], path.T[1], label="Bezier Path") ax.plot(path.T[0], path.T[1], label="Bezier Path")
ax.plot(control_points.T[0], control_points.T[1], '--o', label="Control Points") ax.plot(control_points.T[0], control_points.T[1],
'--o', label="Control Points")
ax.plot(x_target, y_target) ax.plot(x_target, y_target)
ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent") ax.plot(tangent[:, 0], tangent[:, 1], label="Tangent")
ax.plot(normal[:, 0], normal[:, 1], label="Normal") ax.plot(normal[:, 0], normal[:, 1], label="Normal")

10
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -201,10 +201,10 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c) lex, ley, leyaw, c)
px = [math.cos(-syaw) * x + math.sin(-syaw) * px = [math.cos(-syaw) * x + math.sin(-syaw)
y + sx for x, y in zip(lpx, lpy)] * y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw) * py = [- math.sin(-syaw) * x + math.cos(-syaw)
y + sy for x, y in zip(lpx, lpy)] * y + sy for x, y in zip(lpx, lpy)]
pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
return px, py, pyaw, mode, clen return px, py, pyaw, mode, clen
@@ -251,7 +251,7 @@ def generate_course(length, mode, c):
return px, py, pyaw return px, py, pyaw
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
""" """
Plot arrow Plot arrow
""" """

2
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -159,7 +159,7 @@ def dwa_control(x, u, config, goal, ob):
return u, traj return u, traj
def plot_arrow(x, y, yaw, length=0.5, width=0.1): def plot_arrow(x, y, yaw, length=0.5, width=0.1): # pragma: no cover
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
head_length=width, head_width=width) head_length=width, head_width=width)
plt.plot(x, y) plt.plot(x, y)

22
PathPlanning/InformedRRTStar/informed_rrt_star.py
View File

@@ -44,8 +44,8 @@ class InformedRRTStar():
path = None path = None
# Computing the sampling space # Computing the sampling space
cMin = math.sqrt(pow(self.start.x - self.goal.x, 2) + cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
pow(self.start.y - self.goal.y, 2)) + pow(self.start.y - self.goal.y, 2))
xCenter = np.array([[(self.start.x + self.goal.x) / 2.0], xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
[(self.start.y + self.goal.y) / 2.0], [0]]) [(self.start.y + self.goal.y) / 2.0], [0]])
a1 = np.array([[(self.goal.x - self.start.x) / cMin], a1 = np.array([[(self.goal.x - self.start.x) / cMin],
@@ -129,8 +129,8 @@ class InformedRRTStar():
def findNearNodes(self, newNode): def findNearNodes(self, newNode):
nnode = len(self.nodeList) nnode = len(self.nodeList)
r = 50.0 * math.sqrt((math.log(nnode) / nnode)) r = 50.0 * math.sqrt((math.log(nnode) / nnode))
dlist = [(node.x - newNode.x) ** 2 + dlist = [(node.x - newNode.x) ** 2
(node.y - newNode.y) ** 2 for node in self.nodeList] + (node.y - newNode.y) ** 2 for node in self.nodeList]
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
return nearinds return nearinds
@@ -175,8 +175,8 @@ class InformedRRTStar():
node1_y = path[i][1] node1_y = path[i][1]
node2_x = path[i - 1][0] node2_x = path[i - 1][0]
node2_y = path[i - 1][1] node2_y = path[i - 1][1]
pathLen += math.sqrt((node1_x - node2_x) ** pathLen += math.sqrt((node1_x - node2_x)
2 + (node1_y - node2_y)**2) ** 2 + (node1_y - node2_y)**2)
return pathLen return pathLen
@@ -184,8 +184,8 @@ class InformedRRTStar():
return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2) return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)
def getNearestListIndex(self, nodes, rnd): def getNearestListIndex(self, nodes, rnd):
dList = [(node.x - rnd[0])**2 + dList = [(node.x - rnd[0])**2
(node.y - rnd[1])**2 for node in nodes] + (node.y - rnd[1])**2 for node in nodes]
minIndex = dList.index(min(dList)) minIndex = dList.index(min(dList))
return minIndex return minIndex
@@ -220,8 +220,8 @@ class InformedRRTStar():
for i in nearInds: for i in nearInds:
nearNode = self.nodeList[i] nearNode = self.nodeList[i]
d = math.sqrt((nearNode.x - newNode.x)**2 + d = math.sqrt((nearNode.x - newNode.x)**2
(nearNode.y - newNode.y)**2) + (nearNode.y - newNode.y)**2)
scost = newNode.cost + d scost = newNode.cost + d
@@ -275,7 +275,7 @@ class InformedRRTStar():
plt.grid(True) plt.grid(True)
plt.pause(0.01) plt.pause(0.01)
def plot_ellipse(self, xCenter, cBest, cMin, etheta): def plot_ellipse(self, xCenter, cBest, cMin, etheta): # pragma: no cover
a = math.sqrt(cBest**2 - cMin**2) / 2.0 a = math.sqrt(cBest**2 - cMin**2) / 2.0
b = cBest / 2.0 b = cBest / 2.0

2
PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
View File

@@ -19,7 +19,7 @@ cost_th = 0.1
show_animation = False show_animation = False
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
""" """
Plot arrow Plot arrow
""" """

2
PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
View File

@@ -230,7 +230,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
return rx, ry return rx, ry
def plot_road_map(road_map, sample_x, sample_y): def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover
for i, _ in enumerate(road_map): for i, _ in enumerate(road_map):
for ii in range(len(road_map[i])): for ii in range(len(road_map[i])):

10
PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
View File

@@ -153,17 +153,17 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
plot_arrow(sx, sy, syaw) plot_arrow(sx, sy, syaw)
plot_arrow(gx, gy, gyaw) plot_arrow(gx, gy, gyaw)
plot_arrow(rx[i], ry[i], ryaw[i]) plot_arrow(rx[i], ry[i], ryaw[i])
plt.title("Time[s]:" + str(time[i])[0:4] plt.title("Time[s]:" + str(time[i])[0:4] +
+ " v[m/s]:" + str(rv[i])[0:4] " v[m/s]:" + str(rv[i])[0:4] +
+ " a[m/ss]:" + str(ra[i])[0:4] " a[m/ss]:" + str(ra[i])[0:4] +
+ " jerk[m/sss]:" + str(rj[i])[0:4], " jerk[m/sss]:" + str(rj[i])[0:4],
) )
plt.pause(0.001) plt.pause(0.001)
return time, rx, ry, ryaw, rv, ra, rj return time, rx, ry, ryaw, rv, ra, rj
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
""" """
Plot arrow Plot arrow
""" """

10
PathPlanning/RRTStarDubins/dubins_path_planning.py
View File

@@ -200,10 +200,10 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c) lex, ley, leyaw, c)
px = [math.cos(-syaw) * x + math.sin(-syaw) * px = [math.cos(-syaw) * x + math.sin(-syaw)
y + sx for x, y in zip(lpx, lpy)] * y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw) * py = [- math.sin(-syaw) * x + math.cos(-syaw)
y + sy for x, y in zip(lpx, lpy)] * y + sy for x, y in zip(lpx, lpy)]
pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
# print(syaw) # print(syaw)
# pyaw = lpyaw # pyaw = lpyaw
@@ -258,7 +258,7 @@ def generate_course(length, mode, c):
return px, py, pyaw return px, py, pyaw
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
""" """
Plot arrow Plot arrow
""" """

2
PathPlanning/VoronoiRoadMap/voronoi_road_map.py
View File

@@ -229,7 +229,7 @@ def dijkstra_planning(sx, sy, gx, gy, ox, oy, rr, road_map, sample_x, sample_y):
return rx, ry return rx, ry
def plot_road_map(road_map, sample_x, sample_y): def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover
for i, _ in enumerate(road_map): for i, _ in enumerate(road_map):
for ii in range(len(road_map[i])): for ii in range(len(road_map[i])):

26
PathTracking/cgmres_nmpc/cgmres_nmpc.py
View File

@@ -126,8 +126,8 @@ class NMPCSimulatorSystem():
pre_lam_3 = lam_3 + dt * \ pre_lam_3 = lam_3 + dt * \
(- lam_1 * math.sin(yaw) * v + lam_2 * math.cos(yaw) * v) (- lam_1 * math.sin(yaw) * v + lam_2 * math.cos(yaw) * v)
pre_lam_4 = lam_4 + dt * \ pre_lam_4 = lam_4 + dt * \
(lam_1 * math.cos(yaw) + lam_2 (lam_1 * math.cos(yaw) + lam_2 *
* math.sin(yaw) + lam_3 * math.sin(u_2) / WB) math.sin(yaw) + lam_3 * math.sin(u_2) / WB)
return pre_lam_1, pre_lam_2, pre_lam_3, pre_lam_4 return pre_lam_1, pre_lam_2, pre_lam_3, pre_lam_4
@@ -359,8 +359,8 @@ class NMPCController_with_CGMRES():
for i in range(N): for i in range(N):
# ∂H/∂u(xi, ui, λi) # ∂H/∂u(xi, ui, λi)
F.append(u_1s[i] + lam_4s[i] + 2.0 * raw_1s[i] * u_1s[i]) F.append(u_1s[i] + lam_4s[i] + 2.0 * raw_1s[i] * u_1s[i])
F.append(u_2s[i] + lam_3s[i] * v_s[i] F.append(u_2s[i] + lam_3s[i] * v_s[i] /
/ WB * math.cos(u_2s[i])**2 + 2.0 * raw_2s[i] * u_2s[i]) WB * math.cos(u_2s[i])**2 + 2.0 * raw_2s[i] * u_2s[i])
F.append(-PHI_V + 2.0 * raw_1s[i] * dummy_u_1s[i]) F.append(-PHI_V + 2.0 * raw_1s[i] * dummy_u_1s[i])
F.append(-PHI_OMEGA + 2.0 * raw_2s[i] * dummy_u_2s[i]) F.append(-PHI_OMEGA + 2.0 * raw_2s[i] * dummy_u_2s[i])
@@ -371,7 +371,7 @@ class NMPCController_with_CGMRES():
return np.array(F) return np.array(F)
def plot_figures(plant_system, controller, iteration_num, dt): def plot_figures(plant_system, controller, iteration_num, dt): # pragma: no cover
# figure # figure
# time history # time history
fig_p = plt.figure() fig_p = plt.figure()
@@ -421,13 +421,13 @@ def plot_figures(plant_system, controller, iteration_num, dt):
u_2_fig.set_xlabel("time [s]") u_2_fig.set_xlabel("time [s]")
u_2_fig.set_ylabel("u_omega") u_2_fig.set_ylabel("u_omega")
dummy_1_fig.plot(np.arange(iteration_num - 1) dummy_1_fig.plot(np.arange(iteration_num - 1) *
* dt, controller.history_dummy_u_1) dt, controller.history_dummy_u_1)
dummy_1_fig.set_xlabel("time [s]") dummy_1_fig.set_xlabel("time [s]")
dummy_1_fig.set_ylabel("dummy u_1") dummy_1_fig.set_ylabel("dummy u_1")
dummy_2_fig.plot(np.arange(iteration_num - 1) dummy_2_fig.plot(np.arange(iteration_num - 1) *
* dt, controller.history_dummy_u_2) dt, controller.history_dummy_u_2)
dummy_2_fig.set_xlabel("time [s]") dummy_2_fig.set_xlabel("time [s]")
dummy_2_fig.set_ylabel("dummy u_2") dummy_2_fig.set_ylabel("dummy u_2")
@@ -462,7 +462,7 @@ def plot_figures(plant_system, controller, iteration_num, dt):
plt.show() plt.show()
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: no cover
# Vehicle parameters # Vehicle parameters
LENGTH = 0.4 # [m] LENGTH = 0.4 # [m]
@@ -550,9 +550,9 @@ def animation(plant, controller, dt):
plot_car(x, y, yaw, steer=steer) plot_car(x, y, yaw, steer=steer)
plt.axis("equal") plt.axis("equal")
plt.grid(True) plt.grid(True)
plt.title("Time[s]:" + str(round(time, 2)) plt.title("Time[s]:" + str(round(time, 2)) +
+ ", accel[m/s]:" + str(round(accel, 2)) ", accel[m/s]:" + str(round(accel, 2)) +
+ ", speed[km/h]:" + str(round(v * 3.6, 2))) ", speed[km/h]:" + str(round(v * 3.6, 2)))
plt.pause(0.0001) plt.pause(0.0001)
plt.close("all") plt.close("all")

11
PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
View File

@@ -105,9 +105,6 @@ def get_linear_model_matrix(v, phi, delta):
return A, B, C return A, B, C
def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL], outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
[WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
@@ -276,8 +273,8 @@ def linear_mpc_control(xref, xbar, x0, dref):
if t < (T - 1): if t < (T - 1):
cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd) cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd)
constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) <=
<= MAX_DSTEER * DT] MAX_DSTEER * DT]
cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf) cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf)
@@ -438,8 +435,8 @@ def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state):
plot_car(state.x, state.y, state.yaw, steer=di) plot_car(state.x, state.y, state.yaw, steer=di)
plt.axis("equal") plt.axis("equal")
plt.grid(True) plt.grid(True)
plt.title("Time[s]:" + str(round(time, 2)) + plt.title("Time[s]:" + str(round(time, 2))
", speed[km/h]:" + str(round(state.v * 3.6, 2))) + ", speed[km/h]:" + str(round(state.v * 3.6, 2)))
plt.pause(0.0001) plt.pause(0.0001)
return t, x, y, yaw, v, d, a return t, x, y, yaw, v, d, a

6
PathTracking/move_to_pose/move_to_pose.py
View File

@@ -53,8 +53,8 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
# from 0 rad to 2*pi rad with slight turn # from 0 rad to 2*pi rad with slight turn
rho = np.sqrt(x_diff**2 + y_diff**2) rho = np.sqrt(x_diff**2 + y_diff**2)
alpha = (np.arctan2(y_diff, x_diff) - alpha = (np.arctan2(y_diff, x_diff)
theta + np.pi) % (2 * np.pi) - np.pi - theta + np.pi) % (2 * np.pi) - np.pi
beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
v = Kp_rho * rho v = Kp_rho * rho
@@ -76,7 +76,7 @@ def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal):
plot_vehicle(x, y, theta, x_traj, y_traj) plot_vehicle(x, y, theta, x_traj, y_traj)
def plot_vehicle(x, y, theta, x_traj, y_traj): def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover
# Corners of triangular vehicle when pointing to the right (0 radians) # Corners of triangular vehicle when pointing to the right (0 radians)
p1_i = np.array([0.5, 0, 1]).T p1_i = np.array([0.5, 0, 1]).T
p2_i = np.array([-0.5, 0.25, 1]).T p2_i = np.array([-0.5, 0.25, 1]).T