Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-rbprm-corba
Commits
bcfad6ea
Unverified
Commit
bcfad6ea
authored
Mar 26, 2020
by
Fernbach Pierre
Committed by
GitHub
Mar 26, 2020
Browse files
Merge pull request #62 from pFernbach/devel
Add no-viewer mode and tests
parents
810988c1
132ab57b
Changes
26
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
bcfad6ea
...
...
@@ -72,5 +72,6 @@ endif()
PKG_CONFIG_APPEND_LIBS
(
${
PROJECT_NAME
}
)
ADD_SUBDIRECTORY
(
src
)
ADD_SUBDIRECTORY
(
tests
)
CONFIG_FILES
(
include/
${
CUSTOM_HEADER_DIR
}
/doc.hh
)
src/CMakeLists.txt
View file @
bcfad6ea
...
...
@@ -142,7 +142,9 @@ SET(${PROJECT_NAME}_PYTHON_SCENARIOS_DEMOS
talos_navBauzil_hard_path.py
talos_navBauzil_hard.py
talos_navBauzil_path.py
talos_navBauzil_obstacles_path.py
talos_navBauzil.py
talos_navBauzil_obstacles.py
talos_plateformes_path.py
talos_plateformes.py
talos_stairs10_path.py
...
...
src/hpp/corbaserver/rbprm/__main__.py
View file @
bcfad6ea
...
...
@@ -14,6 +14,8 @@ parser.add_argument('scenario_name',
type
=
str
,
help
=
"The name of the scenario script to run. "
"If a relative path is given, hpp.corbaserver.rbprm.scenarios is prepended"
)
parser
.
add_argument
(
"-n"
,
"--no_viewer"
,
help
=
"Run rbprm without visualization."
,
action
=
"store_true"
)
args
=
parser
.
parse_args
()
# retrieve argument
scenario_name
=
args
.
scenario_name
...
...
@@ -31,27 +33,31 @@ except ImportError:
print
(
"Cannot import "
+
scenario_name
+
". Check if the path is correct"
)
sys
.
exit
(
1
)
# kill already existing instance of the viewer/server
subprocess
.
run
([
"killall"
,
"gepetto-gui"
])
# kill already existing instance of the server
subprocess
.
run
([
"killall"
,
"hpp-rbprm-server"
])
# run the
viewer/
server in background
# run the server in background
:
# stdout and stderr outputs of the child process are redirected to devnull (hidden).
# preexec_fn is used to ignore ctrl-c signal send to the main script (otherwise they are forwarded to the child process)
process_viewer
=
subprocess
.
Popen
(
"gepetto-gui"
,
stdout
=
subprocess
.
PIPE
,
stderr
=
subprocess
.
DEVNULL
,
preexec_fn
=
os
.
setpgrp
)
process_server
=
subprocess
.
Popen
(
"hpp-rbprm-server"
,
stdout
=
subprocess
.
PIPE
,
stderr
=
subprocess
.
DEVNULL
,
preexec_fn
=
os
.
setpgrp
)
# register cleanup methods to kill server when exiting python interpreter
atexit
.
register
(
process_server
.
kill
)
# do the same for the viewer, exept if --no-viewer flag is set
if
not
args
.
no_viewer
:
subprocess
.
run
([
"killall"
,
"gepetto-gui"
])
process_viewer
=
subprocess
.
Popen
(
"gepetto-gui"
,
stdout
=
subprocess
.
PIPE
,
stderr
=
subprocess
.
DEVNULL
,
preexec_fn
=
os
.
setpgrp
)
atexit
.
register
(
process_viewer
.
kill
)
# wait a little for the initialization of the server/viewer
time
.
sleep
(
3
)
# register cleanup methods to kill viewer/server when exiting python interpreter
atexit
.
register
(
process_server
.
kill
)
atexit
.
register
(
process_viewer
.
kill
)
# Get ContactGenerator or PathPlanner class from the imported module and run it
if
hasattr
(
module_scenario
,
'ContactGenerator'
):
print
(
"# Run contact generation script ..."
)
...
...
src/hpp/corbaserver/rbprm/scenarios/abstract_contact_generator.py
View file @
bcfad6ea
...
...
@@ -83,7 +83,7 @@ class AbstractContactGenerator:
self
.
fullbody
.
setPostureWeights
(
self
.
weight_postural
)
self
.
fullbody
.
usePosturalTaskContactCreation
(
use_postural_task
)
def
load_limbs
(
self
,
heuristic
=
"fixedStep06"
,
analysis
=
None
,
nb_samples
=
None
,
octree_size
=
None
):
def
load_limbs
(
self
,
heuristic
=
"fixedStep06"
,
analysis
=
"ReferenceConfiguration"
,
nb_samples
=
None
,
octree_size
=
None
):
"""
Generate the samples used for each limbs in 'used_limbs'
:param heuristic: the name of the heuristic used,
...
...
@@ -170,7 +170,7 @@ class AbstractContactGenerator:
self
.
v
(
self
.
q_init
)
print
(
"Generate contact plan ..."
)
t_start
=
time
.
time
()
self
.
configs
=
self
.
fullbody
.
interpolate
(
0.01
,
self
.
configs
=
self
.
fullbody
.
interpolate
(
self
.
dt
,
pathId
=
self
.
pid
,
robustnessTreshold
=
self
.
robustness
,
filterStates
=
self
.
filter_states
,
...
...
src/hpp/corbaserver/rbprm/scenarios/abstract_path_planner.py
View file @
bcfad6ea
...
...
@@ -109,27 +109,11 @@ class AbstractPathPlanner:
vf
=
ViewerFactory
(
self
.
ps
)
self
.
afftool
=
AffordanceTool
()
self
.
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
self
.
afftool
.
loadObstacleModel
(
env_package
,
env_name
,
"planning"
,
vf
,
reduceSizes
=
reduce_sizes
)
try
:
self
.
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
print
(
"No viewer started !"
)
class
FakeViewer
():
def
__init__
(
self
):
return
def
__call__
(
self
,
q
):
return
def
addLandmark
(
self
,
a
,
b
):
return
self
.
v
=
FakeViewer
()
try
:
self
.
pp
=
PathPlayer
(
self
.
v
)
except
Exception
:
pass
self
.
afftool
.
loadObstacleModel
(
"package://"
+
env_package
+
"/urdf/"
+
env_name
+
".urdf"
,
"planning"
,
vf
,
reduceSizes
=
reduce_sizes
)
self
.
v
=
vf
.
createViewer
(
ghost
=
True
,
displayArrows
=
True
)
self
.
pp
=
PathPlayer
(
self
.
v
)
for
aff_type
in
visualize_affordances
:
self
.
afftool
.
visualiseAffordances
(
aff_type
,
self
.
v
,
self
.
v
.
color
.
lightBrown
)
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil.py
View file @
bcfad6ea
...
...
@@ -5,10 +5,12 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact
class
ContactGenerator
(
TalosContactGenerator
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
self
.
robustness
=
2.
def
set_reference
(
self
):
super
().
set_reference
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig_elbowsUp
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
def
load_fullbody
(
self
):
super
().
load_fullbody
()
self
.
fullbody
.
nbSamples
=
100000
self
.
fullbody
.
limbs_names
=
[
self
.
fullbody
.
lLegId
,
self
.
fullbody
.
rLegId
]
# left feet first on this scenario
if
__name__
==
"__main__"
:
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_hard.py
View file @
bcfad6ea
...
...
@@ -5,10 +5,13 @@ from hpp.corbaserver.rbprm.scenarios.talos_contact_generator import TalosContact
class
ContactGenerator
(
TalosContactGenerator
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
self
.
robustness
=
2.
def
load_fullbody
(
self
):
super
().
load_fullbody
()
self
.
fullbody
.
nbSamples
=
100000
self
.
fullbody
.
limbs_names
=
[
self
.
fullbody
.
lLegId
,
self
.
fullbody
.
rLegId
]
# left feet first on this scenario
def
set_reference
(
self
):
super
().
set_reference
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig_elbowsUp
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
def
load_limbs
(
self
,
heuristic
):
super
().
load_limbs
(
heuristic
)
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_hard_path.py
View file @
bcfad6ea
...
...
@@ -4,7 +4,8 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class
PathPlanner
(
TalosPathPlanner
):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
Robot
.
urdfName
+=
"_large"
# load the model with conservative bounding boxes for trunk
Robot
.
urdfName
=
"talos_trunk_large"
# load the model with conservative bounding boxes for trunk
self
.
robot_node_name
=
"talos_trunk_large"
self
.
rbprmBuilder
=
Robot
()
def
init_problem
(
self
):
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_obstacles.py
0 → 100644
View file @
bcfad6ea
from
hpp.corbaserver.rbprm.scenarios.demos.talos_navBauzil_obstacles_path
import
PathPlanner
from
hpp.corbaserver.rbprm.scenarios.talos_contact_generator
import
TalosContactGenerator
class
ContactGenerator
(
TalosContactGenerator
):
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
self
.
robustness
=
2.
def
load_fullbody
(
self
):
super
().
load_fullbody
()
self
.
fullbody
.
nbSamples
=
100000
self
.
fullbody
.
limbs_names
=
[
self
.
fullbody
.
lLegId
,
self
.
fullbody
.
rLegId
]
# left feet first on this scenario
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_obstacles_path.py
0 → 100644
View file @
bcfad6ea
from
hpp.corbaserver.rbprm.scenarios.talos_path_planner
import
TalosPathPlanner
class
PathPlanner
(
TalosPathPlanner
):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
Robot
.
urdfName
=
"talos_trunk_large"
# load the model with conservative bounding boxes for trunk
self
.
robot_node_name
=
"talos_trunk_large"
self
.
rbprmBuilder
=
Robot
()
def
init_problem
(
self
):
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
run
(
self
):
self
.
init_problem
()
self
.
root_translation_bounds
=
[
-
1.5
,
4
,
0.
,
3.3
,
self
.
rbprmBuilder
.
ref_height
,
self
.
rbprmBuilder
.
ref_height
]
self
.
set_joints_bounds
()
self
.
q_init
[:
2
]
=
[
-
0.9
,
1.7
]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
self
.
q_init
[
-
6
:
-
3
]
=
[
0.07
,
0
,
0
]
self
.
q_goal
[:
2
]
=
[
2
,
2.6
]
self
.
init_viewer
(
"multicontact/floor_bauzil_obstacles"
,
visualize_affordances
=
[
"Support"
],
reduce_sizes
=
[
0.02
,
0.
,
0.
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_path.py
View file @
bcfad6ea
...
...
@@ -4,7 +4,8 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class
PathPlanner
(
TalosPathPlanner
):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
Robot
.
urdfName
+=
"_large"
# load the model with conservative bounding boxes for trunk
Robot
.
urdfName
=
"talos_trunk_large"
# load the model with conservative bounding boxes for trunk
self
.
robot_node_name
=
"talos_trunk_large"
self
.
rbprmBuilder
=
Robot
()
def
init_problem
(
self
):
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs10_path.py
View file @
bcfad6ea
...
...
@@ -5,7 +5,8 @@ class PathPlanner(TalosPathPlanner):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
# select ROM model with really conservative ROM shapes
Robot
.
urdfName
+=
"_large_reducedROM"
Robot
.
urdfName
=
"talos_trunk_large_reducedROM"
self
.
robot_node_name
=
"talos_trunk_large_reducedROM"
Robot
.
urdfNameRom
=
[
'talos_lleg_rom_reduced'
,
'talos_rleg_rom_reduced'
]
self
.
used_limbs
=
Robot
.
urdfNameRom
self
.
rbprmBuilder
=
Robot
()
...
...
src/hpp/corbaserver/rbprm/scenarios/hrp2_contact_generator.py
View file @
bcfad6ea
...
...
@@ -13,6 +13,8 @@ class HRP2ContactGenerator(AbstractContactGenerator):
self
.
fullbody
=
Robot
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
weight_postural
=
self
.
fullbody
.
postureWeights
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
fullbody
.
limbs_names
=
[
self
.
fullbody
.
rLegId
,
self
.
fullbody
.
lLegId
]
def
set_joints_bounds
(
self
):
super
().
set_joints_bounds
()
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/talos_bauzil_with_stairs_path.py
View file @
bcfad6ea
...
...
@@ -23,7 +23,8 @@ class PathPlanner(TalosPathPlanner):
def
load_rbprm
(
self
):
# select model with conservative collision geometry for trunk
Robot
.
urdfName
+=
"_large_reducedROM"
Robot
.
urdfName
=
"talos_trunk_large_reducedROM"
self
.
robot_node_name
=
"talos_trunk_large_reducedROM"
# select conservative ROM for feet
Robot
.
urdfNameRom
+=
[
'talos_lleg_rom_reduced'
,
'talos_rleg_rom_reduced'
]
self
.
rbprmBuilder
=
Robot
()
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/talos_mazeEas_path.py
View file @
bcfad6ea
...
...
@@ -13,7 +13,8 @@ class PathPlanner(TalosPathPlanner):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
# select model with conservative collision geometry for trunk
Robot
.
urdfName
+=
"_large"
Robot
.
urdfName
=
"talos_trunk_large"
self
.
robot_node_name
=
"talos_trunk_large"
self
.
rbprmBuilder
=
Robot
()
def
init_problem
(
self
):
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/talos_navBauzil_path.py
View file @
bcfad6ea
...
...
@@ -8,7 +8,8 @@ class PathPlanner(TalosPathPlanner):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
Robot
.
urdfName
+=
"_large"
# load the model with conservative bounding boxes for trunk
Robot
.
urdfName
=
"talos_trunk_large"
# load the model with conservative bounding boxes for trunk
self
.
robot_node_name
=
"talos_trunk_large"
self
.
rbprmBuilder
=
Robot
()
def
init_problem
(
self
):
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/talos_stairs10_random_path.py
View file @
bcfad6ea
...
...
@@ -35,7 +35,8 @@ class PathPlanner(TalosPathPlanner):
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
# select ROM model with really conservative ROM shapes
Robot
.
urdfName
+=
"_large_reducedROM"
Robot
.
urdfName
=
"talos_trunk_large_reducedROM"
self
.
robot_node_name
=
"talos_trunk_large_reducedROM"
Robot
.
urdfNameRom
=
[
'talos_lleg_rom_reduced'
,
'talos_rleg_rom_reduced'
]
self
.
used_limbs
=
Robot
.
urdfNameRom
self
.
rbprmBuilder
=
Robot
()
...
...
src/hpp/corbaserver/rbprm/scenarios/talos_contact_generator.py
View file @
bcfad6ea
...
...
@@ -13,6 +13,7 @@ class TalosContactGenerator(AbstractContactGenerator):
self
.
fullbody
=
Robot
()
self
.
q_ref
=
self
.
fullbody
.
referenceConfig
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
weight_postural
=
self
.
fullbody
.
postureWeights
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
fullbody
.
limbs_names
=
[
self
.
fullbody
.
rLegId
,
self
.
fullbody
.
lLegId
]
def
init_viewer
(
self
):
super
().
init_viewer
()
...
...
src/hpp/corbaserver/rbprm/tools/display_tools.py
View file @
bcfad6ea
...
...
@@ -51,10 +51,11 @@ def displaySurfaceFromPoints(viewer, p_list, color=[0, 0, 0, 1], name=None):
if
len
(
p_list
)
<
2
:
return
gui
=
viewer
.
client
.
gui
if
name
is
None
:
node_list
=
gui
.
getNodeList
()
if
name
is
None
and
node_list
is
not
None
:
i
=
0
name
=
'surface_'
+
str
(
i
)
while
name
in
gui
.
getN
ode
L
ist
()
:
while
name
in
n
ode
_l
ist
:
i
=
i
+
1
name
=
'surface_'
+
str
(
i
)
gui
.
addCurve
(
name
,
p_list
,
color
)
...
...
src/rbprmbuilder.impl.cc
View file @
bcfad6ea
...
...
@@ -975,7 +975,7 @@ short RbprmBuilder::generateContactState(::CORBA::UShort cId, const char* name,
if
(
rep
.
success_
)
{
lastStatesComputed_
.
push_back
(
rep
.
result_
);
lastStatesComputedTime_
.
push_back
(
std
::
make_pair
(
-
1.
,
rep
.
result_
));
return
lastStatesComputed_
.
size
()
-
1
;
return
(
CORBA
::
Short
)(
lastStatesComputed_
.
size
()
-
1
)
;
}
}
return
-
1
;
...
...
@@ -2787,7 +2787,7 @@ CORBA::Short RbprmBuilder::isLimbInContactIntermediary(const char* limbName, uns
}
}
CORBA
::
Short
RbprmBuilder
::
getNumStates
()
throw
(
hpp
::
Error
)
{
return
lastStatesComputed_
.
size
();
}
CORBA
::
Short
RbprmBuilder
::
getNumStates
()
throw
(
hpp
::
Error
)
{
return
(
CORBA
::
Short
)
lastStatesComputed_
.
size
();
}
CORBA
::
Short
RbprmBuilder
::
computeIntermediary
(
unsigned
short
stateFrom
,
unsigned
short
stateTo
)
throw
(
hpp
::
Error
)
try
{
...
...
Prev
1
2
Next
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment