# Advanced pathfinding with robot in 3D

## Recommended Posts

Fuzsy    136
Hello,

[b]My project:[/b]
I have a 2000 pounds real life robot with two arms. The arms are controlled by 15 motors and gets position feedback from 15 potentiometers. The robot is placed in a room with stationary objects which positions are known. The motors are only able to move with full speed.

Get the two arms of the robot from their current point to a selected point.

[b]My current solution:[/b]
Currently the robot is moving by a big handmade table, but the problem is that if an object is moved the robot is going to collide. And it is almost impossible to imagine all possible situations.

[b]Environment:[/b]
I have created a 3D simulation for the robot which I am planning to use when training the robot. The 3D simulation can detect collision with objects and between the two arms.

[b]My question:[/b]
First I looked at some path finding algorithms like A* but with 15 motor in a 3D environment it is not very useful. Then I begin to look at neural network and Q-learning but I am not sure it is the right way to go. I think I need some kind of reinforcement learning.

My input and output could be something like:
Input:
15 x Current motor pos
15 x Goto pos
Obejects in room pos or the 3D simulation

Output:
15 x (maybe 5 states) sub positions for the motors

I am currently trying to figure out which network and learning method to use if I go with neural network and reinforcement learning.
Is it doable?

I hope someone can help me and maybe also point to an example.

With best regards,
Peter

##### Share on other sites
Wow... uh... not really a [color=#ff0000][i]game [/i][/color]AI topic... *shrug* Let's see what happens.

##### Share on other sites
Fuzsy    136
Yea.. it's not a game. [img]http://public.gamedev.net//public/style_emoticons/default/smile.png[/img] But I think the problem in many ways is game related.
I think I need some kind of neural network so I thought AI was the right area to put the post.

##### Share on other sites
My point wasn't that it isn't AI-related but rather that the entire purpose of GameDev.net is related to games. Be that as it may...

##### Share on other sites
Fuzsy    136
Okay. Thanks for letting me post IADaveMark. If I find a solution for my problem I am hope it also will benefit game develops. The 3D simulation works as a sort of a game. [img]http://public.gamedev.net//public/style_emoticons/default/biggrin.png[/img]

By the way, I found a neural network learning algorithms which I may be able to use. It is called Simulated Annealing. As with Genetic algorithms it is able to solve the Traveling Salesman problem. In that problem there are a group of cities and the network decides which is the best route to take. As I understand it Simulated Annealing just do it fast.

I am not sure it is the best way to go so I am open for all ideas. Edited by Fuzsy

##### Share on other sites
jefferytitan    2523
Personally I find no conflict with game vs robot, many techniques used are similar, and there's probably a bigger community of game developers than robot developers. ;) And research flows both ways, so we're not exactly losing out.

I don't understand the capabilities of your robot. You mention two arms. How does it move? Wheels? Using the arms as legs? Grasping objects and pulling? What do the 15 motors do? Or are you saying that the robot itself doesn't move, you're just trying to position the arms? A picture might assist.

If you're trying to figure out how to use the motors to achieve a given effect, and you have the ability to sense the results of your movements, you might be able to use neural nets or genetic algorithms to do self-guided learning, e.g. try stuff, watch the effects, repeat.

If you're talking about physical pathfinding from one side of room to another (for example), I believe D* or D* lite are good for unknown dynamic environments.

##### Share on other sites
slicer4ever    6760
I'd also recommend RVO's/VO's for pathing between two nodes in an environment, if you can decompose the objects into spherical representation, the problem becomes easier to solve.

##### Share on other sites
Fuzsy    136
Thank you Jefferytitan and slicer4ever,

Well, the robot is hanging from the ceiling and moves along a rail. The two arms are operating down on floor level and shall do things like go under a table without touching it.

The question is how to for example move the arms from above the table to under the table without colliding with the table and without the two arms colliding. The arms are never allowed to touch anything in the room. 1 motor is used for moving the robot along the rails and 1 motor is used to turn the main body of the robot. 7 motors in one arm and 6 motors in the other arm.

So I have two problems.
1. Is the pathfinding.
2. How to follow the path with the use of the motors in the arms and the main body.

Thanks again, I will look in to D* and RVO/VO and see if it is an solution I can use.

##### Share on other sites
slicer4ever    6760
[quote name='Fuzsy' timestamp='1347059499' post='4977846']
Thank you Jefferytitan and slicer4ever,

Well, the robot is hanging from the ceiling and moves along a rail. The two arms are operating down on floor level and shall do things like go under a table without touching it.

The question is how to for example move the arms from above the table to under the table without colliding with the table and without the two arms colliding. The arms are never allowed to touch anything in the room. 1 motor is used for moving the robot along the rails and 1 motor is used to turn the main body of the robot. 7 motors in one arm and 6 motors in the other arm.

So I have two problems.
1. Is the pathfinding.
2. How to follow the path with the use of the motors in the arms and the main body.

Thanks again, I will look in to D* and RVO/VO and see if it is an solution I can use.
[/quote]

So, the arms essentially extend downward, and then can extend in any other direction(if i understand correctly?)

generating a heavily subdivided voxel map might be more useful in this situations, where a voxel is either passable or not passable. if the sensor's can decompose the environment into a 3D Scene, and from their, you generate the voxel world. with the voxel world, you can then do A* combined with [url="http://harablog.wordpress.com/2009/01/29/clearance-based-pathfinding/"]clerance based pathfinding[/url]

I think the result would be pretty decent for your needs, and precision would be dependent on how large a single voxel is.

I'm doubtful you'd need to go full NN's to solve this.

RVO's/VO's are probably not as useful in this situation, i'd recommend a look over, but i had thought the entire robot was tasked with reaching a destination, not just the arms(it can still be used, but it'd be much more difficult imo.) Edited by slicer4ever

##### Share on other sites
Fuzsy    136

The problem is the number of axles when it comes to controlling the motors.

I have created 4 screenshot from the simulation so you are able to see have the robot works.

I am very grateful for your suggestions.

Edit:
Sorry, I was asked to remove the pictures from the forum by my boss. Edited by Fuzsy

##### Share on other sites
jefferytitan    2523
Given the rather specific nature of the application, why not keep it simple and assume a box that matches the size of the table top + the occupant? It looks like the number of obstacles is low, they don't move (much) and there is no reason to skirt close to obstacles.Take the arm out, down then back in. If there are other obstacles in that path, wait for them to leave. Assuming this is x-ray or other radiation based, there aren't meant to be other people in the room anyway.

##### Share on other sites
Fuzsy    136

The way the system currently works is by a big handmade table with current position as input. The feedback from the table is something like your description:

1. Move "arm 1" to 75 cm in Z
2. Move body to 600 cm
3. If "arms are free" "turn body toward goal" else if "possible move arm 2 up".
4. etc.

I use the hit boxes show in the image to determine if a movement is possible or will result in collision.
The problem with that solution is that it is not very dynamic. The "stand" can be moved and the table can be moved by the user.

I have also made a system where smaller easy recognizable sub states are recognized:

1. If "Big movement" Move to body to 600 cm
2. If "arms need to switch places" do sequence 132
3. If "Goal position along tableside" do sequence 97
4. etc.

It made the task a little easier but still with the same problem, it is not dynamic. In a new situation where the "stand" is put in a new position and the table is turn, I have to make a lot of new add-ons to the table without affecting the already working part of the table.

Was it something like above you had in mind?

Jeffery you are right about the application of the robot. [img]http://public.gamedev.net//public/style_emoticons/default/smile.png[/img]

I am very open to all ideas and I am grateful for all suggestions.

Edit:
Sorry, I was asked to remove the picture from the forum by my boss. Edited by Fuzsy

##### Share on other sites
jefferytitan    2523
Hmm, okay. I think simplification still applies, just not *as* simple. ;) Perhaps a good approach would be to explore a virtual graph using A* with special actions. It might look a little more like a planner than a spatial graph. So for example, you allow free movement in a safe zone above the table, you have special actions for moving from above to below the table and vice versa, and you have special actions for moving while under the table. For moving above to below and vice-versa, treat it like a thick cylinder, e.g. move the arm so it's in it's most vertical position and move directly up and down. For moving while under the table, maybe treat the end as a sphere so no rotation could lead to collision.

The advantage would be simplicity and smarts. So if the arm is below the foot of the bed and wants to go to below the head of the bed, it would plot a direct course below the bed, sense a future collision (and abandon that node of the A* search), and then search special actions (move arm up), moving above the table, then special actions (move arm down). You may need to use a time slice based A* to avoid arm collisions with each other.

Edit: The up/across/down approach would of course work for arbitrary obstacles as long as they are simple and sparse. You would need to detect the general table position to start with. If you have depth data you could use a Hough transform or similar to detect a cuboid. Edited by jefferytitan

##### Share on other sites
Fuzsy    136
Thanks Jeffery,

It is a great idea. By using the A* as an action planner instead of has a pathfinder, I may be able to solve the problem. It opens a whole new set of options. I could do things like make some movement cost more than others. And as you suggest, it can use my preprogrammed sequences.

I think you are right (again [img]http://public.gamedev.net//public/style_emoticons/default/smile.png[/img] ) - I need to make the A* time slice based. I will start working with your suggestion right away. It will not be easy, but hopefully it will work.

It would be a lot cleaner solution than with ANN.

Once again thanks.

##### Share on other sites
At that point, it is a planner a la GOAP, STRIPS, etc. Of course, planners use A* to backward solve from the goal, so yeah... same thing.

##### Share on other sites
Fuzsy    136
Okay, thanks Dave Mark.

Hmm.. Automated planners is a whole new category of algorithm I didn’t knew about, so I think I need to make some research on the topic before I continue my work on the solution.

[b]Info for other users at Gamedev:[/b]
The idea of planners
[url="http://en.wikipedia.org/wiki/Automated_planning"]http://en.wikipedia.org/wiki/Automated_planning[/url]

STRIPS
[url="http://en.wikipedia.org/wiki/STRIPS"]http://en.wikipedia.org/wiki/STRIPS[/url]

GOAP ( Simplfied version of STRIPS)
[url="http://web.media.mit.edu/~jorkin/goap.html"]http://web.media.mit.edu/~jorkin/goap.html[/url]

Ironic.. here is an game engine named “[b]X-ray[/b]” which uses GOAP of NPC’s [img]http://public.gamedev.net//public/style_emoticons/default/smile.png[/img]
[url="http://en.wikipedia.org/wiki/X-Ray_Engine"]http://en.wikipedia.org/wiki/X-Ray_Engine[/url]

##### Share on other sites
Fuzsy    136
Hello again,

Jefferytitan, your idea with A* as a planner worked. The system is now able to calculate a movement path.

But.. yeah.. there is always a but.. It is very slow.. it takes about 15 seconds to calculate. It is the simulation of the movement which determines if a movement will result in a collision which takes time, so I have to look in to some faster collision detection, but that is a whole other problem.

Thanks again.

##### Share on other sites
jefferytitan    2523
I'm glad to hear it. I did suspect you might experience problems like that. Feel free to post another question on the collision. I imagine that (depending upon the format that you get obstacle data in) there will be easy ways to optimise the problem. If in doubt, try to solve a simplified version of the problem, and then drill down deeper if required. For example if you had obstacle data as voxels, you could do it octree style (e.g. if any child is an obstacle, mark the parent as an obstacle) and check for collisions first on a coarse version. If there may be an obstacle, then drill down.

##### Share on other sites
snowmanZOMG    1205
You're dealing with motion planning here. Check out http://planning.cs.uiuc.edu/

##### Share on other sites
Fuzsy    136
Thanks Jeffery and Snowman,

To Jeffery: I’ll got some ideas on how to optimize collision detection, but if it don’t works I will probably post it as you suggested.

To Snowman: It looks like a very good and well written book. I think I will gain quite a lot by reading it.

##### Share on other sites
snowmanZOMG    1205
The author was one of my professors while I was in school. He's very smart. I find the book very useful for understanding the problems at a theoretical, abstract level, but it has very little to offer (perhaps none) when it comes to code examples. If you're mathematically inclined, you shouldn't have any problems.

##### Share on other sites
Emergent    982
I'm with snowman. As a starting point, I always direct people to Steve LaValle's book. Another very good book is [url="http://www.amazon.com/Principles-Robot-Motion-Implementations-Intelligent/dp/0262033275/ref=sr_1_1?ie=UTF8&qid=1348835130&sr=8-1&keywords=choset+planning+for+robot+motion"]this one[/url].

The state of practice for robot planning is essentially this:

1. Use an [url="http://msl.cs.uiuc.edu/rrt/"]RRT [/url]to generate some path -- any path -- in the configuration space (i.e., space of joint angles) from start to goal. The resulting path will not be optimal in any sense, but it can be computed quickly and it gets from point A to point B, which is the most important thing.

2. Smooth the path. There are varying strategies for this, and there is a little less consensus about the details, but the gist is that you do some combination of throwing out intermediate points in the path, and running splines (usually cubic) through the remaining points. In lower dimensions, this is what algorithms like "string pulling" aim to solve.

3. Write a feedback controller to follow the path. The standard methods (which are very similar) are "inverse Jacobian" and "Jacobian Transpose" controllers (the first reasonable Google hit is here; I also particularly like this book.)

There are some variations for the various items above, especially for part 1: Along similar lines to RRTs, there are also algorithms like RRG* that attempt to grow a random [i]graph[/i], and attempt to converge to a path that is actually optimal. [url="http://arxiv.org/pdf/1005.0416.pdf"]This paper[/url] by Frazzoli's student should point you in the right direction (see the references). There are also some deterministic planners that people have had some success with; these do a combination of graph search and graph refinement (in the style of hierarchical A*).

##### Share on other sites
Emergent    982
Sorry, two links got removed from my last post in point 3; they were:

[i]3. Write a feedback controller to follow the path. The standard methods (which are very similar) are "inverse Jacobian" and "Jacobian Transpose" controllers (the first reasonable Google hit is [b][url="http://math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf"]here[/url][/b]; I also particularly like [b][url="http://www.amazon.com/A-Mathematical-Introduction-Robotic-Manipulation/dp/0849379814/ref=sr_1_1?ie=UTF8&qid=1348835067&sr=8-1&keywords=sastry+mathematical+introduction+to+robotic+manipulation"]this book[/url][/b].)[/i]

##### Share on other sites
Fuzsy    136
Thanks Emergent,

I am currently learning a lot from replies, links and books I got here at the forum.

I have managed to get something working but there is still a long way to go. My current solution is based on Hierarchical A*.

My goal is to be able make a motion plan in less than 1 second. At the moment I am down to a average of 450 ms, but out of 1400 test positions 50 fails to find a plan. At least within 40 seconds.

40 seconds equal approximately 6000 branches searched.

I will look in to RRT. It seems like something I could use. Thanks again.

##### Share on other sites
Emergent    982
Oh! Also: Although I 100% understand if you want to write your motion planner from scratch as a learning experience, you might want to be aware of two existing pieces of software that can make your life easier:

1. [url="http://www.ros.org/wiki/"]ROS[/url]: The Robotic Operating System. This runs on top of Linux and manages message-passing, etc; it gives a modular way to write robotics software.

2. [url="http://ompl.kavrakilab.org/"]OMPL[/url]: The Open Motion Planning Library. This implements a variety of planners including RRTs, and is available as a [url="http://www.ros.org/news/2011/01/open-motion-planning-library-ompl-released.html"]ROS package[/url].