Feature/fastpathplanning #90
Conversation
it runs in sim but the robots are undergoing brownian motion
…oller for opponents for testing
There was a problem hiding this comment.
Pull request overview
This PR introduces a new motion planning algorithm called "fast path planning" inspired by a research paper. The implementation adds a FastPathPlanner class that computes collision-free trajectories by recursively finding subgoals around obstacles, and integrates it with the existing motion control system through a new FastPathPlanningController.
Changes:
- New FastPathPlanner algorithm with recursive obstacle avoidance
- FastPathPlanningController integrating the planner with PID control
- Helper geometry functions for line segment calculations
- Registration of the new "fpp" control scheme
Reviewed changes
Copilot reviewed 6 out of 6 changed files in this pull request and generated 51 comments.
Show a summary per file
| File | Description |
|---|---|
| planner.py | Core implementation of the fast path planning algorithm with obstacle detection and trajectory generation |
| config.py | Configuration constants for the planner (clearances, distances, recursion limits) |
| fastpathplanning.py | Controller class integrating FastPathPlanner with PID control |
| init.py | Exports the new FastPathPlanningController |
| control_schemes.py | Registers "fpp" control scheme in the scheme map |
| math_utils.py | Adds geometry helper functions for line segment distance calculations and intersection detection |
|
|
||
| oren = self.pid_oren.calculate(target_oren, robot.orientation, robot_id) | ||
|
|
||
| pos = self.fpp._path_to(game, robot_id, target_pos)[0][1] |
There was a problem hiding this comment.
The method '_path_to' is being called with 'target_pos' which is a Vector2D, but it's converted to a tuple in line 30 when accessing '[0][1]'. However, the method signature expects a Tuple[float, float] and converts it to np.array internally. The Vector2D object needs to be converted to a tuple (target_pos.x, target_pos.y) before passing it to '_path_to'.
| pos = self.fpp._path_to(game, robot_id, target_pos)[0][1] | |
| pos = self.fpp._path_to(game, robot_id, (target_pos.x, target_pos.y))[0][1] |
There was a problem hiding this comment.
Not needed, as the _path_to function is converting the target position to an np.array in line 127.
| return vel, oren | ||
|
|
||
| def reset(self, robot_id): | ||
| self.pid_oren.reset(robot_id) |
There was a problem hiding this comment.
The method 'reset' only resets 'pid_oren' but does not reset 'pid_trans'. This is inconsistent with the DWAController implementation which resets both controllers. The translational PID should also be reset to maintain consistent state management.
| self.pid_oren.reset(robot_id) | |
| self.pid_oren.reset(robot_id) | |
| self.pid_trans.reset(robot_id) |
| def point_to_segment_distance(point: np.ndarray, seg_start: np.ndarray, seg_end: np.ndarray) -> float: | ||
| """Compute the shortest distance between a point and a line segment.""" | ||
| seg_vec = seg_end - seg_start | ||
| t = np.clip(np.dot(point - seg_start, seg_vec) / np.dot(seg_vec, seg_vec), 0, 1) |
There was a problem hiding this comment.
use
den = np.dot(seg_vec, seg_vec)
if den < 1e-9:
return np.linalg.norm(point - seg_start)
t = np.clip(np.dot(point - seg_start, seg_vec) / den, 0, 1)| robotpos = np.array([r.p.x, r.p.y]) | ||
| if ( | ||
| distance(ourpos, robotpos) < self.LOOK_AHEAD_RANGE | ||
| and distance(robotpos, ourpos) > self.OBSTACLE_CLEARANCE |
There was a problem hiding this comment.
explain why we are doing this. It's not clear why we would filter these out. I know, but people in the future may not. We intend to use DWA when the obstacles overlap our start right?
|
|
||
| for o in obstacles: | ||
| if point_to_segment_distance(subgoal, o[0], o[1]) < self.OBSTACLE_CLEARANCE: | ||
| subgoal = self._find_subgoal( |
There was a problem hiding this comment.
it's possible for an infinite recursion here
There was a problem hiding this comment.
It is not, because after a point, I believe it will find a subgoal outside the bounds of the field, giving a possible subgoal. This needs to be resolved as we make an edit to add bounds to the planner.
| self, segment: Tuple, obstacles, target | ||
| ): # returns None if no obstacles, else it returns the closest obstacle. | ||
| closestobstacle = None | ||
| tempdistance = distance(segment[0], segment[1]) |
There was a problem hiding this comment.
why not use tempdistance = float("inf") here?
| obstaclelist.append((robotpos, robotpos)) | ||
| return obstaclelist | ||
|
|
||
| def _find_subgoal(self, robotpos, target, closestobstacle, obstacles, recursionfactor, multiple) -> np.array: |
There was a problem hiding this comment.
the purpose of recursionfactor is really ambiguous. You mean something like left_or_right right? Reconsider the implementation.
There was a problem hiding this comment.
It is a left-right thing. At the moment, the planner is working, but this implementation is not exactly correct.
Once the submission video is completed I want to update it.
| direction = target - robotpos | ||
|
|
||
| if recursionfactor % 2 == 1: | ||
| perp_dir = rotate_vector(direction[0], direction[1], math.pi / 2) | ||
| else: | ||
| perp_dir = rotate_vector(direction[0], direction[1], math.pi * 3 / 2) |
There was a problem hiding this comment.
| direction = target - robotpos | |
| if recursionfactor % 2 == 1: | |
| perp_dir = rotate_vector(direction[0], direction[1], math.pi / 2) | |
| else: | |
| perp_dir = rotate_vector(direction[0], direction[1], math.pi * 3 / 2) | |
| direction = target - robotpos | |
| norm = np.linalg.norm(direction) | |
| if norm < 1e-6: # already at target | |
| return target | |
| direction /= norm | |
| # perpendicular vector | |
| perp = np.array([-direction[1], direction[0]]) |
There was a problem hiding this comment.
Not updating this at the moment, because it is linked to the previous comment. While _findsubgoal works, its implementation is not exactly correct/the best. So I would like to change it later.
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
New motion planning algorithm inspired by the fast-path-planning algorithm described here.