Rapidly exploring random Trees (RRT) and their much nicer properties.
Rapidly exploring random Trees (RRT) is a randomized data structure that was invented by Professor Dr. Steven LaVelle. This data structure is hugely useful for handling path-planning queries with non-holonomic constraints involving dynamics and high degrees of freedom. RRTs and their variants are highly popular in the robot motion planning community as they tend to facilitate advanced path-planning queries. But most of its nicer properties are often forgotten or overshadowed by its much-glorified applications. This article tries to highlight the properties of the RRTs which make them so useful. Also, this article demonstrates the working of RRT in a 6-DoF Collaborative manipulator (UR5e — Universal Robot) for a pick and place task.
What is an RRT? An RRT is a randomized tree that iteratively expands by attempting to make connections between a randomly spawned node (X_rand) in the state space and a node in a tree that is closest to X_Rand. This might sound abstract and similar to RDT — Random dense trees, but the nuances in the tree expansion process would be made much clearer later in this article.
Why RRT? Even before RRTs came into existence there were methods like Probabilistic Roadmaps (PRM) and Potential fields(PF) that were used to solve path-planning problems. These methods were not much capable of planning with non-holonomic constraints. Potential field approaches depended heavily on heuristics which made it very hard to validate and apply in environments with obstacles and Differential constraints. PRMs had trouble making connections between valid states when it came to non-holonomic planning. RRTs incorporate many good properties from probabilistic roadmaps at the same time they can easily be extended to a planning scenario with non-holonomic constraints as there are not many connections to make. They work with fewer heuristics and can easily be extended to high-dimensional motion planning problems with various constraints.
How does RRT work? This question has the most elaborate answer in this article. For now, let us consider a simple case where we have to plan for a point robot on an R2 state space (the 2D plane in simple terms) from a point X_Start to X_Goal. For higher-dimensional robots, the state space could be Rn where n denotes the dimension of the configuration space.
The small green dot denotes the X_Start and the bigger one denotes the X_Goal, where the blue squares denote the obstacles that have to be avoided in the path.
Initially, the X_Start is the only node in the tree, and a random sample X_Rand is spawned in the collision-free space. Now instead of attempting to connect the X_Rand with the closest node on the tree (in this case the X_Start), we instead take a small and finer step towards the X_Rand. This process of extending the tree in small steps is called steering. The steering process checks for the feasibility of movement and it adds the steered node X_Near into the tree (Note:- It is not the randomly sampled node). The steering factor (step_size) is an input parameter that can be decided accordingly. X_Rand is now chucked off and we keep iterating the process with the expanded tree. Now and then, we pass the X_Goal instead of spawning the random configuration, the rate at which we sample the goal node is called goal bias. This ensures that the tree converges toward the goal node. Once the RRT steers its way to the goal configuration X_Goal, the algorithm is stopped and a feasible path is traced back from the goal by tracking the node’s parent iteratively until the X_Start is reached. This is the abstract idea of the process by which the RRT expands.
When iteratively expanding the RRT avoids obstacles and finds a path from X_Start to X_Goal. The steering function accommodates a motion validity checker that checks if the X_near is in a valid state or not. Mostly it is just a collision-checking function but in many cases, it could even check for other constraints like singularity, etc.
A pseudo-code for RRT is given below:-
def RRT():
nodes = []
path = []
add_node(0, *self.start_pose)
sn = self.nodes[0]
#1: add the start poseas a node
self.gn = None
cnt = 0
while(self.gn is None):
cnt = cnt+1
#2 sample a random node with goal bias
if(random.random() < goal_bias):
q_rand = goal_pose
else:
q_rand = sample_Xrand()
#3 expand
expand(q_rand[0],q_rand[1])
#4 keep checking if self.gn (Goal Node) is still None or not, this is updated when we reach the goal
if(self.gn is not None):
#5 construct the path
construct_path(sn,gn)
break
def expand(x, y):
#1: Find the nearest node to x,y
#2 step towards it and during step see if its reaching the goal, this is already implemented in step but do read the code and understand it
pos =[x, y]
q_near = nearest(pos)
step = step(q_near, pos,step_size=15)
def sample_Xrand(self):
x = float(random.uniform(0, mapx))
y = float(random.uniform(0, mapy))
return x, ySome very nice properties of the RRT:-
- Unlike the naive random tree or random forest, the RRT is not biased toward the previously explored areas, rather it has a strong bias towards the unexplored areas. It can be explained using a great concept from computational geometry called the Voronoi diagram. For that, I have taken all the nodes expanded by the RRT while planning from X_Start and X_Goal, using these nodes as input I have drawn a Voronoi diagram as given below
A lemma states that “the probability that a vertex is selected for extension is proportional to the area of its Voronoi region” — Dr. James Kuffner and Dr. Steven LaVelle. This is a great property for a space-filling tree biased toward exploring unexplored areas in state space. If visualized incrementally, the Larger Voronoi cells are broken down into smaller regions.
2. The RRT is always connected:- Unlike PRMs, the RRT does not suffer from the issue of a tree being disconnected. This ensures reliable planning in various scenarios. Even in scenarios where the state space consists of enclosed or trapped regions the RRT is not likely to form a disconnected tree.
3. The distribution of the vertices of an RRT approaches the sampling distribution. In most cases, as the one demonstrated below, the vertices are randomly sampled in a uniform distribution. Then, It is more likely that the vertices also are in uniform distribution. This property greatly avails in biased steering. In this case, the tree expands to a particularly biased region. If a different smooth probability distribution is given, then the tree is more likely to expand with the same bias.
Hence, these were some really great properties that make RRTs a great tool in sampling-based motion planning methods. Using RRT, a great level of motion planning can be done for various applications. It is also important to mention that there are many variants to RRTs like RRT-Connect, RRT*, etc. They use the RRT concept as a core and build on top of it to achieve near-optimal paths in both holonomic and non-holonomic cases. This greatly helps in kino-dynamic planning approaches.
I have implemented an RRT-based motion planner for devising motion plans with collision avoidance for various pick-and-place tasks. The simulation and collision detections are taken care of by PyBullet, whereas RRT is implemented from Scratch. The UR5e robot has a 3-finger Robotiq gripper as its end-effector.
The code for planning a path from a start state (J1, J2, J3, J4, J5, J6) to a Goal state (J1, J2, J3, J4, J5, J6) in the 6-DoF UR5e collaborative robot using RRT is given below.
This code can be extended to many other articulated arms such as stable Tx2_60, Han’s Elfin robot, etc, just the collision checking part (the usual and major bottlenecks in robot motion planning) has to be handled appropriately.
References:-
- Planning Algorithms by Professor Dr. Steven LaVelle
- Rapidly exploring random trees: A new tool for path planning
- Special thanks to Abdur Javaid’s project in GitHub on various path planners.
