diff --git a/.travis/script.py b/.travis/script.py
index ed975be81e799a3b95cfd12539a04146965a258a..5a4337c36124e2b59e937e148be00aae4284082e 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 95632c9be70f3732b2dbe03b094ea88b4d438102..b6ee4727827eb9bff4f222d69d8c2206c276a8cd 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 5c303b23d747ab0e888e8065167ccf0078497cee..c5a1eee515c67a4dffcac13d54b4e52c2bdac3c3 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 fda35eea0a8bd9e3c03d87f007f34fd8167cd421..702b0cd8fdd3cfa3dff661c8465f5d0135f8dad3 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 395cddadf419e6be2a2a9b3860c63ac65d5f0695..b00bd71da1cbd47edbbb2547272f41bd9506028c 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 4a67b4fe4381cfcd5ea8b5059b16b00591eea1b7..8f430e79f52ebc5b7dbaf1f62aec3bc88f234250 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 21fe6c613caf2fab83db32446bf74fa0a3a344f8..e56fcb89e846761e531ccb9dad6e630ca9c5352b 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 e9df910a5a68ec5bdf483c19070769e179558104..49c0b707f7d561e3142483a68094e78a8a327626 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 735307f9165137719dd188803402f7b40aa14ce8..ddb2584313aac28beb506986f2a3b94089977f5e 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 aa49fa36d0fd9c573b55a450f98e9a7074070517..83c29182de9a1c1b0a987b8d474eedca6a0cbf4c 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 f080decfffa4571d40604e2347a3ed20bd340dd2..8088e4d1d2524e821e7813a93500e63d30cdfd26 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 c0a089421804efc0b7c0563c5147c5b64e21ac8c..eefbecac05d90288024cfe28da5e7f1e1b6ef3f1 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 a4ed2c89f4588ca74688c550487f5fda3eeec1e6..36d026dc6752e3a941bfbecff66ba74e794abf44 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 7cdea5c2b1e39feae65b8ec2d0afbdc1f1085c50..0c95b905709fcbab6261cce5e7547699b7ce3e99 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 144a9932020f8b257fa24fe78f946a4b81081da7..8e4935a04aae443ef121b409e3e25f4d04f7f323 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 7cdea5c2b1e39feae65b8ec2d0afbdc1f1085c50..0c95b905709fcbab6261cce5e7547699b7ce3e99 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 8979db925d0f91eab017ac604d3f2908f4f10bd4..b135654e36d8ce892df871391a2886e2ac6295d5 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 cf0df25416fb9bf9be0a691c346ba68b506d4de2..f6193380edbc332bb85188fdf1a4cdad9bb69270 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 3c22e4a3ab7b51aeb2f5c685aa2629df11389b54..f80f323f6542f181d918d6cce3adfe4f812d2706 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 699242cccd3f2d709e6a23c711dde468068c8ca2..bd9348a4fb1d0fe2147e00b7309123f5a869089b 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 4745971e4184eabb108e48560814d0246dd70b39..c8cd2bf0e254d731c3adb166da15d3c99f3c5810 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>