Commit ed284623 authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #3 from stonneau/master

release of rbprm-corba for ijrr resubmission
parents 2d4fce53 e40047bd
...@@ -30,5 +30,6 @@ ...@@ -30,5 +30,6 @@
*.app *.app
build/ build/
profile/logs
CMakeLists.txt.user CMakeLists.txt.user
...@@ -29,7 +29,7 @@ SET(PROJECT_DESCRIPTION "Corba server for reachability based planning") ...@@ -29,7 +29,7 @@ SET(PROJECT_DESCRIPTION "Corba server for reachability based planning")
SET(PROJECT_URL "") SET(PROJECT_URL "")
# Set to 1 for profiling # Set to 1 for profiling
#~ add_definitions(-DPROFILE) #add_definitions(-DPROFILE)
SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm) SET(CUSTOM_HEADER_DIR hpp/corbaserver/rbprm)
...@@ -50,6 +50,7 @@ ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 3") ...@@ -50,6 +50,7 @@ ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-rbprm") ADD_REQUIRED_DEPENDENCY("hpp-rbprm")
ADD_REQUIRED_DEPENDENCY("omniORB4 >= 4.1.4") ADD_REQUIRED_DEPENDENCY("omniORB4 >= 4.1.4")
ADD_REQUIRED_DEPENDENCY("hpp-model-urdf >= 3") ADD_REQUIRED_DEPENDENCY("hpp-model-urdf >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-affordance-corba")
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME}) PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(src)
...@@ -90,6 +91,7 @@ install(FILES ...@@ -90,6 +91,7 @@ install(FILES
data/urdf/groundcrouch.urdf data/urdf/groundcrouch.urdf
data/urdf/darpa.urdf data/urdf/darpa.urdf
data/urdf/car.urdf data/urdf/car.urdf
data/urdf/polaris.urdf
#~ data/urdf/scene2.urdf #~ data/urdf/scene2.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
) )
...@@ -120,6 +122,7 @@ install(FILES ...@@ -120,6 +122,7 @@ install(FILES
data/srdf/groundcrouch.srdf data/srdf/groundcrouch.srdf
data/srdf/darpa.srdf data/srdf/darpa.srdf
data/srdf/car.srdf data/srdf/car.srdf
data/srdf/polaris.srdf
#~ data/srdf/scene2.srdf #~ data/srdf/scene2.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
) )
...@@ -128,11 +131,13 @@ install(FILES ...@@ -128,11 +131,13 @@ install(FILES
data/meshes/darpareduced.stl data/meshes/darpareduced.stl
data/meshes/car.stl data/meshes/car.stl
data/meshes/car_simple.stl data/meshes/car_simple.stl
data/meshes/ground.stl
data/meshes/ground_table.stl
data/meshes/hrp2_trunk.stl data/meshes/hrp2_trunk.stl
data/meshes/hrp2_trunk_body.stl data/meshes/hrp2_trunk_body.stl
data/meshes/hrp2_trunk_torso.stl data/meshes/hrp2_trunk_torso.stl
data/meshes/hrp2_trunk_body_view.dae data/meshes/hrp2_trunk_body_view.dae
data/meshes/hrp2_trunk_torso_view.dae data/meshes/hrp2_trunk_torso_view.dae
data/meshes/hrp2_rom.stl data/meshes/hrp2_rom.stl
data/meshes/hrp2_larm_rom.stl data/meshes/hrp2_larm_rom.stl
data/meshes/hrp2_rarm_rom.stl data/meshes/hrp2_rarm_rom.stl
...@@ -148,11 +153,13 @@ install(FILES ...@@ -148,11 +153,13 @@ install(FILES
data/meshes/truck.stl data/meshes/truck.stl
data/meshes/truck_vision.stl data/meshes/truck_vision.stl
data/meshes/stair_bauzil.stl data/meshes/stair_bauzil.stl
data/meshes/stair_bauzil_reduced.stl data/meshes/stair_bauzil_reduced.stl
data/meshes/climb.stl data/meshes/climb.stl
data/meshes/stepladder.stl data/meshes/stepladder.stl
data/meshes/chair.stl data/meshes/chair.stl
data/meshes/car.stl data/meshes/car.stl
data/meshes/polaris.stl
data/meshes/polaris_reduced.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
) )
install(FILES install(FILES
...@@ -167,6 +174,5 @@ install(FILES ...@@ -167,6 +174,5 @@ install(FILES
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes/hyq
) )
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
SETUP_PROJECT_FINALIZE() SETUP_PROJECT_FINALIZE()
...@@ -12,22 +12,17 @@ Please refer to this [link](https://github.com/stonneau/hpp-rbprm) for informati ...@@ -12,22 +12,17 @@ Please refer to this [link](https://github.com/stonneau/hpp-rbprm) for informati
To install hpp-rbprm-corba: To install hpp-rbprm-corba:
1. install HPP-RBPRM 1. Install HPP-RBPRM and its dependencies
- see https://github.com/stonneau/hpp-rbprm - 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 3. Use CMake to install the library. For instance:
cd hpp-template-corba/
mkdir build && cd build
cmake ..
make install
2. Use CMake to install the library. For instance:
mkdir $HPP_RBPRM_CORBA_DIR/build mkdir $HPP_RBPRM_CORBA_DIR/build
cd $HPP_RBPRM_CORBA_DIR/build cd $HPP_RBPRM_CORBA_DIR/build
cmake .. cd cmake ..
make install make install
...@@ -48,22 +43,14 @@ To install hpp-rbprm-corba: ...@@ -48,22 +43,14 @@ To install hpp-rbprm-corba:
https://github.com/iit-DLSLab/hyq-description 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/ - 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``` ```$ ./run.sh darpa_hyq_path.py```
- To generate the complete contact sequence, run: 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.
```$ ./run.sh darpa_hyq_interp.py```
The scripts include comments explaining the different calls to the library.
...@@ -88,4 +88,13 @@ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact") ...@@ -88,4 +88,13 @@ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
i = 0; i = 0;
fullBody.draw(configs[i],r); 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
from hpp.gepetto import PathPlayer
pp = PathPlayer (ps.robot.client.basic, r)
...@@ -21,11 +21,10 @@ rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4]) ...@@ -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: # The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ... # 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.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_rhleg_rom', ['Support', 'Lean'])
rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.5) rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.5) rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support', 'Lean'])
rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.5) rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.5)
# We also bound the rotations of the torso. # We also bound the rotations of the torso.
rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3]) rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
...@@ -45,11 +44,15 @@ ps.addPathOptimizer("RandomShortcut") ...@@ -45,11 +44,15 @@ ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init) ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal) 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. # Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used. # Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "darpa", "planning")
# Solve the problem # Solve the problem
t = ps.solve () t = ps.solve ()
...@@ -57,4 +60,4 @@ t = ps.solve () ...@@ -57,4 +60,4 @@ t = ps.solve ()
# Playing the computed path # Playing the computed path
from hpp.gepetto import PathPlayer from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r) pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (1) pp (1)
# 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)
# 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)
# Importing helper class for setting up a reachability planning problem
from hpp.corbaserver.rbprm.rbprmbuilder import Builder from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer from hpp.gepetto import Viewer
rootJointType = 'freeflyer' rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba' packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba' meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hyq_trunk' urdfName = 'hyq_trunk_large'
urdfNameRom = ['hyq_lhleg_rom','hyq_lhleg_rom','hyq_rfleg_rom','hyq_rhleg_rom'] urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = "" urdfSuffix = ""
srdfSuffix = "" srdfSuffix = ""
rbprmBuilder = Builder () rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,4, -1, 1, 0, 2]) rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
rbprmBuilder.setFilter(['hyq_lhleg_rom' , 'hyq_rfleg_rom'])
#~ from hpp.corbaserver.rbprm. import ProblemSolver rbprmBuilder.setFilter(['hyq_rhleg_rom'])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver 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 ) ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps) r = Viewer (ps)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig (); q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-1, 0, 0.4]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_init [0:3] = [-2, 0, 0.63]; 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_goal = q_init [::] q_goal = q_init [::]
#~ q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
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)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init) ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal) 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.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) 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 from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r) pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") pp (0)
#~
#~ pp (2)
#~ pp (0)
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init)
# 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)
# 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.