hpp_tutorial  5.0.0
Tutorial for humanoid path planner platform.
Tutorial 1 - Python

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 script script/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 solutions,

>>> pp(1)
>>> pp(2)

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://hpp_tutorial/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::pathOptimization::RandomShortcut).

loaded = ps.client.problem.loadPlugin("spline-gradient-based.so")
if loaded:
ps.addPathOptimizer("SplineGradientBased_bezier1")
else:
print("Could not load spline-gradient-based.so")

Load a plugin that implements another path optimizer (hpp::core::pathOptimization::SplineGradientBasedAbstract) and add the path optimizer. The two selected path optimizers will be called in sequence. Note that in this example, the second path optimizer will not improve the result of the first one.

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.

Note
Paths can be displayed in gepetto-gui after installing hpp-gui package and loading plugin hppwidgetsplugin.
pp(0)

Display first path, result of RRT.

pp(1)

Display second path after optimization by random shortcut algorithm.

pp(2)

Display third path after optimization by spline gradient based algorithm.