From 63e2891105797737dcd5e33a2ad4ab52cb5eff11 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Wed, 4 May 2022 15:20:25 +0200
Subject: [PATCH] format

---
 .travis/script.py                             |  50 ++--
 include/hpp_tutorial/doc.hh                   |   2 +-
 include/hpp_tutorial/tutorial_1.hh            |  11 +-
 include/hpp_tutorial/tutorial_2.hh            |  67 +++---
 include/hpp_tutorial/tutorial_3.hh            |  10 +-
 meshes/box.dae                                |   2 +-
 script/tutorial_1.py                          |  54 ++---
 script/tutorial_2.py                          |  68 +++---
 script/tutorial_3.py                          | 180 +++++++++------
 script/tutorial_kinodynamic.py                | 169 ++++++++------
 src/CMakeLists.txt                            |   1 -
 .../corbaserver/manipulation/pr2/__init__.py  |   2 +-
 src/hpp/corbaserver/manipulation/pr2/robot.py |  53 +++--
 src/hpp/corbaserver/pr2/__init__.py           |   2 +-
 src/hpp/corbaserver/pr2/robot.py              |  32 +--
 src/hpp/corbaserver/rod/__init__.py           |   2 +-
 src/hpp/corbaserver/rod/robot.py              |  31 +--
 src/tutorial_1.cc                             |  84 +++----
 src/tutorial_2.cc                             | 215 +++++++++---------
 srdf/kitchen_area.srdf                        |   4 +-
 urdf/pr2.urdf                                 |   8 +-
 21 files changed, 553 insertions(+), 494 deletions(-)

diff --git a/.travis/script.py b/.travis/script.py
index ed975be..5a4337c 100644
--- a/.travis/script.py
+++ b/.travis/script.py
@@ -1,40 +1,42 @@
 from hpp.corbaserver.pr2 import Robot
-robot = Robot ('pr2')
-robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3])
-
 from hpp.corbaserver import ProblemSolver
-ps = ProblemSolver (robot)
+
+robot = Robot("pr2")
+robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3])
+
+
+ps = ProblemSolver(robot)
 
 # On travis, we do not compile the viewer (yet?)
 # from hpp.gepetto import ViewerFactory
 # r = ViewerFactory (ps)
 
-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
+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
 # r (q_init)
 
-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
+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
 # r (q_goal)
 
 # r.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
-ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "kitchen/")
+ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "kitchen/")
 
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
+ps.setInitialConfig(q_init)
+ps.addGoalConfig(q_goal)
 
-print ps.solve ()
+print(ps.solve())
 
-ps.addPathOptimizer ("RandomShortcut")
+ps.addPathOptimizer("RandomShortcut")
 
-print ps.optimizePath (0)
+print(ps.optimizePath(0))
diff --git a/include/hpp_tutorial/doc.hh b/include/hpp_tutorial/doc.hh
index 95632c9..b6ee472 100644
--- a/include/hpp_tutorial/doc.hh
+++ b/include/hpp_tutorial/doc.hh
@@ -27,7 +27,7 @@
 /// how to implement a new path planning algorithm.
 ///
 /// \par Setting up your environment
-/// 
+///
 /// Before starting, make sure that
 /// \li \c bash is you default shell script language,
 /// \li the line \code source $DEVEL_DIR/config.sh \endcode is in your .bashrc
diff --git a/include/hpp_tutorial/tutorial_1.hh b/include/hpp_tutorial/tutorial_1.hh
index 5c303b2..c5a1eee 100644
--- a/include/hpp_tutorial/tutorial_1.hh
+++ b/include/hpp_tutorial/tutorial_1.hh
@@ -50,7 +50,9 @@
 /// cd script
 /// python -i tutorial_1.py
 /// \endcode
-/// to run the script<code><a href="script/tutorial_1.py">script/tutorial_1.py</a></code> in an interactive python terminal.
+/// to run the script<code><a
+/// href="script/tutorial_1.py">script/tutorial_1.py</a></code> in an
+/// interactive python terminal.
 ///
 /// To display the scene, type
 /// \code
@@ -83,7 +85,8 @@
 /// \endcode
 ///
 /// \section hpp_tutorial_1_script Detailed explanation
-/// This section presents in more details the content of \c script/tutorial_1.py.
+/// This section presents in more details the content of \c
+/// script/tutorial_1.py.
 ///
 /// \code
 /// from hpp.corbaserver.pr2 import Robot
@@ -139,7 +142,8 @@
 /// Define goal configuration.
 ///
 /// \code
-/// vf.loadObstacleModel ("package://hpp_tutorial/urdf/kitchen_area.urdf", "kitchen")
+/// vf.loadObstacleModel ("package://hpp_tutorial/urdf/kitchen_area.urdf",
+/// "kitchen")
 
 /// \endcode
 /// Load obstacles from urdf file.
@@ -196,4 +200,3 @@
 /// build-folder/src/hpp-tutorial-1
 /// \endcode
 ///
-
diff --git a/include/hpp_tutorial/tutorial_2.hh b/include/hpp_tutorial/tutorial_2.hh
index fda35ee..702b0cd 100644
--- a/include/hpp_tutorial/tutorial_2.hh
+++ b/include/hpp_tutorial/tutorial_2.hh
@@ -25,10 +25,10 @@
 /// The compilation and installation instructions can be found in
 /// \c CMakeLists.txt.
 ///
-/// \section hpp_tutorial_tutorial_2_class_planner Implementation of class Planner
-/// File \c src/tutorial_2.cc implements \c class hpp::tutorial::Planner,
-/// deriving from abstract class hpp::core::PathPlanner. In this section,
-/// we explain some specific parts of the code.
+/// \section hpp_tutorial_tutorial_2_class_planner Implementation of class
+/// Planner File \c src/tutorial_2.cc implements \c class
+/// hpp::tutorial::Planner, deriving from abstract class hpp::core::PathPlanner.
+/// In this section, we explain some specific parts of the code.
 ///
 /// \code HPP_PREDEF_CLASS (Planner);\endcode
 /// is a macro containing the forward declaration of class \c Planner as well as
@@ -48,7 +48,7 @@
 /// protected.
 /// \note method \c create calls protected method \c init that is explained
 ///       later on.
-/// 
+///
 /// \code
 /// virtual void oneStep ()
 /// \endcode
@@ -64,11 +64,12 @@
 /// \code
 /// weakPtr_.lock ();
 /// \endcode
-/// which is the shared pointer equivalent to \c this when using simple pointers.
-/// \note Method \c init always calls the parent implementation so that the
-/// parent part of the object also stores a weak pointer to itself.
+/// which is the shared pointer equivalent to \c this when using simple
+/// pointers. \note Method \c init always calls the parent implementation so
+/// that the parent part of the object also stores a weak pointer to itself.
 ///
-/// \section hpp_tutorial_tutorial_2_hpp_tutorial_server Implementation of executable hpp-tutorial-2-server
+/// \section hpp_tutorial_tutorial_2_hpp_tutorial_server Implementation of
+/// executable hpp-tutorial-2-server
 ///
 /// Now that the new class \c hpp::tutorial::Planner has been implemented, we
 /// are going to use it in a new executable. The new executable is defined by
@@ -127,7 +128,8 @@
 /// SETUP_PROJECT_FINALIZE()
 /// \endcode
 ///
-/// \section hpp_tutorial_tutorial_2_running Running the server and solving a problem.
+/// \section hpp_tutorial_tutorial_2_running Running the server and solving a
+/// problem.
 ///
 /// To run your executable and solve a problem with your path planning
 /// algorithm, you simply need to follow the same steps as in tutorial 1,
@@ -145,7 +147,8 @@
 /// \warning Basic PRM is very inefficient. Resolution can take a long time,
 /// especially if you have compiled in debug mode.
 ///
-/// \section hpp_tutorial_tutorial_2_external_package Moving the code into an external package
+/// \section hpp_tutorial_tutorial_2_external_package Moving the code into an
+/// external package
 ///
 /// To implement the above executable in an external package, you should do the
 /// following steps.
@@ -155,33 +158,21 @@
 /// \endcode
 /// \li create a file \c README.md describing the new package
 /// \code
-/// echo "Implementation of a new path planning algorithm embedded in a CORBA server" > my-hpp-project/README.md
-/// \endcode
-/// \li create an empty \c Doxyfile.extra.in file
-/// \code
-/// mkdir my-hpp-project/doc; touch my-hpp-project/doc/Doxyfile.extra.in
-/// \endcode
-/// \li copy the above \c cmake code into \c my-hpp-project/CMakeLists.txt, after replacing names by the names you have chosen,
-/// \li copy file \c src/tutorial.cc into \c my-hpp-project/src
-/// \code
-/// mkdir my-hpp-project/src
-/// cp hpp_tutorial/src/tutorial.cc my-hpp-project/src/.
-/// \endcode
-/// Go into the project directory and initialize git.
-/// \code
-/// cd my-hpp-project; git init; git add .
-/// \endcode
-/// Import the cmake git sub-module
-/// \code
-/// git submodule add git://github.com/jrl-umi3218/jrl-cmakemodules.git cmake
-/// \endcode
-/// Commit this first version.
-/// \code
-/// git commit -m "My first hpp project"
-/// \endcode
-///
-/// \subsection hpp_tutorial_tutorial_2_external_package_installation Installation
-/// The package is ready for installation. Create a build directory
+/// echo "Implementation of a new path planning algorithm embedded in a CORBA
+/// server" > my-hpp-project/README.md \endcode \li create an empty \c
+/// Doxyfile.extra.in file \code mkdir my-hpp-project/doc; touch
+/// my-hpp-project/doc/Doxyfile.extra.in \endcode \li copy the above \c cmake
+/// code into \c my-hpp-project/CMakeLists.txt, after replacing names by the
+/// names you have chosen, \li copy file \c src/tutorial.cc into \c
+/// my-hpp-project/src \code mkdir my-hpp-project/src cp
+/// hpp_tutorial/src/tutorial.cc my-hpp-project/src/. \endcode Go into the
+/// project directory and initialize git. \code cd my-hpp-project; git init; git
+/// add . \endcode Import the cmake git sub-module \code git submodule add
+/// git://github.com/jrl-umi3218/jrl-cmakemodules.git cmake \endcode Commit this
+/// first version. \code git commit -m "My first hpp project" \endcode
+///
+/// \subsection hpp_tutorial_tutorial_2_external_package_installation
+/// Installation The package is ready for installation. Create a build directory
 /// \code
 /// mkdir build; cd build
 /// \endcode
diff --git a/include/hpp_tutorial/tutorial_3.hh b/include/hpp_tutorial/tutorial_3.hh
index 395cdda..b00bd71 100644
--- a/include/hpp_tutorial/tutorial_3.hh
+++ b/include/hpp_tutorial/tutorial_3.hh
@@ -23,7 +23,8 @@
 /// \c CTRL+SHIFT+T twice. When the terminal is selected, you can select a tab
 /// by typing \c ALT-[1|2|3].
 ///
-/// \section hpp_tutorial_3_starting_hpp_manipulation_server Starting hppcorbaserver
+/// \section hpp_tutorial_3_starting_hpp_manipulation_server Starting
+/// hppcorbaserver
 ///
 /// In the first tab, type
 /// \code
@@ -38,7 +39,8 @@
 /// cd script
 /// python -i tutorial_3.py
 /// \endcode
-/// Script <code><a href="script/tutorial_3.py">script/tutorial_3.py</a></code> defines a manipulation planning problem.
+/// Script <code><a href="script/tutorial_3.py">script/tutorial_3.py</a></code>
+/// defines a manipulation planning problem.
 ///
 /// \section hpp_tutorial_3_starting_gui Starting gepetto-gui
 ///
@@ -61,7 +63,7 @@
 /// The robot and environment should appear in the viewer. If the viewer
 /// window is black, select the window and hit space.
 ///
-/// To solve the problem, type 
+/// To solve the problem, type
 /// \code
 /// >>> ps.solve ()
 /// \endcode
@@ -70,7 +72,7 @@
 /// \code
 /// >>> pp = PathPlayer (v)
 /// >>> pp (0)
-/// \endcode 
+/// \endcode
 ///
 /// \section hpp_tutorial_3_optimization Optimizing the solution path
 ///
diff --git a/meshes/box.dae b/meshes/box.dae
index 4a67b4f..8f430e7 100644
--- a/meshes/box.dae
+++ b/meshes/box.dae
@@ -598,4 +598,4 @@
   <scene>
     <instance_visual_scene url="#Scene"/>
   </scene>
-</COLLADA>
\ No newline at end of file
+</COLLADA>
diff --git a/script/tutorial_1.py b/script/tutorial_1.py
index 21fe6c6..e56fcb8 100644
--- a/script/tutorial_1.py
+++ b/script/tutorial_1.py
@@ -1,41 +1,41 @@
 from hpp.corbaserver.pr2 import Robot
-robot = Robot ('pr2')
-robot.setJointBounds ("root_joint", [-4, -3, -5, -3])
-
 from hpp.corbaserver import ProblemSolver
-ps = ProblemSolver (robot)
-
 from hpp.gepetto import ViewerFactory
-vf = ViewerFactory (ps)
 
-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
+robot = Robot("pr2")
+robot.setJointBounds("root_joint", [-4, -3, -5, -3])
+
+ps = ProblemSolver(robot)
+
+vf = ViewerFactory(ps)
+
+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
 
-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
+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.loadObstacleModel ("package://hpp_tutorial/urdf/kitchen_area.urdf",
-                      "kitchen")
+vf.loadObstacleModel("package://hpp_tutorial/urdf/kitchen_area.urdf", "kitchen")
 
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
+ps.setInitialConfig(q_init)
+ps.addGoalConfig(q_goal)
 
-ps.addPathOptimizer ("RandomShortcut")
+ps.addPathOptimizer("RandomShortcut")
 
 # print (ps.solve ())
 
-## Uncomment this to connect to a viewer server and play solution paths
-# 
+# # Uncomment this to connect to a viewer server and play solution paths
+#
 # v = vf.createViewer()
 # from hpp.gepetto import PathPlayer
 # pp = PathPlayer (v)
diff --git a/script/tutorial_2.py b/script/tutorial_2.py
index e9df910..49c0b70 100644
--- a/script/tutorial_2.py
+++ b/script/tutorial_2.py
@@ -1,43 +1,43 @@
 from hpp.corbaserver.pr2 import Robot
-robot = Robot ('pr2')
-robot.setJointBounds ("root_joint", [-4, -3, -5, -3])
-
+from hpp.gepetto import ViewerFactory
 from hpp.corbaserver import ProblemSolver
-ps = ProblemSolver (robot)
 
-from hpp.gepetto import ViewerFactory
-vf = ViewerFactory (ps)
-
-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)
-
-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)
-
-vf.loadObstacleModel ("package://hpp_tutorial/urdf/kitchen_area.urdf",
-                      "kitchen")
-
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.selectPathPlanner ("PRM")
-ps.addPathOptimizer ("RandomShortcut")
+robot = Robot("pr2")
+robot.setJointBounds("root_joint", [-4, -3, -5, -3])
+
+ps = ProblemSolver(robot)
+
+vf = ViewerFactory(ps)
+
+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)
+
+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)
+
+vf.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
+
+ps.setInitialConfig(q_init)
+ps.addGoalConfig(q_goal)
+
+ps.selectPathPlanner("PRM")
+ps.addPathOptimizer("RandomShortcut")
 
 # print (ps.solve ())
 
-## Uncomment this to connect to a viewer server and play solution paths
+# # Uncomment this to connect to a viewer server and play solution paths
 #
 # v = vf.createViewer()
 # from hpp.gepetto import PathPlayer
diff --git a/script/tutorial_3.py b/script/tutorial_3.py
index 735307f..ddb2584 100644
--- a/script/tutorial_3.py
+++ b/script/tutorial_3.py
@@ -2,103 +2,141 @@
 
 # Import.
 from math import sqrt
-from hpp.gepetto import PathPlayer
+from hpp.gepetto import PathPlayer  # noqa: F401
 from hpp.corbaserver.manipulation.pr2 import Robot
-from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule, \
-  Constraints, ConstraintGraphFactory, Client
+from hpp.corbaserver.manipulation import (
+    ProblemSolver,
+    ConstraintGraph,
+    Rule,
+    Constraints,
+    ConstraintGraphFactory,
+    Client,
+)
 from hpp.gepetto.manipulation import ViewerFactory
 from hpp.corbaserver import loadServerPlugin
-loadServerPlugin ("corbaserver", "manipulation-corba.so")
-Client ().problem.resetProblem ()
+
+loadServerPlugin("corbaserver", "manipulation-corba.so")
+Client().problem.resetProblem()
 
 # Load PR2 and a box to be manipulated.
-class Box (object):
-  rootJointType = 'freeflyer'
-  packageName = 'hpp_tutorial'
-  urdfName = 'box'
-  urdfSuffix = ""
-  srdfSuffix = ""
-
-class Environment (object):
-  packageName = 'hpp_tutorial'
-  urdfName = 'kitchen_area'
-  urdfSuffix = ""
-  srdfSuffix = ""
-
-robot = Robot ('pr2-box', 'pr2')
-ps = ProblemSolver (robot)
-## ViewerFactory is a class that generates Viewer on the go. It means you can
-## restart the server and regenerate a new windows.
-## To generate a window:
-## vf.createViewer ()
-vf = ViewerFactory (ps)
-
-vf.loadObjectModel (Box, 'box')
-vf.loadEnvironmentModel (Environment, "kitchen_area")
-
-robot.setJointBounds ("pr2/root_joint", [-5,-2,-5.2,-2.7]     )
-robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5])
+
+
+class Box(object):
+    rootJointType = "freeflyer"
+    packageName = "hpp_tutorial"
+    urdfName = "box"
+    urdfSuffix = ""
+    srdfSuffix = ""
+
+
+class Environment(object):
+    packageName = "hpp_tutorial"
+    urdfName = "kitchen_area"
+    urdfSuffix = ""
+    srdfSuffix = ""
+
+
+robot = Robot("pr2-box", "pr2")
+ps = ProblemSolver(robot)
+# ViewerFactory is a class that generates Viewer on the go. It means you can
+# restart the server and regenerate a new windows.
+# To generate a window:
+# vf.createViewer ()
+vf = ViewerFactory(ps)
+
+vf.loadObjectModel(Box, "box")
+vf.loadEnvironmentModel(Environment, "kitchen_area")
+
+robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7])
+robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5])
 
 # Initialization.
 
 # Set parameters.
 # robot.client.basic.problem.resetRoadmap ()
-ps.setErrorThreshold (1e-3)
-ps.setMaxIterProjection (40)
+ps.setErrorThreshold(1e-3)
+ps.setMaxIterProjection(40)
 
 # Generate initial and goal configuration.
-q_init = robot.getCurrentConfig ()
-rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
-q_init [rank] = 0.5
-rank = robot.rankInConfiguration ['pr2/l_gripper_r_finger_joint']
-q_init [rank] = 0.5
-q_init [0:2] = [-3.2, -4]
-rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
-q_init [rank] = 0.2
-rank = robot.rankInConfiguration ['box/root_joint']
-q_init [rank:rank+3] = [-2.5, -4, 0.746]
+q_init = robot.getCurrentConfig()
+rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"]
+q_init[rank] = 0.5
+rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"]
+q_init[rank] = 0.5
+q_init[0:2] = [-3.2, -4]
+rank = robot.rankInConfiguration["pr2/torso_lift_joint"]
+q_init[rank] = 0.2
+rank = robot.rankInConfiguration["box/root_joint"]
+q_init[rank : rank + 3] = [-2.5, -4, 0.746]
 
 # Put box in right orientation
-q_init[rank+3:rank+7] = [0, -sqrt(2)/2, 0, sqrt(2)/2]
+q_init[rank + 3 : rank + 7] = [0, -sqrt(2) / 2, 0, sqrt(2) / 2]
 
-q_goal = q_init [::]
-q_goal [0:2] = [-3.2, -4]
-rank = robot.rankInConfiguration ['box/root_joint']
-q_goal [rank:rank+3] = [-2.5, -4.5, 0.746]
+q_goal = q_init[::]
+q_goal[0:2] = [-3.2, -4]
+rank = robot.rankInConfiguration["box/root_joint"]
+q_goal[rank : rank + 3] = [-2.5, -4.5, 0.746]
 
 # Create the constraints.
-locklhand = ['l_l_finger','l_r_finger'];
-ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,])
-ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,])
+locklhand = ["l_l_finger", "l_r_finger"]
+ps.createLockedJoint(
+    "l_l_finger",
+    "pr2/l_gripper_l_finger_joint",
+    [
+        0.5,
+    ],
+)
+ps.createLockedJoint(
+    "l_r_finger",
+    "pr2/l_gripper_r_finger_joint",
+    [
+        0.5,
+    ],
+)
 
 # Create the constraint graph.
 # Define the set of grippers used for manipulation
-grippers = [ "pr2/l_gripper", ]
+grippers = [
+    "pr2/l_gripper",
+]
 # Define the set of objects that can be manipulated
-objects = [ "box", ]
+objects = [
+    "box",
+]
 # Define the set of handles for each object
-handlesPerObject = [ ["box/handle2", ], ]
+handlesPerObject = [
+    [
+        "box/handle2",
+    ],
+]
 # Define the set of contact surfaces used for each object
-contactSurfacesPerObject = [ ["box/box_surface", ], ]
+contactSurfacesPerObject = [
+    [
+        "box/box_surface",
+    ],
+]
 # Define the set of contact surfaces of the environment used to put objects
-envContactSurfaces = [ "kitchen_area/pancake_table_table_top", ]
+envContactSurfaces = [
+    "kitchen_area/pancake_table_table_top",
+]
 # Define rules for associating grippers and handles (here all associations are
 # allowed)
-rules = [ Rule([".*"], [".*"], True), ]
-
-cg = ConstraintGraph (robot, 'graph')
-factory = ConstraintGraphFactory (cg)
-factory.setGrippers (grippers)
-factory.environmentContacts (envContactSurfaces)
-factory.setObjects (objects, handlesPerObject, contactSurfacesPerObject)
-factory.setRules (rules)
-factory.generate ()
-cg.addConstraints (graph = True, constraints = Constraints \
-                   (numConstraints = locklhand))
-cg.initialize ()
-
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
+rules = [
+    Rule([".*"], [".*"], True),
+]
+
+cg = ConstraintGraph(robot, "graph")
+factory = ConstraintGraphFactory(cg)
+factory.setGrippers(grippers)
+factory.environmentContacts(envContactSurfaces)
+factory.setObjects(objects, handlesPerObject, contactSurfacesPerObject)
+factory.setRules(rules)
+factory.generate()
+cg.addConstraints(graph=True, constraints=Constraints(numConstraints=locklhand))
+cg.initialize()
+
+ps.setInitialConfig(q_init)
+ps.addGoalConfig(q_goal)
 
 # uncomment to solve
 # ps.solve()
diff --git a/script/tutorial_kinodynamic.py b/script/tutorial_kinodynamic.py
index aa49fa3..83c2918 100644
--- a/script/tutorial_kinodynamic.py
+++ b/script/tutorial_kinodynamic.py
@@ -1,104 +1,123 @@
 from hpp.corbaserver.robot import Robot
-import time
+from hpp.gepetto import PathPlayer
+from hpp.gepetto import ViewerFactory
+from hpp.corbaserver import ProblemSolver
 
-# This tutorial shows how to use kinodynamic motion planning methods. In the current implementation, only the translation part of a freeflyer is considered by the Kinodynamic methods, all the other degree of freedom use classical geometrical methods. 
+# This tutorial shows how to use kinodynamic motion planning methods.
+# In the current implementation, only the translation part of a freeflyer is considered
+# by the Kinodynamic methods, all the other degree of freedom use classical geometrical
+# methods.
 
 
 # Take a rod with a freeflyer base as robot
-class RobotRod (Robot):
-  rootJointType = 'freeflyer'
-  packageName = "hpp_tutorial"
-  meshPackageName = "hpp_tutorial"
-  urdfName = 'rod'
-  urdfSuffix = ""
-  srdfSuffix = ""
-  def __init__ (self, robotName, load = True):
-        Robot.__init__ (self, robotName, self.rootJointType, load)
+class RobotRod(Robot):
+    rootJointType = "freeflyer"
+    packageName = "hpp_tutorial"
+    meshPackageName = "hpp_tutorial"
+    urdfName = "rod"
+    urdfSuffix = ""
+    srdfSuffix = ""
+
+    def __init__(self, robotName, load=True):
+        Robot.__init__(self, robotName, self.rootJointType, load)
         self.tf_root = "base_footprint"
 
+
 robot = RobotRod("rod")
 
-robot.setJointBounds ("root_joint", [-7, 6.5, -7, 7,0.4,0.4])
+robot.setJointBounds("root_joint", [-7, 6.5, -7, 7, 0.4, 0.4])
 
-# Kinodynamic methods need at least 6 extraConfigs, to store the velocity (first 3) and acceleration (last 3) of the translation of the root
+# Kinodynamic methods need at least 6 extraConfigs, to store the velocity (first 3) and
+# acceleration (last 3) of the translation of the root
 robot.client.robot.setDimensionExtraConfigSpace(6)
 # set the bounds for velocity and acceleration :
-aMax=1.
-vMax=2.
-robot.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
-
-from hpp.corbaserver import ProblemSolver
-ps = ProblemSolver (robot)
-# define the velocity and acceleration bounds used by the steering method. This bounds will be stastified along the whole trajectory.
-ps.setParameter("Kinodynamic/velocityBound",vMax)
-ps.setParameter("Kinodynamic/accelerationBound",aMax)
-ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
-# Uncomment the following line if you want to constraint the orientation of the base of the robot to follow the direction of the motion. Note that in this case the initial and final orientation are not considered.
-#ps.setParameter("Kinodynamic/forceOrientation",True)
-
-# The following line constraint the random sampling method to fix all the extraDOF at 0 during sampling. Comment it if you want to sample states with non-null velocity and acceleration. Note that it increase the complexity of the problem and greatly increase the computation time.
-ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
-
-from hpp.gepetto import ViewerFactory
-vf = ViewerFactory (ps)
-
-q_init = robot.getCurrentConfig ()
-q_goal = q_init [::]
-
-q_init [0:3] = [6.5,-4,0.4] #root position
-q_init[3:7] = [0,0,0,1] #root rotation
-#set initial velocity (along x,y,z axis) : 
-q_init[-6:-3]=[0,0,0]
-
-
-#q_goal[0:3] = [6.5,-1,0.4] # straight line
-q_goal [0:3] = [3,-4,0.4] # easy goal position
-#q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position
-#set goal velocity (along x,y,z axis) : 
-q_goal[-6:-3]=[0,0,0]
-
-vf.loadObstacleModel ("iai_maps", "room", "room")
-# with displayArrow parameter the viewer will display velocity and acceleration of the center of the robot with 3D arrow. The length scale with the amplitude with a length of 1 for the maximal amplitude
-v = vf.createViewer(displayArrows = True)
-ps.setInitialConfig (q_init)
-ps.addGoalConfig (q_goal)
-
-ps.addPathOptimizer ("RandomShortcut")
-#select kinodynamic methods : 
+aMax = 1.0
+vMax = 2.0
+robot.client.robot.setExtraConfigSpaceBounds(
+    [-vMax, vMax, -vMax, vMax, 0, 0, -aMax, aMax, -aMax, aMax, 0, 0]
+)
+
+
+ps = ProblemSolver(robot)
+# define the velocity and acceleration bounds used by the steering method. This bounds
+# will be stastified along the whole trajectory.
+ps.setParameter("Kinodynamic/velocityBound", vMax)
+ps.setParameter("Kinodynamic/accelerationBound", aMax)
+ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
+# Uncomment the following line if you want to constraint the orientation of the base of
+# the robot to follow the direction of the motion. Note that in this case the initial
+# and final orientation are not considered.
+# ps.setParameter("Kinodynamic/forceOrientation",True)
+
+# The following line constraint the random sampling method to fix all the extraDOF at 0
+# during sampling. Comment it if you want to sample states with non-null velocity and
+# acceleration. Note that it increase the complexity of the problem and greatly increase
+# the computation time.
+ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
+
+
+vf = ViewerFactory(ps)
+
+q_init = robot.getCurrentConfig()
+q_goal = q_init[::]
+
+q_init[0:3] = [6.5, -4, 0.4]  # root position
+q_init[3:7] = [0, 0, 0, 1]  # root rotation
+# set initial velocity (along x,y,z axis) :
+q_init[-6:-3] = [0, 0, 0]
+
+
+# q_goal[0:3] = [6.5,-1,0.4] # straight line
+q_goal[0:3] = [3, -4, 0.4]  # easy goal position
+# q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position
+# set goal velocity (along x,y,z axis) :
+q_goal[-6:-3] = [0, 0, 0]
+
+vf.loadObstacleModel("iai_maps", "room", "room")
+# with displayArrow parameter the viewer will display velocity and acceleration of the
+# center of the robot with 3D arrow. The length scale with the amplitude with a length
+# of 1 for the maximal amplitude.
+v = vf.createViewer(displayArrows=True)
+ps.setInitialConfig(q_init)
+ps.addGoalConfig(q_goal)
+
+ps.addPathOptimizer("RandomShortcut")
+# select kinodynamic methods :
 ps.selectSteeringMethod("Kinodynamic")
 ps.selectDistance("Kinodynamic")
-# the Kinodynamic steering method require a planner that build directionnal roadmap (with oriented edges) as the trajectories cannot always be reversed. 
+# the Kinodynamic steering method require a planner that build directionnal roadmap
+# (with oriented edges) as the trajectories cannot always be reverse.
 ps.selectPathPlanner("BiRRTPlanner")
 
 
-print (ps.solve ())
+print(ps.solve())
 
-# display the computed roadmap. Note that the edges are all represented as straight line and may not show the real motion of the robot between the nodes : 
+# display the computed roadmap. Note that the edges are all represented as straight line
+# and may not show the real motion of the robot between the nodes :
 v.displayRoadmap("rm")
 
-#Alternatively, use the following line instead of ps.solve() to display the roadmap as it's computed (note that it slow down a lot the computation)
-#v.solveAndDisplay('rm',1)
+# Alternatively, use the following line instead of ps.solve() to display the roadmap as
+# it's computed (note that it slow down a lot the computation)
+# v.solveAndDisplay('rm',1)
 
 # Highlight the solution path used in the roadmap
-v.displayPathMap('pm',0)
+v.displayPathMap("pm", 0)
 
-# remove the roadmap for the scene : 
-#v.client.gui.removeFromGroup("rm",v.sceneName)
-#v.client.gui.removeFromGroup("pm",v.sceneName)
+# remove the roadmap for the scene :
+# v.client.gui.removeFromGroup("rm",v.sceneName)
+# v.client.gui.removeFromGroup("pm",v.sceneName)
 
 
 # Connect to a viewer server and play solution paths
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (v)
-#play path before optimization
-pp (0)
 
-# Display the optimized path, with a color variation depending on the velocity along the path (green for null velocity, red for maximal velocity)
-pp.dt=0.1
+pp = PathPlayer(v)
+# play path before optimization
+pp(0)
+
+# Display the optimized path, with a color variation depending on the velocity along the
+# path (green for null velocity, red for maximal velocity)
+pp.dt = 0.1
 pp.displayVelocityPath(1)
 # play path after random shortcut
-pp.dt=0.001
-pp (1)
-
-
-
+pp.dt = 0.001
+pp(1)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f080dec..8088e4d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -36,4 +36,3 @@ ADD_EXECUTABLE(hpp-tutorial-2-server tutorial_2.cc)
 TARGET_LINK_LIBRARIES(hpp-tutorial-2-server hpp-corbaserver::hpp-corbaserver)
 # Install executable
 INSTALL(TARGETS hpp-tutorial-2-server DESTINATION ${CMAKE_INSTALL_BINDIR})
-
diff --git a/src/hpp/corbaserver/manipulation/pr2/__init__.py b/src/hpp/corbaserver/manipulation/pr2/__init__.py
index c0a0894..eefbeca 100644
--- a/src/hpp/corbaserver/manipulation/pr2/__init__.py
+++ b/src/hpp/corbaserver/manipulation/pr2/__init__.py
@@ -16,4 +16,4 @@
 # hpp-corbaserver.  If not, see
 # <http://www.gnu.org/licenses/>.
 
-from .robot import Robot
+from .robot import Robot  # noqa: F401
diff --git a/src/hpp/corbaserver/manipulation/pr2/robot.py b/src/hpp/corbaserver/manipulation/pr2/robot.py
index a4ed2c8..36d026d 100644
--- a/src/hpp/corbaserver/manipulation/pr2/robot.py
+++ b/src/hpp/corbaserver/manipulation/pr2/robot.py
@@ -18,32 +18,37 @@
 
 from hpp.corbaserver.manipulation.robot import Robot as Parent
 
-##
-#  Control of robot PR2 in hpp
-#
-#  This class implements a client to the corba server implemented in
-#  hpp-corbaserver. It derive from class hpp.corbaserver.robot.Robot.
-#
-#  This class is also used to initialize a client to rviz in order to
-#  display configurations of the PR2 robot.
-#
-#  At creation of an instance, the urdf and srdf files are loaded using
-#  idl interface hpp::corbaserver::Robot::loadRobotModel.
-class Robot (Parent):
-    ##
+
+class Robot(Parent):
+    """
+    Control of robot PR2 in hpp
+
+    This class implements a client to the corba server implemented in
+    hpp-corbaserver. It derive from class hpp.corbaserver.robot.Robot.
+
+    This class is also used to initialize a client to rviz in order to
+    display configurations of the PR2 robot.
+
+    At creation of an instance, the urdf and srdf files are loaded using
+    idl interface hpp::corbaserver::Robot::loadRobotModel.
+    """
+
     #  Information to retrieve urdf and srdf files.
     urdfFilename = "package://hpp_tutorial/urdf/pr2.urdf"
     srdfFilename = "package://hpp_tutorial/srdf/pr2_manipulation.srdf"
     rootJointType = "planar"
 
-    ## Constructor
-    # \param compositeName name of the composite robot that will be built later,
-    # \param robotName name of the first robot that is loaded now,
-    # \param load whether to actually load urdf files. Set to no if you only
-    #        want to initialize a corba client to an already initialized
-    #        problem.
-    # \param rootJointType type of root joint among ("freeflyer", "planar",
-    #        "anchor"),
-    def __init__ (self, compositeName, robotName, load = True,
-                  rootJointType = "planar", **kwargs):
-        Parent.__init__ (self, compositeName, robotName, rootJointType, load, **kwargs)
+    def __init__(
+        self, compositeName, robotName, load=True, rootJointType="planar", **kwargs
+    ):
+        """
+        Constructor
+        \\param compositeName name of the composite robot that will be built later,
+        \\param robotName name of the first robot that is loaded now,
+        \\param load whether to actually load urdf files. Set to no if you only
+               want to initialize a corba client to an already initialized
+               problem.
+        \\param rootJointType type of root joint among ("freeflyer", "planar",
+               "anchor"),
+        """
+        Parent.__init__(self, compositeName, robotName, rootJointType, load, **kwargs)
diff --git a/src/hpp/corbaserver/pr2/__init__.py b/src/hpp/corbaserver/pr2/__init__.py
index 7cdea5c..0c95b90 100644
--- a/src/hpp/corbaserver/pr2/__init__.py
+++ b/src/hpp/corbaserver/pr2/__init__.py
@@ -16,4 +16,4 @@
 # hpp_tutorial.  If not, see
 # <http://www.gnu.org/licenses/>.
 
-from .robot import Robot
+from .robot import Robot  # noqa: F401
diff --git a/src/hpp/corbaserver/pr2/robot.py b/src/hpp/corbaserver/pr2/robot.py
index 144a993..8e4935a 100644
--- a/src/hpp/corbaserver/pr2/robot.py
+++ b/src/hpp/corbaserver/pr2/robot.py
@@ -18,23 +18,25 @@
 
 from hpp.corbaserver.robot import Robot as Parent
 
-##
-#  Control of robot PR2 in hpp
-#
-#  This class implements a client to the corba server implemented in
-#  hpp-corbaserver. It derive from class hpp.corbaserver.robot.Robot.
-#
-#  This class is also used to initialize a client to rviz in order to
-#  display configurations of the PR2 robot.
-#
-#  At creation of an instance, the urdf and srdf files are loaded using
-#  idl interface hpp::corbaserver::Robot::loadRobotModel.
-class Robot (Parent):
-    ##
+
+class Robot(Parent):
+    """
+    Control of robot PR2 in hpp
+
+    This class implements a client to the corba server implemented in
+    hpp-corbaserver. It derive from class hpp.corbaserver.robot.Robot.
+
+    This class is also used to initialize a client to rviz in order to
+    display configurations of the PR2 robot.
+
+    At creation of an instance, the urdf and srdf files are loaded using
+    idl interface hpp::corbaserver::Robot::loadRobotModel.
+    """
+
     #  Information to retrieve urdf and srdf files.
     urdfFilename = "package://hpp_tutorial/urdf/pr2.urdf"
     srdfFilename = "package://hpp_tutorial/srdf/pr2.srdf"
     rootJointType = "planar"
 
-    def __init__ (self, robotName, load = True, **kwargs):
-        Parent.__init__ (self, robotName, self.rootJointType, load, **kwargs)
+    def __init__(self, robotName, load=True, **kwargs):
+        Parent.__init__(self, robotName, self.rootJointType, load, **kwargs)
diff --git a/src/hpp/corbaserver/rod/__init__.py b/src/hpp/corbaserver/rod/__init__.py
index 7cdea5c..0c95b90 100644
--- a/src/hpp/corbaserver/rod/__init__.py
+++ b/src/hpp/corbaserver/rod/__init__.py
@@ -16,4 +16,4 @@
 # hpp_tutorial.  If not, see
 # <http://www.gnu.org/licenses/>.
 
-from .robot import Robot
+from .robot import Robot  # noqa: F401
diff --git a/src/hpp/corbaserver/rod/robot.py b/src/hpp/corbaserver/rod/robot.py
index 8979db9..b135654 100644
--- a/src/hpp/corbaserver/rod/robot.py
+++ b/src/hpp/corbaserver/rod/robot.py
@@ -18,19 +18,20 @@
 
 from hpp.corbaserver.robot import Robot as Parent
 
-##
-#  Rod robot in hpp
-#
-#  This class implements a client to the corba server implemented in
-#  hpp-corbaserver. It derive from class hpp.corbaserver.robot.Robot.
-#
-#  This class is also used to initialize a client to rviz in order to
-#  display configurations of the PR2 robot.
-#
-#  At creation of an instance, the urdf and srdf files are loaded using
-#  idl interface hpp::corbaserver::Robot::loadRobotModel.
-class Robot (Parent):
-    ##
+
+class Robot(Parent):
+    """
+    Rod robot in hpp
+
+    This class implements a client to the corba server implemented in
+    hpp-corbaserver. It derive from class hpp.corbaserver.robot.Robot.
+
+    This class is also used to initialize a client to rviz in order to
+    display configurations of the PR2 robot.
+
+    At creation of an instance, the urdf and srdf files are loaded using
+    idl interface hpp::corbaserver::Robot::loadRobotModel."""
+
     #  Information to retrieve urdf and srdf files.
     packageName = "hpp_tutorial"
     meshPackageName = "hpp_tutorial"
@@ -41,6 +42,6 @@ class Robot (Parent):
     urdfSuffix = ""
     srdfSuffix = ""
 
-    def __init__ (self, robotName, load = True):
-        Parent.__init__ (self, robotName, self.rootJointType, load)
+    def __init__(self, robotName, load=True):
+        Parent.__init__(self, robotName, self.rootJointType, load)
         self.tf_root = "base_link"
diff --git a/src/tutorial_1.cc b/src/tutorial_1.cc
index cf0df25..f619338 100644
--- a/src/tutorial_1.cc
+++ b/src/tutorial_1.cc
@@ -14,60 +14,64 @@
 // received a copy of the GNU Lesser General Public License along with
 // hpp-core. If not, see <http://www.gnu.org/licenses/>.
 
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/plugin.hh>
+#include <hpp/core/problem-solver.hh>
 #include <hpp/pinocchio/configuration.hh>
 #include <hpp/pinocchio/device.hh>
 #include <hpp/pinocchio/joint.hh>
 #include <hpp/pinocchio/urdf/util.hh>
 
-#include <hpp/core/problem-solver.hh>
-#include <hpp/core/path-vector.hh>
-#include <hpp/core/plugin.hh>
-
 using namespace hpp::pinocchio;
 using namespace hpp::core;
 
-int main ()
-{
-  ProblemSolverPtr_t ps = ProblemSolver::create ();
+int main() {
+  ProblemSolverPtr_t ps = ProblemSolver::create();
 
   // Create robot
-  DevicePtr_t device = ps->createRobot ("pr2");
-  hpp::pinocchio::urdf::loadRobotModel (device,
-      "planar", "hpp_tutorial", "pr2", "", "");
-  device->controlComputation ((Computation_t)(JOINT_POSITION | JACOBIAN));
-  ps->robot (device);
+  DevicePtr_t device = ps->createRobot("pr2");
+  hpp::pinocchio::urdf::loadRobotModel(device, "planar", "hpp_tutorial", "pr2",
+                                       "", "");
+  device->controlComputation((Computation_t)(JOINT_POSITION | JACOBIAN));
+  ps->robot(device);
 
-  device->rootJoint()->lowerBound (0, -4);
-  device->rootJoint()->upperBound (0, -3);
-  device->rootJoint()->lowerBound (1, -5);
-  device->rootJoint()->upperBound (1, -3);
+  device->rootJoint()->lowerBound(0, -4);
+  device->rootJoint()->upperBound(0, -3);
+  device->rootJoint()->lowerBound(1, -5);
+  device->rootJoint()->upperBound(1, -3);
 
   // Add obstacle
-  DevicePtr_t obstacle = Device::create ("kitchen");
-  hpp::pinocchio::urdf::loadUrdfModel (obstacle,
-      "anchor", "iai_maps", "kitchen_area");
-  obstacle->controlComputation (JOINT_POSITION);
-  ps->addObstacle (obstacle, true, true);
+  DevicePtr_t obstacle = Device::create("kitchen");
+  hpp::pinocchio::urdf::loadUrdfModel(obstacle, "anchor", "iai_maps",
+                                      "kitchen_area");
+  obstacle->controlComputation(JOINT_POSITION);
+  ps->addObstacle(obstacle, true, true);
 
   // Create initial and final configuration
-  Configuration_t q_init (device->currentConfiguration());
+  Configuration_t q_init(device->currentConfiguration());
   q_init.head<2>() << -3.2, -4;
-  q_init[device->getJointByName("torso_lift_joint")->rankInConfiguration()] = 0.2;
-  ps->initConfig (ConfigurationPtr_t(new Configuration_t(q_init)));
+  q_init[device->getJointByName("torso_lift_joint")->rankInConfiguration()] =
+      0.2;
+  ps->initConfig(ConfigurationPtr_t(new Configuration_t(q_init)));
 
-  Configuration_t q_goal (q_init);
+  Configuration_t q_goal(q_init);
   q_goal.head<2>() << -3.2, -4;
   q_goal[device->getJointByName("torso_lift_joint")->rankInConfiguration()] = 0;
-  q_goal[device->getJointByName("l_shoulder_lift_joint")->rankInConfiguration()] =  0.5;
-  q_goal[device->getJointByName("l_elbow_flex_joint"   )->rankInConfiguration()] = -0.5;
-  q_goal[device->getJointByName("r_shoulder_lift_joint")->rankInConfiguration()] =  0.5;
-  q_goal[device->getJointByName("r_elbow_flex_joint"   )->rankInConfiguration()] = -0.5;
-  ps->addGoalConfig (ConfigurationPtr_t(new Configuration_t(q_goal)));
+  q_goal[device->getJointByName("l_shoulder_lift_joint")
+             ->rankInConfiguration()] = 0.5;
+  q_goal[device->getJointByName("l_elbow_flex_joint")->rankInConfiguration()] =
+      -0.5;
+  q_goal[device->getJointByName("r_shoulder_lift_joint")
+             ->rankInConfiguration()] = 0.5;
+  q_goal[device->getJointByName("r_elbow_flex_joint")->rankInConfiguration()] =
+      -0.5;
+  ps->addGoalConfig(ConfigurationPtr_t(new Configuration_t(q_goal)));
 
   bool loaded;
   try {
-    std::string filename = plugin::findPluginLibrary ("spline-gradient-based.so");
-    loaded = plugin::loadPlugin (filename, ps);
+    std::string filename =
+        plugin::findPluginLibrary("spline-gradient-based.so");
+    loaded = plugin::loadPlugin(filename, ps);
   } catch (const std::invalid_argument&) {
     loaded = false;
   }
@@ -82,17 +86,17 @@ int main ()
   std::cout << "# Solution path.\n";
 
   std::cout << "path = list ()" << std::endl;
-  PathPtr_t path (ps->paths ().back ());
-  value_type L (path->length ());
+  PathPtr_t path(ps->paths().back());
+  value_type L(path->length());
   bool success;
   Configuration_t q;
-  for (value_type t=0; t<L; t+=.01) {
-    q = path->eval (t, success);
-    assert (success);
-    std::cout << "path.append (" << displayConfig (q) << ")" << std::endl;
+  for (value_type t = 0; t < L; t += .01) {
+    q = path->eval(t, success);
+    assert(success);
+    std::cout << "path.append (" << displayConfig(q) << ")" << std::endl;
   }
-  q = path->eval (L, success);
-  std::cout << "path.append (" << displayConfig (q) << ")" << std::endl;
+  q = path->eval(L, success);
+  std::cout << "path.append (" << displayConfig(q) << ")" << std::endl;
 
   return 0;
 }
diff --git a/src/tutorial_2.cc b/src/tutorial_2.cc
index 3c22e4a..f80f323 100644
--- a/src/tutorial_2.cc
+++ b/src/tutorial_2.cc
@@ -16,10 +16,8 @@
 // hpp_tutorial  If not, see
 // <http://www.gnu.org/licenses/>.
 
-#include <hpp/util/pointer.hh>
-
-#include <hpp/core/configuration-shooter/uniform.hh>
 #include <hpp/core/config-validations.hh>
+#include <hpp/core/configuration-shooter/uniform.hh>
 #include <hpp/core/connected-component.hh>
 #include <hpp/core/constraint-set.hh>
 #include <hpp/core/path-planner.hh>
@@ -28,130 +26,125 @@
 #include <hpp/core/problem.hh>
 #include <hpp/core/roadmap.hh>
 #include <hpp/core/steering-method.hh>
+#include <hpp/util/pointer.hh>
 
 #include "hpp/corbaserver/server.hh"
 
 namespace hpp {
-  namespace tutorial {
-    // forward declaration of class Planner
-    HPP_PREDEF_CLASS (Planner);
-    // Planner objects are manipulated only via shared pointers
-    typedef shared_ptr <Planner> PlannerPtr_t;
-    typedef core::value_type value_type;
+namespace tutorial {
+// forward declaration of class Planner
+HPP_PREDEF_CLASS(Planner);
+// Planner objects are manipulated only via shared pointers
+typedef shared_ptr<Planner> PlannerPtr_t;
+typedef core::value_type value_type;
 
-    /// Example of path planner
-    class Planner : public core::PathPlanner
-    {
-    public:
-      /// Create an instance and return a shared pointer to the instance
-      static PlannerPtr_t create (const core::ProblemConstPtr_t& problem,
-				  const core::RoadmapPtr_t& roadmap)
-      {
-	Planner* ptr = new Planner (problem, roadmap);
-	PlannerPtr_t shPtr (ptr);
-	ptr->init (shPtr);
-	return shPtr;
-      }
+/// Example of path planner
+class Planner : public core::PathPlanner {
+ public:
+  /// Create an instance and return a shared pointer to the instance
+  static PlannerPtr_t create(const core::ProblemConstPtr_t& problem,
+                             const core::RoadmapPtr_t& roadmap) {
+    Planner* ptr = new Planner(problem, roadmap);
+    PlannerPtr_t shPtr(ptr);
+    ptr->init(shPtr);
+    return shPtr;
+  }
 
-      /// One step of extension.
-      ///
-      /// This method implements one step of your algorithm. The method
-      /// will be called iteratively until one goal configuration is accessible
-      /// from the initial configuration.
-      ///
-      /// We will see how to implement a basic PRM algorithm.
-      virtual void oneStep ()
-      {
-	// Retrieve the robot the problem has been defined for.
-	pinocchio::DevicePtr_t robot (problem()->robot ());
-	// Retrieve the path validation algorithm associated to the problem
-	core::PathValidationPtr_t pathValidation (problem()->pathValidation ());
-	// Retrieve configuration validation methods associated to the problem
-	core::ConfigValidationsPtr_t configValidations
-	  (problem()->configValidations ());
-	// Retrieve the steering method
-	core::SteeringMethodPtr_t sm (problem()->steeringMethod ());
-	// Retrieve the constraints the robot is subject to
-	core::ConstraintSetPtr_t constraints (problem()->constraints ());
-	// Retrieve roadmap of the path planner
-	core::RoadmapPtr_t r (roadmap ());
-	// shoot a valid random configuration
-	core::Configuration_t qrand (robot->configSize ());
-	// Report of configuration validation: unused here
-	core::ValidationReportPtr_t validationReport;
-	do {
-	  shooter_->shoot (qrand);
-	} while (!configValidations->validate (qrand, validationReport));
-	// Add qrand as a new node
-	core::NodePtr_t newNode = r->addNode (qrand);
-	// try to connect the random configuration to each connected component
-	// of the roadmap.
-	for (core::ConnectedComponents_t::const_iterator itcc =
-	       r->connectedComponents ().begin ();
-	     itcc != r->connectedComponents ().end (); ++itcc) {
-	  core::ConnectedComponentPtr_t cc = *itcc;
-	  // except its own connected component of course
-	  if (cc != newNode->connectedComponent ()) {
-	    value_type d;
-	    // Get nearest node to qrand in connected component
-	    core::NodePtr_t nearest = r->nearestNode (qrand, cc, d);
-	    core::ConfigurationPtr_t qnear = nearest->configuration ();
-	    // Create local path between qnear and qrand
-	    core::PathPtr_t localPath = (*sm) (*qnear, qrand);
-	    // validate local path
-	    core::PathPtr_t validPart;
-	    // report on path validation: unused here
-	    core::PathValidationReportPtr_t report;
-	    if (pathValidation->validate (localPath, false, validPart,
-					  report)) {
-	      // Create node and edges with qrand and the local path
-	      r->addEdge (nearest, newNode, localPath);
-	      r->addEdge (newNode, nearest, localPath->reverse ());
-	    }
-	  }	  
-	}
-      }
-    protected:
-      /// Protected constructor
-      /// Users need to call Planner::create in order to create instances.
-      Planner (const core::ProblemConstPtr_t& problem,
-	       const core::RoadmapPtr_t& roadmap) :
-	core::PathPlanner (problem, roadmap),
-	shooter_ (core::configurationShooter::Uniform::create
-                  (problem->robot()))
-      {
-      }
-      /// Store weak pointer to itself
-      void init (const PlannerWkPtr_t& weak)
-      {
-	core::PathPlanner::init (weak);
-	weakPtr_ = weak;
+  /// One step of extension.
+  ///
+  /// This method implements one step of your algorithm. The method
+  /// will be called iteratively until one goal configuration is accessible
+  /// from the initial configuration.
+  ///
+  /// We will see how to implement a basic PRM algorithm.
+  virtual void oneStep() {
+    // Retrieve the robot the problem has been defined for.
+    pinocchio::DevicePtr_t robot(problem()->robot());
+    // Retrieve the path validation algorithm associated to the problem
+    core::PathValidationPtr_t pathValidation(problem()->pathValidation());
+    // Retrieve configuration validation methods associated to the problem
+    core::ConfigValidationsPtr_t configValidations(
+        problem()->configValidations());
+    // Retrieve the steering method
+    core::SteeringMethodPtr_t sm(problem()->steeringMethod());
+    // Retrieve the constraints the robot is subject to
+    core::ConstraintSetPtr_t constraints(problem()->constraints());
+    // Retrieve roadmap of the path planner
+    core::RoadmapPtr_t r(roadmap());
+    // shoot a valid random configuration
+    core::Configuration_t qrand(robot->configSize());
+    // Report of configuration validation: unused here
+    core::ValidationReportPtr_t validationReport;
+    do {
+      shooter_->shoot(qrand);
+    } while (!configValidations->validate(qrand, validationReport));
+    // Add qrand as a new node
+    core::NodePtr_t newNode = r->addNode(qrand);
+    // try to connect the random configuration to each connected component
+    // of the roadmap.
+    for (core::ConnectedComponents_t::const_iterator itcc =
+             r->connectedComponents().begin();
+         itcc != r->connectedComponents().end(); ++itcc) {
+      core::ConnectedComponentPtr_t cc = *itcc;
+      // except its own connected component of course
+      if (cc != newNode->connectedComponent()) {
+        value_type d;
+        // Get nearest node to qrand in connected component
+        core::NodePtr_t nearest = r->nearestNode(qrand, cc, d);
+        core::ConfigurationPtr_t qnear = nearest->configuration();
+        // Create local path between qnear and qrand
+        core::PathPtr_t localPath = (*sm)(*qnear, qrand);
+        // validate local path
+        core::PathPtr_t validPart;
+        // report on path validation: unused here
+        core::PathValidationReportPtr_t report;
+        if (pathValidation->validate(localPath, false, validPart, report)) {
+          // Create node and edges with qrand and the local path
+          r->addEdge(nearest, newNode, localPath);
+          r->addEdge(newNode, nearest, localPath->reverse());
+        }
       }
-    private:
-      /// Configuration shooter to uniformly shoot random configurations
-      core::configurationShooter::UniformPtr_t shooter_;
-      /// weak pointer to itself
-      PlannerWkPtr_t weakPtr_;
-    }; // class Planner
-  } // namespace tutorial
-} // namespace hpp
+    }
+  }
+
+ protected:
+  /// Protected constructor
+  /// Users need to call Planner::create in order to create instances.
+  Planner(const core::ProblemConstPtr_t& problem,
+          const core::RoadmapPtr_t& roadmap)
+      : core::PathPlanner(problem, roadmap),
+        shooter_(
+            core::configurationShooter::Uniform::create(problem->robot())) {}
+  /// Store weak pointer to itself
+  void init(const PlannerWkPtr_t& weak) {
+    core::PathPlanner::init(weak);
+    weakPtr_ = weak;
+  }
+
+ private:
+  /// Configuration shooter to uniformly shoot random configurations
+  core::configurationShooter::UniformPtr_t shooter_;
+  /// weak pointer to itself
+  PlannerWkPtr_t weakPtr_;
+};  // class Planner
+}  // namespace tutorial
+}  // namespace hpp
 
 // main function of the corba server
-int main (int argc, const char* argv[])
-{
+int main(int argc, const char* argv[]) {
   // create a ProblemSolver instance.
   // This class is a container that does the interface between hpp-core library
   // and component to be running in a middleware like CORBA or ROS.
   hpp::core::ProblemSolverPtr_t problemSolver =
-    hpp::core::ProblemSolver::create ();
+      hpp::core::ProblemSolver::create();
   // Add the new planner type in order to be able to select it from python
   // client.
-  hpp::core::PathPlannerBuilder_t factory (hpp::tutorial::Planner::create);
-  problemSolver->pathPlanners.add ("PRM", factory);
+  hpp::core::PathPlannerBuilder_t factory(hpp::tutorial::Planner::create);
+  problemSolver->pathPlanners.add("PRM", factory);
   // Create the CORBA server.
-  hpp::corbaServer::Server server (problemSolver, argc, argv, true);
+  hpp::corbaServer::Server server(problemSolver, argc, argv, true);
   // Start the CORBA server.
-  server.startCorbaServer ();
+  server.startCorbaServer();
   // Wait for CORBA requests.
-  server.processRequest (true);
+  server.processRequest(true);
 }
diff --git a/srdf/kitchen_area.srdf b/srdf/kitchen_area.srdf
index 699242c..bd9348a 100644
--- a/srdf/kitchen_area.srdf
+++ b/srdf/kitchen_area.srdf
@@ -239,9 +239,9 @@
   size_x="0.25" size_y="0.38" size_z="0.025"
   />-->
   <!-- Finally, the 5 main blocks of the kitchen -->
-  <!-- 
+  <!--
   <island_block block_pos="-2.1 -4.94 0" block_rpy="0 0 3.141"/>
-  
+
   <oven_block block_pos="-5.27 -6.85 0" block_rpy="0 0 0"/>
   -->
   <contact name="white_counter_top_sink">
diff --git a/urdf/pr2.urdf b/urdf/pr2.urdf
index 4745971..c8cd2bf 100644
--- a/urdf/pr2.urdf
+++ b/urdf/pr2.urdf
@@ -57,10 +57,10 @@
   <!-- Now we can start using the macros included above to define the actual PR2 -->
   <!-- The first use of a macro.  This one was defined in base.urdf.xacro above.
        A macro like this will expand to a set of link and joint definitions, and to additional
-       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name, 
-       that equals "base", and uses it to generate names for its component links and joints 
-       (e.g., base_link).  The included origin block is also an argument to the macro.  By convention, 
-       the origin block defines where the component is w.r.t its parent (in this case the parent 
+       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name,
+       that equals "base", and uses it to generate names for its component links and joints
+       (e.g., base_link).  The included origin block is also an argument to the macro.  By convention,
+       the origin block defines where the component is w.r.t its parent (in this case the parent
        is the world frame). For more, see http://www.ros.org/wiki/xacro -->
   <link name="base_link">
     <inertial>
-- 
GitLab