.gitignore

@@ -6,6 +6,7 @@
 
 # Precompiled Headers
 *.gch
+*.txt
 *.pch
 
 # Compiled Dynamic libraries
@@ -21,8 +22,19 @@
 *.la
 *.a
 *.lib
+*.pyc
+*.~
+*_config
+*_unstable
+*_coplanar
 
 # Executables
 *.exe
 *.out
 *.app
+
+build/
+build-rel/
+profile/logs
+
+CMakeLists.txt.user

.gitlab-ci.yml

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

.gitmodules

+[submodule "cmake"]
+	path = cmake
+	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git

.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,43 +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
-  include/hpp/corbaserver/rbprm/fwd.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("octomap >= 1.8")
 
-PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
-ADD_SUBDIRECTORY(src)
+set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake/find-external/CDD")
+ADD_PROJECT_DEPENDENCY(CDD REQUIRED)
+
+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()
+
+SET(${PROJECT_NAME}_HEADERS
+  include/${CUSTOM_HEADER_DIR}/server.hh
+  )
 
-CONFIG_FILES (include/hpp/corbaserver/rbprm/doc.hh)
+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

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
+
+#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 = ""
+
+#  This time we load the full body model of HyQ
+fullBody = FullBody () 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+
+#  Setting a number of sample configurations used
+nbSamples = 20000
+
+ps = tp.ProblemSolver(fullBody)
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#  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'
+# 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'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+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])
+
+# 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)
+# computing the contact sequence
+configs = fullBody.interpolate(0.1, 1, 0)
+
+r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
+
+# calling draw with increasing i will display the sequence
+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)
+

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)

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)

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_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']) 
+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] = [-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)
+
+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)
+
+# 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/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)

build/src/hpp/corbaserver/rbprm/__init__.py

-# DO NOT EDIT THIS FILE!
-#
-# Python module hpp.corbaserver.rbprm generated by omniidl
-
-import omniORB
-omniORB.updateModule("hpp.corbaserver.rbprm")
-
-# ** 1. Stub files contributing to this module
-import rbprmbuilder_idl
-
-# ** 2. Sub-modules
-
-# ** 3. End

cmake @ ee7a773c

+Subproject commit ee7a773c5c23f83dd21eb0ccfa96277e068b0456

include/hpp/corbaserver/rbprm/fwd.hh

-// Copyright (C) 2014 CNRS-LAAS
-// Author: Florent Lamiraux.
-//
-// This file is part of the hpp-manipulation-corba.
-//
-// hpp-manipulation-corba is free software: you can redistribute
-// it and/or modify it under the terms of the GNU Lesser General
-// Public License as published by the Free Software Foundation, either
-// version 3 of the License, or (at your option) any later version.
-//
-// hpp-manipulation-corba is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public
-// License along with hpp-manipulation-corba.  If not, see
-// <http://www.gnu.org/licenses/>.
-
-#ifndef HPP_MANIPULATION_CORBA_FWD_HH
-# define HPP_MANIPULATION_CORBA_FWD_HH
-
-# include <hpp/manipulation/fwd.hh>
-
-namespace hpp {
-  namespace manipulation {
-    namespace impl {
-      typedef manipulation::ProblemSolver ProblemSolver;
-      class Server;
-      typedef core::ConstraintSet ConstraintSet;
-      typedef core::ConstraintSetPtr_t ConstraintSetPtr_t;
-      typedef core::ConfigProjector ConfigProjector;
-      typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
-    } // namespace impl
-  } // namespace manipulation
-} // namespace hpp
-
-#endif // HPP_MANIPULATION_CORBA_FWD_HH

include/hpp/corbaserver/rbprm/server.hh

@@ -18,36 +18,37 @@
 // <http://www.gnu.org/licenses/>.
 
 #ifndef HPP_RBPRM_CORBA_SERVER_HH
-# define HPP_RBPRM_CORBA_SERVER_HH
+#define HPP_RBPRM_CORBA_SERVER_HH
 
-# include <hpp/corba/template/server.hh>
-# include <hpp/corbaserver/rbprm/fwd.hh>
-# include <hpp/corbaserver/rbprm/config.hh>
+#include <hpp/corba/template/server.hh>
+
+#include <hpp/corbaserver/problem-solver-map.hh>
+#include <hpp/corbaserver/rbprm/config.hh>
+#include <hpp/corbaserver/server-plugin.hh>
 
 namespace hpp {
-  namespace rbprm {
-    namespace impl {
-      class RbprmBuilder;
-    }
-    class HPP_RBPRM_CORBA_DLLAPI Server
-    {
-    public:
-      Server (int argc, char *argv[], bool multiThread = false,
-          const std::string& poaName = "child");
-      ~Server ();
-      /// Set planner that will be controlled by server
-      void setProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
-
-      /// Start corba server
-
-      /// Call hpp::corba::Server <impl::Problem>::startCorbaServer
-      void startCorbaServer(const std::string& contextId,
-                const std::string& contextKind,
-                const std::string& objectId);
-    private:
-      corba::Server <impl::RbprmBuilder>* rbprmBuilder_;
-    }; // class Server
-  } // namespace rbprm
-} // namespace hpp
-
-#endif // HPP_RBPRM_CORBA_SERVER_HH
+namespace rbprm {
+namespace impl {
+class RbprmBuilder;
+}
+class HPP_RBPRM_CORBA_DLLAPI Server : public corbaServer::ServerPlugin {
+ public:
+  Server(corbaServer::Server* parent);
+
+  ~Server();
+
+  /// Start corba server
+  /// Call hpp::corba::Server <impl::Problem>::startCorbaServer
+  void startCorbaServer(const std::string& contextId, const std::string& contextKind);
+
+  std::string name() const;
+
+  ::CORBA::Object_ptr servant (const std::string& name) const;
+
+ public:
+  corba::Server<impl::RbprmBuilder>* rbprmBuilder_;
+};  // class Server
+}  // namespace rbprm
+}  // namespace hpp
+
+#endif  // HPP_RBPRM_CORBA_SERVER_HH

matlab/effectorRomToObj.m

+% Open it on blender with default axis setting and export it with x forward. 
+% Use decimate to reduce the number of faces, don't forget to triangulate faces before exporting
+
+function [] = effectorRomToObj(filename, outname)
+delimiterIn = ',';
+headerlinesIn = 0;
+points = importdata(filename,delimiterIn,headerlinesIn);
+k = convhull(points);
+vertface2obj(points,k,outname)