Lecture 12: Sampling Based Planning

IIT Kanpur July 201
20 followers
2 Video Views·Dec 1, 2023
Hello, and welcome to lecture number 12 in the course Robot Motion Planning.
The last class we looked at cell decomposition methods where you take the whole workspace
and you divide it into small, small cells, find the connectivity between the cells and
then try to find the path from the initial point to the goal point.
Now, if the workspace is very large or they are complex obstacles, then what would happen
the cell decomposition method will take a lot of, may take a lot of time because you
have to explore the full area, right, the full workspace.
So in such cases, what we do is we use the next method which we will be looking at today
that is called sampling based methods.
That means we do not need to search the whole workspace, but we just need to search in areas
which is of interest to us or search in such a way that you can go from the initial point
to the goal point without searching the whole workspace, okay.
So today we will look at sampling based methods, okay.
So today we start off with sampling based methods and as I was mentioning that in such
methods we do not need to explore the whole workspace because it takes a lot of time,
okay.
Now, also in case of systems having higher degrees of freedom, the very large degrees
of freedom.
For example, look at this humanoid robot on the left hand side.
This robot is picking up an object from under the table.
Now how many degrees of freedom does it have?
It can have very large degrees of freedom, somewhere around, say, maybe 50 degrees of
freedom.
Now if you have 50 degree of freedom configuration space, obviously you understand how complex
that is going to be.
You may not be able to even draw it in three dimensional space and then to search for a
path to take the hand and catch that object and take it up, okay, there would be infinite
solutions again.
So if you need to do that, this can actually become a problem which you cannot solve by
other methods, by drawing configuration space and things like that.
The complexity increases exponentially with high degrees of freedom systems, that we have
seen.
So we started off this course by talking about very simple mobile robot, circular mobile
robot or rather a point robot and the point was moving around in space and avoiding obstacles,
right.
But as the degree of freedom starts going up, the complexity starts increasing and it
can increase exponentially, okay.
Now building C space for high degree of freedom systems is very complex and for example, in
the humanoid robot case here, it will be virtually impossible to draw this and then
exactly figure out to find a path to catch that object and take it up.
So this is one example.
There can be other examples.
Let us have a look at another example which is what we call a snake robot or a hyper redundant
manipulator having very large degrees of freedom, so maybe 10 degrees of freedom.
So you can imagine that you have a robot here, very often called a snake robot and the snake
robot is having 10 degrees of freedom.
So here there is one more degree of freedom, it is the end effector and it has to go inside
some position, maybe there is a hole here, okay, there is an obstacle and there is an
object which is kept here on the table, okay.
So let us say this is an object which is kept on the table.
Now it has to go inside this hole, okay, from here and then go and catch that object and
take it out.
Now you have 10 degrees of freedom.
Again this is the case where you have such high degrees of freedom that it will be very
difficult to draw the configuration space and actually try to figure out what is the
best way to go.
Say for example, it can go in from here like this, catch this object, okay, position itself
accurately and then come out, okay.
But that will be a complex task if you are thinking in terms of C space.
So this is another example where, which will be very, very difficult in case of C space
based planning, okay.
One way to go ahead for such problems is to simplify the problem, okay.
Now exhaustive search covering all possibilities is not required.
For example, in the previous case we looked at the humanoid robot which has 50 degrees
of freedom.
Now there will be infinite solutions, okay.
Now if you want to have an exhaustive search, you should look at all the possibilities and
then try to find which is the best possibility.
Now that will be time consuming and may not be even possible in finite time, okay.
So exhaustive search is not required, okay, that is one.
So we can search in a lower dimensional space and we can search in a lower dimensional space
and limit the number of possibilities, okay, either by adding constraints or by reducing
the volume of the free space.
Now if you do that what will happen is you are going to sacrifice what is of the completeness
in terms of not covering, not having a exhaustive search, okay, so no exhaustive search, okay.
So this is something that will happen.
The second is you may sacrifice optimality, okay, that your solution may not be optimal
in the global sense.
It could be locally optimal but need not be globally optimal because you are only looking
at a finite set of solutions, okay.
But by doing this you can simplify the problem.
Now what is the basic way by which we perform this problem is that we use the sampling based
method and as the name indicates we randomly explore a small subset of possibilities while
keeping track of progress.
So for example, let me draw a workspace here, okay, this is my workspace and these are obstacles
let us say, these are obstacles and we want to go from one point to another point, we
want to go from this point, initial point to goal point, okay.
So one way of searching for this is instead of looking everywhere or searching in the
whole space I do a sampling based search, okay.
Say for example, from the initial point I go to the next point which is here, the next
point here, next point here, okay.
So I sample these points, possible points, say for example a point is not possible inside
the obstacle so there is no point there.
So I sample a few points and then I make this network of paths again, okay.
So these are my sample points and I can see that I can go from one point to another point
by sampling in this space.
So what has happened is I have not gone here, I have not gone there, okay.
So I have randomly explored a smaller subset of possibilities.
Now obviously it has to be directed, okay.
It will be, otherwise it will become, if it is completely random it can go in any direction.
So if you know the goal is in this direction or if you know the obstacle is this side then
you can sample accordingly.
We will see this as we go along.
So first of all you can randomly explore a smaller subset of possibilities while keeping
track of progress and you can probe deeper into the search tree much earlier than any
exhaustive search can.
Say for example, this is my search tree.
So I started from here, I went there, I went there, I went here, like that, right.
So this is my search tree and I can probe deeper in the search tree much earlier than
any other exhaustive algorithm.
Now we are sacrificing completeness and optimality here.
So there is basically a trade-off between solution quality and run time performance,
okay.
So this is basically how we do our sampling based methods, okay, by searching for collision
free paths only by sampling at points, okay.
So as I was explaining, first we sample points, possible points, that means they are not inside
an obstacle.
Then we connect these points by possible paths and make this network and then try and go
from the initial point to the final point, okay.
Now for example, let us take the case of this 2 degree of freedom arm.
We have 2 degree of freedom arm which is going to avoid obstacles, okay.
Now this is a 2 degree of freedom arm which is going to avoid obstacle.
Let us say there is an end effector there.
Now then it becomes 3 degree, so let us keep it at 2 degree only, okay.
So this is my end point and there is an obstacle which is there.
So this is my obstacle.
Now I want to go from one point to another point without hitting this obstacle.
Now I want to go from this point to this point.
So this is my initial point and this is my goal point.
How many solutions are there?
There can be many solutions.
It can go like this.
It can go from this side, okay.
So these are all possible solutions, right.
But instead of trying to search for solutions everywhere, that means even trying to look
at these solutions which I do not need, I can simply look at solutions near the obstacle
which will take me from the initial point to the goal point.
So basically I can do sampling at these points, say maybe this point, this point, this point,
this point.
These are points I am sampling which are possible.
There is no possibility of a point inside the obstacle, so I do not sample there, okay.
And then what I do is I connect it with paths.
So these are my paths which are connecting and then see whether I can go from one point
to another point, from the initial point to the goal point by connecting to these points.
Now obviously we have, we are not looking at the complete search space.
This may not be an optimal solution, but it is a solution and there might be no solution
also.
So as you can imagine very easily here that if your sampling is not dense enough, you
may miss the initial point and the goal point itself, okay, or if a path exists, you may
not be able to get the path.
Similarly for, this is 2 degree of freedom which is much simpler.
Now if you have a 7 degree of freedom system, it is a very large 7 degree of freedom system.
So we have 7 degrees of freedom and you want to avoid an obstacle, again similar case where
there is an obstacle here and you want to go from one point to another point, I want
to go from this point, its initial point to goal point.
So what I do is I sample at these points which are possible near the obstacle, okay, and
I connect them by paths, making a network of paths and then what we do is we try and
see if I can connect the initial point to the goal point to this network of paths.
And if a path exists, then it can take you from the initial point to the goal point.
Now in this case there was infinite solutions, in this case also there is a very large number
of solutions, okay.
So we are sampling such that we get only a few solutions, not all the solutions.
So there is some good news and bad news as shown in the example here.
So the C obstacles are the obstacles.
So we have sampled at these points and these are the sample points, what we can call nodes
and we connect them by edges or paths and then I connect the start point to this, the
nearest node and I connect the goal point to this nearest node and then I see if I can
find a path like this, like this, like this, like this, like this that will take me there.
Now the good news is that it is probabilistically complete.
We are not constructing the C space.
So for high dimensional space like redundant manipulator or a humanoid robot, this is fine
because we are not constructing the C space.
This can be very easily applied to high dimensional C spaces, okay, because we do not have to
search the whole space.
We can tune our search such that we cover a very small part of the space.
Now it supports fast queries and with enough preprocessing, okay.
Now what is the meaning of fast queries?
For example, if I have a start point here and a goal point there and I want to, the
query that whether you can reach the goal from the start point is very easy.
You just search in this way.
So this is a query and then you can go to the goal point.
Now what is the bad news?
Look at this figure here.
Now I have, these are obstacles, okay, these are all obstacles, C obstacles.
Now I have sampled and after getting the samples, I am getting this network of paths which are
not connected.
Why?
Because there are these narrow passages there.
As you can imagine, I did not sample there.
So there is no connection of path there, okay.
Similarly, there is narrow passage here, narrow passage here.
Now if I want to go from the start point to goal point, I will not be able to go, there
is no connected path.
So as you can understand, it depends on how you have sampled, whether your sampling is
dense enough or not, whether you have covered the whole space.
So the bad news here is that, the good news is that it is faster, it is you do not need
to construct the C space.
But the bad news here is that it does not work well for some problems, especially this
kind of problems where there are narrow passages and you have not sampled there, okay.
Now it is also hard to sample connect nodes and constraint surfaces and no optimality
or completeness.
So this is the plus point and the minus point of this method that we are using.
Now this is basically where the study started off from, higher dimensional planning 1999,
single query and multi query.
So there are these famous papers by Latombe and others, okay.
So single query, for example, you are at the start point here and you want to go to the
goal point, okay.
So in the potential field method basically, which we will be coming to a little bit later,
is that you start going towards the goal and if there is an obstacle, you get pushed away
from the obstacle and you keep going towards the goal, avoiding the obstacle and then finally
you go to the goal.
This is a single query, okay, that means that you started off with one query and went off
in that direction.
In case of multiple query, your start point here, goal point here, you start off in multiple
points and start branching out and then see if you can connect the start point to the
goal point, okay.
So basically there are two ways, single query and multi query, ways of looking at this problem.
Now let us look at a few more, the exact procedure that we are going to follow, the step one
and the step two.
So the step one is, in such cases we have to generate, the first thing is we need to
generate the points, possible points.
So this is my workspace, these are triangles.
So we are going to generate the sample points.
So these are sample points which we are generating, possible sample points, okay.
So a possible sample point means it is not inside an obstacle.
So possible sample point or what we can call a node, okay, is the one that is not inside
an obstacle.
And then what we do is we have a learning phase in which we connect the nodes by edges.
So we are connecting this nodes with edges like this, okay.
Then we are generation of a connected graph of paths, which is a query for this, we are
connecting all the nodes by what we call edges, okay.
So maybe I have one more here, one more here, one more here, like this.
Then so first sample points are generated, then we are connecting them with a set of
paths and then there is a query phase which is going to take me from, suppose this is
my initial point and that is my goal point, okay.
So I want to go from initial point, whether it is possible to go from the initial point
to the goal point, that is basically what we call the query phase, okay, connecting
the start and goal points to the graph of paths and whether you can go from the initial
point to the goal point.
Now so this is the first way in which it goes.
The second is the learning phase after generating your sample points.
So you construct a probabilistic roadmap by generating random free configurations
of the robot and connecting them using simple but very fast motion planner, this is also
called the local planner, okay.
So you generate these points and connect them by means of the local planner.
Now store this as a graph whose nodes are the configuration, so this is my nodes are
the configurations and whose edges are the paths computed by the local planner, okay.
So first we create the nodes, then we store this as a graph such that the configurations,
where the nodes are the configurations and whose edges are the paths computed by the
local planner.
Now let us look at it this way here.
So initially, so we are in terms of a program, let me, let me discuss this in the terms of
a program now.
So at the end of the day, a path planning or motion planning is executed by a program.
So this is a program and how does the program operate, okay, it is a computer program.
So initially, it starts off by this graph, having a graph or a set G is equal to V and
E, V is a vertex and E is an edge, okay, so each of the points is a vertex, so this point
is a vertex and these connections are edges.
So we are making this graph and to start off in this graph, we first make, initially this
is V and E is empty because it is in free space, okay, only the obstacles are there
and we have to start generating this graph.
Then repeatedly a random free configuration is generated and added to V, okay.
So suppose I am here, just for example, I am starting a random new configuration which
is here and this is taken as my node, this is my node 0, this is my node 1, this is my
node, this is another node, this is another node 2, 3 and this is added to the set V here,
okay, so these are nodes.
Now for every new nodes, select a number of nodes from V and try to connect C to each
of them using a local planar.
So I have generated my nodes, I have kept this set, now what would be, V be equal to
this will be coordinates, okay, suppose this is my x-axis, suppose this is my x-axis and
that is my y-axis, then these are going to be node coordinates, right.
If a path is found between C and the selected node V, the edge CV is added to E, okay, that
means if the path exists between this point and this point, then that edge is added to
E here now, okay.
So this is how we are generating a graph, we start off V and E are initially empty.
Now we start off by populating the V, the vertices, okay and vertices mean basically
the coordinates of those points which are possible not intersecting with the obstacle.
So coordinate not intersecting with obstacle, okay and then we connect between the points,
between these two points by edges and we connect this graph and then we keep adding onto this
graph, we start populating the values of V and E, where E are the edges and V is the
vertices, okay.
So now we have all these edges which are connected like this, okay, so we have made our set of
connected graphs or network of paths.
Now how to determine a random free configuration, okay.
So we want the nodes of V to be rather uniform sampling of Q free.
So draw each of its coordinates from the interval of values of the corresponding degrees of
freedom, okay.
So for example, let us take this example of a 2 degree of freedom manipulator, okay.
So I want to have some kind of uniform sampling.
So I take theta 1 and theta 2 here and I can sample theta 1 as from 0 degrees to 1
degree to 360 degrees.
Similarly, I sample theta 2, 0 degrees, 1 degree and 360 degrees, okay and then see
where the end effector is.
So those are my possible samples.
So if there is an obstacle here, wherever it is hits the obstacle, it is not going there,
okay.
So basically if I have a for loop, 2 for loops rather, so one will vary theta 1 from 0 degrees
to 360 degrees, intervals of 1 degree or 2 degrees.
See which sample points are possible and then this is how we get a uniform sampling of Q
free.
Now check for collision both with robot itself and with obstacles.
Now not only must you check for collision with the robot or with the obstacle, but even
with the robot.
For example, the link should not hit the other link, okay.
So that is also an obstacle for itself.
So if collision is free, add to V, otherwise discard.
That is how we make our point.
That is how we sample our points.
So this is the way we go ahead.
Now after this, we have made the points, sample points, we need a local planner.
Now we need to make sure that the start and goal configurations can connect to graph which
requires a somewhat dense roadmap, okay.
We can reuse the local planner, okay, to connect the various points and generate the edges.
That is what the local planner does.
Now let us see how this is done here.
So these are the points I have generated, okay.
So sample points I have generated which are not hitting any obstacle.
There is no obstacle here, okay.
So these are my sample points which I have generated.
Next is where I connect the points by means of an edge.
So this point is connected to this one, this is connected to this one and I proceed that
way.
So I choose a point connected to the nearest point.
So these are points which I am connecting.
By doing this, what we get is a set of paths.
Now the paths may not be connected, okay.
So this is the end of construction step.
So I have had two networks but they are not connected.
Then what we need to do is we need to make the connection between them.
So we use a distance function for making connections between points.
Now D should reflect the likelihood that the planner will fail to find a path, okay.
So choose points which are likely to succeed for connection, okay.
For example, so typically we can look at Euclidean distance on some embedding of C. For example,
let us, let us come back here, let me just go there.
So what we basically do is I want to make my connections now.
So when I am making the connections, from one point we should choose points which are,
which are closer to that point which is likely to succeed, okay and not to choose points
which are far away.
That is how I am going to try to make my connections now.
Now typical approach could be Euclidean distance on some embedding of C. For example, let us
look at, so here I choose points, now I had, I had in the, in the previous case if you
remember the blue lines, they were not connected, now I want to connect them.
So what I do is from here I choose points which are near to that, their Euclidean distance
is near to this point and not far away.
Suppose I choose a point there, then the possibility of connecting this to that is less and is
likely to fail.
So basically I choose points which are near here and then I try and make a connection.
So I have made my additional points here which are near to this and now I have made
my connection.
Now I find that the two graphs, the two blue graphs are connected, okay.
So let us come back here and look at this.
So if you have an unconnected graph like we had, to expand the node C, we compute the
shortest random bounce walks starting from C, means repeatedly pick at random a direction
of motion in C space and move in this direction until an obstacle is hit.
When a collision occurs, choose a new random direction.
The final configuration N with the edge C N are inserted into R and the path is memorized.
What it means is that here you start with a random direction here and C and go until
you hit an obstacle, okay.
Now if you do not hit an obstacle, now this is a possible path, so you add this to your
edge N node graph now again and proceed like that.
Now we will see that there can be different ways by which you can, these branches can
shoot off, okay.
But this is the basic idea of how we make the connections in the, or the network of
paths.
So after you have made your connections, so first we have done sample, that means we created
nodes N, then we got edges.
So we have got edges E and we have made our, we have made our graph, okay, G, a connection
of pathways as shown here.
Now next is the, is the query phase where you have to find a path from the start and
the goal configurations to nodes of the roadmap, okay.
So search the graph to find a sequence of edges connecting those nodes in the roadmap.
So for example, I have my initial point here and my goal point here.
So what I need to do now is find a path from the start and goal configurations to two nodes
on the roadmap.
So for example, I find to this, again I can use Euclidean distance which is nearest and
from here I find this one, okay.
So now we have connected to the roadmap, okay.
Now connect the successive segments, gives a feasible path for the robot.
For example, I go like this, like this, like this, like this, like this, like this, like
this, coming this way, this way, this way, coming all this way and then coming here,
okay.
So this is basically the query phase where I am connecting the initial point and the
goal point to the network of paths already generated and then what we are doing is, we
are finding the final path to take us from the initial point to the goal point, okay.
So as shown here, we have a start point and a goal point, I basically take the start,
connect it to the nearest node, take the goal, connect to the nearest node and then try and
find a path which will take me from there to there, a connected path.
Now path may exist or path may not exist, depending on how you have sampled and things
like.
So this is basically the idea of how the sampling based methods work.
Now obviously the question comes up that in this way, in case of no path, suppose you
cannot find a path, then what?
So maybe the roadmap was not adequate.
Now this is tricky because are you sure that there is no path?
How are you sure that there is no path, that no path exists, okay?
Just because it could not find a path does not mean that there is no path, right?
In this particular case, it is for example here, okay, we have obstacles like this, we
have obstacles like this, we have obstacles like this and so suppose we have a workspace
like this and we have obstacles which are here, here, here and I have sampled like this,
okay?
And I have sampled this side, I have sampled this side, I have sampled this side and I
sample that side and what we have ended up with is a set of graphs which are not connected.
So I have a set of graphs like this which are not connected, okay.
Now I want to go from a start point, I want to go from this point, my initial point to
my goal point here, okay.
Now we can see with our eyes because we have eyes that there is a path but this road map
is not being able to find the path only because the sampling was not good enough, it did not
cover everywhere but that does not mean that the path was not, the path is not there, okay.
So it basically means that maybe the road map was not adequate, adequate means that
it did not cover the whole space, okay.
So now you could spend more time in the learning phase and you could do another learning phase
and reuse R constructed in the first learning phase to sample more of the points, okay.
So this is something that we could do and then try again, okay.
So you can see that one of the disadvantages of this method is that it can fail, okay very
simply because it depends on your sampling now.
So there are some sampling strategies that are used.
So sampling strategies that are used is uniform sampling is easy to implement but it is bad
because it needs to cover all the workspace and needs more time.
Free space need not be sampled densely.
So divide workspace and sample could be one way.
So uniform sampling means you are covering everything, okay which may not be very good
always, okay because it is going to take more time, you are covering the full workspace.
So what was the point anyway, okay and free space need not be sampled densely.
So for example divide workspace and sample uniform sampling, what we can do is we have
the workspace like this.
We can divide the workspace into 2 parts, one part is this side, okay and then I start
sampling at and I divide it into, I take half of this one point, then I have another
point which is half of this, is half here, another half here.
Then I draw a line like this, take the half of here, half of here, half of here, half
of here, half of this line, half of this line.
So basically what I am doing is I am taking, I am dividing this equally and then I am sampling.
So what is happening is that I am getting a uniform kind of sampling as you can see
here, okay.
So basically I just take, divide the workspace and sample, okay into equal halves, this is
something very similar to the cell decomposition methods.
But this is not a good idea because you are ultimately covering the whole workspace which
is actually not required.
Now uniform sampling using grids, so divide the x-axis and the y-axis into equal parts
and take a sample at the center of each grid.
For example, I have the workspace like this, this is my x-axis and this is my y-axis, okay.
So what I can do is I can make it into grids.
So we are familiar with grids, like in the graph paper you have grids.
So basically I divide the workspace into grids.
So these are my grids, okay and then what we can do is, we can take the center of each
of the grids.
So this is something like my uniform sampling, each of those points, suppose let us put an
obstacle also, it becomes clearer, suppose there is an obstacle here.
So these are obstacles.
So basically what we can see is that at the center point of these obstacles we cannot
sampling points which are inside the obstacle, you cannot sample there of course, right.
So that is what we are seeing that we have generated our sample of possible points using
a grid now.
Now the other possibility here is that you can take the intersection also, I could have
taken these points also, then also it is uniform sampling, right, it is more or less the same.
You take the center of the grid or you take the intersection points of these straight
lines, okay.
Then also we have covered the whole space using grids.
Now as we have seen that if there is an obstacle and we start doing uniform sampling, then
I have covered the whole workspace and if the workspace is very large, then what was
the point anyway, because we are supposed to be doing a sampling based method, not cell
decomposition method.
So there are some sampling strategies which are used which will tend to make your search
faster or make the samples faster so that the whole process becomes faster.
So the first method is near obstacles, now obviously that is clear, right.
So if you have a workspace like this where you have obstacles, where you have obstacles
like this, then basically what you are doing is there is, you are sampling here and this
is free space, right.
So there is no point in sampling very closely here because this is all free space, okay.
So what is the point in sampling there?
So a good idea would be to sample near the obstacles because you want to avoid the obstacles.
So I am going to sample near the obstacles such that and not in the free space, okay.
So that is going to enhance my, so this, this, this, this is near obstacles, okay.
So I am going to sample more near obstacles so that my network of paths are actually going
avoiding the obstacles and I do not care about the free space.
So if I am going from here, I can go straight there, I do not need to sample anywhere here.
So sampling near obstacles tends to help you in reducing the number of samples that are
required and also helping you to avoid the obstacles.
Narrow passages, now of course we have seen in the previous case that specially narrow
passage is the one that causes lot of problem that if we have obstacle like this and we
have obstacle like this, okay.
In that case, what we can do is if there is a narrow passage here, let us, let us make
it narrower, okay.
If there is a narrow passage here or there is a narrow passage here, this is my narrow
passage.
So sample more in narrow passages, okay, and less here.
So maybe 1, 2, 3 samples here and narrow passage is the one that I need to sample more, narrow
passage here, narrow passage there, okay.
Visibility based, if you remember the visibility graph method, so visibility based would be
if you are standing here, which vertices can you see?
So based on that you sample.
Manipulation ability based, we will come to that.
So manipulation ability is the ease of motion.
So the direction in which you can move easily, okay, you sample such that you move more in
that direction, okay, and that, we will see that as we go along.
Then quasi-random sampling, instead of doing complete random sampling, you can do quasi-static,
quasi-random sampling in which it can be biased in a particular direction, for example more
towards obstacles or more towards goal, okay.
So these are some of the sampling strategies that are used.
And let me now give you a few more examples as we go along.
So sampling near obstacles is very clear.
It says that if you have obstacles, we can sample near obstacles as I just explained,
okay.
So there is an obstacle here, there is an obstacle here, there is an obstacle here.
So what we can do here is you simply sample near obstacles, okay, and not in the free
space.
That will give you a road map which is going to avoid the obstacles, okay, and will take
you from one point to another point.
So this is a road map which can take you from one point to another point by mainly sampling
near the obstacles.
Then sampling in narrow passages as is shown here, okay.
So if you want to navigate inside narrow passages, then obviously you have to sample inside them,
which means that in this particular case, these are narrow passages.
So if you do not sample in there, what will happen, you will not find a connected graph.
So although a path exists like in the left hand side from the start point to the end
point, you will not be able to find this because you have to go like this and you have not
sampled inside the, okay, narrow passages.
So sampling in narrow passages essentially to find the path that will take you from the
initial point to the goal point by sampling inside narrow passages.
Now sampling inside narrow passages using generalized Voronoi diagram, okay, GVD planners.
This is again by looking at, let us see this is my, these are obstacles, this one obstacle
here, this one obstacle here.
So using GVD, suppose I come to this point, okay, so I can see this one, I can see this
one, so I sample here, okay.
Now there is no vertex here, so I do not sample here.
Essentially it is almost coming back to saying that you are sampling in narrow passages only
by using the generalized Voronoi diagram, okay.
The other is by using deformation retracts.
Now deformation retract is something which we have seen that can you find a path which
is equidistant.
So the retract is what we saw is if you have something like a donut and you shrink, what
will happen if this side shrinks and this side shrinks, ultimately it will become a
one line here with a circle.
So this will come and shrink here, this will come and shrink there.
This surface will come to the circle and this surface will also come to the circle.
So what you will get, the deformation retract is a circle, okay.
So it is like from 2D it has become 1D.
This we saw in case of our GVD planning.
So can we find a set of paths which are in between these obstacles like this?
They are at equal distance, this is something which we have seen earlier.
So we can use the generalized Voronoi diagram, this is something like the retract samples
and sample in these positions, okay.
So now I know that there is a path from one point to another point and this is my network
of paths.
So I did not have to sample everywhere else.
Now the ease of manipulation ability.
Now manipulation ability is the ease of motion in a particular direction.
That basically means that we have seen that from kinematics, we have seen that x dot,
tool link manipulator, okay.
So this is my tool link manipulator, this is theta 1, this is theta 2, this is xy.
So x dot is equal to J into theta dot, right.
And we have seen that theta dot is equal to J inverse of x dot, which means that theta
dot is equal to adjoint of J, where determinant of J into x dot, okay.
Now wherever determinant of J becomes equal to 0, it ends up in a singular position.
That means you should avoid singularities, okay.
Now singularities basically mean that your energy requirement becomes infinity and that
is a point where you cannot go.
So for more details, please refer to the earlier lectures and also for into the introduction
to the robotics course or the textbook, okay.
So very briefly speaking, this relationship is a relationship between the end-effector
velocity x dot here and the joint velocity theta dot.
And if determinant of J becomes equal to 0, then what will happen is your joint velocity
will become infinity.
And to avoid that, we say that there is a measure which is called the manipulation ability
measure, which is defined by the determinant of, determinant of square root of determinant
of J, J into J transpose for redundant manipulators.
So this is my W or W is equal to simply for determinant of J.
So this is for redundant systems and this is for non-redundant.
Why they are different?
Because in case of redundant manipulators, okay, J is non-square, it is m into n, okay.
So this is a non-square matrix.
So basically if you multiply J into J transpose, what will happen, it will become a square
matrix.
Then you can take the determinant and then find the square root.
Whereas in the case of non-redundant system, basically J is a square matrix, m into m,
okay.
So it is a square matrix, so there is no problem.
So it is determinant of J.
So this is a measure which tells us, which indicates the ease of motion in a particular
direction.
So if you take the singular value decomposition of J, okay, you can get SVD of J will give
you 3 matrices, U, sigma, VT.
This is called my singular matrix, singular, which will be coming out like this, singular
values, sigma 1 by sigma n and this is basically the direction of an ellipse.
This is the major axis and that is the minor axis of the ellipse, this is my sigma 1 and
this is my sigma n.
So this sigma is called the singular values of a matrix and sigma is equal to square root
of eigenvalues and the determinant of a matrix, determinant of J is equal to the product of
singular values, sigma 1 into sigma 2 into sigma n.
And what does it basically indicate, it gives the direction of an ellipse and the direction
of the axis is given by U and VT, okay, which indicates which direction it can move easily.
So in this direction, it can move easily because a sigma 1 is much larger than sigma 2.
So the sampling based on manipulation ability should be such that you should sample more
in the direction in which the manipulator can move easily, rather than trying to sample
in a direction where it cannot move.
And also if it is going near a singular position, there is no point in going near a singularity,
you know, it is like an obstacle, you cannot go there.
So do not sample near singularities because the manipulation ability is very low there.
One way of trying to sample by using this manipulability measure is to sample near in
directions in which manipulation ability is increased, okay, direction in which the manipulator
can move very quickly, okay.
So this is manipulation ability based sampling.
Quasi-radic static sampling is basically you are sampling the largest unoccupied space,
okay.
So suppose I have workspace like this and this is the unoccupied space.
So you do sampling there in a quasi-random sampling method.
Now that is one way by which we did our sampling, we generated.
So we did sampling, then we generated our basically nodes, then here we generated our
edges and from the edges we made our graph and then we found a path, that is how we did.
Now there can be other ways of doing this.
One is what we call the rapidly exploring random trick, also in short it is called RRT.
Now this RRT is a very interesting way by which it operates, is that it is like a tree
as the name indicates, okay.
So if you have a workspace, suppose we have an obstacle here, another obstacle there.
So in this particular case what we are doing is, suppose I start, this is my initial point.
So I start growing a tree, it is like this is the trunk of the tree, these are branches
of the tree and the branches of the tree are increasing in that direction.
It is like you are growing a tree, now branches cannot go obviously inside the obstacle, right.
So this is the tree I am growing and these are all branches of the tree.
So it is going from initial point to the goal point and then in order to find the connection
what we can do is, we can go along a particular branch and then, so I do not take this one,
I take this one, like this, like this, like this and then I see if I can go from here,
I can go to the goal, wherever there is a path.
Let us assume that there is a path there, so let me draw a path point here.
Now this can be unidirectional, so I have grown a single tree which we have gone from
one direction to another direction.
Now you can also have trees growing in multiple directions like in a forest.
So I have the obstacle here and we have a triangle here and we are starting from the
position, of the initial position here to the goal point there, okay.
So what we are basically doing is, we are going from the initial point to the goal point
and we are growing two trees.
So this is a tree which is growing in this direction, so this is a tree which is growing
in this direction and there is another tree which is growing in that direction, from that
direction.
Let us say there is another tree which is growing from here.
Two trees are growing, bidirectional.
So now what is happening is, somewhere they will come and meet.
Now you can find the path and try and, these are all the trees, branches of the trees.
Now basically you can find the connection like this, like this, like this, like this,
like this, like this and to the goal.
So it is a bidirectional tree.
Now you can have multiple trees growing in different directions.
One more tree from here, okay, and which can actually be a forest.
So this is basically called, you know, RRT or Rapidly Exploring Random Tree.
Basically what we do is, in, so this is a Rapidly Exploring Random Tree, you can see
here.
The tree is growing in different directions.
It has started from the center and it is going in all directions.
Now you can connect start point to this one and goal point to this one and then connect
it.
Now path planning with RRTs, suppose we are starting off again, as in earlier case, we
need to have nodes not intersecting with obstacles, with obstacles, okay, so nodes that are not
intersecting with obstacles.
So we have started from here and then these are all nodes which are not intersecting with
obstacles and we have come here, this is a new node, okay.
Now what I need to do is, I need to generate another node which is here, let us say Q random
and then see the connection between them.
So there can be bias, for example, it can be biased towards larger spaces, biased towards
the goal.
Biased towards the goal is a good idea.
The tree should grow towards the goal.
Otherwise if I am growing a tree, I have a tree and I start from here, there is an obstacle
here and another obstacle there and my goal is here, okay.
And I start growing the tree in this direction, okay, in the reverse direction.
The goal is in that direction.
So it should be biased towards the goal or it can be biased towards larger spaces.
So bias towards goal can help you, help the tree grow towards the goal, it will make your
search faster.
Now you can grow two other trees towards each other.
So from the goal I start one and from the initial point I start another one.
Now this is one tree, this is another tree, I want to connect them now, okay.
Now how do I connect these two trees?
For example, in this particular case, we have initial point here, goal point here, I want
to connect them.
Now I am going to grow two trees.
One tree can probably come from here, like this, I am just giving an example.
Another tree is coming from here and I need to connect the trees now.
So I need to connect this node and this node now.
Now how do you connect these nodes?
That is something which we are looking at.
So one tree grown using random target, I want to see whether this can hit any point there,
it can hit any point of the initial tree.
So what we do is a new node becomes target for the other tree, okay.
So I generate another node which is going to become a target for the other tree.
So now how do I calculate?
I can calculate the nearest node to the target.
So from here, I want to make a connection.
So how do I get the nearest node?
Now this again we have eyes, so you can see, this is a computer program, please remember.
These are coordinates in space, okay, x dash, y dash, okay.
So the computer program cannot see this directly, right.
So from here, what it does is it has to find a target node to connect.
So it can calculate the nearest node to the target, okay.
So from the target, this is my target.
So I find the nearest node to the node, this is my Euclidean distance, so this is my distance
which is the nearest to that, I am going to connect to this tree, okay.
Now try to add new collision-free branches.
So I have a q new such that this is a collision-free branch which is connecting the q target and
the q new, and q new is connected to this one now.
So this is how, and if this is successful, keep extending the branch.
So what we are doing is we are making this new nodes from the target, okay, and putting
this new q new such that ultimately it goes and hits the q targets.
And then finally we have made the connection.
So if I just go back of this process that I have a q target, the first thing I do is
I need to, this is my q target, I need to identify which is the node nearest to it in
terms of distance.
So I identify this is the shortest distance.
Then what we do is from here, along this direction I generate a node, okay.
So I have generated a node here, then on this line I generate another node here.
So in this way I am generating new news and then finally I am making this branch and connecting
this with that.
So now I have connected and made my tree.
Now from the initial point to the goal point we are coming here and we are getting our
path.
Now this is very useful in the case of articulated robots.
This is also used for mobile robots in path planning, okay.
Now there are a lot of examples.
Highly articulated robots, large chain manipulators like this, they have to go from one configuration
to another configuration.
So RRT is a method by which it can go very very easily.
These are examples.
We will come in terms of kinematic constraints little bit later.
Let us have a look at a few videos in which, for example here.
So this is a workspace.
You can see here and this is an obstacle.
The blue one is an obstacle here.
So this blue triangle here is an obstacle.
I want to go from this point to this point.
Again I am making the assumption that this is not having any kinematic constraint in
terms of non-kinematic constraint.
If this is a mobile robot, it can move in any direction.
It is not constrained, okay.
So how do we start?
I start making a tree from one side and you can see the tree is increasing and it is increasing
slowly and until it goes to the other side.
So this is the increase of the tree increasing, increasing, increasing, increasing and then
it is connecting the goal point.
Now once it has connected the goal point, I have gone from the initial point to the
goal point, okay.
So this is a unidirectional tree.
Now I can also have a bidirectional tree.
Now this is sampling inside a hollow passage.
So this is a hollow passage, okay and we are sampling inside this hollow passage.
Now let me play the video from here let us say, okay from here.
So next is what we see is sampling inside hollow passages.
So this is a hollow passage which is shown and it is going, finding the nodes and then
going from one point to another point inside the hollow passage.
So you can see that it is generating the node and then going from one point to another point.
Now this is another case where there are narrow passages and the RRT is going in one direction
from one side and is finally going and reaching the other side.
So you can see how the network of paths is increasing.
Now it has reached, now the robot is going from one point to the other point, okay.
So this is one way of finding a path by using the rapidly, the RRT method.
There are a few more examples, too many of them actually.
So these are different configurations of the obstacles where we are going from one point
to another point, exactly the same procedure.
You can see it is increasing from left side and going up.
Now there can be cases where it fails, let us look at a few such cases.
Here this is an interesting case, please have a look at this one.
So here what is happening is that there is a very narrow passage and you want to go from
the initial point to the goal point on the right and it is sampling but it is not being
able to find the narrow passage because the narrow passage is very, very narrow there.
Now this is another case in which you are having the same RRT which is going from one
point to another point trying to go through the narrow passage and it says path not found.
Now this is an interesting case where instead of unidirectional, we are using bidirectional,
okay.
So the next one is bidirectional.
So the trees are going from two directions, one from the initial point and the other from
the goal point and what is happening is that you can see that if it is able to successfully
sample in that narrow space, then it is going to get the connection, okay, not otherwise,
okay.
So here also it is sampling but then it is not getting a path because it has not managed
to reach the corner of the samples, so it is still sampling.
This is also showing you the importance of being able to sample in the narrow passages.
Now let us look at it, so it is saying path not found but you can very clearly see that
there is a path.
Now in the next one what we are seeing is, okay, the next video is basically for a serial
arm manipulator.
Now for the serial arm, it has to take the manipulator end effector and put it inside
here, put it through there, okay.
So what it will do is it is trying to find configurations which are possible which will
take the end effector and put it through there using RRT only, okay.
So see as the video goes on, you see how it is finding, on the right side is a program
which is basically finding the sample points and connecting them by edges, okay.
So you can see that the end effector is bending and then finally it is able to go through
the target point, that is that hollow section there.
So now it is moving forward and it is able to go through the, that passage there, narrow
passage, okay, that we can see, right.
So it has found a way to go through the, put the end effector through that passage and
also it can come out, now it is coming out.
Now this is another example of dynamic obstacle avoidance using RRT where we have the application
of course, as we come along we will see more application, this is actually an autonomous
car.
So in this autonomous car you can see that the car is behind, okay, on the right side
you see is the simulation, okay.
So you can see the car coming out here, that is the car, okay.
So it has a lighter and a camera in front and the simulation is shown here on the right
side, on here.
So the moment it sees something in front it will stop, it will find another path by shooting
out another ray.
So there is a pedestrian who is approaching, so you can see that immediately it will see,
so that was the person.
So it has changed its path exactly by using RRT and then comes back again.
So this is doing obstacle avoidance using RRT.
So these rays which are going out are like the branches of the tree that I was talking
about.
So this gives us an idea of how the system actually works in a real world situation.
So autonomous cars also use RRT, okay.
So what we have seen is this autonomous vehicle actually driving by itself and avoiding obstacles,
okay.
And this is an application of RRT.
In today's class what we looked at is sampling based planning, that is in which you do not
have to search the whole workspace, you do not have to make C spaces and you can sample
a part of the workspace based on whether they are near obstacles, manipulation ability
based sampling or sampling in narrow passages and then you can connect these paths and find
a path, okay.
So this is a better way of or faster way of finding a path to take you from a initial
point to a goal point, especially in cases where you have very high degrees of freedom
of system, very high degrees of freedom systems.
So we will stop here today and we will continue in subsequent lectures.
Thank you.
