To run the tutorial, open a terminal and open 3 tabs by typing CTRL+SHIFT+T twice. When the terminal is selected, you can select a tab by typing ALT-[1|2|3].

Starting hppcorbaserver

In the first tab, type

hppcorbaserver

See package hpp-corbaserver for details.

Starting gepetto-gui

In the second tab, type

gepetto-gui -c basic

A window opens and is ready to display the scene containing the robot. The robot and environment will appear later.

Note that gepetto-gui and hppcorbaserver executables are completely independent.

Controlling via a python terminal

In the third tab, type

cd script

python -i tutorial_1.py

to run the scriptscript/tutorial_1.py in an interactive python terminal.

To display the scene, type

>>> v = vf.createViewer ()

gepetto-gui window should now display a scene containing a PR2 robot in a kitchen environment.

To display initial and goal configurations type the following commands

>>> v (q_init)

>>> v (q_goal)

To solve the path planning problem between those configurations, type

>>> ps.solve ()

To display the resulting of RRT, type

>>> from hpp.gepetto import PathPlayer
>>> pp = PathPlayer (v)
>>> pp (0)

To display an optimized solution,

>>> pp (1)

Detailed explanation

This section presents in more details the content of script/tutorial_1.py.

from hpp.corbaserver.pr2 import Robot
robot = Robot ('pr2')
robot.setJointBounds ("root_joint", [-4, -3, -5, -3])

Import class pr2.robot.Robot and create an instance and set bounds of translation degrees of freedom of the base. Note that the constructor of the instance calls idl method hpp::corbaserver::Robot::loadRobotModel. This triggers the loading of the urdf/srdf model of the PR2 robot in hppcorbaserver executable.

from hpp.corbaserver import ProblemSolver

ps = ProblemSolver (robot)

Import class hpp.corbaserver.problem_solver.ProblemSolver and create an instance. This class is a helper class to define and solve path planning problems.

from hpp.gepetto import ViewerFactory

vf = ViewerFactory (ps)

Import class gepetto.viewerFactory.ViewerFactory and create an instance. This object takes as input the ProblemSolver instance that enables the viewer client to also control hppcorbaserver executable

q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2

Define initial configuration.

Note: Initial configuration is built from configuration of the robot at construction, and by modification of joints retrieved by name. This method is more robust than specifying a hard-coded configuration vector since the ordering of joints in the configuration vector is not unique.

q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5

Define goal configuration.

vf.loadObstacleModel ("package://iai_maps/urdf/kitchen_area.urdf", "kitchen")

Load obstacles from urdf file.

Note: this method loads the objects defined in the urdf file both in hppcorbaserver and in gepetto-viewer-server.

ps.setInitialConfig (q_init)

ps.addGoalConfig (q_goal)

Define initial and goal configurations.

ps.addPathOptimizer ("RandomShortcut")

Add a path optimizer (hpp::core::RandomShortcut).

print (ps.solve ())

Solve problem and print the results.

v = vf.createViewer()
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)

Create the display window. Import and create an instance of PathPlayer. This class samples a path in hppcorbaserver and displays it in gepetto-viewer-server.

pp (0)

Display first path, result of RRT.

pp (1)

Display second path after optimization.