Motion Planning HW #7

Due : Nov/26/2007

Problem

Implement a PRM planner for a multi-link (at least four) robot arm. The arm can be a simple planar arm (which will simplify the graphics), or a 3D arm. The arm can be composed of line segments (which will make collision checking easier) rather than finite volume links. All you need to do is write code to detect the intersection between line segments and polygons. If you want, you can use collision checking software that is available on the web.

On your web page, you should show the following:

• A figure that shows the environment (without the robot),
• A figure that shows the robot at all of the randomly sampled configurations
• A movie of the robot executing a plan

Solution

I implemented the PRM and A* algorithm to find the path using GUI of MATLAB.

Workspace Description : Workspace has 2-Dimension(no orientation) which is the same as before (bug, potential etc.). However, the robot has 6 degree of freedom which is composed of 4 links plannar(x, y, theta1, theta2, theta3, theta4).

PRM(Probablistic Roadmaps) is one of sampling-based algorithm. Through many sampling, the planner interpolates between available nodes. It divides planning into two phase: the learning phase, during which a roadmap in Q_free is built; and the query phase, during which user-defined query configurations are connected with the precomputed roadmap[1].

- Random Sampling and Connecting nodes

Planner generates random sampling(uniform distribution) in configuration space and classifies nodes depending on whether nodes collide with obstacles. Note that even though a node is on obstacle space, we do not discard it because it will be used to resampling afterward. After sampling nodes, it finds several closest nodes and connects them. In this process, the distance is computed as below (Euclidean distance).

w is selected properly. This depends on scale of workspace thus, in my algorithm it is set to 0.9~1 since workspace is confined 0~1 but angles varies from 0 ~ 2*phi which is relatively big.

- Roadmap construction algorithm

• NEW VSet
• NEW ESet
• WHILE size(VSet) < num_node
  • REPEAT
    • q = uniform random sampling in configuration space
  • UNTIL q = isCollisionFree
  • AddVSet(q)
• ENDWHILE
• FOR all q in VSet
  • neighbor = the kth closest neighbors of q chosen from VSet according to dist
  • FOR all q' in neighbor
    • IF !isESet(q, q') && LocalPlanner(q, q') ~= NIL
      • AddESet(q, q')
    • ENDIF
  • ENDFOR
• ENDFOR

The VSet, ESet represent set of available(visible) nodes which in Q_free and set of connectable edges which does not collide with obstacle in configuration space respectly. LocalPlanner is connecting planner between two nodes in workspace. In my algorithm, incremental search is used.

Mapping from configuration space to workspace

- configuration space variables : X, Y, theta1, theta2, theta3, theta4

- workspace variable : X, Y, X1, Y1, X2, Y2, X3, Y3, X4, Y4

In order for collision check between two nodes, first local planner interpolates configuration space variable and then transforms them to workspace variables. Through this process, the neighbors can be determined whether it is connected each other

- Solve query

Once the random sampling is generated, the planner tries to find accessibility and departability with roadmap. It connects several nearest node with start/goal points

• AddVSet(q_init)
• AddVSet(q_goal)
• neighborInit = the kth closest neighbors of q_init chosen from VSet according to dist
• neighborGoal = the kth closest neighbors of q_goal chosen from VSet according to dist
• q' = nearest neighbor in neighborInit from q_init
• REPEAT
  • IF LocalPlanner(q_init, q') != NIL
    • AddESet(q_init, q')
  • ELSE
    • ExcludeESet(q')
  q' = nearest neighbor in neighborInit from q_init
  • ENDIF
• UNTIL isConnected(q_init) || isEmpty(neighborInit)
• q' = nearest neighbor in neighborInit from q_goal
• REPEAT
  • IF LocalPlanner(q_goal, q') != NIL
    • AddESet(q_goal, q')
  • ELSE
    • ExcludeESet(q')
  q' = nearest neighbor in neighborInit from q_goal
  • ENDIF
• UNTIL isConnected(q_init) || isEmpty(neighborInit)
• P = ShortestPath
• IF P
  • RETURN P
• ELSE
  • RETURN FAILURE
• ENDIF

ShortestPath subroutine computes A* algorithm using heuristic function. When this routine returns failure then we need more samples in order to connect from start point to goal point.

- Resampling

As I mention before, planner generates resampling when the path is not found. In this sampling case, it starts from unavailable nodes which are determined it collides with obstacles. These nodes are near to obstacles so the result of this processing achieves more sample near obstacles. Through having more sample near obstacles, path can be found in narrow shape free space. First random nodes which are the normal distribution from unavailable nodes (mean : unavailable nodes, variance : depending on workspace) are generated. Since my workspace is small scale, so I set variance of X, Y very small relative to angle.

• FOR all unavaiable node
  • REPEAT
    • q = random sampling according to normal distribution (mean = unavailable node)
  • UNTIL !isCollisionFree(q)
  • AddVSet(q)
• ENDFOR
• REPEAT
  • IF LocalPlanner(q_init, q') != NIL
    • AddESet(q_init, q')
  • ELSE
    • ExcludeESet(q')
  q' = nearest neighbor in neighborInit from q_init
  • ENDIF
• UNTIL isConnected(q_init) || isEmpty(neighborInit)
• FOR all q in VSet
  • neighbor = the kth closest neighbors of q chosen from VSet according to dist
  • FOR all q' in neighbor
    • IF !isESet(q, q') && LocalPlanner(q, q') ~= NIL
      • AddESet(q, q')
    • ENDIF
  • ENDFOR
• ENDFOR

- Demo

black triangle : base point

red circle : each joint

blue line : each link

black dot line : connection between nodes

- Workspace

- Uniform Sampling : 200 samples

- Resampling (Normal distribution) : 5 samples for each failed node

NOTE : Red line is trajectory of base position

3 WALLS - DIFFICULT

3 WALLS - EASY

NARROW CHANNEL : w = 1 of Euclidean distance, least action of base position

NARROW CHANNEL : w = 0.9 of Euclidean distance, combination of base position and angles

- Future works

There are a number of sampling based algorithm such as RRT. After I implemented PRM, I got to have really great interest in sampling based algorithm because it might be able to work with high degree of freedom robot. As I prepared paper presentation in class which was random motion technique of potential function, actually I could not understand how to create configuration space with high degree of freedom. Now everything becomes clear and I think I need to work more on probability.

- Reference

[1] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations,
MIT Press, Boston, 2005.