From 80ebc55c2f8c986ef58aae28c43dcf0bbbaa85d1 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 1 Mar 2020 21:05:04 +0900 Subject: [PATCH 01/12] clean up codes --- PathPlanning/BSplinePath/bspline_path.py | 25 +++++++++++------------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/PathPlanning/BSplinePath/bspline_path.py b/PathPlanning/BSplinePath/bspline_path.py index 0c453ad4..2c3d7ad6 100644 --- a/PathPlanning/BSplinePath/bspline_path.py +++ b/PathPlanning/BSplinePath/bspline_path.py @@ -1,6 +1,6 @@ """ -Path Plannting with B-Spline +Path Planning with B-Spline author: Atsushi Sakai (@Atsushi_twi) @@ -8,16 +8,13 @@ author: Atsushi Sakai (@Atsushi_twi) import numpy as np import matplotlib.pyplot as plt -import scipy.interpolate as si - -# parameter -N = 3 # B Spline order +import scipy.interpolate as scipy_interpolate -def bspline_planning(x, y, sn): +def b_spline_planning(x, y, sn, degree=3): t = range(len(x)) - x_tup = si.splrep(t, x, k=N) - y_tup = si.splrep(t, y, k=N) + x_tup = scipy_interpolate.splrep(t, x, k=degree) + y_tup = scipy_interpolate.splrep(t, y, k=degree) x_list = list(x_tup) xl = x.tolist() @@ -28,8 +25,8 @@ def bspline_planning(x, y, sn): y_list[1] = yl + [0.0, 0.0, 0.0, 0.0] ipl_t = np.linspace(0.0, len(x) - 1, sn) - rx = si.splev(ipl_t, x_list) - ry = si.splev(ipl_t, y_list) + rx = scipy_interpolate.splev(ipl_t, x_list) + ry = scipy_interpolate.splev(ipl_t, y_list) return rx, ry @@ -37,14 +34,14 @@ def bspline_planning(x, y, sn): def main(): print(__file__ + " start!!") # way points - x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) - y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) + way_x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) + way_y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) sn = 100 # sampling number - rx, ry = bspline_planning(x, y, sn) + rx, ry = b_spline_planning(way_x, way_y, sn) # show results - plt.plot(x, y, '-og', label="Waypoints") + plt.plot(way_x, way_y, '-og', label="way points") plt.plot(rx, ry, '-r', label="B-Spline path") plt.grid(True) plt.legend() From 6cb903a8143064c8eaba811b7f894bd1539ee2ae Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Mar 2020 22:27:07 +0900 Subject: [PATCH 02/12] add mypy setting and update bspline_path.py --- .github/workflows/mypycheck.yml | 3 ++ PathPlanning/BSplinePath/bspline_path.py | 57 ++++++++++++++++++------ mypy.ini | 2 + 3 files changed, 49 insertions(+), 13 deletions(-) create mode 100644 .github/workflows/mypycheck.yml create mode 100644 mypy.ini diff --git a/.github/workflows/mypycheck.yml b/.github/workflows/mypycheck.yml new file mode 100644 index 00000000..26e55660 --- /dev/null +++ b/.github/workflows/mypycheck.yml @@ -0,0 +1,3 @@ +- name: Mypy Check + uses: jpetrucciani/mypy-check@0.761 + diff --git a/PathPlanning/BSplinePath/bspline_path.py b/PathPlanning/BSplinePath/bspline_path.py index 2c3d7ad6..d4b24c76 100644 --- a/PathPlanning/BSplinePath/bspline_path.py +++ b/PathPlanning/BSplinePath/bspline_path.py @@ -1,6 +1,6 @@ """ -Path Planning with B-Spline +Path Planner with B-Spline author: Atsushi Sakai (@Atsushi_twi) @@ -11,38 +11,69 @@ import matplotlib.pyplot as plt import scipy.interpolate as scipy_interpolate -def b_spline_planning(x, y, sn, degree=3): +def approximate_b_spline_path(x: list, y: list, n_path_points: int, + degree: int = 3) -> tuple: + """ + approximate points with a B-Spline path + + :param x: x position list of approximated points + :param y: y position list of approximated points + :param n_path_points: number of path points + :param degree: (Optional) B Spline curve degree + :return: x and y position list of the result path + """ t = range(len(x)) x_tup = scipy_interpolate.splrep(t, x, k=degree) y_tup = scipy_interpolate.splrep(t, y, k=degree) x_list = list(x_tup) - xl = x.tolist() - x_list[1] = xl + [0.0, 0.0, 0.0, 0.0] + x_list[1] = x + [0.0, 0.0, 0.0, 0.0] y_list = list(y_tup) - yl = y.tolist() - y_list[1] = yl + [0.0, 0.0, 0.0, 0.0] + y_list[1] = y + [0.0, 0.0, 0.0, 0.0] - ipl_t = np.linspace(0.0, len(x) - 1, sn) + ipl_t = np.linspace(0.0, len(x) - 1, n_path_points) rx = scipy_interpolate.splev(ipl_t, x_list) ry = scipy_interpolate.splev(ipl_t, y_list) return rx, ry +def interpolate_b_spline_path(x: list, y: list, n_path_points: int, + degree: int = 3) -> tuple: + """ + interpolate points with a B-Spline path + + :param x: x positions of interpolated points + :param y: y positions of interpolated points + :param n_path_points: number of path points + :param degree: B-Spline degree + :return: x and y position list of the result path + """ + ipl_t = np.linspace(0.0, len(x) - 1, len(x)) + spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree) + spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree) + + travel = np.linspace(0.0, len(x) - 1, n_path_points) + return spl_i_x(travel), spl_i_y(travel) + + def main(): print(__file__ + " start!!") # way points - way_x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) - way_y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) - sn = 100 # sampling number + way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0] + way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0] + n_course_point = 100 # sampling number - rx, ry = b_spline_planning(way_x, way_y, sn) + rax, ray = approximate_b_spline_path(way_point_x, way_point_y, + n_course_point) + rix, riy = interpolate_b_spline_path(way_point_x, way_point_y, + n_course_point) # show results - plt.plot(way_x, way_y, '-og', label="way points") - plt.plot(rx, ry, '-r', label="B-Spline path") + plt.plot(way_point_x, way_point_y, '-og', label="way points") + plt.plot(rax, ray, '-r', label="Approximated B-Spline path") + plt.plot(rix, riy, '-b', label="Interpolated B-Spline path") plt.grid(True) plt.legend() plt.axis("equal") diff --git a/mypy.ini b/mypy.ini new file mode 100644 index 00000000..1215375e --- /dev/null +++ b/mypy.ini @@ -0,0 +1,2 @@ +[mypy] +ignore_missing_imports = True \ No newline at end of file From ba13817862c503c79da0e847d1afd7555cb8015e Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Mar 2020 22:29:03 +0900 Subject: [PATCH 03/12] fixed git hub action --- .github/workflows/mypycheck.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/mypycheck.yml b/.github/workflows/mypycheck.yml index 26e55660..64902867 100644 --- a/.github/workflows/mypycheck.yml +++ b/.github/workflows/mypycheck.yml @@ -1,3 +1,3 @@ -- name: Mypy Check - uses: jpetrucciani/mypy-check@0.761 +name: Mypy Check +uses: jpetrucciani/mypy-check@0.761 From 5f253ef56229b44cfb5e6ba5a317de5c042efad9 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Mar 2020 22:31:17 +0900 Subject: [PATCH 04/12] fixed git hub action --- .github/workflows/mypycheck.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/mypycheck.yml b/.github/workflows/mypycheck.yml index 64902867..0e26c1e2 100644 --- a/.github/workflows/mypycheck.yml +++ b/.github/workflows/mypycheck.yml @@ -1,3 +1,4 @@ name: Mypy Check +on: [push] uses: jpetrucciani/mypy-check@0.761 From aad8d4face65a0adf571b01a54b32f307876dcb2 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Mar 2020 22:34:00 +0900 Subject: [PATCH 05/12] fixed git hub action --- .github/workflows/mypycheck.yml | 4 ---- .github/workflows/pythonpackage.yml | 6 +++++- 2 files changed, 5 insertions(+), 5 deletions(-) delete mode 100644 .github/workflows/mypycheck.yml diff --git a/.github/workflows/mypycheck.yml b/.github/workflows/mypycheck.yml deleted file mode 100644 index 0e26c1e2..00000000 --- a/.github/workflows/mypycheck.yml +++ /dev/null @@ -1,4 +0,0 @@ -name: Mypy Check -on: [push] -uses: jpetrucciani/mypy-check@0.761 - diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml index a3203ff1..d03d5ea1 100644 --- a/.github/workflows/pythonpackage.yml +++ b/.github/workflows/pythonpackage.yml @@ -29,6 +29,10 @@ jobs: # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics - name: install coverage - run: pip install coverage + run: pip install coverage + - name: Mypy Check + uses: jpetrucciani/mypy-check@0.761 - name: Test run: bash runtests.sh + + From d7dddc8772b04a6895902ea6e3f72be1ebf21a86 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Mar 2020 22:51:47 +0900 Subject: [PATCH 06/12] mypy fail test --- PathPlanning/BSplinePath/bspline_path.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/BSplinePath/bspline_path.py b/PathPlanning/BSplinePath/bspline_path.py index d4b24c76..7c59e99b 100644 --- a/PathPlanning/BSplinePath/bspline_path.py +++ b/PathPlanning/BSplinePath/bspline_path.py @@ -11,7 +11,7 @@ import matplotlib.pyplot as plt import scipy.interpolate as scipy_interpolate -def approximate_b_spline_path(x: list, y: list, n_path_points: int, +def approximate_b_spline_path(x: int, y: list, n_path_points: int, degree: int = 3) -> tuple: """ approximate points with a B-Spline path From 0b9a0c88d6c5fc029bf9b0fb305e977c3b641d3c Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Mar 2020 23:00:32 +0900 Subject: [PATCH 07/12] mypy fail test --- .github/workflows/pythonpackage.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml index d03d5ea1..af761ec7 100644 --- a/.github/workflows/pythonpackage.yml +++ b/.github/workflows/pythonpackage.yml @@ -32,6 +32,8 @@ jobs: run: pip install coverage - name: Mypy Check uses: jpetrucciani/mypy-check@0.761 + with: + path: './AerialNavigation ./PathPlanning' - name: Test run: bash runtests.sh From 8dc23fec2135f8c376d7aaa7696104d3deb40a8b Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Mar 2020 23:15:13 +0900 Subject: [PATCH 08/12] mypy fail test --- .github/workflows/pythonpackage.yml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml index af761ec7..c81ce893 100644 --- a/.github/workflows/pythonpackage.yml +++ b/.github/workflows/pythonpackage.yml @@ -30,11 +30,12 @@ jobs: flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics - name: install coverage run: pip install coverage - - name: Mypy Check - uses: jpetrucciani/mypy-check@0.761 - with: - path: './AerialNavigation ./PathPlanning' - - name: Test + - name: install mypy + run: pip install mypy + - name: mypy check + run: | + - find . -name "*.py" | xargs mypy + - name: do all unit tests run: bash runtests.sh From c36aa27d605628238ff4d05e66b2f59d91fc30b3 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Mar 2020 23:20:10 +0900 Subject: [PATCH 09/12] mypy fail test --- .github/workflows/pythonpackage.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml index c81ce893..1844e538 100644 --- a/.github/workflows/pythonpackage.yml +++ b/.github/workflows/pythonpackage.yml @@ -34,7 +34,7 @@ jobs: run: pip install mypy - name: mypy check run: | - - find . -name "*.py" | xargs mypy + find . -name "*.py" | xargs mypy - name: do all unit tests run: bash runtests.sh From 14ffc4a2d4e202c49c0ae385b3ba62144a1cf86c Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 9 Mar 2020 22:57:35 +0900 Subject: [PATCH 10/12] mypy fix test --- .../{NLinkArm.py => NLinkArm3d.py} | 116 +++++---- ArmNavigation/n_joint_arm_3d/__init__py.py | 0 .../random_forward_kinematics.py | 21 +- .../random_inverse_kinematics.py | 15 +- .../n_joint_arm_to_point_control/__init__.py | 0 PathPlanning/AStar/a_star.py | 12 +- PathPlanning/BSplinePath/bspline_path.py | 2 +- PathPlanning/CubicSpline/__init__.py | 0 .../cubic_spline_planner.py | 239 ------------------ .../frenet_optimal_trajectory.py | 109 ++++---- PathPlanning/HybridAStar/__init__.py | 0 .../{a_star.py => a_star_heuristic.py} | 0 PathPlanning/HybridAStar/hybrid_a_star.py | 2 +- 13 files changed, 150 insertions(+), 366 deletions(-) rename ArmNavigation/n_joint_arm_3d/{NLinkArm.py => NLinkArm3d.py} (75%) create mode 100644 ArmNavigation/n_joint_arm_3d/__init__py.py create mode 100644 ArmNavigation/n_joint_arm_to_point_control/__init__.py create mode 100644 PathPlanning/CubicSpline/__init__.py delete mode 100644 PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py create mode 100644 PathPlanning/HybridAStar/__init__.py rename PathPlanning/HybridAStar/{a_star.py => a_star_heuristic.py} (100%) diff --git a/ArmNavigation/n_joint_arm_3d/NLinkArm.py b/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py similarity index 75% rename from ArmNavigation/n_joint_arm_3d/NLinkArm.py rename to ArmNavigation/n_joint_arm_3d/NLinkArm3d.py index da562d9f..b411cd71 100644 --- a/ArmNavigation/n_joint_arm_3d/NLinkArm.py +++ b/ArmNavigation/n_joint_arm_3d/NLinkArm3d.py @@ -7,6 +7,7 @@ import math from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as plt + class Link: def __init__(self, dh_params): self.dh_params_ = dh_params @@ -28,16 +29,22 @@ class Link: return trans - def basic_jacobian(self, trans_prev, ee_pos): - pos_prev = np.array([trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]]) - z_axis_prev = np.array([trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]]) + @staticmethod + def basic_jacobian(trans_prev, ee_pos): + pos_prev = np.array( + [trans_prev[0, 3], trans_prev[1, 3], trans_prev[2, 3]]) + z_axis_prev = np.array( + [trans_prev[0, 2], trans_prev[1, 2], trans_prev[2, 2]]) - basic_jacobian = np.hstack((np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev)) + basic_jacobian = np.hstack( + (np.cross(z_axis_prev, ee_pos - pos_prev), z_axis_prev)) return basic_jacobian - + class NLinkArm: def __init__(self, dh_params_list): + self.fig = plt.figure() + self.ax = Axes3D(self.fig) self.link_list = [] for i in range(len(dh_params_list)): self.link_list.append(Link(dh_params_list[i])) @@ -47,7 +54,7 @@ class NLinkArm: for i in range(len(self.link_list)): trans = np.dot(trans, self.link_list[i].transformation_matrix()) return trans - + def forward_kinematics(self, plot=False): trans = self.transformation_matrix() @@ -57,15 +64,12 @@ class NLinkArm: alpha, beta, gamma = self.euler_angle() if plot: - self.fig = plt.figure() - self.ax = Axes3D(self.fig) - x_list = [] y_list = [] z_list = [] - + trans = np.identity(4) - + x_list.append(trans[0, 3]) y_list.append(trans[1, 3]) z_list.append(trans[2, 3]) @@ -74,14 +78,15 @@ class NLinkArm: x_list.append(trans[0, 3]) y_list.append(trans[1, 3]) z_list.append(trans[2, 3]) - - self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5) + + self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, + mew=0.5) self.ax.plot([0], [0], [0], "o") - + self.ax.set_xlim(-1, 1) self.ax.set_ylim(-1, 1) self.ax.set_zlim(-1, 1) - + plt.show() return [x, y, z, alpha, beta, gamma] @@ -89,41 +94,45 @@ class NLinkArm: def basic_jacobian(self): ee_pos = self.forward_kinematics()[0:3] basic_jacobian_mat = [] - - trans = np.identity(4) + + trans = np.identity(4) for i in range(len(self.link_list)): - basic_jacobian_mat.append(self.link_list[i].basic_jacobian(trans, ee_pos)) + basic_jacobian_mat.append( + self.link_list[i].basic_jacobian(trans, ee_pos)) trans = np.dot(trans, self.link_list[i].transformation_matrix()) - + return np.array(basic_jacobian_mat).T def inverse_kinematics(self, ref_ee_pose, plot=False): for cnt in range(500): ee_pose = self.forward_kinematics() diff_pose = np.array(ref_ee_pose) - ee_pose - + basic_jacobian_mat = self.basic_jacobian() alpha, beta, gamma = self.euler_angle() - - K_zyz = np.array([[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)], - [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)], - [1, 0, math.cos(beta)]]) + + K_zyz = np.array( + [[0, -math.sin(alpha), math.cos(alpha) * math.sin(beta)], + [0, math.cos(alpha), math.sin(alpha) * math.sin(beta)], + [1, 0, math.cos(beta)]]) K_alpha = np.identity(6) K_alpha[3:, 3:] = K_zyz - theta_dot = np.dot(np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), np.array(diff_pose)) + theta_dot = np.dot( + np.dot(np.linalg.pinv(basic_jacobian_mat), K_alpha), + np.array(diff_pose)) self.update_joint_angles(theta_dot / 100.) - + if plot: self.fig = plt.figure() self.ax = Axes3D(self.fig) - + x_list = [] y_list = [] z_list = [] - + trans = np.identity(4) - + x_list.append(trans[0, 3]) y_list.append(trans[1, 3]) z_list.append(trans[2, 3]) @@ -132,30 +141,36 @@ class NLinkArm: x_list.append(trans[0, 3]) y_list.append(trans[1, 3]) z_list.append(trans[2, 3]) - - self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5) + + self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, + mew=0.5) self.ax.plot([0], [0], [0], "o") - + self.ax.set_xlim(-1, 1) self.ax.set_ylim(-1, 1) self.ax.set_zlim(-1, 1) - - self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], "o") + + self.ax.plot([ref_ee_pose[0]], [ref_ee_pose[1]], [ref_ee_pose[2]], + "o") plt.show() - + def euler_angle(self): trans = self.transformation_matrix() - + alpha = math.atan2(trans[1][2], trans[0][2]) - if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2): + if not (-math.pi / 2 <= alpha <= math.pi / 2): alpha = math.atan2(trans[1][2], trans[0][2]) + math.pi - if not (-math.pi / 2 <= alpha and alpha <= math.pi / 2): + if not (-math.pi / 2 <= alpha <= math.pi / 2): alpha = math.atan2(trans[1][2], trans[0][2]) - math.pi - beta = math.atan2(trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), trans[2][2]) - gamma = math.atan2(-trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha), -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha)) - + beta = math.atan2( + trans[0][2] * math.cos(alpha) + trans[1][2] * math.sin(alpha), + trans[2][2]) + gamma = math.atan2( + -trans[0][0] * math.sin(alpha) + trans[1][0] * math.cos(alpha), + -trans[0][1] * math.sin(alpha) + trans[1][1] * math.cos(alpha)) + return alpha, beta, gamma - + def set_joint_angles(self, joint_angle_list): for i in range(len(self.link_list)): self.link_list[i].dh_params_[0] = joint_angle_list[i] @@ -163,17 +178,17 @@ class NLinkArm: def update_joint_angles(self, diff_joint_angle_list): for i in range(len(self.link_list)): self.link_list[i].dh_params_[0] += diff_joint_angle_list[i] - + def plot(self): self.fig = plt.figure() self.ax = Axes3D(self.fig) - + x_list = [] y_list = [] z_list = [] trans = np.identity(4) - + x_list.append(trans[0, 3]) y_list.append(trans[1, 3]) z_list.append(trans[2, 3]) @@ -182,15 +197,16 @@ class NLinkArm: x_list.append(trans[0, 3]) y_list.append(trans[1, 3]) z_list.append(trans[2, 3]) - - self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, mew=0.5) + + self.ax.plot(x_list, y_list, z_list, "o-", color="#00aa00", ms=4, + mew=0.5) self.ax.plot([0], [0], [0], "o") self.ax.set_xlabel("x") self.ax.set_ylabel("y") - self.ax.set_zlabel("z") - + self.ax.set_zlabel("z") + self.ax.set_xlim(-1, 1) self.ax.set_ylim(-1, 1) - self.ax.set_zlim(-1, 1) + self.ax.set_zlim(-1, 1) plt.show() diff --git a/ArmNavigation/n_joint_arm_3d/__init__py.py b/ArmNavigation/n_joint_arm_3d/__init__py.py new file mode 100644 index 00000000..e69de29b diff --git a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py index 87907f80..b3426e53 100644 --- a/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py +++ b/ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py @@ -3,9 +3,10 @@ Forward Kinematics for an n-link arm in 3D Author: Takayuki Murooka (takayuki5168) """ import math -from NLinkArm import NLinkArm +from NLinkArm3d import NLinkArm import random + def random_val(min_val, max_val): return min_val + random.random() * (max_val - min_val) @@ -14,17 +15,17 @@ if __name__ == "__main__": print("Start solving Forward Kinematics 10 times") # init NLinkArm with Denavit-Hartenberg parameters of PR2 - n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.], - [math.pi/2, math.pi/2, 0., 0.], - [0., -math.pi/2, 0., .4], - [0., math.pi/2, 0., 0.], - [0., -math.pi/2, 0., .321], - [0., math.pi/2, 0., 0.], + n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.], + [math.pi / 2, math.pi / 2, 0., 0.], + [0., -math.pi / 2, 0., .4], + [0., math.pi / 2, 0., 0.], + [0., -math.pi / 2, 0., .321], + [0., math.pi / 2, 0., 0.], [0., 0., 0., 0.]]) # execute FK 10 times for i in range(10): - n_link_arm.set_joint_angles([random_val(-1, 1) for j in range(len(n_link_arm.link_list))]) - - ee_pose = n_link_arm.forward_kinematics(plot=True) + n_link_arm.set_joint_angles( + [random_val(-1, 1) for j in range(len(n_link_arm.link_list))]) + ee_pose = n_link_arm.forward_kinematics(plot=True) diff --git a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py index 44be2270..a3838a6a 100644 --- a/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py +++ b/ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py @@ -3,23 +3,24 @@ Inverse Kinematics for an n-link arm in 3D Author: Takayuki Murooka (takayuki5168) """ import math -from NLinkArm import NLinkArm +from NLinkArm3d import NLinkArm import random def random_val(min_val, max_val): return min_val + random.random() * (max_val - min_val) + if __name__ == "__main__": print("Start solving Inverse Kinematics 10 times") # init NLinkArm with Denavit-Hartenberg parameters of PR2 - n_link_arm = NLinkArm([[0., -math.pi/2, .1, 0.], - [math.pi/2, math.pi/2, 0., 0.], - [0., -math.pi/2, 0., .4], - [0., math.pi/2, 0., 0.], - [0., -math.pi/2, 0., .321], - [0., math.pi/2, 0., 0.], + n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.], + [math.pi / 2, math.pi / 2, 0., 0.], + [0., -math.pi / 2, 0., .4], + [0., math.pi / 2, 0., 0.], + [0., -math.pi / 2, 0., .321], + [0., math.pi / 2, 0., 0.], [0., 0., 0., 0.]]) # execute IK 10 times diff --git a/ArmNavigation/n_joint_arm_to_point_control/__init__.py b/ArmNavigation/n_joint_arm_to_point_control/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 2229d633..55881f4e 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -41,7 +41,8 @@ class AStarPlanner: self.pind = pind def __str__(self): - return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind) + return str(self.x) + "," + str(self.y) + "," + str( + self.cost) + "," + str(self.pind) def planning(self, sx, sy, gx, gy): """ @@ -72,7 +73,10 @@ class AStarPlanner: break c_id = min( - open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, open_set[o])) + open_set, + key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal, + open_set[ + o])) current = open_set[c_id] # show graph @@ -81,7 +85,8 @@ class AStarPlanner: self.calc_grid_position(current.y, self.miny), "xc") # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + lambda event: [exit( + 0) if event.key == 'escape' else None]) if len(closed_set.keys()) % 10 == 0: plt.pause(0.001) @@ -104,7 +109,6 @@ class AStarPlanner: current.cost + self.motion[i][2], c_id) n_id = self.calc_grid_index(node) - # If the node is not safe, do nothing if not self.verify_node(node): continue diff --git a/PathPlanning/BSplinePath/bspline_path.py b/PathPlanning/BSplinePath/bspline_path.py index 7c59e99b..d4b24c76 100644 --- a/PathPlanning/BSplinePath/bspline_path.py +++ b/PathPlanning/BSplinePath/bspline_path.py @@ -11,7 +11,7 @@ import matplotlib.pyplot as plt import scipy.interpolate as scipy_interpolate -def approximate_b_spline_path(x: int, y: list, n_path_points: int, +def approximate_b_spline_path(x: list, y: list, n_path_points: int, degree: int = 3) -> tuple: """ approximate points with a B-Spline path diff --git a/PathPlanning/CubicSpline/__init__.py b/PathPlanning/CubicSpline/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py b/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py deleted file mode 100644 index 98ef8c4e..00000000 --- a/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py +++ /dev/null @@ -1,239 +0,0 @@ -""" -cubic spline planner - -Author: Atsushi Sakai - -""" -import math -import numpy as np -import bisect - - -class Spline: - """ - Cubic Spline class - """ - - def __init__(self, x, y): - self.b, self.c, self.d, self.w = [], [], [], [] - - self.x = x - self.y = y - - self.nx = len(x) # dimension of x - h = np.diff(x) - - # calc coefficient c - self.a = [iy for iy in y] - - # calc coefficient c - A = self.__calc_A(h) - B = self.__calc_B(h) - self.c = np.linalg.solve(A, B) - # print(self.c1) - - # calc spline coefficient b and d - for i in range(self.nx - 1): - self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) - tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ - (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 - self.b.append(tb) - - def calc(self, t): - """ - Calc position - - if t is outside of the input x, return None - - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = self.a[i] + self.b[i] * dx + \ - self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 - - return result - - def calcd(self, t): - """ - Calc first derivative - - if t is outside of the input x, return None - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 - return result - - def calcdd(self, t): - """ - Calc second derivative - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx - return result - - def __search_index(self, x): - """ - search data segment index - """ - return bisect.bisect(self.x, x) - 1 - - def __calc_A(self, h): - """ - calc matrix A for spline coefficient c - """ - A = np.zeros((self.nx, self.nx)) - A[0, 0] = 1.0 - for i in range(self.nx - 1): - if i != (self.nx - 2): - A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) - A[i + 1, i] = h[i] - A[i, i + 1] = h[i] - - A[0, 1] = 0.0 - A[self.nx - 1, self.nx - 2] = 0.0 - A[self.nx - 1, self.nx - 1] = 1.0 - # print(A) - return A - - def __calc_B(self, h): - """ - calc matrix B for spline coefficient c - """ - B = np.zeros(self.nx) - for i in range(self.nx - 2): - B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ - h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] - # print(B) - return B - - -class Spline2D: - """ - 2D Cubic Spline class - - """ - - def __init__(self, x, y): - self.s = self.__calc_s(x, y) - self.sx = Spline(self.s, x) - self.sy = Spline(self.s, y) - - def __calc_s(self, x, y): - dx = np.diff(x) - dy = np.diff(y) - self.ds = [math.sqrt(idx ** 2 + idy ** 2) - for (idx, idy) in zip(dx, dy)] - s = [0] - s.extend(np.cumsum(self.ds)) - return s - - def calc_position(self, s): - """ - calc position - """ - x = self.sx.calc(s) - y = self.sy.calc(s) - - return x, y - - def calc_curvature(self, s): - """ - calc curvature - """ - dx = self.sx.calcd(s) - ddx = self.sx.calcdd(s) - dy = self.sy.calcd(s) - ddy = self.sy.calcdd(s) - k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) - return k - - def calc_yaw(self, s): - """ - calc yaw - """ - dx = self.sx.calcd(s) - dy = self.sy.calcd(s) - yaw = math.atan2(dy, dx) - return yaw - - -def calc_spline_course(x, y, ds=0.1): - sp = Spline2D(x, y) - s = list(np.arange(0, sp.s[-1], ds)) - - rx, ry, ryaw, rk = [], [], [], [] - for i_s in s: - ix, iy = sp.calc_position(i_s) - rx.append(ix) - ry.append(iy) - ryaw.append(sp.calc_yaw(i_s)) - rk.append(sp.calc_curvature(i_s)) - - return rx, ry, ryaw, rk, s - - -def main(): # pragma: no cover - print("Spline 2D test") - import matplotlib.pyplot as plt - x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] - y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] - - sp = Spline2D(x, y) - s = np.arange(0, sp.s[-1], 0.1) - - rx, ry, ryaw, rk = [], [], [], [] - for i_s in s: - ix, iy = sp.calc_position(i_s) - rx.append(ix) - ry.append(iy) - ryaw.append(sp.calc_yaw(i_s)) - rk.append(sp.calc_curvature(i_s)) - - plt.subplots(1) - plt.plot(x, y, "xb", label="input") - plt.plot(rx, ry, "-r", label="spline") - plt.grid(True) - plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") - plt.legend() - - plt.subplots(1) - plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("yaw angle[deg]") - - plt.subplots(1) - plt.plot(s, rk, "-r", label="curvature") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("curvature [1/m]") - - plt.show() - - -if __name__ == '__main__': # pragma: no cover - main() diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index b113bcbe..ad3da208 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -6,9 +6,11 @@ author: Atsushi Sakai (@Atsushi_twi) Ref: -- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame](https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf) +- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame] +(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf) -- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame](https://www.youtube.com/watch?v=Cj6tAQe7UCY) +- [Optimal trajectory generation for dynamic street scenarios in a Frenet Frame] +(https://www.youtube.com/watch?v=Cj6tAQe7UCY) """ @@ -16,19 +18,20 @@ import numpy as np import matplotlib.pyplot as plt import copy import math -import cubic_spline_planner import sys import os sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../QuinticPolynomialsPlanner/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../CubicSpline/") try: from quintic_polynomials_planner import QuinticPolynomial + import cubic_spline_planner except ImportError: raise - SIM_LOOP = 500 # Parameter @@ -38,36 +41,35 @@ MAX_CURVATURE = 1.0 # maximum curvature [1/m] MAX_ROAD_WIDTH = 7.0 # maximum road width [m] D_ROAD_W = 1.0 # road width sampling length [m] DT = 0.2 # time tick [s] -MAXT = 5.0 # max prediction time [m] -MINT = 4.0 # min prediction time [m] +MAX_T = 5.0 # max prediction time [m] +MIN_T = 4.0 # min prediction time [m] TARGET_SPEED = 30.0 / 3.6 # target speed [m/s] D_T_S = 5.0 / 3.6 # target speed sampling length [m/s] N_S_SAMPLE = 1 # sampling number of target speed ROBOT_RADIUS = 2.0 # robot radius [m] # cost weights -KJ = 0.1 -KT = 0.1 -KD = 1.0 -KLAT = 1.0 -KLON = 1.0 +K_J = 0.1 +K_T = 0.1 +K_D = 1.0 +K_LAT = 1.0 +K_LON = 1.0 show_animation = True -class quartic_polynomial: - - def __init__(self, xs, vxs, axs, vxe, axe, T): +class QuarticPolynomial: + def __init__(self, xs, vxs, axs, vxe, axe, time): # calc coefficient of quartic polynomial self.a0 = xs self.a1 = vxs self.a2 = axs / 2.0 - A = np.array([[3 * T ** 2, 4 * T ** 3], - [6 * T, 12 * T ** 2]]) - b = np.array([vxe - self.a1 - 2 * self.a2 * T, + A = np.array([[3 * time ** 2, 4 * time ** 3], + [6 * time, 12 * time ** 2]]) + b = np.array([vxe - self.a1 - 2 * self.a2 * time, axe - 2 * self.a2]) x = np.linalg.solve(A, b) @@ -75,19 +77,19 @@ class quartic_polynomial: self.a4 = x[1] def calc_point(self, t): - xt = self.a0 + self.a1 * t + self.a2 * t**2 + \ - self.a3 * t**3 + self.a4 * t**4 + xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ + self.a3 * t ** 3 + self.a4 * t ** 4 return xt def calc_first_derivative(self, t): xt = self.a1 + 2 * self.a2 * t + \ - 3 * self.a3 * t**2 + 4 * self.a4 * t**3 + 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 return xt def calc_second_derivative(self, t): - xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 + xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 return xt @@ -97,7 +99,7 @@ class quartic_polynomial: return xt -class Frenet_path: +class FrenetPath: def __init__(self): self.t = [] @@ -121,15 +123,14 @@ class Frenet_path: def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0): - frenet_paths = [] # generate path to each offset goal for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W): # Lateral motion planning - for Ti in np.arange(MINT, MAXT, DT): - fp = Frenet_path() + for Ti in np.arange(MIN_T, MAX_T, DT): + fp = FrenetPath() # lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti) lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti) @@ -141,9 +142,10 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0): fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t] # Longitudinal motion planning (Velocity keeping) - for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S): + for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, + TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S): tfp = copy.deepcopy(fp) - lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti) + lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti) tfp.s = [lon_qp.calc_point(t) for t in fp.t] tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t] @@ -154,11 +156,11 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0): Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk # square of diff from target speed - ds = (TARGET_SPEED - tfp.s_d[-1])**2 + ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2 - tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1]**2 - tfp.cv = KJ * Js + KT * Ti + KD * ds - tfp.cf = KLAT * tfp.cd + KLON * tfp.cv + tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2 + tfp.cv = K_J * Js + K_T * Ti + K_D * ds + tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv frenet_paths.append(tfp) @@ -166,7 +168,6 @@ def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0): def calc_global_paths(fplist, csp): - for fp in fplist: # calc global positions @@ -174,10 +175,10 @@ def calc_global_paths(fplist, csp): ix, iy = csp.calc_position(fp.s[i]) if ix is None: break - iyaw = csp.calc_yaw(fp.s[i]) + i_yaw = csp.calc_yaw(fp.s[i]) di = fp.d[i] - fx = ix + di * math.cos(iyaw + math.pi / 2.0) - fy = iy + di * math.sin(iyaw + math.pi / 2.0) + fx = ix + di * math.cos(i_yaw + math.pi / 2.0) + fy = iy + di * math.sin(i_yaw + math.pi / 2.0) fp.x.append(fx) fp.y.append(fy) @@ -199,12 +200,11 @@ def calc_global_paths(fplist, csp): def check_collision(fp, ob): - for i in range(len(ob[:, 0])): - d = [((ix - ob[i, 0])**2 + (iy - ob[i, 1])**2) + d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2) for (ix, iy) in zip(fp.x, fp.y)] - collision = any([di <= ROBOT_RADIUS**2 for di in d]) + collision = any([di <= ROBOT_RADIUS ** 2 for di in d]) if collision: return False @@ -213,38 +213,38 @@ def check_collision(fp, ob): def check_paths(fplist, ob): - - okind = [] + ok_ind = [] for i, _ in enumerate(fplist): if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check continue - elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]): # Max accel check + elif any([abs(a) > MAX_ACCEL for a in + fplist[i].s_dd]): # Max accel check continue - elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check + elif any([abs(c) > MAX_CURVATURE for c in + fplist[i].c]): # Max curvature check continue elif not check_collision(fplist[i], ob): continue - okind.append(i) + ok_ind.append(i) - return [fplist[i] for i in okind] + return [fplist[i] for i in ok_ind] def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob): - fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0) fplist = calc_global_paths(fplist, csp) fplist = check_paths(fplist, ob) # find minimum cost path - mincost = float("inf") - bestpath = None + min_cost = float("inf") + best_path = None for fp in fplist: - if mincost >= fp.cf: - mincost = fp.cf - bestpath = fp + if min_cost >= fp.cf: + min_cost = fp.cf + best_path = fp - return bestpath + return best_path def generate_target_course(x, y): @@ -282,7 +282,7 @@ def main(): c_speed = 10.0 / 3.6 # current speed [m/s] c_d = 2.0 # current lateral position [m] c_d_d = 0.0 # current lateral speed [m/s] - c_d_dd = 0.0 # current latral acceleration [m/s] + c_d_dd = 0.0 # current lateral acceleration [m/s] s0 = 0.0 # current course position area = 20.0 # animation area length [m] @@ -304,8 +304,9 @@ def main(): if show_animation: # pragma: no cover plt.cla() # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.plot(tx, ty) plt.plot(ob[:, 0], ob[:, 1], "xk") plt.plot(path.x[1:], path.y[1:], "-or") diff --git a/PathPlanning/HybridAStar/__init__.py b/PathPlanning/HybridAStar/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/PathPlanning/HybridAStar/a_star.py b/PathPlanning/HybridAStar/a_star_heuristic.py similarity index 100% rename from PathPlanning/HybridAStar/a_star.py rename to PathPlanning/HybridAStar/a_star_heuristic.py diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 725976dc..41e8b393 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -14,7 +14,7 @@ import matplotlib.pyplot as plt import sys sys.path.append("../ReedsSheppPath/") try: - from a_star import dp_planning # , calc_obstacle_map + from a_star_heuristic import dp_planning # , calc_obstacle_map import reeds_shepp_path_planning as rs from car import move, check_car_collision, MAX_STEER, WB, plot_car except: From ae2593b44425f805da85e245d925554cd8336700 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 9 Mar 2020 23:04:47 +0900 Subject: [PATCH 11/12] mypy fix test --- .github/workflows/pythonpackage.yml | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml index 1844e538..b3f1aa36 100644 --- a/.github/workflows/pythonpackage.yml +++ b/.github/workflows/pythonpackage.yml @@ -9,7 +9,7 @@ jobs: strategy: max-parallel: 4 matrix: - python-version: [3.6, 3.7] + python-version: [3.7, 3.8] steps: - uses: actions/checkout@v1 @@ -34,7 +34,15 @@ jobs: run: pip install mypy - name: mypy check run: | - find . -name "*.py" | xargs mypy + find AerialNavigation -name "*.py" | xargs mypy + find ArmNavigation -name "*.py" | xargs mypy + find Bipedal -name "*.py" | xargs mypy + find InvertedPendulumCart -name "*.py" | xargs mypy + find Localization -name "*.py" | xargs mypy + find Mapping -name "*.py" | xargs mypy + find PathPlanning -name "*.py" | xargs mypy + find PathTracking -name "*.py" | xargs mypy + find SLAM -name "*.py" | xargs mypy - name: do all unit tests run: bash runtests.sh From 4107c424067fbbac8a855066e5cae03c9f8c4334 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 9 Mar 2020 23:06:48 +0900 Subject: [PATCH 12/12] mypy fix test --- .github/workflows/pythonpackage.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pythonpackage.yml b/.github/workflows/pythonpackage.yml index b3f1aa36..574c7be9 100644 --- a/.github/workflows/pythonpackage.yml +++ b/.github/workflows/pythonpackage.yml @@ -9,7 +9,7 @@ jobs: strategy: max-parallel: 4 matrix: - python-version: [3.7, 3.8] + python-version: [3.7] steps: - uses: actions/checkout@v1