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 -c basic

In the third terminal, type

cd script
python -i rrt.py

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 create 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

While running, your RRT algorithm will produce configurations and paths to store in the roadmap edges. To display a configuration q, first create a client to gepetto-gui in the python terminal:

>>> v = vf.createViewer ()
hpp-gui graphical interface

You should see the above window displaying a manipulator robot surrounded by obstacles.

Then, typing in the python terminal

>>> v (q)

You shoud see the robot in configuration q.

Displaying a path

Functions that create a path (such as directPath below) append 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 = PathPlayer (v)
>>> pp (pid)

You can also use gepetto-gui path player to display a path. For that, in menu Tools, select Reset connections then Refresh. In menu Windows, select Path player.

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