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
hpp-gui graphical interface

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