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
2d0a5781
Commit
2d0a5781
authored
Mar 03, 2020
by
Pierre Fernbach
Browse files
[Script] update abstract template scripts
parent
74bc5725
Changes
4
Hide whitespace changes
Inline
Side-by-side
script/scenarios/abstract_contact_generator.py
View file @
2d0a5781
...
...
@@ -6,13 +6,13 @@ from abc import abstractmethod
class
AbstractContactGenerator
:
fullbody
=
None
ps
=
None
v
=
None
q_ref
=
None
weight_postural
=
None
q_init
=
None
q_goal
=
None
fullbody
=
None
# rborpm.FullBody instance
ps
=
None
# ProblemSolver instance
v
=
None
# gepetto.Viewer instance
q_ref
=
None
# reference configuration used (depending on the settings)
weight_postural
=
None
# weight used for the postural task (depending on the setting)
q_init
=
None
# Initial whole body configuration
q_goal
=
None
# Desired final whole body configuration
def
__init__
(
self
,
path_planner
):
"""
...
...
@@ -21,34 +21,65 @@ class AbstractContactGenerator:
"""
path_planner
.
run
()
self
.
path_planner
=
path_planner
# ID of the guide path used in problemSolver, default to the last one
self
.
pid
=
self
.
path_planner
.
ps
.
numberPaths
()
-
1
self
.
used_limbs
=
path_planner
.
used_limbs
self
.
dt
=
0.01
self
.
robustness
=
0
self
.
filter_states
=
True
self
.
test_reachability
=
True
self
.
quasi_static
=
True
self
.
erase_previous_states
=
True
self
.
init_contacts
=
self
.
used_limbs
self
.
end_contacts
=
self
.
used_limbs
self
.
configs
=
[]
self
.
root_translation_bounds
=
self
.
path_planner
.
root_translation_bounds
# increase bounds from path planning, to leave room for the root motion during the steps
for
i
in
range
(
3
):
self
.
root_translation_bounds
[
2
*
i
]
-=
0.1
self
.
root_translation_bounds
[
2
*
i
+
1
]
+=
0.1
# settings related to the 'interpolate' method:
self
.
dt
=
0.01
# discretisation step used
self
.
robustness
=
0
# robustness treshold
# (see https://github.com/humanoid-path-planner/hpp-centroidal-dynamics/blob/master/include/hpp/centroidal-dynamics/centroidal_dynamics.hh#L215)
self
.
filter_states
=
True
# if True, after contact generation try to remove all the redundant steps
self
.
test_reachability
=
True
# if True, check feasibility of the contact transition during contact generation
self
.
quasi_static
=
True
# if True, use the 2-PAC method to check feasibility, if False use CROC
self
.
erase_previous_states
=
True
# if False, keep^the previously computed states if 'interpolate' is called a second time
self
.
static_stability
=
True
# if True, the acceleration computed by the guide is ignored during contact planning
self
.
init_contacts
=
self
.
used_limbs
# limbs in contact in the initial configuration
# the order of the limbs in the list define the initial order in which the contacts are removed when then cannot be maintained anymore
self
.
end_contacts
=
self
.
used_limbs
# limbs in contact in the final configuration
self
.
configs
=
[]
# will contains the whole body configuration computed after calling 'interpolate'
@
abstractmethod
def
load_fullbody
(
self
):
"""
Load the robot from urdf and instanciate a fullBody object
Also initialize the q_ref and weight_postural vectors
"""
pass
def
set_joints_bounds
(
self
):
self
.
fullbody
.
setJointBounds
(
"root_joint"
,
self
.
path_planner
.
root_translation_bounds
)
"""
Define the root translation bounds and the extra config bounds
"""
self
.
fullbody
.
setJointBounds
(
"root_joint"
,
self
.
root_translation_bounds
)
self
.
fullbody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
self
.
path_planner
.
extra_dof
)
self
.
fullbody
.
client
.
robot
.
setExtraConfigSpaceBounds
(
self
.
path_planner
.
extra_dof_bounds
)
def
set_reference
(
self
,
use_postural_task
=
True
):
"""
Set the reference configuration used and the weight for the postural task
:param use_postural_task: if True, when projecting configurations to contact a postural task is added to the cost function
Disabling this setting will greatly reduce the computation time, but may result in weird configurations in contact
"""
self
.
fullbody
.
setReferenceConfig
(
self
.
q_ref
)
self
.
fullbody
.
setCurrentConfig
(
self
.
q_ref
)
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
):
"""
Generate the samples used for each limbs in 'used_limbs'
:param heuristic: the name of the heuristic used,
see https://github.com/humanoid-path-planner/hpp-rbprm/blob/master/src/sampling/heuristic.cc#L272-L285
:param analysis: The name of the analysis used,
see https://github.com/humanoid-path-planner/hpp-rbprm/blob/master/src/sampling/analysis.cc#L318-L335
:param nb_samples: The number of samples for each limb database. Default is set in the Robot python class
:param octree_size: The size of each cell of the octree. Default is set in the Robot python class
"""
self
.
fullbody
.
limbs_names
=
self
.
used_limbs
if
nb_samples
is
None
:
nb_samples
=
self
.
fullbody
.
nbSamples
...
...
@@ -62,6 +93,9 @@ class AbstractContactGenerator:
def
init_problem
(
self
):
"""
Create a ProblemSolver instance, and set the velocity and acceleration bounds
"""
self
.
ps
=
ProblemSolver
(
self
.
fullbody
)
if
self
.
path_planner
.
v_max
>=
0
:
self
.
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
self
.
path_planner
.
v_max
)
...
...
@@ -69,9 +103,18 @@ class AbstractContactGenerator:
self
.
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
self
.
path_planner
.
a_max
)
def
init_viewer
(
self
):
"""
Create a Viewer instance
"""
self
.
v
=
Viewer
(
self
.
ps
,
viewerClient
=
self
.
path_planner
.
v
.
client
,
displayCoM
=
True
)
def
compute_configs_from_guide
(
self
,
use_acc_init
=
True
,
use_acc_end
=
False
):
def
compute_configs_from_guide
(
self
,
use_acc_init
=
True
,
use_acc_end
=
False
,
set_ref_height
=
True
):
"""
Compute the wholebody configurations from the reference one and the guide init/goal config
:param use_acc_init: if True, use the initial acceleration from the guide path
:param use_acc_end: if True, use the final acceleration from the guide path
:param set_ref_height: if True, set the root Z position of q_init and q_goal to be equal to q_ref
"""
self
.
q_init
=
self
.
q_ref
[::]
self
.
q_init
[
0
:
7
]
=
self
.
path_planner
.
ps
.
configAtParam
(
self
.
pid
,
0.001
)[
0
:
7
]
# do not use 0 but an epsilon in order to avoid the orientation discontinuity that may happen at t=0
...
...
@@ -93,12 +136,22 @@ class AbstractContactGenerator:
self
.
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
q_goal_guide
[
index_ecs
+
3
:
index_ecs
+
6
]
else
:
self
.
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
if
set_ref_height
:
# force root height to be at the reference position:
self
.
q_init
[
2
]
=
self
.
q_ref
[
2
]
self
.
q_goal
[
2
]
=
self
.
q_ref
[
2
]
self
.
v
(
self
.
q_init
)
def
interpolate
(
self
):
"""
Compute the sequence of configuration in contact between q_init and q_goal
"""
# specify the full body configurations as start and goal state of the problem
self
.
fullbody
.
setStaticStability
(
self
.
static_stability
)
self
.
fullbody
.
setStartState
(
self
.
q_init
,
self
.
init_contacts
)
self
.
fullbody
.
setEndState
(
self
.
q_goal
,
self
.
end_contacts
)
self
.
v
(
self
.
q_init
)
print
(
"Generate contact plan ..."
)
t_start
=
time
.
time
()
self
.
configs
=
self
.
fullbody
.
interpolate
(
0.01
,
...
...
@@ -113,6 +166,10 @@ class AbstractContactGenerator:
print
(
"number of configs :"
,
len
(
self
.
configs
))
def
display_sequence
(
self
):
"""
Display the sequence of configuration in contact,
requires that self.configs is not empty
"""
displayContactSequence
(
self
.
v
,
self
.
configs
)
@
abstractmethod
...
...
@@ -129,9 +186,6 @@ class AbstractContactGenerator:
self.init_problem()
self.init_viewer()
self.compute_configs_from_guide()
# force root height to be at the reference position:
self.q_init[2] = self.q_ref[2]
self.q_goal[2] = self.q_ref[2]
self.interpolate()
"""
pass
script/scenarios/abstract_path_planner.py
View file @
2d0a5781
...
...
@@ -11,6 +11,7 @@ class AbstractPathPlanner:
v
=
None
afftool
=
None
pp
=
None
extra_dof_bounds
=
None
def
__init__
(
self
):
self
.
v_max
=
-
1
# bounds on the linear velocity for the root, negative values mean unused
...
...
@@ -23,13 +24,9 @@ class AbstractPathPlanner:
self
.
used_limbs
=
[]
# names of the limbs that must be in contact during all the motion
self
.
size_foot_x
=
0
# size of the feet along the x axis
self
.
size_foot_y
=
0
# size of the feet along the y axis
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
0
,
0
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
0
,
0
]
self
.
q_init
=
[]
self
.
q_goal
=
[]
@
abstractmethod
def
load_rbprm
(
self
):
"""
...
...
@@ -37,6 +34,18 @@ class AbstractPathPlanner:
"""
pass
def
set_configurations
(
self
):
self
.
rbprmBuilder
.
client
.
robot
.
setDimensionExtraConfigSpace
(
self
.
extra_dof
)
self
.
q_init
=
self
.
rbprmBuilder
.
getCurrentConfig
()
self
.
q_goal
=
self
.
rbprmBuilder
.
getCurrentConfig
()
self
.
q_init
[
2
]
=
self
.
rbprmBuilder
.
ref_height
self
.
q_goal
[
2
]
=
self
.
rbprmBuilder
.
ref_height
def
compute_extra_config_bounds
(
self
):
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
0
,
0
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
0
,
0
]
def
set_joints_bounds
(
self
):
"""
Set the root translation and rotation bounds as well as the the extra dofs bounds
...
...
@@ -61,6 +70,8 @@ class AbstractPathPlanner:
The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
"""
self
.
load_rbprm
()
self
.
set_configurations
()
self
.
compute_extra_config_bounds
()
self
.
set_joints_bounds
()
self
.
set_rom_filters
()
self
.
ps
=
ProblemSolver
(
self
.
rbprmBuilder
)
...
...
@@ -77,17 +88,19 @@ class AbstractPathPlanner:
# sample only configuration with null velocity and acceleration :
self
.
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
def
init_viewer
(
self
,
env_name
,
env_package
=
"hpp_environments"
,
visualize_affordances
=
[]):
def
init_viewer
(
self
,
env_name
,
env_package
=
"hpp_environments"
,
reduce_sizes
=
[
0
,
0
,
0
],
visualize_affordances
=
[]):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
"""
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
)
self
.
afftool
.
loadObstacleModel
(
env_package
,
env_name
,
"planning"
,
vf
,
reduceSizes
=
reduce_sizes
)
try
:
self
.
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
...
...
script/scenarios/template_path.py
deleted
100644 → 0
View file @
74bc5725
from
abc
import
abstractmethod
from
hpp.gepetto
import
ViewerFactory
,
PathPlayer
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
from
hpp.corbaserver
import
ProblemSolver
import
time
class
AbstractPathPlanning
:
rbprmBuilder
=
None
ps
=
None
v
=
None
afftool
=
None
pp
=
None
q_init
=
[]
q_goal
=
[]
def
__init__
(
self
):
self
.
v_max
=
-
1
# bounds on the linear velocity for the root, negative values mean unused
self
.
a_max
=
-
1
# bounds on the linear acceleration for the root, negative values mean unused
self
.
root_translation_bounds
=
[
0
]
*
6
# bounds on the root translation position (-x, +x, -y, +y, -z, +z)
self
.
root_rotation_bounds
=
[
-
3.14
,
3.14
,
-
0.01
,
0.01
,
-
0.01
,
0.01
]
# bounds on the rotation of the root (-z, z, -y, y, -x, x)
# The rotation bounds are only used during the random sampling, they are not enforced along the path
self
.
extra_dof
=
6
# number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
self
.
mu
=
0.5
# friction coefficient between the robot and the environment
self
.
used_limbs
=
[]
# names of the limbs that must be in contact during all the motion
self
.
size_foot_x
=
0
# size of the feet along the x axis
self
.
size_foot_y
=
0
# size of the feet along the y axis
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
0
,
0
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
0
,
0
]
@
abstractmethod
def
load_robot
(
self
):
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
pass
def
set_joints_bounds
(
self
):
"""
Set the root translation and rotation bounds as well as the the extra dofs bounds
"""
self
.
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
self
.
root_translation_bounds
)
self
.
rbprmBuilder
.
boundSO3
(
self
.
root_rotation_bounds
)
self
.
rbprmBuilder
.
client
.
robot
.
setExtraConfigSpaceBounds
(
self
.
extra_dof_bounds
)
def
set_rom_filters
(
self
):
"""
Define which ROM must be in collision at all time and with which kind of affordances
By default it set all the roms in used_limbs to be in contact with 'support' affordances
"""
self
.
rbprmBuilder
.
setFilter
(
self
.
used_limbs
)
for
limb
in
self
.
used_limbs
:
self
.
rbprmBuilder
.
setAffordanceFilter
(
limb
,
[
'Support'
])
def
init_problem
(
self
):
"""
Load the robot, set the bounds and the ROM filters and then
Create a ProblemSolver instance and set the default parameters.
The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
"""
self
.
load_robot
()
self
.
set_joints_bounds
()
self
.
set_rom_filters
()
self
.
ps
=
ProblemSolver
(
self
.
rbprmBuilder
)
# define parameters used by various methods :
if
self
.
v_max
>=
0
:
self
.
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
self
.
v_max
)
if
self
.
a_max
>=
0
:
self
.
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
self
.
a_max
)
if
self
.
size_foot_x
>
0
:
self
.
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
self
.
size_foot_x
)
if
self
.
size_foot_y
>
0
:
self
.
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
self
.
size_foot_y
)
self
.
ps
.
setParameter
(
"DynamicPlanner/friction"
,
0.5
)
# sample only configuration with null velocity and acceleration :
self
.
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
def
init_viewer
(
self
,
env_name
,
env_package
=
"hpp_environments"
,
visualize_affordances
=
[]):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param visualize_affordances: list of affordances type to visualize, default to none
"""
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
)
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
()
self
.
pp
=
PathPlayer
(
self
.
v
)
for
aff_type
in
visualize_affordances
:
self
.
afftool
.
visualiseAffordances
(
aff_type
,
self
.
v
,
self
.
v
.
color
.
lightBrown
)
def
init_planner
(
self
,
kinodynamic
=
True
):
"""
Select the rbprm methods, and the kinodynamic ones if required
:param kinodynamic: if True, also select the kinodynamic methods
"""
self
.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
self
.
ps
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
if
kinodynamic
:
self
.
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
self
.
ps
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
self
.
ps
.
selectDistance
(
"Kinodynamic"
)
self
.
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
def
solve
(
self
):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
"""
if
len
(
self
.
q_init
)
!=
self
.
rbprmBuilder
.
getConfigSize
():
raise
ValueError
(
"Initial configuration vector do not have the right size"
)
if
len
(
self
.
q_goal
)
!=
self
.
rbprmBuilder
.
getConfigSize
():
raise
ValueError
(
"Goal configuration vector do not have the right size"
)
self
.
ps
.
setInitialConfig
(
self
.
q_init
)
self
.
ps
.
addGoalConfig
(
self
.
q_goal
)
self
.
v
(
self
.
q_init
)
t
=
self
.
ps
.
solve
()
print
(
"Guide planning time : "
,
t
)
def
display_path
(
self
,
path_id
=
-
1
,
dt
=
0.1
):
"""
Display the path in the viewer, if no path specified display the last one
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.1)
"""
if
path_id
<
0
:
path_id
=
self
.
ps
.
numberPaths
()
-
1
self
.
pp
.
dt
=
dt
self
.
pp
.
displayVelocityPath
(
path_id
)
def
play_path
(
self
,
path_id
=
-
1
,
dt
=
0.01
):
"""
play the path in the viewer, if no path specified display the last one
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.01)
"""
if
path_id
<
0
:
path_id
=
self
.
ps
.
numberPaths
()
-
1
self
.
pp
.
dt
=
dt
self
.
pp
(
path_id
)
def
hide_rom
(
self
):
"""
Move visual ROM far above the meshs, as we cannot just delete it.
"""
q_far
=
self
.
q_init
[::]
q_far
[
2
]
=
-
3
self
.
v
(
q_far
)
\ No newline at end of file
script/scenarios/template_talos_path.py
deleted
100644 → 0
View file @
74bc5725
from
.template_path
import
AbstractPathPlanning
class
TalosPathPlanning
(
AbstractPathPlanning
):
def
__init__
(
self
):
super
().
__init__
()
# set default bounds to a large workspace on x,y with small interval around reference z value
self
.
root_translation_bounds
=
[
-
5.
,
5.
,
-
5.
,
5.
,
0.95
,
1.05
]
# set default used limbs to be both feet
self
.
used_limbs
=
[
'talos_lleg_rom'
,
'talos_rleg_rom'
]
self
.
size_foot_x
=
0.2
# size of the feet along the x axis
self
.
size_foot_y
=
0.12
# size of the feet along the y axis
self
.
v_max
=
0.3
self
.
a_max
=
0.1
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
0
,
0
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
0
,
0
]
def
load_robot
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
self
.
rbprmBuilder
=
Robot
()
self
.
rbprmBuilder
.
client
.
robot
.
setDimensionExtraConfigSpace
(
self
.
extra_dof
)
self
.
q_init
=
self
.
rbprmBuilder
.
getCurrentConfig
()
self
.
q_goal
=
self
.
rbprmBuilder
.
getCurrentConfig
()
self
.
q_init
[
2
]
=
self
.
rbprmBuilder
.
ref_height
self
.
q_goal
[
2
]
=
self
.
rbprmBuilder
.
ref_height
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