Robot Motion Planning—Home Work 5

 

Q) Implement visibility graph for a two-dimensional polygonal robot and work space.

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

                                                                                                                             

 

 

 

 

                                                  

                           Success (Click here for the Movie)                                                 Failure (Click here for the Movie)

 

                 

Note: Right-Click and Save the movies if you have problems in running them directly. 

 

My Implementation is as follows:

 

· I have assumed an open workspace in this implementation. The user can choose the number of polygonal obstacles, their shapes, robot’s polygonal shape, start and goal positions. I have assumed that all the obstacles are convex.

 

· The first step in the implementation is constructing the configuration space obstacles. It is done using Star Algorithm, which is described below:

· The robot’s and obstacle’s reference points are chosen.

· The inward angles for the robot edges are calculated, by moving anti-clockwise from one vertex to the next and adding 90 degrees to that angle. The <inward angle,edge> pair is saved in an angle array.

· The outward angles for the workspace obstacle edges are calculated, by moving clockwise from one vertex to the next and adding 90 degrees to that angle. The <outward angle,edge> pair is saved in the angle array.

· The angle array is sorted on the basis of angle and the first starting pair is found by the smallest angle in the array.

· The obstacle reference point is the first configuration space obstacle vertex.

· The chosen edge vector is added to the current configuration space vertex to get the next configuration space vertex. The next pair is retrieved from the array and this process repeats until all the (angle,edge) pairs are evaluated.

 

· The second step in the implementation is the visibility graph generation. It is implemented as follows:

· An array of all the configuration space obstacle vertices is created. Both start and goal points are added to the array.

· For every point in the array, the visibility of every other point in the array is checked. This is done by checking whether there exists a point on the line connecting the two points of interest, which lies within any of the configuration space obstacles. If it does, then the points are not visible to each other. An adjacency matrix is created based on this information, which will be used in path planning.

 

· The third step is the path planning using A*. The implementation is as follows:

· The adjacency matrix generated in the second step gives the map of the configuration free space.

· A 2-D version of the A* implemented in Homework 4 is used here to find a path to reach the goal.

· The heuristic used is Euclidean distance and the path cost is also calculated using Euclidean distance.

· If a path exists, it is found and plotted.

· If there is no path existing then it reports “NO PATH FOUND”.

· The figures and movies provided above show a success and a failure case.