Add files via upload

Author: ss48

PathPlanning/DynamicWindowApproach/dwa.py

@@ -0,0 +1,151 @@
+import math
+import numpy as np
+from enum import Enum
+import matplotlib.pyplot as plt
+
+class RobotType(Enum):
+    circle = 0
+    rectangle = 1
+
+class Config:
+    def __init__(self):
+        self.max_speed = 1.0  # [m/s]
+        self.min_speed = -0.5  # [m/s]
+        self.max_yaw_rate = 40.0 * math.pi / 180.0  # [rad/s]
+        self.max_accel = 0.2  # [m/ss]
+        self.max_delta_yaw_rate = 40.0 * math.pi / 180.0  # [rad/ss]
+        self.v_resolution = 0.01  # [m/s]
+        self.yaw_rate_resolution = 0.1 * math.pi / 180.0  # [rad/s]
+        self.dt = 0.1  # [s] Time tick for motion prediction
+        self.predict_time = 3.0  # [s]
+        self.to_goal_cost_gain = 0.15
+        self.speed_cost_gain = 1.0
+        self.obstacle_cost_gain = 1.0
+        self.robot_stuck_flag_cons = 0.001  # constant to prevent robot stucked
+        self.robot_type = RobotType.circle
+        self.robot_radius = 1.0  # [m] for collision check
+        self.robot_width = 0.5  # [m] for collision check
+        self.robot_length = 1.2  # [m] for collision check
+        self.ob = np.array([[51.74531, 0.47058], [51.73484, 0.45986], [51.715071, 0.4858], [51.75032, 0.48405], [51.74856, 0.48624], [51.73793, 0.48606], [51.76216, 0.47748], [51.76928, 0.46855]])  # Initialize with empty array
+
+    @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):
+    x[2] += u[1] * dt
+    x[0] += u[0] * math.cos(x[2]) * dt
+    x[1] += u[0] * math.sin(x[2]) * dt
+    x[3] = u[0]
+    x[4] = u[1]
+    return x
+
+def dwa_control(x, config, goal, ob):
+    dw = calc_dynamic_window(x, config)
+    u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob)
+    return u, trajectory
+
+def calc_dynamic_window(x, config):
+    Vs = [config.min_speed, config.max_speed,
+          -config.max_yaw_rate, config.max_yaw_rate]
+    Vd = [x[3] - config.max_accel * config.dt,
+          x[3] + config.max_accel * config.dt,
+          x[4] - config.max_delta_yaw_rate * config.dt,
+          x[4] + config.max_delta_yaw_rate * config.dt]
+    dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
+          max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
+    return dw
+
+def predict_trajectory(x_init, v, y, config):
+    x = np.array(x_init)
+    trajectory = np.array(x)
+    time = 0
+    while time <= config.predict_time:
+        x = motion(x, [v, y], config.dt)
+        trajectory = np.vstack((trajectory, x))
+        time += config.dt
+    return trajectory
+
+def calc_control_and_trajectory(x, dw, config, goal, ob):
+    x_init = x[:]
+    min_cost = float("inf")
+    best_u = [0.0, 0.0]
+    best_trajectory = np.array([x])
+    for v in np.arange(dw[0], dw[1], config.v_resolution):
+        for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):
+            trajectory = predict_trajectory(x_init, v, y, config)
+            to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)
+            speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])
+            ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)
+            final_cost = to_goal_cost + speed_cost + ob_cost
+            if min_cost >= final_cost:
+                min_cost = final_cost
+                best_u = [v, y]
+                best_trajectory = trajectory
+                if abs(best_u[0]) < config.robot_stuck_flag_cons and abs(x[3]) < config.robot_stuck_flag_cons:
+                    best_u[1] = -config.max_delta_yaw_rate
+    return best_u, best_trajectory
+
+def calc_obstacle_cost(trajectory, ob, config):
+    ox = ob[:, 0]
+    oy = ob[:, 1]
+    dx = trajectory[:, 0] - ox[:, None]
+    dy = trajectory[:, 1] - oy[:, None]
+    r = np.hypot(dx, dy)
+    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])
+        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 == RobotType.circle:
+        if np.array(r <= config.robot_radius).any():
+            return float("Inf")
+    min_r = np.min(r)
+    return 1.0 / min_r
+
+def calc_to_goal_cost(trajectory, goal):
+    dx = goal[0] - trajectory[-1, 0]
+    dy = goal[1] - trajectory[-1, 1]
+    error_angle = math.atan2(dy, dx)
+    cost_angle = error_angle - trajectory[-1, 2]
+    cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))
+    return cost
+
+def plot_robot(x, y, yaw, config):
+    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],
+                            [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 == 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]) +
+                        np.array([np.cos(yaw), np.sin(yaw)]) * config.robot_radius)
+        plt.plot([x, out_x], [y, out_y], "-k")
+