In this video, we will see how to play an obstacle free parts for mobile robots that the RRT algorithm using a reference example in MATLAB and navigation tool box. This is the MATLAB 2020 B version that I'm using here and you must have navigation tool box installed to run the example that we are going to use. Let's see how to open the example first. One way to do that is to type dock nav in the command window where nav is the short form for navigation toolbox. This takes us to the navigation tool box documentation page or the help page where you can find description for all the available planning algorithms functions.
And we'll look at motion planning examples to open this one. So we can directly open live script. And this automatically changes the part to the folder where this example is stored. Along with the helper functions and the map file required to run this example. Let's start with loading the occupancy map of an office environment. So the load function loads the map file into an occupancy map. We can see that the occupancy map is loaded with different parameters like the world limits, grid size and resolution. The show function visualizes the map. Next we are providing the start and goal states in the form of xy and theta. This should be a value that lies inside the occupancy map.
Now, we plot the start and goal on the map as well as we are also plotting the robots heading using the theta from the start and goal states. So now we have the map plotted with the stark goal and the headings. Next we're defining the state space by using the state space Dubin's object. It takes the bounds of the occupancy map as input to know the boundary limits inside which the states should be sampled. Since the state space also need the information on where to sample the next state that means how far the consecutive states should be, we need to provide turning radius of the robot as a parameter to the state space. The turning radius will depend on what robot we are using. Here we are setting the minimum turning radius as 0.4 meters.
Now we can create a state validator. So validated occupancy map object we can create this object with specifying the state space as SS. Then we can set the map property to the loaded occupancy map object or grid. we need a metric that measures the distance between each state in the state space to check for collision. Let's set a validation distance of 0.05 meters. This distance discridizes the path connections and checks obstacles in the map at each 0.05 meter interval. So now we will create the path planner and we will set the parameters for the planner.
We can set maximum connection distance, which is the maximum length of motion allowed in the tree. The lesser the maximum connection distance, the more number of states will be sampled and connected to each other. Let's set it S2 for now. And if we don't set any value for S2 planner will assume the default values. We also need to set the maximum number of iterations for sampling states. If we said this as a lower number the planner can stop before exploring the entire space and eventually couldn't reach to the goal. The goal reached function now is a callback function that tells the planner if it has reached to the goal or not. This is also an inbuilt parameter but we can also customize it as per the requirement. So here we use a helper function to customize it to check if a feasible part reaches the goal within a set threshold value of 0.1.
Now we use plan function that takes the planner object as input along with the start and goal point. And it returns to outputs. The first output is that object which restores all the robot state that leads the robot to
the goal state. It is stored in the nav but object. We can go to the path object variable and see all the states. The second output includes solution information such as number of nodes that were explored to find the part or number of iterations and the tree data. The tree data is useful to plot the tree and that object is used to plot the path in the next section. So now we plot the results of the occupancy map using the available information from planners output. In order to show the path in a uniform way, we can interpolate the path. So here we are. We have a resulting map with a tree as well as a plan pad from start to goal and we can also see the elapsed time.
Now let's change some parameters and see how this changes the part and the solution info. You can try changing maximum connection distance let's reduce it 2.5. We will notice that the tree is more dense now since there are more number of nodes explored and we can see that the number of iterations were also increased. This led to an increased amount of elapsed time as well. Now, if you're wondering what if I have to compare results of two planners such as RRT and RRT*. Do I need to implement it all from scratch? The answer is no. If it is a planner that can be implemented in the same infrastructure using state space and state validators. For example, you can just replace planner RRT with already RRT* and you can see the results immediately.
For a planner which uses different parameters you will also have to specify those parameters. But the overall idea will be the same. Now let's see the customization part of the example. Until now, we used the building stage space for houben's motion Dublin's motion is a left right and straight car-like motion. But what if your robot can only make left turns? We can customize the building stage space for Dubin's motion to become a one sided state space. For this, we have the example helper function here that shows how to do that.
Since the built-in state space only takes the bounds as inputs, we define a constructor of the class inherited from the state based Dubin's class. And we can add another input argument. This input argument limited to turning direction to either left or right based on a Boolean property. If the property go left is set to true, the Triton's part will be disabled and the robot will make only left turns. And if the property go left is set to false, the robot will only turn to right.
The rest of the process is the same as we saw above. But let's run this and see the difference. We can see that the tree is even more denser because we need to find the path with only left turns now which needs more sampling and more number of iterations. This makes the elapsed time for this type of planning even higher. The execution time for any such planning can be further improved with optimization techniques.
Finally, we can summarize the results of different test cases we solved. As I mentioned earlier, RRT* performs the best with lesser connection distance that is the value of 0.5. The execution time as well as the number of nodes explored increases when we restrict the state space to left turn only. This was the case when we customized the Dubin's state space. RRT with maximum connection distance value of two performs great as well. But the time and number of nodes increases when we decrease the value of max connection distance to 0.5.
So this is how you can quickly implement and summarize the results of that planning algorithms with MATLAB and navigation tool box. The same example that we saw in MATLAB can be deployed on a real robot using MATLAB coder. You can also generate Ross node from the MATLAB code using Ross toolbox. This video shows an interactive app built in MATLAB that takes and LiDAR data from a robot. And builds a map of the environment. And then we can use our algorithm to plan a path in that
environment. We further see how the robot is localized and it follows the path to reach a destination goal to deliver this book.
So finally, I would like to thank you to watch this session where we covered some concepts of motion planning algorithms and how you can use MATLAB, and navigation toolbox to implement them. In the next part of the session, YJ will walk us through the example on our RRT motion planner for robot manipulators.
