From 23c73567d6f20f539e860d03d0ce2e7bbe74cad5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Sat, 12 Oct 2019 15:08:20 +0300 Subject: [PATCH 1/7] Optimize the obstacle cost calculation. The calculation was too slow to work with more obstacles. The calculation function is vectorized to increase the speed. --- .../dynamic_window_approach.py | 24 +++++++------------ 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index a7d74332a8..7b5eba1743 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -139,22 +139,14 @@ def calc_obstacle_cost(traj, ob, config): calc obstacle cost inf: collision """ - skip_n = 2 # for speed up - minr = float("inf") - - for ii in range(0, len(traj[:, 1]), skip_n): - for i in range(len(ob[:, 0])): - ox = ob[i, 0] - oy = ob[i, 1] - dx = traj[ii, 0] - ox - dy = traj[ii, 1] - oy - - r = math.sqrt(dx**2 + dy**2) - if r <= config.robot_radius: - return float("Inf") # collision - - if minr >= r: - minr = r + ox = ob[:, 0] + oy = ob[:, 1] + dx = traj[:, 0] - ox[:, None] + dy = traj[:, 1] - oy[:, None] + r = np.sqrt(np.square(dx) + np.square(dy)) + if (r <= config.robot_radius).any(): + return float("Inf") + minr = np.min(r) return 1.0 / minr # OK From 4cedb7aad4d2b4f6431536a8c749f1980cc8f84e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Tue, 15 Oct 2019 21:55:56 +0300 Subject: [PATCH 2/7] Add rectangle robot type and collision check Add collision check for rectangle robots Add new function to draw robot depending on the type --- .../dynamic_window_approach.py | 56 ++++++++++++++++++- 1 file changed, 53 insertions(+), 3 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 7b0882ab0c..1fad0e91cf 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -45,8 +45,16 @@ def __init__(self): self.to_goal_cost_gain = 0.15 self.speed_cost_gain = 1.0 self.obstacle_cost_gain = 1.0 + self.robot_type = 'circle' # circle or rectangle + + # if robot_type == circle + # Also used to check if goal is reached in both types self.robot_radius = 1.0 # [m] for collision check + # if robot_type == rectangle + self.robot_width = 0.5 # [m] for collision check + self.robot_length = 1.2 # [m] for collision check + def motion(x, u, dt): """ @@ -141,8 +149,25 @@ def calc_obstacle_cost(trajectory, ob, config): dx = trajectory[:, 0] - ox[:, None] dy = trajectory[:, 1] - oy[:, None] r = np.sqrt(np.square(dx) + np.square(dy)) - if (r <= config.robot_radius).any(): - return float("Inf") + + if config.robot_type == 'rectangle': + yaw = trajectory[:, 2] + rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) + local_ob = ob[:, None] - trajectory[:, 0:2] + local_ob = local_ob.reshape(-1, local_ob.shape[-1]) + local_ob = np.array([local_ob @ -x for x in rot.T]) + local_ob = local_ob.reshape(-1, local_ob.shape[-1]) + upper_check = local_ob[:, 0] <= config.robot_length/2 + right_check = local_ob[:, 1] <= config.robot_width/2 + bottom_check = local_ob[:, 0] >= -config.robot_length/2 + left_check = local_ob[:, 1] >= -config.robot_width/2 + if (np.logical_and(np.logical_and(upper_check, right_check), + np.logical_and(bottom_check, left_check))).any(): + return float("Inf") + elif config.robot_type == 'circle': + if (r <= config.robot_radius).any(): + return float("Inf") + min_r = np.min(r) return 1.0 / min_r # OK @@ -166,7 +191,30 @@ def plot_arrow(x, y, yaw, length=0.5, width=0.1): # pragma: no cover plt.plot(x, y) -def main(gx=10.0, gy=10.0): +def plot_robot(x, y, yaw, config): # pragma: no cover + if config.robot_type == 'rectangle': + outline = np.array([[-config.robot_length/2, config.robot_length/2, + (config.robot_length/2), -config.robot_length/2, + -config.robot_length/2], + [config.robot_width / 2, config.robot_width / 2, + - config.robot_width / 2, -config.robot_width / 2, + config.robot_width / 2]]) + Rot1 = np.array([[math.cos(yaw), math.sin(yaw)], + [-math.sin(yaw), math.cos(yaw)]]) + outline = (outline.T.dot(Rot1)).T + outline[0, :] += x + outline[1, :] += y + plt.plot(np.array(outline[0, :]).flatten(), + np.array(outline[1, :]).flatten(), "-k") + elif config.robot_type == 'circle': + circle = plt.Circle((x, y), config.robot_radius, color="b") + plt.gcf().gca().add_artist(circle) + out_x, out_y = (np.array([x, y]) + + np.array([np.cos(yaw), np.sin(yaw)]) * config.robot_radius) + plt.plot([x, out_x], [y, out_y], "-k") + + +def main(gx=10.0, gy=10.0, robot_type='circle'): print(__file__ + " start!!") # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0]) @@ -193,6 +241,7 @@ def main(gx=10.0, gy=10.0): # input [forward speed, yawrate] u = np.array([0.0, 0.0]) config = Config() + config.robot_type = robot_type trajectory = np.array(x) while True: @@ -206,6 +255,7 @@ def main(gx=10.0, gy=10.0): plt.plot(x[0], x[1], "xr") plt.plot(goal[0], goal[1], "xb") plt.plot(ob[:, 0], ob[:, 1], "ok") + plot_robot(x[0], x[1], x[2], config) plot_arrow(x[0], x[1], x[2]) plt.axis("equal") plt.grid(True) From d98604158c56f67d47ad2f9481e6498ee469b415 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Tue, 15 Oct 2019 22:03:28 +0300 Subject: [PATCH 3/7] add test for the rectangle robot type --- tests/test_dynamic_window_approach.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tests/test_dynamic_window_approach.py b/tests/test_dynamic_window_approach.py index ab2e0a27ac..de371f4a2b 100644 --- a/tests/test_dynamic_window_approach.py +++ b/tests/test_dynamic_window_approach.py @@ -17,7 +17,12 @@ def test1(self): m.show_animation = False m.main(gx=1.0, gy=1.0) + def test2(self): + m.show_animation = False + m.main(gx=1.0, gy=1.0, robot_type='rectangle') + if __name__ == '__main__': # pragma: no cover test = Test() test.test1() + test.test2() From bcdf088684b1b47ef9867c39263f624f677fe869 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Wed, 16 Oct 2019 19:19:11 +0300 Subject: [PATCH 4/7] Change robot_type comparison to enum class named RobotType - Instead of using string based comparison for robot_type, use Enum class. - Append authors --- .../dynamic_window_approach.py | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 1fad0e91cf..0313b0f1ef 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -2,10 +2,11 @@ Mobile robot motion planning sample with Dynamic Window Approach -author: Atsushi Sakai (@Atsushi_twi) +author: Atsushi Sakai (@Atsushi_twi), Goktug Karakasli """ +from enum import Enum import math import matplotlib.pyplot as plt @@ -25,6 +26,9 @@ def dwa_control(x, config, goal, ob): return u, trajectory +class RobotType(Enum): + circle = 0 + rectangle = 1 class Config: """ @@ -45,13 +49,13 @@ def __init__(self): self.to_goal_cost_gain = 0.15 self.speed_cost_gain = 1.0 self.obstacle_cost_gain = 1.0 - self.robot_type = 'circle' # circle or rectangle + self.robot_type = RobotType.circle - # if robot_type == circle + # if robot_type == RobotType.circle # Also used to check if goal is reached in both types self.robot_radius = 1.0 # [m] for collision check - # if robot_type == rectangle + # if robot_type == RobotType.rectangle self.robot_width = 0.5 # [m] for collision check self.robot_length = 1.2 # [m] for collision check @@ -150,7 +154,7 @@ def calc_obstacle_cost(trajectory, ob, config): dy = trajectory[:, 1] - oy[:, None] r = np.sqrt(np.square(dx) + np.square(dy)) - if config.robot_type == 'rectangle': + if config.robot_type == RobotType.rectangle: yaw = trajectory[:, 2] rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) local_ob = ob[:, None] - trajectory[:, 0:2] @@ -164,7 +168,7 @@ def calc_obstacle_cost(trajectory, ob, config): if (np.logical_and(np.logical_and(upper_check, right_check), np.logical_and(bottom_check, left_check))).any(): return float("Inf") - elif config.robot_type == 'circle': + elif config.robot_type == RobotType.circle: if (r <= config.robot_radius).any(): return float("Inf") @@ -192,7 +196,7 @@ def plot_arrow(x, y, yaw, length=0.5, width=0.1): # pragma: no cover def plot_robot(x, y, yaw, config): # pragma: no cover - if config.robot_type == 'rectangle': + if config.robot_type == RobotType.rectangle: outline = np.array([[-config.robot_length/2, config.robot_length/2, (config.robot_length/2), -config.robot_length/2, -config.robot_length/2], @@ -206,7 +210,7 @@ def plot_robot(x, y, yaw, config): # pragma: no cover outline[1, :] += y plt.plot(np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), "-k") - elif config.robot_type == 'circle': + elif config.robot_type == RobotType.circle: circle = plt.Circle((x, y), config.robot_radius, color="b") plt.gcf().gca().add_artist(circle) out_x, out_y = (np.array([x, y]) + @@ -214,7 +218,7 @@ def plot_robot(x, y, yaw, config): # pragma: no cover plt.plot([x, out_x], [y, out_y], "-k") -def main(gx=10.0, gy=10.0, robot_type='circle'): +def main(gx=10.0, gy=10.0, robot_type=RobotType.circle): print(__file__ + " start!!") # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0]) From 457b4086ef4e900f53a104a6baf36246ef57889e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Wed, 16 Oct 2019 19:24:37 +0300 Subject: [PATCH 5/7] Change string to RobotType enum --- tests/test_dynamic_window_approach.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_dynamic_window_approach.py b/tests/test_dynamic_window_approach.py index de371f4a2b..890f000475 100644 --- a/tests/test_dynamic_window_approach.py +++ b/tests/test_dynamic_window_approach.py @@ -19,7 +19,7 @@ def test1(self): def test2(self): m.show_animation = False - m.main(gx=1.0, gy=1.0, robot_type='rectangle') + m.main(gx=1.0, gy=1.0, robot_type=m.RobotType.rectangle) if __name__ == '__main__': # pragma: no cover From 0ec56812be8e6811cc0a3b2486528b1fb2c64685 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Wed, 16 Oct 2019 20:17:46 +0300 Subject: [PATCH 6/7] Add type checking for robot_type --- .../DynamicWindowApproach/dynamic_window_approach.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 0313b0f1ef..267ca1676d 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -59,6 +59,15 @@ def __init__(self): self.robot_width = 0.5 # [m] for collision check self.robot_length = 1.2 # [m] for collision check + @property + def robot_type(self): + return self._robot_type + + @robot_type.setter + def robot_type(self, value): + if not isinstance(value, RobotType): + raise TypeError("robot_type must be an instance of RobotType") + self._robot_type = value def motion(x, u, dt): """ From adb4e99ceb5628f103c486b9eb84ef35d1c4b3d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Wed, 16 Oct 2019 21:03:18 +0300 Subject: [PATCH 7/7] Fix transpose error of the rotation matrix --- PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 267ca1676d..9216892aee 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -166,9 +166,10 @@ def calc_obstacle_cost(trajectory, ob, config): if config.robot_type == RobotType.rectangle: yaw = trajectory[:, 2] rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) + rot = np.transpose(rot, [2, 0, 1]) local_ob = ob[:, None] - trajectory[:, 0:2] local_ob = local_ob.reshape(-1, local_ob.shape[-1]) - local_ob = np.array([local_ob @ -x for x in rot.T]) + local_ob = np.array([local_ob @ -x for x in rot]) local_ob = local_ob.reshape(-1, local_ob.shape[-1]) upper_check = local_ob[:, 0] <= config.robot_length/2 right_check = local_ob[:, 1] <= config.robot_width/2