.gitignore

@@ -6,6 +6,7 @@
 
 # Precompiled Headers
 *.gch
+*.txt
 *.pch
 
 # Compiled Dynamic libraries
@@ -23,6 +24,9 @@
 *.lib
 *.pyc
 *.~
+*_config
+*_unstable
+*_coplanar
 
 # Executables
 *.exe
@@ -30,5 +34,7 @@
 *.app
 
 build/
+build-rel/
+profile/logs
 
 CMakeLists.txt.user

.gitlab-ci.yml

+include: http://rainboard.laas.fr/project/hpp-rbprm-corba/.gitlab-ci.yml

.yapfignore

+script
+profile
+affordance-tests

CMakeLists.txt

-# Copyright (c) 2012 CNRS
-# Author: Florent Lamiraux
+# Copyright (c) 2012, 2020, CNRS
+# Authors: Florent Lamiraux, Guilhem Saurel
 #
 # This file is part of hpp-rbprm-corba.
 # hpp-rbprm-corba is free software: you can redistribute it
@@ -15,155 +15,68 @@
 # hpp-rbprm-corba.  If not, see
 # <http://www.gnu.org/licenses/>.
 
-# Requires at least CMake 2.6 to configure the package.
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
 
+SET(PROJECT_NAME hpp-rbprm-corba)
+SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
+
+SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
+SET(PROJECT_USE_CMAKE_EXPORT TRUE)
 SET(CXX_DISABLE_WERROR true)
 
-INCLUDE(cmake/base.cmake)
+INCLUDE(cmake/hpp.cmake)
 INCLUDE(cmake/idl.cmake)
 INCLUDE(cmake/python.cmake)
 
-SET(PROJECT_NAME hpp-rbprm-corba)
-SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
-SET(PROJECT_URL "")
+COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
+PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
-SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
+LIST(APPEND PKG_CONFIG_ADDITIONAL_VARIABLES cmake_plugin)
 
-SETUP_PROJECT ()
+FINDPYTHON()
 
-SET(${PROJECT_NAME}_HEADERS
-  include/hpp/corbaserver/rbprm/server.hh
-)
-
-# Activate hpp-util logging if requested
-SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
-IF (HPP_DEBUG)
-  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
-ENDIF()
-
-ADD_DOC_DEPENDENCY("hpp-core >= 3")
-ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 3")
-ADD_REQUIRED_DEPENDENCY("hpp-rbprm")
+ADD_PROJECT_DEPENDENCY("hpp-rbprm" REQUIRED)
+ADD_PROJECT_DEPENDENCY("hpp-affordance-corba" REQUIRED)
+ADD_PROJECT_DEPENDENCY("hpp-gepetto-viewer")
 ADD_REQUIRED_DEPENDENCY("omniORB4 >= 4.1.4")
-ADD_REQUIRED_DEPENDENCY("hpp-model-urdf >= 3")
-
-PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
-ADD_SUBDIRECTORY(src)
+ADD_REQUIRED_DEPENDENCY("octomap >= 1.8")
 
-CONFIG_FILES (include/hpp/corbaserver/rbprm/doc.hh)
+set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/find-external/CDD")
+ADD_PROJECT_DEPENDENCY(CDD REQUIRED)
 
-SET(CATKIN_PACKAGE_SHARE_DESTINATION
-  ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME})
+if(OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)
+        include_directories(${OCTOMAP_INCLUDE_DIRS})
+        link_directories(${OCTOMAP_LIBRARY_DIRS})
+  string(REPLACE "." ";" VERSION_LIST ${OCTOMAP_VERSION})
+  list(GET VERSION_LIST 0 OCTOMAP_MAJOR_VERSION)
+  list(GET VERSION_LIST 1 OCTOMAP_MINOR_VERSION)
+  list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION)
+  add_definitions (-DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION}
+                   -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION}
+                   -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}  -DHPP_FCL_HAVE_OCTOMAP=1)
+        message(STATUS "FCL uses Octomap" ${OCTOMAP_MINOR_VERSION})
+else()
+        message(STATUS "FCL does not use Octomap")
+endif()
 
-install(FILES
-  data/package.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  )
-install(FILES
-  data/urdf/hrp2_trunk.urdf
-  data/urdf/hrp2_trunk_flexible.urdf
-  data/urdf/hrp2_rom.urdf
-  data/urdf/hrp2_larm_rom.urdf
-  data/urdf/hrp2_rarm_rom.urdf
-  data/urdf/hrp2_lleg_rom.urdf
-  data/urdf/hrp2_rleg_rom.urdf
-  data/urdf/hyq/hyq_trunk.urdf
-  data/urdf/hyq/hyq_trunk_large.urdf
-  data/urdf/hyq/hyq_rhleg_rom.urdf
-  data/urdf/hyq/hyq_rfleg_rom.urdf
-  data/urdf/hyq/hyq_lhleg_rom.urdf
-  data/urdf/hyq/hyq_lfleg_rom.urdf
-  data/urdf/hyq/hyq_rom.urdf
-  data/urdf/box_rom.urdf
-  data/urdf/box.urdf
-  data/urdf/scene.urdf
-  data/urdf/scene_wall.urdf
-  data/urdf/truck.urdf
-  data/urdf/stair_bauzil.urdf
-  data/urdf/climb.urdf
-  data/urdf/stepladder.urdf
-  data/urdf/ground.urdf
-  data/urdf/groundcrouch.urdf
-  data/urdf/darpa.urdf
-  #~ data/urdf/scene2.urdf
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
-  )
-install(FILES
-  data/srdf/hrp2_trunk.srdf
-  data/srdf/hrp2_trunk_flexible.srdf
-  data/srdf/hrp2_rom.srdf
-  data/srdf/hrp2_larm_rom.srdf
-  data/srdf/hrp2_rarm_rom.srdf
-  data/srdf/hrp2_lleg_rom.srdf
-  data/srdf/hrp2_rleg_rom.srdf
-  data/srdf/hyq/hyq_trunk.srdf
-  data/srdf/hyq/hyq_trunk_large.srdf
-  data/srdf/hyq/hyq_rhleg_rom.srdf
-  data/srdf/hyq/hyq_rfleg_rom.srdf
-  data/srdf/hyq/hyq_lhleg_rom.srdf
-  data/srdf/hyq/hyq_lfleg_rom.srdf
-  data/srdf/hyq/hyq_rom.srdf
-  data/srdf/box_rom.srdf
-  data/srdf/box.srdf
-  data/srdf/scene.srdf
-  data/srdf/scene_wall.srdf
-  data/srdf/truck.srdf
-  data/srdf/stair_bauzil.srdf
-  data/srdf/climb.srdf
-  data/srdf/stepladder.srdf
-  data/srdf/ground.srdf
-  data/srdf/groundcrouch.srdf
-  data/srdf/darpa.srdf
-  #~ data/srdf/scene2.srdf
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
-  )
-install(FILES
-	data/meshes/darpa.stl
-	data/meshes/darpareduced.stl
-	data/meshes/car.stl
-	data/meshes/car_simple.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_rom.stl
-	data/meshes/hrp2_larm_rom.stl
-	data/meshes/hrp2_rarm_rom.stl
-	data/meshes/hrp2_lleg_rom.stl
-	data/meshes/hrp2_rleg_rom.stl
-	data/meshes/ground.stl
-	data/meshes/box.stl
-	data/meshes/box_rom.stl
-	data/meshes/robot_box.stl
-	data/meshes/chair_simple.stl
-	data/meshes/ground.stl
-	data/meshes/groundcrouch.stl
-	data/meshes/truck.stl
-	data/meshes/truck_vision.stl
-	data/meshes/stair_bauzil.stl
-        data/meshes/stair_bauzil_reduced.stl
-	data/meshes/climb.stl
-	data/meshes/stepladder.stl
-	data/meshes/chair.stl
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
-  )
-install(FILES
-	data/meshes/hyq/hyq_all.stl
-	data/meshes/hyq/hyq_trunk.stl
-	data/meshes/hyq/hyq_trunk_large.stl
-	data/meshes/hyq/hyq_rom.stl
-	data/meshes/hyq/hyq_rhleg_rom.stl
-	data/meshes/hyq/hyq_rfleg_rom.stl
-	data/meshes/hyq/hyq_lhleg_rom.stl
-	data/meshes/hyq/hyq_lfleg_rom.stl
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq
+SET(${PROJECT_NAME}_HEADERS
+  include/${CUSTOM_HEADER_DIR}/server.hh
   )
-  
-install(DIRECTORY data/hyq_description
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/../
+
+SET(${PROJECT_NAME}_SOURCES
+  src/${PROJECT_NAME}.cc
   )
-  
-SETUP_PROJECT_FINALIZE()
 
+# Stand alone corba server
+ADD_EXECUTABLE(hpp-rbprm-server ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
+TARGET_INCLUDE_DIRECTORIES(hpp-rbprm-server PUBLIC $<INSTALL_INTERFACE:include>)
+TARGET_LINK_LIBRARIES(hpp-rbprm-server hpp-corbaserver::hpp-corbaserver)
+INSTALL(TARGETS hpp-rbprm-server EXPORT ${TARGETS_EXPORT_NAME} DESTINATION bin)
+
+ADD_SUBDIRECTORY(src)
+ADD_SUBDIRECTORY(tests)
+
+CONFIG_FILES (include/${CUSTOM_HEADER_DIR}/doc.hh)
+PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
+
+INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})

NEWS

+                                                                -*- outline -*-
+New in 4.6.0
+* Update installation instructions in README.
+* Update to API change of configuration shooters.
+

README.md

-# hpp-rbprm-corba
-python bindings for the rbprm library
+#  Humanoid Path Planner - RBPRM-CORBA module
+
+[![Pipeline status](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-rbprm-corba/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-rbprm-corba/commits/master)
+[![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-rbprm-corba/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-rbprm-corba/master/coverage/)
+
+Copyright 2015-2020 LAAS-CNRS
+
+Authors: Steve Tonneau, Pierre Fernbach
+
+## Description
+hpp-rbprm-corba implements python bindings for hpp-rbprm, and presents a few example files.
+Please refer to this [link](https://github.com/humanoid-path-planner/hpp-rbprm) for information on hpp-rbprm.
+
+## Installation from binary package repository
+
+1. Add robotpkg/wip to your apt configuration: http://robotpkg.openrobots.org/robotpkg-wip.html
+2. `sudo apt update && sudo apt install robotpkg-pyXX-hpp-rbprm-corba` (replace pyXX with your python version)
+3. Then, you will need to export some variables to allow you system to find the executables:
+
+`export PATH=${PATH:+$PATH:}/opt/openrobots/bin:/opt/openrobots/sbin`
+
+`export MANPATH=${MANPATH:+$MANPATH:}/opt/openrobots/man`
+
+`export PYTHONPATH=/opt/openrobots/lib/pythonXX/site-packages:$PYTHONPATH`(replace XX with your python version)
+
+`export ROS_PACKAGE_PATH="$ROS_PACKAGE_PATH:/opt/openrobots/share"`
+
+`export DEVEL_HPP_DIR=/opt/openrobots/`
+
+(you may want to add these to your .bashrc file)
+
+## Installation From source on ubuntu-16.04 64 bit with ros-kinetic
+
+1. Follow this instructions : http://humanoid-path-planner.github.io/hpp-doc/download.html (select 'Devellopement" in the list)
+2. Once this installation is complete, run `make rbprm`
+
+## Optional: installing viewer and python bindings for dependencies
+
+If you are planning to use the visualization tools used by the Gepetto team, along with python examples, you may need a few extra steps:
+
+1. Install the gepetto-viewer server
+
+`sudo apt install -qqy robotpkg-pyXX-qt4-gepetto-viewer-corba`
+
+`sudo apt install -qqy robotpkg-pyXX-qt4-hpp-gepetto-viewer`
+
+2. Install the pinocchio bindings
+
+`sudo apt install -qqy robotpkg-pyXX-pinocchio`
+
+3. Install the dae extension for osg
+
+`sudo apt install -qqy robotpkg-osg-dae`
+
+## Documentation
+
+  Open $DEVEL_DIR/install/share/doc/hpp-rbprm-corba/doxygen-html/index.html in a web brower and you
+  will have access to the code documentation. If you are using ipython, the documentation of the methods implemented
+  is also directly available in a python console.
+
+## Example
+
+  To see the planner in action, one example from our IJRR submission with HyQ is available. Examples with HRP-2 are also provided, though they can only be executed if you have access to HRP-2 model.
+
+  - You can find the scripts in your install directory, in `lib/pythonXX/dist-packages/hpp/corbaserver/rbprm/scenarios/demos` folder.
+
+  - 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.
+
+  - In order to start a scenario, run:
+  
+  `python -im hpp.corbaserver.rbprm demos.hyq_darpa`
+  
+   Replace demos.hyq_darpa with the name of any file in the demos or memmo folder to try different scenarios.
+  
+  - Once the script have been executed, you can display the results in the viewer (if you installed it):
+  
+      - If it was a `\*_path.py` script, you can run:
+      
+          - `planner.play_path()` to display the computed guide path
+          
+          - `planner.v(planner.q_init)` or `planner.v(planner.q_goal)` to put the robot at the initial / goal position of the planning
+          
+      - If it was a a contact generation script, you can run:
+      
+          - `cg.display_sequence()` to display the sequence of configurations in contact computed
+          
+          - `cg.display_init_config()` or `cg.display_end_config()` to put the robot at the initial / final whole body configuration
+          
+          - `cg.v(cg.configs[i])` to display the i-th wholebody configuration of the sequence
+          
+          - `cg.play_guide_path()` to display the guide path
+
+
+## Creating a new scenario script
+
+Start from one of the scripts in the scenarios/demos folder, eg `talos_flatGround.py`. 
+
+* All the `\*_path.py` scripts must define a class called `PathPlanner` that inherit from one of the `{robot}_path_planner` classes defined in the scenarios folder.
+    
+    * In the run() method, define the environment used and the initial/goal position
+    
+    * If further customization is needed, override the required methods from the parent class.
+    
+* All the contact generation scripts must define a class called `ContactGenerator` that inherit from one of the `{robot}_contact_generator` classes defined in the scenarios folder.
+
+    * In the constructor of this class, the parent constructor must be called with an instance of the desired `PathPlanner` class, defining the environment and the guide trajectory. 
+    
+    * This class may override any method from the parent class in order to change the default settings/choices regarding the contact generation

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

+#Importing helper class for RBPRM
 from hpp.corbaserver.rbprm.rbprmbuilder import Builder
 from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
 from hpp.gepetto import Viewer
 
-import ground_crouch_hyq_path as tp
+#calling script darpa_hyq_path to compute root path
+import darpa_hyq_path as tp
 
 packageName = "hyq_description"
 meshPackageName = "hyq_description"
 rootJointType = "freeflyer"
-##
+
 #  Information to retrieve urdf and srdf files.
 urdfName = "hyq"
 urdfSuffix = ""
 srdfSuffix = ""
 
-fullBody = FullBody ()
- 
+#  This time we load the full body model of HyQ
+fullBody = FullBody () 
 fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
 
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
-
+#  Setting a number of sample configurations used
 nbSamples = 20000
 
-ps = tp.ProblemSolver( fullBody )
+ps = tp.ProblemSolver(fullBody)
 r = tp.Viewer (ps)
 
 rootName = 'base_joint_xyz'
 
-#~ AFTER loading obstacles
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
 rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
 rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
 rfoot = 'rf_foot_joint'
-rLegOffset = [0.,0,0.]
-rLegNormal = [0,1,0]
-rLegx = 0.02; rLegy = 0.02
-fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.05)
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.1, cType)
 
 lLegId = 'lhleg'
 lLeg = 'lh_haa_joint'
 lfoot = 'lh_foot_joint'
-lLegOffset = [0,0,0]
-lLegNormal = [0,1,0]
-lLegx = 0.02; lLegy = 0.02
-fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "forward", 0.05)
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
 
 rarmId = 'rhleg'
 rarm = 'rh_haa_joint'
 rHand = 'rh_foot_joint'
-rArmOffset = [0.,0,-0.]
-rArmNormal = [0,1,0]
-rArmx = 0.02; rArmy = 0.02
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "forward", 0.05)
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
 
 larmId = 'lfleg'
 larm = 'lf_haa_joint'
 lHand = 'lf_foot_joint'
-lArmOffset = [0.,0,-0.]
-lArmNormal = [0,1,0]
-lArmx = 0.02; lArmy = 0.02
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "forward", 0.05)
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
 
 q_0 = fullBody.getCurrentConfig(); 
 q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
 q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
 
-
+# Randomly generating a contact configuration at q_init
 fullBody.setCurrentConfig (q_init)
 q_init = fullBody.generateContacts(q_init, [0,0,1])
-q_0 = fullBody.getCurrentConfig(); 
 
+# Randomly generating a contact configuration at q_end
 fullBody.setCurrentConfig (q_goal)
 q_goal = fullBody.generateContacts(q_goal, [0,0,1])
 
+# specifying the full body configurations as start and goal state of the problem
 fullBody.setStartState(q_init,[])
 fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
 
-r(q_init)
 
-configs = fullBody.interpolate(0.1,0)
+r(q_init)
+# computing the contact sequence
+configs = fullBody.interpolate(0.1, 1, 0)
 
-r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
+r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
 
+# calling draw with increasing i will display the sequence
 i = 0;
-r (configs[i]); i=i+1; i-1
+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
 
-q0 = configs[2]
-q0 = fullBody.generateContacts(q0, [0,0,1])
-r(q0)
-c = fullBody.getContactSamplesIds("rfleg",q_init, [0,0,1])
-r(fullBody.getSample("rfleg",int(c[i]))); i = i+1
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (ps.robot.client.basic, r)
 

affordance-tests/darpa_hyq_path.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'])
+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)
+
+# Choosing a path optimizer
+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)
+
+# 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/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)

script/scenarios/stair_bauzil_hyq_path.py → 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'
-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']) # , 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
 
-ps = ProblemSolver( rbprmBuilder )
+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] = [-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)
 
+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)
-r.loadObstacleModel (packageName, "stair_bauzil", "planning")
-ps.solve ()
 
+# 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)

script/scenarios/darpa_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
 
@@ -10,49 +11,43 @@ 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.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.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])
 
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
+# 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_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.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, "darpa", "planning")
-#~ ps.solve ()
 
+# 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)

script/scenarios/ground_crouch_hyq_path.py → 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'
+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", [-6,5, -4, 4, 0.6, 2])
-rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
-rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9)
-rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9)
-rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9)
-rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
-rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
-
-#~ from hpp.corbaserver.rbprm. import ProblemSolver
-from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
 
-ps = ProblemSolver( rbprmBuilder )
+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] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
-
+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)
 
-q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
-
-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.001)
-r.loadObstacleModel (packageName, "groundcrouch", "planning")
-ps.solve ()
+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)
-#~ pp (1)
-r (q_init)

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 @ ee7a773c

-Subproject commit d9578712fbc457a528e121bff38ffaa294819686
+Subproject commit ee7a773c5c23f83dd21eb0ccfa96277e068b0456