diff --git a/.gitignore b/.gitignore index aaebcf8..d835c18 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ __pycache__/ build/ devel/ +video/ # local env files .env.local diff --git a/README.md b/README.md index 4338dcc..9b778a6 100644 --- a/README.md +++ b/README.md @@ -96,9 +96,9 @@ Planner | Version | Animation ## Local Planner | Planner | Version | Animation | | ------- | -------------------------------------------------------- | -------------------------------------------------------- | -| **PID** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/matlab_motion_planning/blob/master/local_planner/pid.m) | ![pid_matlab.gif](gif/pid_matlab.gif) -| **APF** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) -| **DWA** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/matlab_motion_planning/blob/master/local_planner/dwa.m) | ![dwa_matlab.gif](gif/dwa_matlab.gif) +| **PID** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/matlab_motion_planning/blob/master/local_planner/pid_plan.m) | ![pid_matlab.gif](gif/pid_matlab.gif) +| **APF** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/matlab_motion_planning/blob/master/local_planner/apf_plan.m) | ![apf_matlab.gif](gif/apf_matlab.gif) +| **DWA** | [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/matlab_motion_planning/blob/master/local_planner/dwa_plan.m) | ![dwa_matlab.gif](gif/dwa_matlab.gif) | **TEB** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) | **MPC** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) | **Lattice** | ![Status](https://img.shields.io/badge/develop-v1.0-red) | ![Status](https://img.shields.io/badge/gif-none-yellow) @@ -134,4 +134,5 @@ Planner | Version | Animation ## Local Planning -* [DWA: ](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf) The Dynamic Window Approach to Collision Avoidance \ No newline at end of file +* [DWA: ](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf) The Dynamic Window Approach to Collision Avoidance +* [APF: ](https://ieeexplore.ieee.org/document/1087247)Real-time obstacle avoidance for manipulators and mobile robots \ No newline at end of file diff --git a/examples/simulation_global.mlx b/examples/simulation_global.mlx index d5729dc..0de4b0a 100644 Binary files a/examples/simulation_global.mlx and b/examples/simulation_global.mlx differ diff --git a/examples/simulation_global_replan.mlx b/examples/simulation_global_replan.mlx deleted file mode 100644 index b13920e..0000000 Binary files a/examples/simulation_global_replan.mlx and /dev/null differ diff --git a/examples/simulation_local.mlx b/examples/simulation_local.mlx index 1234b53..d8e818f 100644 Binary files a/examples/simulation_local.mlx and b/examples/simulation_local.mlx differ diff --git a/examples/simulation_total.mlx b/examples/simulation_total.mlx deleted file mode 100644 index 1af3974..0000000 Binary files a/examples/simulation_total.mlx and /dev/null differ diff --git a/gif/apf_matlab.gif b/gif/apf_matlab.gif new file mode 100644 index 0000000..5f8bb3b Binary files /dev/null and b/gif/apf_matlab.gif differ diff --git a/gif/pid_matlab.gif b/gif/pid_matlab.gif index fa3f0b0..6dfe692 100644 Binary files a/gif/pid_matlab.gif and b/gif/pid_matlab.gif differ diff --git a/local_planner/apf_plan.m b/local_planner/apf_plan.m new file mode 100644 index 0000000..81202e8 --- /dev/null +++ b/local_planner/apf_plan.m @@ -0,0 +1,192 @@ +function [pose, traj, flag] = apf_plan(start, goal, varargin) +%% +% @file: apf_plan.m +% @breif: Artificial Potential Field motion planning +% @paper: The Artificial Potential Field to Collision Avoidance +% @author: Winter +% @update: 2023.10.24 + +%% + p = inputParser; + addParameter(p, 'path', "none"); + addParameter(p, 'map', "none"); + parse(p, varargin{:}); + + if isstring(p.Results.path) || isstring(p.Results.map) + exception = MException('MyErr:InvalidInput', 'parameter `path` or `map` must be set.'); + throw(exception); + end + + % path + path = flipud(p.Results.path); + path_length = size(path, 1); + plan_idx = 1; + + % map + map = p.Results.map; + + % obstacle + [m, ~] = size(map); + obs_index = find(map==2); + obstacle = [mod(obs_index - 1, m) + 1, fix((obs_index - 1) / m) + 1]; + + % initial robotic state + robot.x = start(1); + robot.y = start(2); + robot.theta = start(3); + robot.v = 0; + robot.w = 0; + max_v = 0.4; + + % parameters + zeta = 1.0; + eta = 0.8; + d_0 = 1.5; + + dt = 0.1; + p_window = 0.5; + p_precision = 0.5; + o_precision = pi / 4; + e_v_ = 0; i_v_ = 0; + e_w_ = 0; i_w_ = 0; + max_iter = 1000; + + % return value + flag = false; + pose = []; + traj = []; + + iter = 0; + % main loop + while (1) + iter = iter + 1; + if (iter > max_iter) + break; + end + + % break until goal reached + if (norm([robot.x, robot.y] - goal(:, 1:2)) < p_precision) + flag = true; + break; + end + + % compute the tatget pose and force at the current step + rep_force = getRepulsiveForce([robot.x, robot.y], obstacle, d_0); + while (plan_idx <= path_length) + tgt_pos = path(plan_idx, :); + attr_force = getAttractiveForce([robot.x, robot.y], tgt_pos); + net_force = zeta * attr_force + eta * rep_force; + + % in body frame + b_x_d = path(plan_idx, 1) - robot.x; + b_y_d = path(plan_idx, 2) - robot.y; + + if (norm([b_x_d, b_y_d]) > p_window) + break; + end + plan_idx = plan_idx + 1; + end + + new_v = [robot.v * cos(robot.theta), robot.v * sin(robot.theta)] + net_force; + new_v = new_v ./ norm(new_v); + new_v = new_v .* max_v; + + theta_d = atan2(new_v(2), new_v(1)); + + % calculate velocity command + if (norm([robot.x, robot.y] - goal(:, 1:2)) < p_precision) + if (abs(robot.theta - goal(3)) < o_precision) + u = [0, 0]; + else + [w, e_w_, i_w_] = angularController(robot, goal(3), dt, e_w_, i_w_); + u = [0, w]; + end + elseif (abs(theta_d - robot.theta) > pi / 2) + [w, e_w_, i_w_] = angularController(robot, theta_d, dt, e_w_, i_w_); + u = [0, w]; + else + [v, e_v_, i_v_] = linearController(robot, norm(new_v), dt, e_v_, i_v_); + [w, e_w_, i_w_] = angularController(robot, theta_d, dt, e_w_, i_w_); + u = [v, w]; + end + + % input into robotic kinematic + robot = f(robot, u, dt); + pose = [pose; robot.x, robot.y, robot.theta]; + end +end + +%% +function attr_force = getAttractiveForce(cur_pos, tgt_pos) + attr_force = tgt_pos - cur_pos; + if ~all(attr_force == 0) + attr_force = attr_force ./ norm(attr_force); + end +end + +function rep_force = getRepulsiveForce(cur_pos, obstacle, d_0) + D = dist(obstacle, cur_pos'); + rep_force = (1 ./ D - 1 / d_0) .* (1 ./ D) .^ 2 .* (cur_pos - obstacle); + valid_mask = (1 ./ D - 1 / d_0) > 0; + rep_force = sum(rep_force(valid_mask, :), 1); + + if ~all(rep_force == 0) + rep_force = rep_force ./ norm(rep_force); + end +end + +function [v, e_v_, i_v_] = linearController(robot, v_d, dt, e_v_, i_v_) + e_v = v_d - robot.v; + i_v_ = i_v_ + e_v * dt; + d_v = (e_v - e_v_) / dt; + e_v_ = e_v; + + k_v_p = 1.00; + k_v_i = 0.00; + k_v_d = 0.00; + v_inc = k_v_p * e_v_ + k_v_i * i_v_ + k_v_d * d_v; + + v = robot.v + v_inc; +end + +function [w, e_w_, i_w_] = angularController(robot, theta_d, dt, e_w_, i_w_) + e_theta = theta_d - robot.theta; + if (e_theta > pi) + e_theta = e_theta - 2 * pi; + elseif (e_theta < -pi) + e_theta = e_theta + 2 * pi; + end + + w_d = e_theta / dt / 10; + e_w = w_d - robot.w; + i_w_ = i_w_ + e_w * dt; + d_w = (e_w - e_w_) / dt; + e_w_ = e_w; + + k_w_p = 1.00; + k_w_i = 0.00; + k_w_d = 0.01; + w_inc = k_w_p * e_w_ + k_w_i * i_w_ + k_w_d * d_w; + + w = robot.w + w_inc; +end + +function robot = f(robot, u, dt) +%@breif: robotic kinematic + F = [ 1 0 0 0 0 + 0 1 0 0 0 + 0 0 1 0 0 + 0 0 0 0 0 + 0 0 0 0 0]; + + B = [dt * cos(robot.theta) 0 + dt * sin(robot.theta) 0 + 0 dt + 1 0 + 0 1]; + + x = [robot.x; robot.y; robot.theta; robot.v; robot.w]; + x_star = F * x + B * u'; + robot.x = x_star(1); robot.y = x_star(2); robot.theta = x_star(3); + robot.v = x_star(4); robot.w = x_star(5); +end \ No newline at end of file diff --git a/local_planner/dwa.m b/local_planner/dwa_plan.m similarity index 82% rename from local_planner/dwa.m rename to local_planner/dwa_plan.m index 1f7b8c0..d2edc71 100644 --- a/local_planner/dwa.m +++ b/local_planner/dwa_plan.m @@ -1,12 +1,32 @@ -function [pose, traj, flag] = dwa(map, start, goal, kinematic) +function [pose, traj, flag] = dwa_plan(start, goal, varargin) %% -% @file: dwa.m +% @file: dwa_plan.m % @breif: DWA motion planning % @paper: The Dynamic Window Approach to Collision Avoidance % @author: Winter % @update: 2023.1.30 %% + p = inputParser; + addParameter(p, 'path', "none"); + addParameter(p, 'map', "none"); + parse(p, varargin{:}); + + if isstring(p.Results.map) + exception = MException('MyErr:InvalidInput', 'parameter `map` must be set.'); + throw(exception); + end + + map = p.Results.map; + + % kinematic + kinematic.V_MAX = 1.0; % maximum velocity [m/s] + kinematic.W_MAX = 20.0 * pi /180; % maximum rotation speed[rad/s] + kinematic.V_ACC = 0.2; % acceleration [m/s^2] + kinematic.W_ACC = 50.0 * pi /180; % angular acceleration [rad/s^2] + kinematic.V_RESOLUTION = 0.01; % velocity resolution [m/s] + kinematic.W_RESOLUTION = 1.0 * pi /180; % rotation speed resolution [rad/s]] + % return value flag = false; pose = []; diff --git a/local_planner/pid.m b/local_planner/pid_plan.m similarity index 87% rename from local_planner/pid.m rename to local_planner/pid_plan.m index b6b8b04..ce86579 100644 --- a/local_planner/pid.m +++ b/local_planner/pid_plan.m @@ -1,7 +1,29 @@ -function [pose, flag] = pid(path, start, goal) +function [pose, traj, flag] = pid_plan(start, goal, varargin) +%% +% @file: pid_plan.m +% @breif: PID motion planning +% @paper: The PID to path tracking +% @author: Winter, Gzy +% @update: 2023.1.30 + +%% + p = inputParser; + addParameter(p, 'path', "none"); + addParameter(p, 'map', "none"); + parse(p, varargin{:}); + + if isstring(p.Results.path) + exception = MException('MyErr:InvalidInput', 'parameter `path` must be set, using global planner.'); + throw(exception); + end + + % path + path = p.Results.path; + % return value flag = false; - pose = []; + pose = []; + traj = []; % reverse path path = flipud(path); @@ -38,7 +60,7 @@ % break until goal reached if (norm([robot.x, robot.y] - goal(:, 1:2)) < p_precision) -% && ... abs(robot.theta - goal(3)) < o_precision + flag = true; break; end diff --git a/utils/animation/animation_dwa.m b/utils/animation/animation.m similarity index 71% rename from utils/animation/animation_dwa.m rename to utils/animation/animation.m index 89e58cc..bc8b58d 100644 --- a/utils/animation/animation_dwa.m +++ b/utils/animation/animation.m @@ -1,7 +1,7 @@ -function animation_dwa(pose, traj, delta, record_video) +function animation(planner_name, pose, traj, delta, record_video) % -% @file: animation_dwa.m -% @breif: DWA algorithm animation +% @file: animation.m +% @breif: local planning algorithm animation % @author: Winter % @update: 2023.1.30 % @@ -9,14 +9,16 @@ function animation_dwa(pose, traj, delta, record_video) [frames, ~] = size(pose); if record_video - process = VideoWriter('./animation/video/dwa.mp4', 'MPEG-4'); + process = VideoWriter(sprintf("%s%s%s", "./utils/animation/video/", planner_name, ".mp4"), 'MPEG-4'); open(process); movie = moviein(frames); end for i=1:frames handler = plot_robot([pose(i, 2) + delta, pose(i, 1) + delta, pose(i, 3)], 0.8, 0.4, 'r'); - handler2 = plot_trajectory(traj(i).info, delta); + if ~isempty(traj) + handler2 = plot_trajectory(traj(i).info, delta); + end plot(pose(i, 2) + delta, pose(i, 1) + delta, 'Marker', '.', 'color', "#f00"); drawnow; if record_video @@ -24,7 +26,10 @@ function animation_dwa(pose, traj, delta, record_video) writeVideo(process, movie(:, i)); end delete(handler); - delete(handler2); + + if ~isempty(traj) + delete(handler2); + end end if record_video diff --git a/utils/animation/animation_pid.m b/utils/animation/animation_pid.m deleted file mode 100644 index 05583ae..0000000 --- a/utils/animation/animation_pid.m +++ /dev/null @@ -1,27 +0,0 @@ -function animation_pid(pose, traj, delta, record_video) - hold on - [frames, ~] = size(pose); - - if record_video - process = VideoWriter('./animation/video/pid.avi'); - open(process); - movie = moviein(frames); - end - - for i=1:frames - handler = plot_robot([pose(i, 2) + delta, pose(i, 1) + delta, pose(i, 3)], 0.8, 0.4, 'r'); -% handler2 = plot_trajectory(traj(i).info, delta); - plot(pose(i, 2) + delta, pose(i, 1) + delta, 'Marker', '.', 'color', "#f00"); - drawnow; - if record_video - movie(:, i) = getframe; - writeVideo(process, movie(:, i)); - end - delete(handler); -% delete(handler2); - end - - if record_video - close(process); - end -end