Description Assignment 3: Planning

Objectives

Planning is one of the core functionalities of an autonomous system. There are many varieties of planning, from high-level planning where the systems plans how to and in which order to best accomplice its tasks, to more low-level planning, such a path planning from place A to place B. In this assignment we will look at path planning to extend the autonomy of our TurtleBot!

Prerequisite

First make sure that you have done the assignments setup.

Submission

You submit your assignment at Assignment 3: Tasks which also points you to the report template with questions to answer.

 

Background

One of the key references in planning is the freely available book by Steve Lavalle Links to an external site.. If you wonder about something it is quite likely described there.

Planning can be done at many different levels of abstraction. In this course we will focus on path planning, which means we are looking for a collision free path from a start point to an end point. This course is about autonomous systems and therefore the application would be, for example, a robot moving. However, you could also make a plan for how the people on the assembly floor should move a part to be able to get it into a half built car.

Before executing the path we need to generate a trajectory, i.e. assign speed at each point. Slight variations of more or less the same problem are referred to local planning and trajectory generation. The path that is output by the the path planner provides the lower levels with sub goals and the local planner works out how to actually move. This often also includes taking into account the presence of obstacles that were not considered during the path planning (people for example). A name for local planning that makes this more explicit is collision avoidance.

Sometimes one has an intermediate step between path planning (creating a global plan) and executing this plan with the purpose of modifying the path so that it can actually be executed. The latter you should be able to see clearly in the tasks below.

Motion planning and path planning are often used to mean the same thing but what that thing is varies depending on who is talking. Looking for a formal definition most would agree that path planning is about finding, as the name suggests, the path from A to B. This is what Google Maps shows you when you ask for direction. Take a look at this page Links to an external site. goes into a bit more details about the difference between motion planning and path planning. This presentation Links to an external site. by Mathworks also describes planning at different levels.

At a level of abstract above path/motion/local planning you have task planning or behavior planning. The task might be the same, i.e. going from A to B but here one needs to work out how to for example sequence tasks like going up a stair, open a door, pick up a key. As you can imagine depending on what you consider to be a task you can do task planning at quite low level but also very high level. For an example if this look at the book and examples at the webpage for the book "Behavior Trees in Robotics and AI" Links to an external site. by Michele Colledanchise and WASP faculty Petter Ögren.

In the assignment we will formulate the path planning problem as a Search problem. We will look at two families of approaches,

• Discrete:
  • These methods rely on discretizing the world. In our case we will look at a grid representation where a solution connects grid cells from the start to the end.
  • Since the world is discretized there will be a trade-off between how accurate we represent the world and the costs of the search (higher resolution --> more cells --> longer time).
  • Struggles when the number of dimensions increase.
  • In the example below (from Wikipedia) we see how the search progresses in the grid from search to end. How the search is expanded is defined the be the specific search algorithm used and how certain properties are defined.
    
• Sample based:
  • These methods draw random samples in the state space (for example different robot positions) and tries to connect these to find a path.
  • One of the key operations when using this approach will be collision checking to answer the question "can the robot be at the position?". If it cannot we cannot place a sample there and we cannot connect two samples if there are collisions on the way between them.
  • Sampling based method are able to solve very complex search problems and define the state-of-the-art today.
  • Just like the discrete methods results in paths that need post-processing before being executed the same is foten true for the sampling based methods as you will see in the assignment.
  • The example below from Maurice Rahme Links to an external site. shows how the search progresses in this 2D example of a method that is able to direct the search towards the goal more than an uninformed method would (like what you will explore).
    

Methods covered in the assignment

• Breadth-First search (BFS)
  • You find a description of the algorithm at Wikipedia Links to an external site.
  • BFS looks at the problem as searching through a tree. If you consider the start position to be the root of the tree the next level in the tree consist of all cells that you can reach from there.
  • Breadth-first search refers to how this tree is traversed to find a solution. Each level in the tree is expanded before the next.
• Dijkstra's algorithm
  • In BFS we expand the nodes in the tree (the cells in the grid) level by level. It is the level that determines which node is expanded next. This corresponds to considering the cost to move from each cell to be the same. In Dijkstra's algorithm we can have different costs between different nodes, to model for example that it might be more expensive to climb a stair than drive on flat ground even if the distance is the same.
  • The algorithm expands the node with the lowest cost first. If the costs for each step is 1 the result should be very similar to BFS.
  • See the example at Wikipedia Links to an external site..
• A*
  • Both BFS and Dijkstra's algorithm are so called uninformed methods. This means that they expand the search only based on the level of a node (BFS) or the accumulated cost (Dijkstra's algorithm) but disregarding the location of the goal and any information we might have on how to get there.
  • A* addresses this by expanding the node that stands the best chance according to a heuristic to be the shortest path.
  • Each node, $n$, that is considered to further search is given a score, $f(n)$, that is the sum of the cost to get from start to $n$, $g(n)$ which is something we know and an estimate of the cost to go from $n$ to the goal $h(n)$. That is we expand the node $n$ with the lowest $f(n) = g(n) + h(n)$. The function $h$ is called the heuristic and has to fulfill certain conditions to assure that the result is optimal.
  • The algorithm is described on Wikipedia Links to an external site.. The illustration above in the introduction to discrete methods shows A* in action.
• Rapidly exploring Random Tree (RRT)
  • Introduced by Steve Lavelle here Links to an external site. and described on Wikipedia Links to an external site..
  • It is a (some would say THE) sample based path planning algorithm.
  • The basics of the algorithm is simple
    • You want to connect the start and the goal with a path
    • You put a node at the start
    • Iterate over
      • Sample random positions in the world and try to connect them with already existing and close by nodes.
      • Terminate if i) a path can be found in the generated tree from the start node to the goal OR ii) you exceeded the allocated number of iterations, in this case RRT does not return a path.
• RRT*
  • An extension of RRT where the tree is rewired to find increasingly better (shorter in our case) solutions.
  • RRT* is introduced in the paper by Sertac Karaman and Emilio Frazzoli Links to an external site.. In a later paper Links to an external site. some of the proofs where revised.
  • It is asymptotically optimal, i.e., the cost of the returned solution converges almost surely to the optimum.

 

For the discrete methods operating on grids some additional concepts are needed

• Euclidean distance
  
  • This is what most people think of when you say distance, i.e., the straight line distance from point A to B.
• Manhattan distance
  • The name comes from how streets are arrange on Manhattan with almost only streets going either north/south or east/west forming a grid. It is sometime called taxicab distance.
  • The distance from a point A to B in such a grid world is then the sum of street segments you need to travel. If you travel only north/south or only east/west there is no difference between the Euclidean distance and the Manhattan distance but as soon as you change in both $x$ and $y$ there will be a difference.
  • Example: Assume all buildings have the same size. If you need to go from the north-east corner of a building to the south-west corner the Manhattan distance would be 2 (as the car has to drive) whereas Euclidean distance would be $\sqrt{2}$.
  • Look at the examples on Wikipedia Links to an external site..
• 4/8-connectivity
  • When we search through a grid we need to determine which cells that we can move to next.
  • When using 4-connectivety we say that we can only travel north/south and east/west.
  • When using 8-connectivety we also allow traveling along the $45^\circ$ diagonals. That is each node in the search is connected to 8 neighbors.
  • Note that when we use 8-connectivety the cost (assume that encodes distance) is no longer the same when going up/down/left/right (1) and on the diagonals ($\sqrt{2}$).

 

 

 

Running the Assignment

You will need three terminals for this assignment. In the first terminal, open RViz with the supplied configuration file:

rviz2 -d ~/ros2_ws/install/assignment_3/share/assignment_3/rviz/planning.rviz

In the second terminal, open rqt  reconfigure:

ros2 run rqt_reconfigure rqt_reconfigure

In the third terminal, run the path planner:

ros2 run assignment_3 path_planner

If you now press refresh in rqt reconfigure, path_planner should appear. Press it and you will see the parameters you can configure (see image below).

If you hover your mouse cursor over the parameters you will see a more detailed description (see image below).

Your RViz window should look like the image below.

You can continue the assignment if everything looks okay on your end.

The image below explains what you see in rViz and how to define the start and goal positions for the path planner. 

 

The video below gives an introduction to the interface we developed for you to be able to explore the different algorithm.

 

Task 3.1

In the first task, we will look at different path planning algorithms, such as breadth-first search, Dijkstra's algorithm, and A*. We have supplied you with a fully implemented breadth-first search; however, the latter two are not complete and therefore you will have to finish the implementation of them.

Try different settings in rqt for breadth-first search ("bf") regarding resolution and maps (id=0..4) and experiment with adding new walls. 

Task 3.2

When selecting Dijkstra's algorithm ("dijkstra") or A* ("astar") you will notice that the path is not what you would probably expect. This is because the cost function is not implemented for the two algorithms. Your job is to implement the cost function by editing that function in the file ~/ros2_ws/src/wasp_autonomous_systems/assignment_3/assignment_3/dijkstra.py and ~/ros2_ws/src/wasp_autonomous_systems/assignment_3/assignment_3/astar.py. When you open the files you will see multiple places marked with # TODO: Fill in, you only have to care about those in the cost function for now. You can use the same cost function for both. You know that your cost function is OK when the solution looks like you saw above.

For now, 4-connected grid planning is performed (see image below); can you take advantage of this in the cost function?

Task 3.3

A* uses a heuristic function to inform the search towards the goal. You task here is to implement the function heuristic in the same astar.py file as you worked with in Task 3.2. When you have a correct heuristic function you should see that the visited cells are fewer. A simple test is to put the start and goal on a straight axis-aligned line.

 

Task 3.4

Extend Dijkstra and A* so that the search uses 8-connectivity. You do that by extending the function neighbors in the two file. You can use the same code in both files. Remember to also also alter the cost (both methods) and heuristic (A*) if you made it specific to 4-connectivity.

Task 3.5

The paths that you generated so far have been planned very close to obstacles. The planning algorithms treat the robot as if it was a point. To correct for this we need to inflate the obstacles so that they appear larger to the algorithm. In our case we have a circular robot that is has a radius of 0.6m, which means that if we inflate the obstacles with 0.6m a path that does not collide with the inflated obstacles will correspond to a path where the real robot stays clear of the real obstacle. Think about what to do if the robot is not circular. 

You activate the inflation of obstacles in rqt by checking the box map.inflate. The inflation will be marked in orange. The image below shows an example from map with id=1.

Change to map id=2 and study the result of planning with and without inflation.  If you look for a path without inflation the path would suggest incorrectly that the robot can move through the opening. 

Change to map id=3 and the opening is now larger and the robot will fit through even when you inflate the map to account for the size of the robot.

 

Task 3.6

Change the planner to RRT ("rrt" in rqt). Investigate how changing the number of samples and max edge length influence the solutions. 

How does the grid resolution influence the solution?

Hint: You can toggle the planner.replan checkbox to make the planner replan.

 

Task 3.7

Change the planner to RRT* ("rrt*" in rqt) and do the same as in Task 3.6. Do you notice a difference in the quality of the solution if you check the box to stop sampling as soon as a path is found? Why?

 

Extra task (if you miss the deadline)

One optimization you can do in RRT/RRT* is to add a check if you can reach the goal from a sampled node. Implement this for RRT or RRT* and investigate how it changes the performance.

Submission: Include your results in the report by extending the template appropriately.

 

Submission

This assignment is reported here Assignment 3: Tasks