# How can I get the right sample in RRT star Dubins?

I am trying to find a solution in S(1)*R^2 (x,y, orientation) with obstacles (refer to image) using RRT star and Dubins Model. The code takes a lot of time to find a suitable random sample with x,y, theta such that a successful Dubins path can be connected between the two points without the vehicle (a rectangle colliding any of the obstacles). The fact that the random sample needs to be at the correct angle so that the vehicle's path is collision free is 1 out of 100,000 random samples. This makes the code very slow even when my computer is at its full processing power. None of my internal codes take much time. I timed all of them, only the fact of achieving that 1 out of 100,000 sample causes the code to take so much time. I tried decreasing my discretization space by half but the problem still exists.

• By "S(1)*R^2" Do you mean $\mathbb{S}^1\ \text{x}\ \mathbb{R}^2$? – koverman47 Jul 16 '18 at 21:52
• Can you describe in more details how you extend your RRT? – Petch Puttichai Jul 16 '18 at 23:59
• @koverman47 yes where each point in the configuration space with a different orientation will be considered as a different point. – Ajin2305 Jul 17 '18 at 9:47
• @Petch, I perform random sampling over the entire configuration space i.e (random x, random y, random \theta), then the nearest node of the tree which can be connected by a Dubins Segment is connected, which is followed by rewiring the neighbors present in the shrinking ball. – Ajin2305 Jul 17 '18 at 9:58
• Let's say your sampled configuration is $q_\text{sample}$ and the nearest configuration on the tree is $q_\text{near}$. Do you attempt to connect $q_\text{near}$ directly with $q_\text{sample}$ or with some nearer configuration based on a step size? – Petch Puttichai Jul 17 '18 at 11:19

Apart from the fact that the collision-free region looks pretty tight, one of the main reasons why you got very few usable samples is that a connection attempt is made directly between a node in the tree and the sampled configuration. Due to that, in order for a sampled configuration to have any chance at all of a successful connection, it has to have its x and y coordinates being on the gray area, which is already relatively small compared to the whole map. Even when you have a sampled configuration falling in the gray area, you still need to take care of the rotation as well as making sure that those configurations can be connected by a relatively constrained Dubin path.

A better approach would be making use of a steering function. (Actually the RRT$^*$ paper also describes this a bit.) To make the point, let's consider when we use a straight line to connect two configurations. Let's say a sampled configuration is $q_\text{rand}$ and its nearest neighbor on the tree is $q_\text{near}$. Instead of connecting $q_\text{near}$ directly to $q_\text{rand}$, you try to connect $q_\text{near}$ to some $q'$ that lies on the straight line connecting $q_\text{near}$ and $q_\text{rand}$ but is closer to $q_\text{near}$. This is kind of steering a configuration from $q_\text{near}$ towards $q_\text{rand}$. It will increase the chance of success as it is not too greedy to always try to go all the way to a sampled configuration.

Of course, with Dubin paths, the actual implementation details are definitely going to be different. But the core idea stays the same: limit the RRT extension to be some step size from existing nodes on the tree.

• Things sped up after decreasing my sample space, sampling my collision check and using global data ( like take a left or right). I get your idea, but there an issue of allotting an angle while taking steps towards the random sample, how can I go about doing that? – Ajin2305 Aug 1 '18 at 13:25
• You can also do linear interpolation, the same way that you do for the positions. – Petch Puttichai Aug 2 '18 at 12:49

According to the given picture, the car should move from start to goal and has to fulfill the constraints, that means the car must stay between the colored boxes. A plain RRT algorithm doesn't make much sense here, because the state space is huge. In each time step the car is able to move a small amount forward, a large amount and it can steer. If the sequence takes 20 seconds and the car can execute an action, each second two times, we will get a lot of possibilities.

The problem can be solved with an algorithm called Random-MMP, which is a multi-modal RRT pathplanner which is utilizing a motion model as heuristic. Explaining Random-MMP is easier if we assume only one mode. A mode is similar to a motion primtive which results into a sandbox like game. We call the mode “putcaronthemiddle”. If we are executing this mode interactively, the car gets the position direct on on the crossroad. The first RRT graph is generated until the mode, and the second graph is created from the mode to the goal.

In the case with only one mode, Random-MMP is equal to RRTconnect which is also growing two trees at the same time. But with more motion primitives the advantage will become more obvious. The general idea behind Random-MMP is called in the literature “task & motion planning grounding” and means to combine a geometric planner with a symbolic planner.

At the end, i want to send some greetings – with respect to the admin of this forum – to Master yoda, Dr. Emmett Brown and Chuckie Sullivan.