===== T1d-plan - Grid-based path planning ======
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.
|**Deadline** | November 24, 2021, 23:59 PST |
|**Points** | 3 |
|**Label in BRUTE** | t1d-plan |
|**Files to submit** | archive with ''HexapodExplorer.py'' file |
|**Resources** | {{ :courses:uir:hw:b4m36uir_21_t1_resource_pack.zip | B4M36UIR_t1_resource_pack}} |
----
===Assignment===
In class ''HexapodExplorer.py'' implement the following basic steps of the grid-based navigation pipeline:
- Implement the obstacle growing (method ''grow_obstacles'') to take into account 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'') which purpose is to provide a smoother path for the robot
The detailed description and guidelines on individual steps follow.
----
== Obstacle growing ==
The purpose of the ''grow_obstacles'' function is to pre-process the grid map into a binary map by rejecting obstacles, unknown and inaccessible areas for the robot given its embodiment. The robot is considered to be a circle with $r_\mathrm{robot}$ radius ''robot_size''. The result of the obstacle growing is a binary map with the used binary values 1 (for occupied, inaccessible and unknown cells) and 0 (for traversable cells).
For each ''grid_map'' cell $m_i$ 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 the following rules:
- **Filtering all obstacles** - if $P(m_i = occupied \vert z) > 0.5$ the cell is considered occupied $b_i$ = 1
- **Filtering all unknown areas** - if $P(m_i = occupied \vert z) = 0.5$ 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| (considering precise euclidean distance on the grid is not practical, therefore approximations using mathematical morphology or distance transform are used)++ of the cell $m_i$ and the closest obstacle is lower then $r_\mathrm{robot}$, then the cell is considered occupied $b_i$ = 1
- **Otherwise** the cell is considered free $b_i=0$
The input parameters of the function are:
- ''grid_map'' - [[courses:uir:hw:support:messages|OccupancyGrid message]] - grid map representation
- ''robot_size'' - the robot radius in metres
The function returns:
- the [[courses:uir:hw:support:messages|OccupancyGrid message]] with the processed grid map - binary map with filtered obstacles, unknown and inaccessible areas
The ''grow_obstacles'' function in the ''HexapodExplorer.py'' class has a following prescription:
def grow_obstacles(self, grid_map, robot_size):
""" Method to grow the obstacles to take into account the robot embodiment
Args:
grid_map: OccupancyGrid - gridmap for obstacle growing
robot_size: float - size of the robot
Returns:
grid_map_grow: OccupancyGrid - gridmap with considered robot body embodiment
"""
Example obstacle growing for first evaluation scenario is depicted in following figure. From left to right:
* Original map
* Map grown for robot diameter 0.3m
* Map grown for robot diameter 0.5m
{{:courses:uir:hw:map_base.png?300|}}
{{:courses:uir:hw:map_grown.png?300|}}
{{:courses:uir:hw:map_grown2.png?300|}}
++++ Suggested approach (click to view)|
**Obstacle growing** - The robot has non-zero dimensions that represent its embodiment which has to be taken into account when planning. Moreover, the plan execution in real experiments has a limited precision which further enlarges demands on the clearance to the obstacles for the robot to safely navigate the environment. The planning with a large robot in grid environment is complex, therefore the trick is to grow the borders of the obstacles by a diameter of the robot and plan in the resulting map that ensures a 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
++++
----
== Path planning ==
The purpose of the ''plan_path'' function is to plan the path between the start and goal poses on the pre-processed grid map considering planning with 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 function are:
- ''grid_map'' - [[courses:uir:hw:support:messages|OccupancyGrid message]] - pre-processed grid map representation (contains 1 and 0 only)
- ''start'' - [[courses:uir:hw:support:messages|Pose message]] - start robot pose
- ''goal'' - [[courses:uir:hw:support:messages|Pose message]] - goal robot pose
The function returns:
- the [[courses:uir:hw:support:messages|Path message]] with the found path or ''None'' when the path cannot be found. It is supposed that the path contains only 2D coordinates and not the heading, i.e., 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 ''HexapodExplorer.py'' class has a following prescription:
def plan_path(self, grid_map, start, goal):
""" Method to plan the path from start to the goal pose on the grid
Args:
grid_map: OccupancyGrid - gridmap 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 correctly planned path with A* algorithm for different sizes of of robot is visualized in following figure. From left to right, the planning for:
* Original map
* Map grown for robot diameter 0.3m
* Map grown for robot diameter 0.5m
{{:courses:uir:hw:map_grown_plan.png?300|}}
{{:courses:uir:hw:map_grown_plan2.png?300|}}
{{:courses:uir:hw:map_grown_plan3.png?300|}}
Note, the task is about planning in robotics and not about implementing A* planner again. 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 graphs and recognizing sub-optimal paths, wrongly set heuristics, etc.
++++ 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 shortest path)
* ARA* (Any-time repairing A*)
* D* lite
* Floyd-Warshall
Pay attention to the planning algorithm used. If you decided to implement * planner it is assumed, that the path is optimal!
++++
----
== Path simplification ==
Path simplification is usually done by excluding navigation points from the path, that are not necessary in the sense that the robot does not have to visit them precisely. A typical approach to trajectory smoothing is to connect the neighboring segments one by one using straight-line segments (using Bresenham line algorithm) up to the point where the straight-line segment collide with an obstacle (grown obstacle) and then follow with another straight-line segment.
The input parameters of the function are:
- ''grid_map'' - [[courses:uir:hw:support:messages|OccupancyGrid message]] - pre-processed grid map representation (contains 1 and 0 only)
- ''path'' - [[courses:uir:hw:support:messages|Path message]] - path to be simplified
The function returns:
- the [[courses:uir:hw:support:messages|Path message]] simplified path with the processed path. The path is returned in the world coordinates!
The ''simplify_path'' function in the ''HexapodExplorer.py'' class has a following prescription:
def simplify_path(self, grid_map, path):
""" Method to simplify the found path on the grid
Args:
grid_map: OccupancyGrid - gridmap for obstacle growing
path: Path - path to be simplified
Returns:
path_simple: Path - simplified path
"""
The result of path simplification may vary, but an exmple of correctly planned and simplified path using the suggested approach is visualized in following figure. From left to right, the planning and simplification for:
* Original map
* Map grown for robot diameter 0.3m
* Map grown for robot diameter 0.5m
{{:courses:uir:hw:map_grown_plan_simple.png?300|}}
{{:courses:uir:hw:map_grown_plan_simple2.png?300|}}
{{:courses:uir:hw:map_grown_plan_simple3.png?300|}}
++++ Suggested approach (click to view)|
* The path simplification is very simple, hence, the suggested approach is described in the assignment. With the pseudocode it can be written, e.g., 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 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 collision
path_simple append previous_pose
break
++++
----
=== Evaluation ===
The evaluation focus on correct behavior of the above described robot navigation pipeline.
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import sys
import os
import math
import numpy as np
sys.path.append('hexapod_robot')
sys.path.append('hexapod_explorer')
#import hexapod robot and explorer
import HexapodRobot as hexapod
import HexapodExplorer as explorer
#import communication messages
from messages import *
#pickle
import pickle
ROBOT_SIZE = 0.3
if __name__=="__main__":
#instantiate the explorer robot
explor = explorer.HexapodExplorer()
#load scenarios
scenarios = pickle.load( open( "scenarios.bag", "rb" ) )
#evaluate individual scenarios
for gridmap, start, goal in scenarios:
#obstacle growing
gridmap_processed = explor.grow_obstacles(gridmap, ROBOT_SIZE)
#path planning
path = explor.plan_path(gridmap_processed, start, goal)
#path simplification
path_simple = explor.simplify_path(gridmap_processed, path)
#plot the result
def plot_path(path, ax, clr):
""" simple method to draw the path
"""
if path is not None:
poses = np.asarray([(pose.position.x,pose.position.y) for pose in path.poses])
ax.plot(poses[:,0], poses[:,1], '.',color=clr)
ax.plot(poses[:,0], poses[:,1], '-',color=clr)
fig, ax = plt.subplots()
gridmap.plot(ax)
plot_path(path, ax, 'r')
plot_path(path_simple, ax, 'b')
plt.xlabel('x[m]')
plt.ylabel('y[m]')
plt.axis('square')
plt.show()
**FAQ and known issues**\\
- The length of the found solution may vary depending on the conversion between the map coordinates and world coordinates. In the reference solution the Python's simple ''int()'' conversion is used to transform to the grid coordinates.