TASK 4: GRADIENT PATH PLANNING

 INTRODUCTION: 

The objective of this task is to implement the logic of the Gradient Path Planning algorithm. 

The GPP consists of selecting a destination and finding the shortest path to it. To do so, I have decided to implement GPP navigation algorithm following the next steps: 

SET VALUES:

First of all, to start setting the values it is necessary to get the map image (which is made of 400x400 pixels), the target converted from the world into the map coordinates as the world is made of 500x500 pixels and the car coordinates also converted from the world into the map. 

As in opencv white is 255 and black is 0 , it is easy to increment values from 0 rather than decrementing values so, I have decided that the  new map will have inverted weights : when in the original map  there is a black pixel,  now it is white and viceversa.  To fill the map values, I have created a grid of 400x400 pixels filled with 0's (black) .

To initially fill the grid, I use the logic explained before: if a pixel from the map is black, in the grid will be white and viceversa. Then, to set cells securities to avoid car going closer to walls, I have set 3 cells with values around 255 to simulate walls. I know it would be easier to set those values different but I was not able to mix those values with the algorithm that creates the path. 

After initially setting the values that I am going to work with, it is time to set the values for the path. 

I start the bidimensional list from the target with an init weight of 0; then, I get the shortest distance from the values that are inside of the list and iterate along its neighbours and if one of them is 0, set the new value with is its distance + cell value. If you reach to the car position in the map, means that it is the end of the wave. 

Not to forget that, it is not recommended to just finish in the car pose, so around -20 to 20 pixels to the car and if those cells have 0 value, set them to 255 to make easier the creation of the path. 

I show the grid with the wave and bounds.

CREATE PATH:

I have obtained again the car coordinates in the map and I take from the grid the neighbours of the car pose and check the ones with the minimum distance and so, I insert them  into the list used for the path. It stops when it reaches to the target.

Now, the path obtained is backwards due to its first pose is the target so it is necessary to invert it. I show the path (which is green by default) to have a visual guide.

I tried to fix that in intersections, in the path appears peaks and I found it impossible to fix it. The only thing I could do is increasing the security cells to decrease the size of the peak. 

NAVIGATION:

I have created a function used for eliminating duplicates in the array to avoid bad navigation.

The navigation logic I have decided to implement is the following: 

Until the car does not arrive to the target, I subtract each coord point with I the car coordinate in the map and that differences I store them and set some cases: ahead, back, ahead-right, ahead-left, center-right, center-left, back-left, back-right.

Down below  you can see what I am trying to explain: 

          

            The 8 cases I have decided to implement

     If previous direction is different from the current direction, means that the robot must change its orientation to go to the correct direction. With those 8 possible cases I have defined, by experimentation I have set different bounds to each case:

Besides, depending where the robot is and needs to go to a different case, turn in one direction or another. When both orientations are the same, start moving using a constant linear velocity, makes a sleep and set that velocity to 0, if not, it makes weird things.

CONCLUSION:

I know it works but I wish it would go faster.

VIDEO:

Down below you can find a video which its speed is modified from 1x to 2x because the video was too long and I could not save that. Sorry about the inconveniences. 

- Complete planification: 

I hope you liked it!

TASK 3: LOCAL NAVIGATION WITH VFF

 INTRODUCTION: 

The objective of this task is to implement the logic of the VFF navigation algorithm.

The VFF consists of a type of local navigation with repulsive force in obstacles and attractive force in destination. Calculate robot situation every time. To do so, I have decided to implement VFF navigation algorithm following the next steps:

PROCESSING IMAGE:

In this section just to mention that I use getImage() and showImage(img) to see what is watching the pilot and it has helped me to debug some errors.

 FORCES: 

• ATTRACTIVE FORCE VECTOR:

As I said in the introduction, this algorithm needs an attractive force in destination. To get that I followed the next steps:

By the functions given, you can know how to get the coords (x and y) of the target we are looking for: current_target.getPose().x and current_target.getPose().y .

Besides, you can also know the coords  and the orientation of the robot (these functions are given in the statement): HAL.getPose3d().x, HAL.getPose3d().y and HAL.getPose3d().yaw.

But, it is necessary to transform those absolute coordinates into relative to know the coords of the target respect to the robot. It has dealt using the function called: absolute2relative (that function is given in the statement but return x, y instead of x and y because the second one just returns x).

At this point, we have the coords of the target respect to the robot, but, how can we get the distance to that point? 

It is calculating the vector's module:

Example:

Target's position (2, 3) (obtained from the function absolute2relative)

It has the following formula: np.sqrt(x² +y²)

 

 

The distance from robot to the target in this case is the 3.61.

This image has been done using Geogebra.  

 

Now, I assume that if the distance if less than 3, set as reached destination and I get the following target.

Also, I take into account that if the target is too far away (farther than 8) , set a constant value for the vector. I make this to make the attractive behaviour. 

Let's remember how attraction works: 

    - Far -> constant 

    - Close -> decreases

So, when it is closer than 8, I use coords target.

The attractive force has an alpha value which multiplies the attract vector. That alpha value is 0.7 which was obtained by experimenting. But, that was not enough, I could not control the force so I had to set some boundaries to eliminate that inefficient execution. That value is always the same and to behave as the VFF, it is multiplied by the alpha. 

That boundaries are set in the four possible situations ( x and y positive, x and y negative, x positive and y negative, x negative and y positive).

The only thing left to do is to represent the force in the screen. I decided to use the green arrow.  

• REPULSIVE FORCE VECTOR:

Now, the algorithm needs a repulsive force in the obstacles.  To do so. I have done the following:

 

Let's remember how repulsion works: 

    - Far -> decreases

    - Close -> increases

In this part, I need to detect the obstacles so I need the laser and process its values. I used getlaserData() and parse_laser_data(laser_data) to get an array of each laser_distance and laser_angle.

 

In parse_laser_data(laser_data) I have decided to use the one given in the statement but I changed when calculating the angle  i-90  to just i just because, is the same function given when using the vacuum and those values are familiar with me.

Now, let's vectorize the laser and that function I obtained from the vacuum but I have just modified the distance because the value was so big.

That vector mean should be modified to make a repulsive force.

 

To do so, I have created the function called get_repulsive_force(laser) which modifies the vector to work as a repulsive force; that is making a 270º rotation.

 

Example: 

Using the same point as the other example, its rotation will be:

             

 
 This image has been done using Geogebra. 

 

The repulsive force has a beta value which multiplies the repulsive vector. That beta value is 8 which was obtained by experimenting.

 

The last thing to do is to represent the force in the screen. I decided to use the red arrow. 

 

• RESULTANT FORCE VECTOR:  

This vector is just the sum of the attractive and repulsive vector and births the equation of VFF:

   

In the screen it is represented with the black arrow.

VELOCITIES:

After getting the resultant force, the last thing to do is translating that resultant force into velocities. I decided to make a conversion from cartesian to polar coordinates. 

That polar coordinate are the velocities. polar[0] = linear and polar[1] = angular. 

CONCLUSION:

The f1 takes about 5 mins to complete the lap as you can see in the video, down below.

VIDEOS:

- Complete lap video:

-Gazebo simulation:

 

I hope you liked them!