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
4ae0a4c7
Commit
4ae0a4c7
authored
Feb 27, 2020
by
Pierre Fernbach
Browse files
[Scripts] add template for contact generation
parent
d6c5cee5
Changes
4
Hide whitespace changes
Inline
Side-by-side
script/scenarios/abstract_contact_generator.py
0 → 100644
View file @
4ae0a4c7
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
displayContactSequence
from
hpp.corbaserver
import
ProblemSolver
import
time
from
abc
import
abstractmethod
class
AbstractContactGenerator
:
fullbody
=
None
ps
=
None
v
=
None
q_ref
=
None
weight_postural
=
None
q_init
=
None
q_goal
=
None
def
__init__
(
self
,
path_planner
):
"""
Constructor, run the guide path and save the results
:param path_planner: an instance of a child class of AbstractPathPlanner
"""
path_planner
.
run
()
self
.
path_planner
=
path_planner
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
=
[]
@
abstractmethod
def
load_fullbody
(
self
):
pass
def
set_joints_bounds
(
self
):
self
.
fullbody
.
setJointBounds
(
"root_joint"
,
self
.
path_planner
.
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
):
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
):
self
.
fullbody
.
limbs_names
=
self
.
used_limbs
if
nb_samples
is
None
:
nb_samples
=
self
.
fullbody
.
nbSamples
if
octree_size
is
None
:
octree_size
=
self
.
fullbody
.
octreeSize
print
(
"Generate limb DB ..."
)
t_start
=
time
.
time
()
self
.
fullbody
.
loadAllLimbs
(
heuristic
,
analysis
,
nb_samples
,
octree_size
)
t_generate
=
time
.
time
()
-
t_start
print
(
"Databases generated in : "
+
str
(
t_generate
)
+
" s"
)
def
init_problem
(
self
):
self
.
ps
=
ProblemSolver
(
self
.
fullbody
)
if
self
.
path_planner
.
v_max
>=
0
:
self
.
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
self
.
path_planner
.
v_max
)
if
self
.
path_planner
.
a_max
>=
0
:
self
.
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
self
.
path_planner
.
a_max
)
def
init_viewer
(
self
):
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
):
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
self
.
q_goal
=
self
.
q_init
[::]
self
.
q_goal
[
0
:
7
]
=
self
.
path_planner
.
ps
.
configAtParam
(
self
.
pid
,
self
.
path_planner
.
ps
.
pathLength
(
self
.
pid
))[
0
:
7
]
# copy extraconfig for start and init configurations
configSize
=
self
.
fullbody
.
getConfigSize
()
-
self
.
fullbody
.
client
.
robot
.
getDimensionExtraConfigSpace
()
q_init_guide
=
self
.
path_planner
.
ps
.
configAtParam
(
self
.
pid
,
0
)
q_goal_guide
=
self
.
path_planner
.
ps
.
configAtParam
(
self
.
pid
,
self
.
path_planner
.
ps
.
pathLength
(
self
.
pid
))
index_ecs
=
len
(
q_init_guide
)
-
self
.
path_planner
.
extra_dof
self
.
q_init
[
configSize
:
configSize
+
3
]
=
q_init_guide
[
index_ecs
:
index_ecs
+
3
]
if
use_acc_init
:
self
.
q_init
[
configSize
+
3
:
configSize
+
6
]
=
q_init_guide
[
index_ecs
+
3
:
index_ecs
+
6
]
else
:
self
.
q_init
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
self
.
q_goal
[
configSize
:
configSize
+
3
]
=
q_goal_guide
[
index_ecs
:
index_ecs
+
3
]
if
use_acc_end
:
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
]
def
interpolate
(
self
):
# specify the full body configurations as start and goal state of the problem
self
.
fullbody
.
setStartState
(
self
.
q_init
,
self
.
init_contacts
)
self
.
fullbody
.
setEndState
(
self
.
q_goal
,
self
.
end_contacts
)
print
(
"Generate contact plan ..."
)
t_start
=
time
.
time
()
self
.
configs
=
self
.
fullbody
.
interpolate
(
0.01
,
pathId
=
self
.
pid
,
robustnessTreshold
=
self
.
robustness
,
filterStates
=
self
.
filter_states
,
testReachability
=
self
.
test_reachability
,
quasiStatic
=
self
.
quasi_static
,
erasePreviousStates
=
self
.
erase_previous_states
)
t_interpolate_configs
=
time
.
time
()
-
t_start
print
(
"Contact plan generated in : "
+
str
(
t_interpolate_configs
)
+
" s"
)
print
(
"number of configs :"
,
len
(
self
.
configs
))
def
display_sequence
(
self
):
displayContactSequence
(
self
.
v
,
self
.
configs
)
@
abstractmethod
def
run
(
self
):
"""
Must be defined in the child class to run all the methods with the correct arguments.
"""
# example of definition:
"""
self.load_fullbody()
self.set_joints_bounds()
self.set_reference(True)
self.load_limbs("fixedStep06")
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
0 → 100644
View file @
4ae0a4c7
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
AbstractPathPlanner
:
rbprmBuilder
=
None
ps
=
None
v
=
None
afftool
=
None
pp
=
None
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
]
self
.
q_init
=
[]
self
.
q_goal
=
[]
@
abstractmethod
def
load_rbprm
(
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_rbprm
()
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
run
(
self
):
self
.
init_problem
()
self
.
init_viewer
()
self
.
init_planner
()
self
.
solve
()
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
)
@
abstractmethod
def
run
(
self
):
"""
Must be defined in the child class to run all the methods with the correct arguments.
"""
# example of definition:
"""
self.init_problem()
# define initial and goal position
self.q_init[0:2] = [0, 0]
self.q_goal[0:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
self.play_path()
"""
pass
\ No newline at end of file
script/scenarios/demos/talos_flatGround.py
View file @
4ae0a4c7
from
talos_rbprm.talos
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
(
"Plan guide trajectory ..."
)
from
.
import
talos_flatGround_path
as
tp
print
(
"Done."
)
import
time
from
scenarios.demos.talos_flatGround_path
import
PathPlanner
from
scenarios.talos_contact_generator
import
TalosContactGenerator
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
Robot
()
class
ContactGenerator
(
TalosContactGenerator
):
# Set the bounds for the root
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.05
])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
fullBody
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
tp
.
vMax
,
tp
.
vMax
,
-
tp
.
vMax
,
tp
.
vMax
,
0
,
0
,
-
tp
.
aMax
,
tp
.
aMax
,
-
tp
.
aMax
,
tp
.
aMax
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
tp
.
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
tp
.
aMax
)
#load the viewer
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
def
__init__
(
self
):
super
().
__init__
(
PathPlanner
())
# load a reference configuration
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_ref
)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
fullBody
.
usePosturalTaskContactCreation
(
True
)
def
run
(
self
):
self
.
load_fullbody
()
self
.
set_joints_bounds
()
self
.
set_reference
(
True
)
self
.
load_limbs
(
"fixedStep06"
)
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
()
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
# generate databases :
fullBody
.
limbs_names
=
tp
.
used_limbs
fullBody
.
loadAllLimbs
(
"fixedStep06"
)
tGenerate
=
time
.
time
()
-
tStart
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
#define initial and final configurations :
q_init
=
q_ref
[::]
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
0.001
)[
0
:
7
]
q_goal
=
q_init
[::]
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
# force root height to be at the reference position:
q_init
[
2
]
=
q_ref
[
2
]
q_goal
[
2
]
=
q_ref
[
2
]
# copy extraconfig for start and init configurations
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
configSize
:
configSize
+
3
]
=
tp
.
ps
.
configAtParam
(
pId
,
0
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
tp
.
ps
.
configAtParam
(
pId
,
0
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
q_goal
[
configSize
:
configSize
+
3
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
fullBody
.
setStaticStability
(
True
)
v
.
addLandmark
(
'talos/base_link'
,
0.3
)
v
(
q_init
)
# specify the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,[
fullBody
.
rLegId
,
fullBody
.
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
rLegId
,
fullBody
.
lLegId
])
print
(
"Generate contact plan ..."
)
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
True
,
quasiStatic
=
True
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
(
"Done."
)
print
(
"Contact plan generated in : "
+
str
(
tInterpolateConfigs
)
+
" s"
)
print
(
"number of configs :"
,
len
(
configs
))
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
script/scenarios/demos/talos_flatGround_path.py
View file @
4ae0a4c7
from
scenarios.
template_
talos_path
import
TalosPathPlann
ing
from
scenarios.talos_path
_planner
import
TalosPathPlann
er
class
PathPlann
ing
(
TalosPathPlann
ing
):
class
PathPlann
er
(
TalosPathPlann
er
):
def
run
(
self
):
self
.
init_problem
()
...
...
@@ -12,8 +12,9 @@ class PathPlanning(TalosPathPlanning):
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
self
.
play_path
()
#
self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
PathPlanning
().
run
()
\ No newline at end of file
planner
=
PathPlanner
()
planner
.
run
()
\ No newline at end of file
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