mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 06:28:00 -05:00
dwa pr (#390)
* dwa pr * dwa_pr * dwa_pr * dwa_pr * dwa_pr * make changes rerun CI * rerun CI...again.. * rerun CI..... * rerun CI..... * rerun CI final time! * modified const to class variable * put back missing comment * add test for dwa stuck case * add test dwa stuck case * add test dwa stuck case * add test dwa stuck case * add stuck test in original test file
This commit is contained in:
weicent
@@ -19,7 +19,6 @@ def dwa_control(x, config, goal, ob):
|
||||
"""
|
||||
Dynamic Window Approach control
|
||||
"""
|
||||
|
||||
dw = calc_dynamic_window(x, config)
|
||||
|
||||
u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob)
|
||||
@@ -51,6 +50,7 @@ class Config:
|
||||
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
|
||||
|
||||
# if robot_type == RobotType.circle
|
||||
@@ -60,6 +60,23 @@ class Config:
|
||||
# if robot_type == RobotType.rectangle
|
||||
self.robot_width = 0.5 # [m] for collision check
|
||||
self.robot_length = 1.2 # [m] for collision check
|
||||
# obstacles [x(m) y(m), ....]
|
||||
self.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]
|
||||
])
|
||||
|
||||
@property
|
||||
def robot_type(self):
|
||||
@@ -72,6 +89,9 @@ class Config:
|
||||
self._robot_type = value
|
||||
|
||||
|
||||
config = Config()
|
||||
|
||||
|
||||
def motion(x, u, dt):
|
||||
"""
|
||||
motion model
|
||||
@@ -139,7 +159,6 @@ def calc_control_and_trajectory(x, dw, config, goal, ob):
|
||||
for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):
|
||||
|
||||
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])
|
||||
@@ -152,13 +171,19 @@ def calc_control_and_trajectory(x, dw, config, goal, ob):
|
||||
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:
|
||||
# to ensure the robot do not get stuck in
|
||||
# best v=0 m/s (in front of an obstacle) and
|
||||
# best omega=0 rad/s (heading to the goal with
|
||||
# angle difference of 0)
|
||||
best_u[1] = -config.max_delta_yaw_rate
|
||||
return best_u, best_trajectory
|
||||
|
||||
|
||||
def calc_obstacle_cost(trajectory, ob, config):
|
||||
"""
|
||||
calc obstacle cost inf: collision
|
||||
calc obstacle cost inf: collision
|
||||
"""
|
||||
ox = ob[:, 0]
|
||||
oy = ob[:, 1]
|
||||
@@ -238,29 +263,12 @@ def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
|
||||
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)
|
||||
|
||||
ob = config.ob
|
||||
while True:
|
||||
u, predicted_trajectory = dwa_control(x, config, goal, ob)
|
||||
x = motion(x, u, config.dt) # simulate robot
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
import os
|
||||
import sys
|
||||
import numpy as np
|
||||
from unittest import TestCase
|
||||
|
||||
sys.path.append(os.path.dirname(__file__) + "/../")
|
||||
@@ -20,8 +21,38 @@ class TestDynamicWindowApproach(TestCase):
|
||||
m.show_animation = False
|
||||
m.main(gx=1.0, gy=1.0, robot_type=m.RobotType.rectangle)
|
||||
|
||||
def test_stuck_main(self):
|
||||
m.show_animation = False
|
||||
# adjust cost
|
||||
m.config.to_goal_cost_gain = 0.2
|
||||
m.config.obstacle_cost_gain = 2.0
|
||||
# obstacles and goals for stuck condition
|
||||
m.config.ob = -1 * np.array([[-1.0, -1.0],
|
||||
[0.0, 2.0],
|
||||
[2.0, 6.0],
|
||||
[2.0, 8.0],
|
||||
[3.0, 9.27],
|
||||
[3.79, 9.39],
|
||||
[7.25, 8.97],
|
||||
[7.0, 2.0],
|
||||
[3.0, 4.0],
|
||||
[6.0, 5.0],
|
||||
[3.5, 5.8],
|
||||
[6.0, 9.0],
|
||||
[8.8, 9.0],
|
||||
[5.0, 9.0],
|
||||
[7.5, 3.0],
|
||||
[9.0, 8.0],
|
||||
[5.8, 4.4],
|
||||
[12.0, 12.0],
|
||||
[3.0, 2.0],
|
||||
[13.0, 13.0]
|
||||
])
|
||||
m.main(gx=-5.0, gy=-7.0)
|
||||
|
||||
|
||||
if __name__ == '__main__': # pragma: no cover
|
||||
test = TestDynamicWindowApproach()
|
||||
test.test_main1()
|
||||
test.test_main2()
|
||||
test.test_stuck_main()
|
||||
|
||||
Reference in New Issue
Block a user