Warning

This page is located in archive.
Go to the latest version of this course pages.
Go the latest version of this page.

The main task is to implement a simple grid-based planning approach. You will learn how to plan on the grid such that the robot embodiment is considered.

Files | `Planner.py` - File to be implemented `bresenham.py` - Implementation of the Bresenham's line algorithm `utils.py` - Helpers for evaluation, drawing, and saving figures `data` - Scenarios and reference solutions in bag files `t1c-plan.py` - Evaluation script |

Resources | redcp.zip, t1c-plan.zip - Initial files and evaluation script |

Implement

`Planner.grow_obstacles()`

in`Planner.py`

,`Planner.plan_path()`

in`Planner.py`

,`Planner.simplify_path()`

in`Planner.py`

.

In `Planner.py`

, implement the following steps of grid-based planning.

- Implement the obstacle growing (method
`grow_obstacles`

) to account for the robot embodiment. - Implement a grid-based planning method (method
`plan_path`

) to find a path between the start and goal position (considering planning with 8-neighborhood). - Implement the trajectory smoothing method (method
`simplify_path`

) that simplifies the path as a sequence of waypoints

The `grow_obstacles`

function processes the occupancy grid map into a binary map that encompasses areas inaccessible for the robot considering its embodiment.
Using obstacles growing by a disk representing the robot shape, we can use such a grid map for grid-based planning with a single-cell robot.
The robot shape can be modeled as a disk with $r_\mathrm{robot}$ radius `robot_size`

.
The result of the obstacle growing is a binary map with the value 1 for occupied and unknown cells and 0 for traversable cells; thus, we threshold the occupancy grid in addition to obstacle growing.
For each cell $m_i$ of the `grid_map`

the new binary value $b_i$ of the `grid_map_grow`

is based on its Bayesian occupancy probability $P(m_i = occupied \vert z) = 0.5$ and distance to the nearby obstacles according to:

**Filtering all obstacles**- if $P(m_i = occupied \vert z) > 0.5$, then the cell is considered occupied $b_i$ = 1;**Filtering all unknown areas**- if $P(m_i = occupied \vert z) = 0.5$, then the cell is considered occupied $b_i$ = 1;**Filtering cells close to the obstacles**- if $P(m_i = occupied \vert z) < 0.5$ and the approximated Euclidean distance on the grid of the cell $m_i$ and the closest obstacle is shorter than $r_\mathrm{robot}$, then the cell is considered occupied $b_i$ = 1;**Otherwise**the cell is considered free $b_i=0$.

We can further denote the mid probability of a cell being occupied as `CELL_MID`

.

The input parameters of the obstacle-growing function are:

`grid_map`

- OccupancyGrid - grid map representation;`robot_size`

- the robot radius in meters.

The function returns

- OccupancyGrid with the processed grid map - binary map with filtered obstacles, unknown and inaccessible areas.

The `grow_obstacles`

function in the `Planner.py`

class has the following prescription.

def grow_obstacles(self, grid_map: OccupancyGrid, robot_size) -> OccupancyGrid: """ Method to grow the obstacles to take into account the robot embodiment Args: grid_map: OccupancyGrid - grid map for obstacle growing robot_size: float - the size of the robot Returns: grid_map_grow: OccupancyGrid - grid map with considered robot body embodiment """The following figure depicts an example of the obstacle growing for the first evaluation scenario. From left to right:

- Original map;
- Map grown for robot diameter 0.3 m;
- Map grown for robot diameter 0.5 m.

Suggested approach (click to view)

**Obstacle growing** - The robot has non-zero dimensions representing its embodiment, which must be considered when planning. Moreover, the plan execution in real experiments has a limited precision that further enlarges demands on the clearance to the obstacles for the robot to navigate the environment safely. We can grow the borders of the obstacles by the diameter of the robot and plan in the resulting map that ensures sufficient clearance to the obstacles. The obstacle growing can be achieved by one of the following approaches:

- Using convolution masks;
- Using mathematical morphology;
- Using distance transform, implemented using, e.g.,
`scipy.ndimage.distance_transform_edt`

(suggested approach).

Path planning is to be implemented in the `plan_path`

function that provides a path between the given start and goal poses on the grid map considering 8-neighborhood. The function returns a path on the output. If the path is not found, the planner returns `None`

.

The input parameters of the `plan_path`

function are:

`grid_map`

- OccupancyGrid - pre-processed grid map representation (contains 1 and 0 only);`start`

- Pose - start robot pose;`goal`

- Pose - goal robot pose.

The function returns

- Path with the found path or
`None`

when the path is not found. It is supposed that the path contains only 2D coordinates and not the heading. Only`position.x`

and`position.y`

entries of individual poses in the path are filled in. The path is returned in the world coordinates!

The `plan_path`

function in the `Planner.py`

class has the following prescription.

def plan_path(self, grid_map: OccupancyGrid, start: Pose, goal: Pose) -> Path: """ Method to plan the path from the start to the goal pose on the grid Args: grid_map: OccupancyGrid - grid map for obstacle growing start: Pose - robot start pose goal: Pose - robot goal pose Returns: path: Path - path between the start and goal Pose on the map """An example of the planned paths with the A* algorithm for different robot sizes is visualized in the following figure. From left to right, the planning for:

- Original map;
- Map grown for robot diameter 0.3 m;
- Map grown for robot diameter 0.5 m;

Note that the task concerns planning in robotics and not implementing an A* planner. Hence, with a proper citation of the source in the source files, you are encouraged to use any publicly available implementation of the planner you want. Rather, get the intuition behind looking at the resulting paths and recognizing sub-optimal paths, wrongly set heuristics, and so on.

Suggested approach (click to view)

Considered planning methods

- Breadth First Search (BFS), Depth First Search (DFS)
- Dijkstra
- A*
- Jump Point Search (JPS)
- Theta* (any-angle path planning - returns the shortest path)
- ARA* (Any-time repairing A*)
- D* lite
- Floyd-Warshall

Pay attention to the planning algorithm used. If you decided to implement A* planner, it is assumed that the path is optimal!

Path simplification can be done by excluding unnecessary cells (locations) from the path. The robot does not have to visit them precisely or will visit them along the navigation to the waypoint, where it needs to change direction. A possible approach to path simplification is to employ the ray-casting technique to connect the neighboring segments one by one using straight-line segments up to the point where the straight-line segment collides with an obstacle (grown obstacle) and then follows with another straight-line segment.

$\qquad\bullet\,$ Bresenham's line algorithm (click to view)

def bresenham_line(self, start: (int, int), goal: (int, int)) -> (int, int): """Bresenham's line algorithm Args: start: (int, int) - start coordinate goal: (int, int) - goal coordinate Returns: Interlining points between the start and goal coordinate """ (x0, y0) = start (x1, y1) = goal line = [] dx = abs(x1 - x0) dy = abs(y1 - y0) x, y = x0, y0 sx = -1 if x0 > x1 else 1 sy = -1 if y0 > y1 else 1 if dx > dy: err = dx / 2.0 while x != x1: line.append((x,y)) err -= dy if err < 0: y += sy err += dx x += sx else: err = dy / 2.0 while y != y1: line.append((x,y)) err -= dx if err < 0: x += sx err += dy y += sy x = goal[0] y = goal[1] return line

You can use the provided

`bresenham_line`

function from the `bresenham.py`

.
The input parameters of the function are

`grid_map`

- OccupancyGrid - pre-processed grid map representation (contains 1 and 0 only);`path`

- Path - path to be simplified.

The function returns:

- the Path simplified path with the processed path. The path is returned in the world coordinates!

The `simplify_path`

function in the `Planner.py`

class has a following prescription:

def simplify_path(self, grid_map: OccupancyGrid, path: Path) -> Path: """ Method to simplify the found path on the grid Args: grid_map: OccupancyGrid - grid map for obstacle growing path: Path - path to be simplified Returns: path_simple: Path - simplified path """The result of path simplification may vary, but an example of the planned and simplified path using the suggested approach is visualized in the following figure. From left to right, the planning and simplification for

- Original map;
- Map grown for robot diameter 0.3 m;
- Map grown for robot diameter 0.5 m.

Suggested approach (click to view)

- Implementation of the path simplification can be relatively straightforward. In a pseudocode, it can be as follows.

#add the start position path_simple add path[0] #iterate through the path and simplify the path while not path_simple[end] == path[end]: #until the goal is not reached #find the connected segment previous_pose = path_simple[end] for each pose in the rest of the path: if bresenham_line(path_simple[end], pose) not collide: #there is no collision previous_pose = pose #the goal is reached if pose = path[end]: path_simple append pose break else: #there is a collision path_simple append previous_pose break

The evaluation focuses on the correct behavior of the above-described robot navigation pipeline; see `t1c-plan.py`

for the full evaluation script.

ROBOT_SIZE = 0.5 if __name__=="__main__": planner = Planner() scenarios = pickle.load( open( "data/scenarios.bag", "rb" ) ) for gridmap, start, goal in scenarios: gridmap_processed = planner.grow_obstacles(gridmap, radius) path = planner.plan_path(gridmap_processed, start, goal) path_simple = planner.simplify_path(gridmap_processed, path)

courses/crl-courses/redcp/tasks/t1c-plan.txt · Last modified: 2022/11/25 12:33 by faiglj