Explanation about 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.

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
vf (q_init)

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
vf (q_goal)

Define goal configuration.

vf.loadObstacleModel ("iai_maps", "kitchen_area", "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.