Merge pull request #3 from stonneau/master

release of rbprm-corba for ijrr resubmission

.gitignore

@@ -30,5 +30,6 @@
 *.app
 
 build/
+profile/logs
 
 CMakeLists.txt.user

CMakeLists.txt

@@ -29,7 +29,7 @@ SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
 SET(PROJECT_URL "")
 
 # Set to 1 for profiling
-#~ add_definitions(-DPROFILE)
+#add_definitions(-DPROFILE)
 
 SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
 
@@ -50,6 +50,7 @@ ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 3")
 ADD_REQUIRED_DEPENDENCY("hpp-rbprm")
 ADD_REQUIRED_DEPENDENCY("omniORB4 >= 4.1.4")
 ADD_REQUIRED_DEPENDENCY("hpp-model-urdf >= 3")
+ADD_REQUIRED_DEPENDENCY("hpp-affordance-corba")
 
 PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
 ADD_SUBDIRECTORY(src)
@@ -90,6 +91,7 @@ install(FILES
   data/urdf/groundcrouch.urdf
   data/urdf/darpa.urdf
   data/urdf/car.urdf
+  data/urdf/polaris.urdf
   #~ data/urdf/scene2.urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
   )
@@ -120,6 +122,7 @@ install(FILES
   data/srdf/groundcrouch.srdf
   data/srdf/darpa.srdf
   data/srdf/car.srdf
+  data/srdf/polaris.srdf
   #~ data/srdf/scene2.srdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
   )
@@ -128,11 +131,13 @@ install(FILES
 	data/meshes/darpareduced.stl
 	data/meshes/car.stl
 	data/meshes/car_simple.stl
+	data/meshes/ground.stl
+	data/meshes/ground_table.stl
 	data/meshes/hrp2_trunk.stl
 	data/meshes/hrp2_trunk_body.stl
 	data/meshes/hrp2_trunk_torso.stl
-        data/meshes/hrp2_trunk_body_view.dae
-        data/meshes/hrp2_trunk_torso_view.dae
+	data/meshes/hrp2_trunk_body_view.dae
+	data/meshes/hrp2_trunk_torso_view.dae
 	data/meshes/hrp2_rom.stl
 	data/meshes/hrp2_larm_rom.stl
 	data/meshes/hrp2_rarm_rom.stl
@@ -148,11 +153,13 @@ install(FILES
 	data/meshes/truck.stl
 	data/meshes/truck_vision.stl
 	data/meshes/stair_bauzil.stl
-        data/meshes/stair_bauzil_reduced.stl
+	data/meshes/stair_bauzil_reduced.stl
 	data/meshes/climb.stl
 	data/meshes/stepladder.stl
 	data/meshes/chair.stl
 	data/meshes/car.stl
+        data/meshes/polaris.stl
+        data/meshes/polaris_reduced.stl
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
   )
 install(FILES
@@ -167,6 +174,5 @@ install(FILES
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq
   )
 
-PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
 SETUP_PROJECT_FINALIZE()
 

README.md

@@ -12,22 +12,17 @@ Please refer to this [link](https://github.com/stonneau/hpp-rbprm) for informati
 
 To install hpp-rbprm-corba:
 
-  1. install HPP-RBPRM
+  1. Install HPP-RBPRM and its dependencies
 	- see https://github.com/stonneau/hpp-rbprm
 
-  2. If necessary, install hpp-corba-template
+  2. Install HPP-AFFORDANCE-CORBA along with its dependencies
+  - see https://github.com/anna-seppala/hpp-affordance-corba
 
-			git clone --recursive https://github.com/laas/hpp-template-corba.git
-			cd hpp-template-corba/
-			mkdir build && cd build
-			cmake ..
-			make install
-
-  2. Use CMake to install the library. For instance:
+  3. Use CMake to install the library. For instance:
 
 			mkdir $HPP_RBPRM_CORBA_DIR/build
 			cd $HPP_RBPRM_CORBA_DIR/build
-			cmake ..	
+			cd cmake ..	
 			make install
 	
 
@@ -48,22 +43,14 @@ To install hpp-rbprm-corba:
 	https://github.com/iit-DLSLab/hyq-description
 
 
-    ```$ rosrun xacro xacro.py  hyq_description/robots/hyq.urdf.xacro  -o  hyq.urdf```
+    ```$ rosrun xacro xacro.py  hyq_description/robots/hyq_model.urdf.xacro -o  hyq.urdf```
 
   - Make sure to install hyq.urdf in $HPP_DEVEL_DIR/install/share/hpp-rbprm-corba/
-  
-  - Also, create an empty hyq.srdf file in  $HPP_DEVEL_DIR/install/share/hpp-rbprm-corba/srdf
-
-  - The planning is decomposed in two phases / scripts. First, a root path is computed (\*_path.py files). Then, the contacts are generated along the computed path (\*_interp.py files). The scripts are located in the folder /scripts/scenarios.
 
-  - To only plan and see the root path, run:
+  - The planning is decomposed in two phases / scripts. First, a root path is computed (\*_path.py files). Then, the contacts are generated along the computed path (\*_interp.py files). The scripts are located in the folder /scripts/scenarios/demos.
 
+  - To see the different steps of the process run
 
     ```$ ./run.sh darpa_hyq_path.py```
 
-  - To generate the complete contact sequence, run:
-
-
-    ```$ ./run.sh darpa_hyq_interp.py```
-
-  The scripts include comments explaining the different calls to the library.
+  The script include comments explaining the different calls to the library. You can call the different methods a() ... d() to see the different steps of the planning.

script/scenarios/darpa_hyq_interp.py → affordance-tests/darpa_hyq_interp.py

@@ -88,4 +88,13 @@ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
 i = 0;
 fullBody.draw(configs[i],r); i=i+1; i-1
 
+#~ collisions = 0;
+#~ for config in configs:
+  #~ if fullBody.isConfigValid(config)[0] == False:
+    #~ collisions += 1
+#~ 
+#~ collisions
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (ps.robot.client.basic, r)
 

script/scenarios/darpa_hyq_path.py → affordance-tests/darpa_hyq_path.py

@@ -21,11 +21,10 @@ rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
 # The following lines set constraint on the valid configurations:
 # a configuration is valid only if all limbs can create a contact ...
 rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
-# ... and only if a contact surface includes the gravity
-rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.5)
-rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.5)
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support', 'Lean'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
 # We also bound the rotations of the torso.
 rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
 
@@ -45,11 +44,15 @@ ps.addPathOptimizer("RandomShortcut")
 ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+
 # Choosing RBPRM shooter and path validation methods.
 # Note that the standard RRT algorithm is used.
 ps.client.problem.selectConFigurationShooter("RbprmShooter")
 ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-r.loadObstacleModel (packageName, "darpa", "planning")
 
 # Solve the problem
 t = ps.solve ()
@@ -57,4 +60,4 @@ t = ps.solve ()
 # Playing the computed path
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
-#~ pp (1)
+pp (1)

affordance-tests/test1.py

+# Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+
+# Importing Gepetto viewer helper class
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+# URDF file describing the trunk of the robot HyQ
+urdfName = 'hyq_trunk_large'
+# URDF files describing the reachable workspace of each limb of HyQ
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+# Creating an instance of the helper class, and loading the robot
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+# The following lines set constraint on the valid configurations:
+# a configuration is valid only if all limbs can create a contact ...
+rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+# ... and only if a contact surface includes the gravity
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean'])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+r = Viewer (ps)
+r.loadObstacleModel (packageName, "darpa", "planning")
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+
+# Choosing a path optimizer
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance import Client
+c = Client ()
+c.affordance.analyseAll ()
+
+objs = c.affordance.getAffordancePoints ("Support")
+
+import random
+count = 0
+for aff in objs:
+	colour = random.random()
+	for tri in aff:
+		r.client.gui.addTriangleFace('tri' + str(count), tri[0], tri[1], tri[2], [colour, 1, 0.5, 1])
+		r.client.gui.addToGroup('tri' + str(count), 'planning')
+		count += 1
+
+# Choosing RBPRM shooter and path validation methods.
+# Note that the standard RRT algorithm is used.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+# Solve the problem
+t = ps.solve ()
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp (1)

affordance-tests/test2.py

+# Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hyq_trunk_large'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+
+rbprmBuilder.setFilter(['hyq_rhleg_rom']) # , 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
+
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean'])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+r = Viewer (ps)
+r.loadObstacleModel (packageName, "darpa", "planning")
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance import Client
+c = Client ()
+c.affordance.analyseAll ()
+
+objs = c.affordance.getAffordancePoints ("Support")
+
+import random
+count = 0
+for aff in objs:
+	colour = random.random()
+	for tri in aff:
+		r.client.gui.addTriangleFace('tri' + str(count), tri[0], tri[1], tri[2], [colour, 1, 0.5, 1])
+		r.client.gui.addToGroup('tri' + str(count), 'planning')
+		count += 1
+
+# Choosing RBPRM shooter and path validation methods.
+# Note that the standard RRT algorithm is used.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+# Solve the problem
+t = ps.solve ()
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp (0)

script/scenarios/stair_bauzil_hyq_path.py → affordance-tests/test3.py

+# Importing helper class for setting up a reachability planning problem
 from hpp.corbaserver.rbprm.rbprmbuilder import Builder
 from hpp.gepetto import Viewer
 
 rootJointType = 'freeflyer'
 packageName = 'hpp-rbprm-corba'
 meshPackageName = 'hpp-rbprm-corba'
-urdfName = 'hyq_trunk'
-urdfNameRom = ['hyq_lhleg_rom','hyq_lhleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfName = 'hyq_trunk_large'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
 urdfSuffix = ""
 srdfSuffix = ""
 
 rbprmBuilder = Builder ()
-
 rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
-rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,4, -1, 1, 0, 2])
-rbprmBuilder.setFilter(['hyq_lhleg_rom' , 'hyq_rfleg_rom'])
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
 
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+rbprmBuilder.setFilter(['hyq_rhleg_rom']) 
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean'])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
 
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 ps = ProblemSolver( rbprmBuilder )
-
 r = Viewer (ps)
 
-
+# Setting initial and goal configurations
 q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-1, 0, 0.4]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-#~ q_init [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
-
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
 q_goal = q_init [::]
-#~ q_goal [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
-q_goal [0:3] = [3.6, 0, 1.1]; r (q_goal)
-#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
 
-#~ ps.addPathOptimizer("GradientBased")
-ps.addPathOptimizer("RandomShortcut")
 ps.setInitialConfig (q_init)
 ps.addGoalConfig (q_goal)
 
+import random
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.visualiseAffordances('Support', r, 'planning', [0.25, 0.5, 0.5])
+
+# Choosing RBPRM shooter and path validation methods.
+# Note that the standard RRT algorithm is used.
 ps.client.problem.selectConFigurationShooter("RbprmShooter")
 ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
-r.loadObstacleModel (packageName, "stair_bauzil", "planning")
-t = ps.solve ()
-
-print ("solving time " + t);
 
+# Solve the problem
+t = ps.solve ()
 
+# Playing the computed path
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
-#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-#~ 
-#~ pp (2)
-#~ pp (0)
-
-#~ pp (1)
-#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
-r (q_init)
+pp (0)

affordance-tests/test4.py

+# Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hyq_trunk_large'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+
+rbprmBuilder.setFilter(['hyq_lhleg_rom', 'hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']) 
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support', 'Lean'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+r = Viewer (ps)
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+
+# Choosing RBPRM shooter and path validation methods.
+# Note that the standard RRT algorithm is used.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+# Solve the problem
+t = ps.solve ()
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp (0)

affordance-tests/test5.py

+# Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hyq_trunk_large'
+urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+
+rbprmBuilder.setFilter(['hyq_lhleg_rom', 'hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']) 
+rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean'])
+rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support', 'Lean'])
+rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
+# We also bound the rotations of the torso.
+rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
+
+# Creating an instance of HPP problem solver and the viewer
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+ps = ProblemSolver( rbprmBuilder )
+r = Viewer (ps)
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_goal = q_init [::]
+q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel (packageName, "darpa", "planning", r)
+afftool.loadObstacleModel ("hpp-ompl-benchmark", "cubicles_robot", "robo", r)
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+
+afftool.deleteAffordances(r,'robo/base_link_0')
+ps.moveObstacle('robo/base_link_0', [0,-0.75,0, 0.5,0.5,0.5,0.5])
+r.computeObjectPosition()
+afftool.analyseObject('robo/base_link_0')
+afftool.visualiseAffordances('Support', r, [0.75, 0.75, 0.1], 'robo/base_link_0')
+
+# Choosing RBPRM shooter and path validation methods.
+# Note that the standard RRT algorithm is used.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+# Solve the problem
+t = ps.solve ()
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp (0)

cmake @ d9578712

-Subproject commit 9965dbcd2a37edbc98707573c05512d838e64c10
+Subproject commit d9578712fbc457a528e121bff38ffaa294819686

data/srdf/polaris.srdf

+<?xml version="1.0"?>
+<robot name="polaris">
+</robot>

data/srdf/scene_wall2.srdf

+<?xml version="1.0"?>
+<robot name="scene_wall2">
+  <handle name="handle">
+    <position> 0 0 0 1 0 0 0 </position>
+    <link name="base_link"/>
+  </handle>
+  <handle name="handle2">
+    <position> 0 0 0
+      0 0 0.7071067811865476 0.7071067811865476 </position>
+    <link name="base_link"/>
+  </handle>
+  <contact name="box_surface">
+    <link name="base_link"/>
+    <point>-0.025 -0.025 -0.025   -0.025 0.025 -0.025   -0.025 -0.025 0.025   -0.025 0.025 0.025
+           +0.025 -0.025 -0.025   +0.025 0.025 -0.025   +0.025 -0.025 0.025   +0.025 0.025 0.025 </point>
+    <triangle> 0 2 1 4 5 6</triangle>
+  </contact>
+</robot>

data/urdf/ground.urdf

@@ -3,7 +3,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package:///../hpp-rbprm-corba/meshes/ground.stl"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/ground.stl"/>
       </geometry>
       <material name="white">
         <color rgba="1 1 1 1"/>