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
afd0446c
Commit
afd0446c
authored
Mar 05, 2020
by
Pierre Fernbach
Browse files
[Scripts] save 'guide' problem to be able to display path after
parent
7a71de96
Changes
6
Hide whitespace changes
Inline
Side-by-side
script/scenarios/abstract_contact_generator.py
View file @
afd0446c
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
displayContactSequence
from
hpp.corbaserver
import
ProblemSolver
from
hpp.corbaserver
import
ProblemSolver
,
Client
import
time
from
abc
import
abstractmethod
...
...
@@ -13,6 +13,7 @@ class AbstractContactGenerator:
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
robot_node_name
=
None
# name of the robot in the node list of the viewer
def
__init__
(
self
,
path_planner
):
"""
...
...
@@ -21,8 +22,18 @@ class AbstractContactGenerator:
"""
path_planner
.
run
()
self
.
path_planner
=
path_planner
self
.
path_planner
.
hide_rom
()
# ID of the guide path used in problemSolver, default to the last one
self
.
pid
=
self
.
path_planner
.
ps
.
numberPaths
()
-
1
# Save the guide planning problem in a specific instance,
# such that we can use it again even after creating the "fullbody" problem:
self
.
cl
=
Client
()
self
.
cl
.
problem
.
selectProblem
(
"guide_planning"
)
path_planner
.
load_rbprm
()
ProblemSolver
(
path_planner
.
rbprmBuilder
)
self
.
cl
.
problem
.
selectProblem
(
"default"
)
self
.
cl
.
problem
.
movePathToProblem
(
self
.
pid
,
"guide_planning"
,
self
.
path_planner
.
rbprmBuilder
.
getAllJointNames
()[
1
:])
# copy bounds and limbs used from path planning :
self
.
used_limbs
=
path_planner
.
used_limbs
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
...
...
@@ -143,15 +154,19 @@ class AbstractContactGenerator:
self
.
q_goal
[
2
]
=
self
.
q_ref
[
2
]
self
.
v
(
self
.
q_init
)
def
set_start_end_states
(
self
):
"""
Set the current q_init and q_goal as initial/goal state for the contact planning
"""
self
.
fullbody
.
setStartState
(
self
.
q_init
,
self
.
init_contacts
)
self
.
fullbody
.
setEndState
(
self
.
q_goal
,
self
.
end_contacts
)
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
.
set_start_end_states
()
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
()
...
...
@@ -166,12 +181,40 @@ class AbstractContactGenerator:
print
(
"Contact plan generated in : "
+
str
(
t_interpolate_configs
)
+
" s"
)
print
(
"number of configs :"
,
len
(
self
.
configs
))
def
display_sequence
(
self
):
"""
Helper methods used to display results
"""
def
display_sequence
(
self
,
dt
=
0.5
):
"""
Display the sequence of configuration in contact,
requires that self.configs is not empty
:param dt: the pause (in second) between each configuration
"""
displayContactSequence
(
self
.
v
,
self
.
configs
)
displayContactSequence
(
self
.
v
,
self
.
configs
,
dt
)
def
display_init_config
(
self
):
self
.
v
(
self
.
q_init
)
def
display_end_config
(
self
):
self
.
v
(
self
.
q_goal
)
def
play_guide_path
(
self
):
"""
display the guide path planned
"""
self
.
v
.
client
.
gui
.
setVisibility
(
self
.
robot_node_name
,
"OFF"
)
self
.
path_planner
.
show_rom
()
self
.
cl
.
problem
.
selectProblem
(
"guide_planning"
)
# this is not the same problem as in the guide_planner. We only added the final path in this problem,
# thus there is only 1 path in this problem
self
.
path_planner
.
pp
(
0
)
self
.
cl
.
problem
.
selectProblem
(
"default"
)
self
.
path_planner
.
hide_rom
()
self
.
v
.
client
.
gui
.
setVisibility
(
self
.
robot_node_name
,
"ON"
)
def
run
(
self
):
"""
...
...
script/scenarios/abstract_path_planner.py
View file @
afd0446c
...
...
@@ -12,6 +12,7 @@ class AbstractPathPlanner:
afftool
=
None
pp
=
None
extra_dof_bounds
=
None
robot_node_name
=
None
# name of the robot in the node list of the viewer
def
__init__
(
self
):
self
.
v_max
=
-
1
# bounds on the linear velocity for the root, negative values mean unused
...
...
@@ -180,6 +181,7 @@ class AbstractPathPlanner:
: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)
"""
self
.
show_rom
()
if
self
.
pp
is
not
None
:
if
path_id
<
0
:
path_id
=
self
.
ps
.
numberPaths
()
-
1
...
...
@@ -188,11 +190,16 @@ class AbstractPathPlanner:
def
hide_rom
(
self
):
"""
Move visual ROM far above the meshs, as we cannot just delete it.
Remove the current robot from the display
"""
q_far
=
self
.
q_init
[::]
q_far
[
2
]
=
-
3
self
.
v
(
q_far
)
self
.
v
.
client
.
gui
.
setVisibility
(
self
.
robot_node_name
,
"OFF"
)
def
show_rom
(
self
):
"""
Add the current robot to the display
"""
self
.
v
.
client
.
gui
.
setVisibility
(
self
.
robot_node_name
,
"ON"
)
@
abstractmethod
def
run
(
self
):
...
...
script/scenarios/hrp2_contact_generator.py
View file @
afd0446c
...
...
@@ -6,6 +6,7 @@ class HRP2ContactGenerator(AbstractContactGenerator):
def
__init__
(
self
,
path_planner
):
super
().
__init__
(
path_planner
)
self
.
robustness
=
1
self
.
robot_node_name
=
"hrp2_14"
def
load_fullbody
(
self
):
...
...
script/scenarios/hrp2_path_planner.py
View file @
afd0446c
...
...
@@ -16,13 +16,10 @@ class HRP2PathPlanner(AbstractPathPlanner):
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
]
self
.
robot_node_name
=
"hrp2_trunk_flexible"
def
load_rbprm
(
self
):
from
hpp.corbaserver.rbprm.hrp2_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
script/scenarios/talos_contact_generator.py
View file @
afd0446c
...
...
@@ -6,7 +6,7 @@ class TalosContactGenerator(AbstractContactGenerator):
def
__init__
(
self
,
path_planner
):
super
().
__init__
(
path_planner
)
self
.
robustness
=
2
self
.
robot_node_name
=
"talos"
def
load_fullbody
(
self
):
from
talos_rbprm.talos
import
Robot
...
...
script/scenarios/talos_path_planner.py
View file @
afd0446c
...
...
@@ -15,6 +15,7 @@ class TalosPathPlanner(AbstractPathPlanner):
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
]
self
.
robot_node_name
=
"talos_trunk"
def
load_rbprm
(
self
):
from
talos_rbprm.talos_abstract
import
Robot
...
...
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