mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 16:57:58 -05:00
297 lines
8.9 KiB
Python
297 lines
8.9 KiB
Python
"""
|
|
|
|
Mobile robot motion planning sample with Dynamic Window Approach
|
|
|
|
author: Atsushi Sakai (@Atsushi_twi), Goktug Karakasli
|
|
|
|
"""
|
|
|
|
import math
|
|
from enum import Enum
|
|
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
show_animation = True
|
|
|
|
|
|
def dwa_control(x, config, goal, ob):
|
|
"""
|
|
Dynamic Window Approach control
|
|
"""
|
|
|
|
dw = calc_dynamic_window(x, config)
|
|
|
|
u, trajectory = calc_final_input(x, dw, config, goal, ob)
|
|
|
|
return u, trajectory
|
|
|
|
|
|
class RobotType(Enum):
|
|
circle = 0
|
|
rectangle = 1
|
|
|
|
|
|
class Config:
|
|
"""
|
|
simulation parameter class
|
|
"""
|
|
|
|
def __init__(self):
|
|
# robot parameter
|
|
self.max_speed = 1.0 # [m/s]
|
|
self.min_speed = -0.5 # [m/s]
|
|
self.max_yawrate = 40.0 * math.pi / 180.0 # [rad/s]
|
|
self.max_accel = 0.2 # [m/ss]
|
|
self.max_dyawrate = 40.0 * math.pi / 180.0 # [rad/ss]
|
|
self.v_reso = 0.01 # [m/s]
|
|
self.yawrate_reso = 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_type = RobotType.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 == RobotType.rectangle
|
|
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):
|
|
"""
|
|
motion model
|
|
"""
|
|
|
|
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 calc_dynamic_window(x, config):
|
|
"""
|
|
calculation dynamic window based on current state x
|
|
"""
|
|
|
|
# Dynamic window from robot specification
|
|
Vs = [config.min_speed, config.max_speed,
|
|
-config.max_yawrate, config.max_yawrate]
|
|
|
|
# Dynamic window from motion model
|
|
Vd = [x[3] - config.max_accel * config.dt,
|
|
x[3] + config.max_accel * config.dt,
|
|
x[4] - config.max_dyawrate * config.dt,
|
|
x[4] + config.max_dyawrate * config.dt]
|
|
|
|
# [vmin,vmax, yaw_rate min, yaw_rate max]
|
|
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):
|
|
"""
|
|
predict trajectory with an input
|
|
"""
|
|
|
|
x = np.array(x_init)
|
|
traj = np.array(x)
|
|
time = 0
|
|
while time <= config.predict_time:
|
|
x = motion(x, [v, y], config.dt)
|
|
traj = np.vstack((traj, x))
|
|
time += config.dt
|
|
|
|
return traj
|
|
|
|
|
|
def calc_final_input(x, dw, config, goal, ob):
|
|
"""
|
|
calculation final input with dynamic window
|
|
"""
|
|
|
|
x_init = x[:]
|
|
min_cost = float("inf")
|
|
best_u = [0.0, 0.0]
|
|
best_trajectory = np.array([x])
|
|
|
|
# evaluate all trajectory with sampled input in dynamic window
|
|
for v in np.arange(dw[0], dw[1], config.v_reso):
|
|
for y in np.arange(dw[2], dw[3], config.yawrate_reso):
|
|
|
|
trajectory = predict_trajectory(x_init, v, y, config)
|
|
|
|
# calc cost
|
|
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
|
|
|
|
# search minimum trajectory
|
|
if min_cost >= final_cost:
|
|
min_cost = final_cost
|
|
best_u = [v, y]
|
|
best_trajectory = trajectory
|
|
|
|
return best_u, best_trajectory
|
|
|
|
|
|
def calc_obstacle_cost(trajectory, ob, config):
|
|
"""
|
|
calc obstacle cost inf: collision
|
|
"""
|
|
ox = ob[:, 0]
|
|
oy = ob[:, 1]
|
|
dx = trajectory[:, 0] - ox[:, None]
|
|
dy = trajectory[:, 1] - oy[:, None]
|
|
r = np.sqrt(np.square(dx) + np.square(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 (r <= config.robot_radius).any():
|
|
return float("Inf")
|
|
|
|
min_r = np.min(r)
|
|
return 1.0 / min_r # OK
|
|
|
|
|
|
def calc_to_goal_cost(trajectory, goal):
|
|
"""
|
|
calc to goal cost with angle difference
|
|
"""
|
|
|
|
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_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),
|
|
head_length=width, head_width=width)
|
|
plt.plot(x, y)
|
|
|
|
|
|
def plot_robot(x, y, yaw, config): # pragma: no cover
|
|
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")
|
|
|
|
|
|
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])
|
|
# goal position [x(m), y(m)]
|
|
goal = np.array([gx, gy])
|
|
# obstacles [x(m) y(m), ....]
|
|
ob = np.array([[-1, -1],
|
|
[0, 2],
|
|
[4.0, 2.0],
|
|
[5.0, 4.0],
|
|
[5.0, 5.0],
|
|
[5.0, 6.0],
|
|
[5.0, 9.0],
|
|
[8.0, 9.0],
|
|
[7.0, 9.0],
|
|
[8.0, 10.0],
|
|
[9.0, 11.0],
|
|
[12.0, 13.0],
|
|
[12.0, 12.0],
|
|
[15.0, 15.0],
|
|
[13.0, 13.0]
|
|
])
|
|
|
|
# input [forward speed, yaw_rate]
|
|
config = Config()
|
|
config.robot_type = robot_type
|
|
trajectory = np.array(x)
|
|
|
|
while True:
|
|
u, predicted_trajectory = dwa_control(x, config, goal, ob)
|
|
x = motion(x, u, config.dt) # simulate robot
|
|
trajectory = np.vstack((trajectory, x)) # store state history
|
|
|
|
if show_animation:
|
|
plt.cla()
|
|
plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g")
|
|
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)
|
|
plt.pause(0.0001)
|
|
|
|
# check reaching goal
|
|
dist_to_goal = math.sqrt((x[0] - goal[0]) ** 2 + (x[1] - goal[1]) ** 2)
|
|
if dist_to_goal <= config.robot_radius:
|
|
print("Goal!!")
|
|
break
|
|
|
|
print("Done")
|
|
if show_animation:
|
|
plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")
|
|
plt.pause(0.0001)
|
|
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main(robot_type=RobotType.rectangle)
|