8/6/2019 A study on Rapidly-growing Random Trees

1/3

A study on Rapidly-growing Random Trees

Kadir Firat UyanikKOVAN Research Lab.

Dept. of Computer Eng.

Middle East Technical Univ.

Ankara, Turkey

kadir@ceng.metu.edu.tr

Abstract In this study Rapidly-growing Random Treesalgorithm is investigated for a robot having an articulated-body. This report explains the implementation details and givespreliminary test results.

I. INTRODUCTION

In the previous homeworks, I implemented tangent bug al-

gorithm(TBA), artificial potential fields(APF), and visibility

graphs(vg). TBA [1] doesnt require a complete environment

model, but makes use of local sensori information and global

information of where it is with respect to the goal configura-

tion. APF [2] on the other hand uses the complete environ-

ment model to generate potential functions, but suffers from

getting stuck in local minima (additive/repulsive potential

function) or requires carefully tuned parameters (navigation

potential function). VG [3], on the other hand, found the

optimal(least cost) path from an initial configuration to the

goal configuration. It makes use of a compete environment

model (polygonal approximation of the obstacles), but it

cannot scale well with the complexity of the environment

(viz. the number of polygon vertices). Besides, its complexity

is exponential in the dimension of the configuration spacedue to the complex intersection tests.

There are different ways to simplify the path finding

problem, such as projecting search to a lower dimensional

space, limiting the number of possibilities(i.e. adding con-

straints, reducing the volume of free space), or sacrificing

the precision, optimality, even completeness.

Random sampling based methods are one way to relax

the problem. It also lets designer to control the precision

and solution time trade-off easily. The key idea behind the

sampling based techniques is that rather than exhaustively

exploring all possibilities, randomly explore a smaller subset

of possibilities as the progress is being tracked. They can godown in the search tree much earlier than any exhaustive

algorithm, which eases taking control of the search process.

However, the downside is that depending on the planner

configuration, optimality and completeness can be com-

promised considerably. Moreover, narrow passages in the

configuration space can be a real problem since the sampled

nodes might not be connected together unless the sampling

is focused around these passages.

In this study, basic Rapidly-growing Random Trees(RRT)

algorithm [4] will be used, though RRT has many variations,

such as RRT-connect [5], ERRT [6], and Bidirectional Multi-

Bridge ERRT[7].

II . RAPIDLY GROWING RANDOM TREES

An RRT is basically a data structure and algorithm which

efficiently searches nonconvex high-dimensional spaces. The

way RRT is constructed can be given as follows:

Start with the initial state as the root of a trees

Pick a random state in anywhere or in the direction of

the target

Find the closest node in the current trees Extend that node toward the target if possible

To mention other RRT-variations briefly:

RRT-connect expands two trees towards each other,

one from the start node, and the other from the goal

node. They interchange the roles of expanding-towards-

the-other and expanding-randomly. The algorithm ter-

minates ones a connection is established between trees.

ERRT makes use of the idea of way-point caching.

Therefore search operation becomes biased towards the

paths found in the earlier successful executions.

Multi-Bridge ERRT is similar to the RRT-connect but

it continues to execution although a connection is estab-lished between the trees until a pre-specified number of

connections are established. Then the resultant structure

is not a tree anymore. To find the shortest path an

optimal graph search algorithm is used, such as A*.

III. EXPERIMENTAL SETUP

Similar to the previous homeworks, Ive used Webots[8]

simulation environment and CGAL[10] as a library to test

and visualize the results. However, the core of this study is

based on a recently-released open source motion planning

library, OMPL[11].

OMPL consists of many state-of-the-art sampling-based

motion planning algorithms. It doesnt include any collision

checking or visualization routine which is done on purpose

to abstract the algorithms from the problem definition and

any other visual load.

The simulated robot and the its simplified version is shown

in the figures 1 and 2.

IV. IMPLEMENTATION DETAILS

Setting up the planning problem in OMPL is as follows:

identify the space we are planning in: C= R2S1S1,

select a corresponding state manifold from the available

ones, or implement one,

8/6/2019 A study on Rapidly-growing Random Trees

2/3

Fig. 1. The simulated robot in Webots simulation environment with severalobstacles.

Fig. 2. The simulated robot in Webots simulation environment withseveral obstacles. Orange and blue balls show the start and goal positionrespectively. Joint angles start and goal configurations are set inside theprogram.

since C contains an R2 component, we need to define

bounds,

define the notion of state validity, define start states and a goal representation.

Once these steps are complete, the specification of the

problem is conceptually done.

The state manifold that is used in this work can be given

as follows:

StateManifoldPtr cm;

cm = StateManifoldPtr(new RealVectorStateManifold(2)) +

StateManifoldPtr(new ompl::base::SO2StateManifold()) +

StateManifoldPtr(new ompl::base::SO2StateManifold());

OMPL requires a state validity method to be implemented

which is called during the sampling if any sample configu-

ration is collided with a configuration space obstacle. To do

this, Ive polygonized all the obstacles in the environment

and check if any intersection occurs in the state validity

checking routine. These are done by CGAL::do Intersect

method.

Start and goal configuration settings are similar to the

state manifold representation. Planners in OMPL requires

space information and problem definition information as

well. Space information is acquired via the state manifold

representation that is done at the very first. Problem defini-

tion takes the start, goal and space information to become

ready for planner configuration.

One other great feature is that, planning time can be

limited by giving a time interval input argument to the

planners solve method. This is very helpful for a time-

critical application.

V. RESULTS AND DISCUSSION

Although current setup allows exhaustive testing and

benchmarking the algorithms due to a known-bug in thecode, simulation crashes after a couple of planning is run.

This is due to the CGALs exact-inexact kernel usage issue,

and happens during the polygon intersection tests.

The figures below show sample runs of the algorithm:

Fig. 3. Sample run. Please notice the visualization bug.

Another known bug is about the visualization as it can be

seen in the figure 5 where a vertex is missing, and edges were

not drawn. This happens since in Webots the single message

size limit is 1024Bytes that is sent from the supervisor

controller process to the simulation process(physics plugin).

Yet, this issue can be solved by casting float numbers to

short integers by multiplying them with a known scalar andthen dividing them with the same amount when the package

is received by the other end and casting them again to the

float numbers. This is a quick-n-dirty solution that Ive done

for this study. Better way is sending multiple packages and

sending the overall size of the package that is intended to

be sent at the very first header of the package-sequences so

that the receiver can wait until the transmission is finished.

Yet another issue is that the states being shown in the

figures reflect only the main checkpoints. Actually, OMPL

finds many states but for memory issues it doesnt store

them, but they can be reconstructed by OMPLs interpolation

methods.

The solution can be improved by changing the weights

of the sub manifolds that are added at the first of the

construction. These weights are used while calculating the

nearest node of the tree to a randomly sampled goal position.

This also affects the precision of the algorithm since the

tolerance is calculated by taking difference of the values of

each sub manifold dimensions with the goal configuration.

When OMPL picks a node to expand, it performs many

local collision checks if that expansion is collision free. This

can be reduced by changing the step size to a more suitable

value. But, for sure, this requires an expert knowledge and

should be calculated carefully.

8/6/2019 A study on Rapidly-growing Random Trees

3/3

Fig. 4. Sample run 2. Notice that the path being found can vary due tothe random nature of the algorithm.

Fig. 5. Sample run 3. Among convex/concave polygons. Planning requiresat most 0.5 seconds in this setup, given that the environment bounded

between [0,2] meters.

A sample planning output is given below:

Info: RRT: Maximum motion extension distance is assumed

to be 1.822322

Info: RRT: Starting with 1 states

Warning: RRT: Found approximate solution

Info: RRT: Created 45 states

Found solution:

collision check cnt: 1565

collision occured: 58

n states in the tree: 45

VI. CONCLUSIONS

In this study, RRT-base algorithm is tested on a simulation

environment with an articulated robot. A recently-released

motion planning library, OMPL, is studied. It seems that

OMPL is going to be a standard planning library in Robotics

community in the near future as it is in the case of Computer

Vision community and OpenCV library.

The most powerful side of the OMPL is that it gives the

designer freedom to use any kind of a robotic platform to

plan its action by using various planning algorithms without

making no change in the planning side of the code as

soon as the state and control manifold representations, and

state validity checking routines are done correctly for that

particular platform.

REFERENCES

[1] I. Kamon, E. Rivlin, E.Rimon. A new range-sensor based globallyconvergent navigation algorithm for mobile robots. Robotics andAutomation, 1996. Proceedings., 1996 IEEE International Conference

on In Robotics and Automation, 1996. Proceedings., 1996 IEEEInternational Conference on, Vol. 1 (1996), pp. 429-435 vol.1.

[2] Khatib, O. 1986. Real-time obstacle avoidance for manipulators andmobile robots. Int. J. Rob. Res. 5(1):9098.

[3] Lozano-Perez T. and Michael A. Wesley. 1979. An algorithm forplanning collision-free paths among polyhedral obstacles. Commun.ACM 22, 10 (October 1979), 560-570

[4] S. M. LaValle. Rapidly-exploring random trees: A new tool for pathplanning. TR 98-11, Computer Science Dept., Iowa State Univ., Oct.1998

[5] Kuffner, J.J., Jr.; LaValle, S.M.; , RRT-connect: An efficient approachto single-query path planning , Robotics and Automation, 2000.Proceedings. ICRA 00. IEEE International Conference on , vol.2,no., pp.995-1001 vol.2, 2000

[6] Bruce, J.; Veloso, M.; , Real-time randomized path planning forrobot navigation, Intelligent Robots and Systems, 2002. IEEE/RSJInternational Conference on , vol.3, no., pp. 2383- 2388 vol.3, 2002

[7] Based on: Bruce J. R., Real-Time Motion Planning and Safe Naviga-tion in Dynamic Multi-Robot Environments, PhD. Thesis, 2006

[8] Michel, O. Webots: Professional Mobile Robot Simulation, Interna-tional Journal of Advanced Robotic Systems, Vol. 1, Num. 1, pages39-42, 2004

[9] Principles of Robot Motion by Howie Choset etal., MIT Press, 2005,ISBN: 0-262-03327-5

[10] R. C. Veltkamp. Generic programming in CGAL, the ComputationalGeometry Algorithms Library. In F. Arbab and P. Slusallek, editors,Proceedings of the 6th Eurographics Workshop on ProgrammingParadigms in Graphics, Budapest, Hungary, 8 September 1997, pages127-138, 1997.

[11] The Open Motion Planning Library (OMPL),http://ompl.kavrakilab.org, 2010