Objective
Program RRT algorithm in python.
Introduction
Open a terminal cd into hpp-practicals directory and open 3 tab by typing CTRL+SHIFT+T twice. In the first terminal, type
hppcorbaserver
In the second terminal, type
gepetto-gui
In the third terminal, type
cd script
python -i rrt.py

You should see the above window displaying a manipulator robot surrounded by obstacles.
In the third terminal, you should see the message "Method solveBiRRT is not implemented yet". Open file script/motion_planner.py
in a text editor. The message is produced by method solveBiRRT
of class MotionPlanner
.
Before starting
All classes and functions are implemented by the remote process hppcorbaserver
called a server
.
Instructions in python terminal trigger computation on the server side that stores some objects.
The server stores a roadmap
composed of nodes and of edges. Nodes contain
configurations. Edges contain paths. The roadmap can be extended using methods
-
ps.addConfigToRoadmap
, -
ps.addEdgeToRoadmap
.
Calls to method ps.directPath
creates a new path between two configurations. This path is stored in a vector on the server side and can be accessed by the
index in the vector.
See Section "Some useful methods" below.
Displaying a configuration or a path
While running, your RRT algorithm will produce configurations and paths to store in the roadmap
edges. To display a configuration q
in the viewer, use
v (q)
Functions that create a path (such as directPath
below) appends
the created path to a vector. The path can be displayed by the path
player pp
. To display the result of your algorithm, type
pid = ps.numberPaths () - 1 if pid < 0: raise RuntimeError ("No path in vector") pp (pid)
Hint
You can use methods of objects robot
and ps
. To get the list of these
methods with documentation, type in the python terminal
help (robot) help (ps)
In class MotionPlanner
, you can access these object by
self.robot self.ps
Some useful methods
# # Note for all the methods below, # - configurations are represented by lists of float, # - "index of the path" means index in the vector of paths stored on the server side, # # Shoot a random configuration within bounds of robot # # return: a configuration robot.shootRandomConfig () # Get nearest node of given input configuration in a connected component of the current roadmap # # config: the input configuration # connectedComponentId: the index of a connected component in the roadmap, # if is negative, considers the whole roadmap # default value: -1 # return: nearest configuration, # distance between nearest configuration and input configuration ps.getNearestConfig (config, connectedComponentId=-1) # Build direct path between two configurations # # q1, q2: start and end configurations of the direct path, # validation: whether the path should be tested for collision, # # return: whether the path is valid (True if validation is set to False), # index of the path, # a string describing why the path is not valid, or empty string. # # note: When the path between q1 and q2 is not valid, the method returns # a part of the path starting at q1 and ending before collision. ps.directPath (q1, q2, validation) # Add a configuration to the current roadmap # # q: configuration ps.addConfigToRoadmap (q) # Add an edge to the current roadmap # # q1, q2: configurations stored in the nodes to be linked by the edge, # pathId: index of the path linking q1 and q2 to be stored in the edge, # bothEdges: whether an edge between q2 and q1 should also be added. ps.addEdgeToRoadmap (q1, q2, pathId, bothEdges) # Get length of path # # pathId: index of the path # # return: length of the path. The interval of definition of the path starts at # 0 and ends at the path length. ps.pathLength (pathId) # Get configuration along a path # # pathId: index of the path # parameter: parameter in interval of definition of the path # (see method pathLength) # # return: configuration of path at given parameter ps.configAtParam (pathId, parameter) # Get the number of connected components of the current roadmap # # return: number of connected components ps.numberConnectedComponents ()
Before starting we recommend that you play a little with the above methods,
creating and displaying some configurations and paths in the python
terminal.
Exercise 1
In file script/motion_planner.py
, remove instruction
print ("Method solveBiRRT is not implemented yet")
and implement RRT algorithm between markers
#### RRT begin #### RRT end