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
6fc60fcd
Commit
6fc60fcd
authored
Mar 09, 2020
by
Pierre Fernbach
Browse files
[Scripts] format changes according to reviews
parent
68e950c3
Changes
24
Hide whitespace changes
Inline
Side-by-side
src/hpp/corbaserver/rbprm/scenarios/abstract_contact_generator.py
View file @
6fc60fcd
...
...
@@ -128,10 +128,10 @@ class AbstractContactGenerator:
: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
]
self
.
q_init
[:
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
]
self
.
q_goal
[:
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
()
...
...
@@ -181,9 +181,7 @@ class AbstractContactGenerator:
print
(
"Contact plan generated in : "
+
str
(
t_interpolate_configs
)
+
" s"
)
print
(
"number of configs :"
,
len
(
self
.
configs
))
"""
Helper methods used to display results
"""
# Helper methods used to display results
def
display_sequence
(
self
,
dt
=
0.5
):
"""
...
...
src/hpp/corbaserver/rbprm/scenarios/abstract_path_planner.py
View file @
6fc60fcd
...
...
@@ -2,8 +2,6 @@ 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
:
...
...
@@ -33,8 +31,8 @@ class AbstractPathPlanner:
@
abstractmethod
def
load_rbprm
(
self
):
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
pass
def
set_configurations
(
self
):
...
...
@@ -45,35 +43,41 @@ class AbstractPathPlanner:
self
.
q_goal
[
2
]
=
self
.
rbprmBuilder
.
ref_height
def
compute_extra_config_bounds
(
self
):
"""
Compute extra dof bounds from the current values of v_max and a_max
By default, set symmetrical bounds on x and y axis and bounds z axis values to 0
"""
# 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
.
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
"""
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
"""
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
"""
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_configurations
()
self
.
compute_extra_config_bounds
()
...
...
@@ -95,13 +99,13 @@ class AbstractPathPlanner:
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
"""
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
])
...
...
@@ -131,10 +135,10 @@ class AbstractPathPlanner:
def
init_planner
(
self
,
kinodynamic
=
True
,
optimize
=
True
):
"""
Select the rbprm methods, and the kinodynamic ones if required
:param kinodynamic: if True, also select the kinodynamic methods
:param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
"""
Select the rbprm methods, and the kinodynamic ones if required
:param kinodynamic: if True, also select the kinodynamic methods
:param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
"""
self
.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
self
.
ps
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
if
kinodynamic
:
...
...
@@ -149,9 +153,9 @@ class AbstractPathPlanner:
def
solve
(
self
):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
"""
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
():
...
...
@@ -162,18 +166,13 @@ class AbstractPathPlanner:
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)
"""
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
self
.
pp
is
not
None
:
if
path_id
<
0
:
path_id
=
self
.
ps
.
numberPaths
()
-
1
...
...
@@ -182,10 +181,10 @@ class AbstractPathPlanner:
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)
"""
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)
"""
self
.
show_rom
()
if
self
.
pp
is
not
None
:
if
path_id
<
0
:
...
...
@@ -195,32 +194,32 @@ class AbstractPathPlanner:
def
hide_rom
(
self
):
"""
Remove the current robot from the display
"""
Remove the current robot from the display
"""
self
.
v
.
client
.
gui
.
setVisibility
(
self
.
robot_node_name
,
"OFF"
)
def
show_rom
(
self
):
"""
Add the current robot to the display
"""
Add the current robot to the display
"""
self
.
v
.
client
.
gui
.
setVisibility
(
self
.
robot_node_name
,
"ON"
)
@
abstractmethod
def
run
(
self
):
"""
Must be defined in the child class to run all the methods with the correct arguments.
"""
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()
"""
self.init_problem()
# define initial and goal position
self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
self.play_path()
"""
pass
src/hpp/corbaserver/rbprm/scenarios/demos/hrp2_flatGround_path.py
View file @
6fc60fcd
...
...
@@ -5,7 +5,7 @@ class PathPlanner(HRP2PathPlanner):
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[
0
:
2
]
=
[
0
,
0
]
self
.
q_init
[:
2
]
=
[
0
,
0
]
self
.
q_goal
[
0
]
=
1.5
self
.
init_viewer
(
"multicontact/ground"
,
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/hrp2_plateformes_path.py
View file @
6fc60fcd
...
...
@@ -12,8 +12,8 @@ class PathPlanner(HRP2PathPlanner):
self
.
v_max
=
0.3
self
.
root_translation_bounds
=
[
-
5
,
5
,
-
1.5
,
1.5
,
0.70
,
0.8
]
self
.
init_problem
()
self
.
q_init
[
0
:
3
]
=
[
0.12
,
0.25
,
0.75
]
self
.
q_goal
[
0
:
3
]
=
[
1.08
,
0.25
,
0.75
]
self
.
q_init
[:
3
]
=
[
0.12
,
0.25
,
0.75
]
self
.
q_goal
[:
3
]
=
[
1.08
,
0.25
,
0.75
]
self
.
init_viewer
(
"multicontact/plateforme_surfaces"
,
reduce_sizes
=
[
0.1
,
0
,
0
],
visualize_affordances
=
[
"Support"
])
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/hyq_darpa_path.py
View file @
6fc60fcd
...
...
@@ -11,8 +11,8 @@ class PathPlanner(HyqPathPlanner):
self
.
root_rotation_bounds
=
[
-
0.4
,
0.4
,
-
0.3
,
0.3
,
-
0.3
,
0.3
]
self
.
init_problem
()
self
.
q_init
[
0
:
2
]
=
[
-
2
,
0
]
self
.
q_goal
[
0
:
2
]
=
[
3
,
0
]
self
.
q_init
[:
2
]
=
[
-
2
,
0
]
self
.
q_goal
[:
2
]
=
[
3
,
0
]
self
.
init_viewer
(
"multicontact/darpa"
,
visualize_affordances
=
[
"Support"
],
reduce_sizes
=
[
0.06
,
0
,
0
])
self
.
init_planner
(
kinodynamic
=
False
)
self
.
solve
()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/hyq_slalom_debris_path.py
View file @
6fc60fcd
...
...
@@ -13,9 +13,9 @@ class PathPlanner(HyqPathPlanner):
self
.
root_translation_bounds
=
[
-
1.7
,
2.5
,
-
0.2
,
2
,
0.64
,
0.64
]
self
.
init_problem
()
self
.
q_init
[
0
:
2
]
=
[
-
1.5
,
0
]
self
.
q_init
[:
2
]
=
[
-
1.5
,
0
]
self
.
q_init
[
-
6
]
=
0.05
self
.
q_goal
[
0
:
2
]
=
[
2.2
,
0
]
self
.
q_goal
[:
2
]
=
[
2.2
,
0
]
self
.
q_goal
[
-
6
]
=
0.05
self
.
init_viewer
(
"multicontact/slalom_debris"
)
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_flatGround_path.py
View file @
6fc60fcd
...
...
@@ -5,14 +5,14 @@ class PathPlanner(TalosPathPlanner):
def
run
(
self
):
self
.
init_problem
()
self
.
q_init
[
0
:
2
]
=
[
0
,
0
]
self
.
q_goal
[
0
:
2
]
=
[
1
,
0
]
self
.
q_init
[:
2
]
=
[
0
,
0
]
self
.
q_goal
[:
2
]
=
[
1
,
0
]
self
.
init_viewer
(
"multicontact/ground"
,
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
#self.play_path()
#
self.play_path()
self
.
hide_rom
()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_hard_path.py
View file @
6fc60fcd
...
...
@@ -21,14 +21,14 @@ class PathPlanner(TalosPathPlanner):
]
self
.
set_joints_bounds
()
self
.
q_init
[
0
:
2
]
=
[
-
0.7
,
2
]
self
.
q_goal
[
0
:
2
]
=
[
0
,
-
1
]
self
.
q_init
[:
2
]
=
[
-
0.7
,
2
]
self
.
q_goal
[:
2
]
=
[
0
,
-
1
]
self
.
init_viewer
(
"multicontact/floor_bauzil"
,
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
#self.play_path()
#
self.play_path()
self
.
hide_rom
()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_navBauzil_path.py
View file @
6fc60fcd
...
...
@@ -19,17 +19,17 @@ class PathPlanner(TalosPathPlanner):
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
[
0
:
2
]
=
[
-
0.9
,
1.7
]
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
[
0
:
2
]
=
[
3.6
,
1.2
]
self
.
q_goal
[:
2
]
=
[
3.6
,
1.2
]
self
.
q_goal
[
-
6
:
-
3
]
=
[
0
,
-
0.1
,
0
]
self
.
init_viewer
(
"multicontact/floor_bauzil"
,
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
#self.play_path()
#
self.play_path()
self
.
hide_rom
()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_plateformes_path.py
View file @
6fc60fcd
...
...
@@ -12,8 +12,8 @@ class PathPlanner(TalosPathPlanner):
self
.
root_translation_bounds
=
[
-
5
,
5
,
-
1.5
,
1.5
,
0.95
,
1.3
]
self
.
init_problem
()
self
.
q_init
[
0
:
3
]
=
[
0.16
,
0.25
,
1.14
]
self
.
q_goal
[
0
:
3
]
=
[
1.09
,
0.25
,
1.14
]
self
.
q_init
[:
3
]
=
[
0.16
,
0.25
,
1.14
]
self
.
q_goal
[:
3
]
=
[
1.09
,
0.25
,
1.14
]
self
.
init_viewer
(
"multicontact/plateforme_surfaces"
,
reduce_sizes
=
[
0.18
,
0
,
0
],
...
...
@@ -21,7 +21,7 @@ class PathPlanner(TalosPathPlanner):
self
.
init_planner
(
kinodynamic
=
False
)
self
.
solve
()
self
.
display_path
()
#self.play_path()
#
self.play_path()
self
.
hide_rom
()
...
...
src/hpp/corbaserver/rbprm/scenarios/demos/talos_stairs10_path.py
View file @
6fc60fcd
...
...
@@ -31,14 +31,14 @@ class PathPlanner(TalosPathPlanner):
self
.
root_translation_bounds
=
[
-
1.5
,
3
,
0.
,
3.3
,
0.95
,
2.
]
self
.
init_problem
()
self
.
q_init
[
0
:
3
]
=
[
0.05
,
1.2
,
0.98
]
self
.
q_goal
[
0
:
3
]
=
[
1.87
,
1.2
,
1.58
]
self
.
q_init
[:
3
]
=
[
0.05
,
1.2
,
0.98
]
self
.
q_goal
[:
3
]
=
[
1.87
,
1.2
,
1.58
]
self
.
init_viewer
(
"multicontact/bauzil_stairs"
,
reduce_sizes
=
[
0.11
,
0.
,
0.
],
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
#self.play_path()
#
self.play_path()
self
.
hide_rom
()
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/anymal_circle_oriented_path.py
View file @
6fc60fcd
from
hpp.corbaserver.rbprm.scenarios.anymal_path_planner
import
AnymalPathPlanner
from
pinocchio
import
Quaternion
import
numpy
as
np
import
random
import
sys
class
PathPlanner
(
AnymalPathPlanner
):
...
...
@@ -23,17 +25,16 @@ class PathPlanner(AnymalPathPlanner):
"""
randomly sample initial and goal configuration :
"""
self
.
q_init
[
0
:
3
]
=
[
0
,
0
,
0.465
]
self
.
q_init
[:
3
]
=
[
0
,
0
,
0.465
]
self
.
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
self
.
q_init
[
-
6
]
=
self
.
v_init
import
random
random
.
seed
()
alpha
=
random
.
uniform
(
0.
,
2.
*
np
.
pi
)
#alpha = 4.
print
(
"Test on a circle, alpha = "
,
alpha
)
self
.
q_goal
=
self
.
q_init
[::]
self
.
q_goal
[
0
:
3
]
=
[
self
.
radius
*
np
.
sin
(
alpha
),
-
self
.
radius
*
np
.
cos
(
alpha
),
0.465
]
self
.
q_goal
[:
3
]
=
[
self
.
radius
*
np
.
sin
(
alpha
),
-
self
.
radius
*
np
.
cos
(
alpha
),
0.465
]
# set final orientation to be along the circle :
vx
=
np
.
matrix
([
1
,
0
,
0
]).
T
v_goal
=
np
.
matrix
([
self
.
q_goal
[
0
],
self
.
q_goal
[
1
],
0
]).
T
...
...
@@ -62,7 +63,6 @@ class PathPlanner(AnymalPathPlanner):
success
=
self
.
ps
.
client
.
problem
.
prepareSolveStepByStep
()
if
not
success
:
print
(
"planning failed."
)
import
sys
sys
.
exit
(
1
)
self
.
ps
.
client
.
problem
.
finishSolveStepByStep
()
self
.
display_path
()
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/anymal_circle_path.py
View file @
6fc60fcd
from
hpp.corbaserver.rbprm.scenarios.anymal_path_planner
import
AnymalPathPlanner
from
pinocchio
import
Quaternion
import
numpy
as
np
import
random
import
sys
class
PathPlanner
(
AnymalPathPlanner
):
...
...
@@ -17,18 +19,17 @@ class PathPlanner(AnymalPathPlanner):
"""
randomly sample initial and goal configuration :
"""
self
.
q_init
[
0
:
3
]
=
[
0
,
0
,
0.465
]
self
.
q_init
[:
3
]
=
[
0
,
0
,
0.465
]
self
.
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
import
random
random
.
seed
()
alpha
=
random
.
uniform
(
0.
,
2.
*
np
.
pi
)
print
(
"Test on a circle, alpha = "
,
alpha
)
self
.
q_goal
=
self
.
q_init
[::]
self
.
q_goal
[
0
:
3
]
=
[
self
.
radius
*
np
.
sin
(
alpha
),
-
self
.
radius
*
np
.
cos
(
alpha
),
0.465
]
self
.
q_goal
[:
3
]
=
[
self
.
radius
*
np
.
sin
(
alpha
),
-
self
.
radius
*
np
.
cos
(
alpha
),
0.465
]
self
.
v
(
self
.
q_init
)
print
(
"initial root position : "
,
self
.
q_init
[
0
:
3
])
print
(
"final root position : "
,
self
.
q_goal
[
0
:
3
])
print
(
"initial root position : "
,
self
.
q_init
[:
3
])
print
(
"final root position : "
,
self
.
q_goal
[:
3
])
self
.
ps
.
setInitialConfig
(
self
.
q_init
)
self
.
ps
.
addGoalConfig
(
self
.
q_goal
)
...
...
@@ -46,7 +47,6 @@ class PathPlanner(AnymalPathPlanner):
success
=
self
.
ps
.
client
.
problem
.
prepareSolveStepByStep
()
if
not
success
:
print
(
"planning failed."
)
import
sys
sys
.
exit
(
1
)
self
.
ps
.
client
.
problem
.
finishSolveStepByStep
()
self
.
display_path
()
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/anymal_contact_generator.py
View file @
6fc60fcd
from
hpp.corbaserver.rbprm.scenarios.anymal_contact_generator
import
AnymalContactGenerator
as
Parent
import
sys
class
AnymalContactGenerator
(
Parent
):
...
...
@@ -49,8 +50,6 @@ class AnymalContactGenerator(Parent):
f
.
write
(
"cg_too_many_states: "
+
str
(
cg_too_many_states
)
+
"
\n
"
)
f
.
close
()
if
(
not
cg_success
)
or
cg_too_many_states
:
import
sys
sys
.
exit
(
1
)
if
throw_if_goal_not_reached
and
not
cg_reach_goal
:
import
sys
sys
.
exit
(
1
)
src/hpp/corbaserver/rbprm/scenarios/memmo/anymal_platform_random_path.py
View file @
6fc60fcd
from
hpp.corbaserver.rbprm.scenarios.anymal_path_planner
import
AnymalPathPlanner
from
pinocchio
import
Quaternion
import
numpy
as
np
import
random
import
sys
class
PathPlanner
(
AnymalPathPlanner
):
...
...
@@ -32,16 +34,15 @@ class PathPlanner(AnymalPathPlanner):
"""
randomly sample initial and goal configuration :
"""
import
random
random
.
seed
()
self
.
q_init
[
0
:
3
]
=
[
self
.
q_init
[:
3
]
=
[
random
.
uniform
(
self
.
X_BOUNDS
[
0
],
self
.
X_BOUNDS
[
1
]),
random
.
uniform
(
self
.
Y_BOUNDS
[
0
],
self
.
Y_BOUNDS
[
1
]),
self
.
Z_VALUE
]
self
.
q_goal
=
self
.
q_init
[::]
for
i
in
range
(
random
.
randint
(
0
,
1000
)):
random
.
uniform
(
0.
,
1.
)
self
.
q_goal
[
0
:
3
]
=
[
self
.
q_goal
[:
3
]
=
[
random
.
uniform
(
self
.
X_BOUNDS
[
0
],
self
.
X_BOUNDS
[
1
]),
random
.
uniform
(
self
.
Y_BOUNDS
[
0
],
self
.
Y_BOUNDS
[
1
]),
self
.
Z_VALUE
]
...
...
@@ -54,8 +55,8 @@ class PathPlanner(AnymalPathPlanner):
self
.
q_init
[
3
:
7
]
=
quat
.
coeffs
().
tolist
()
self
.
q_goal
[
3
:
7
]
=
self
.
q_init
[
3
:
7
]
self
.
v
(
self
.
q_init
)
print
(
"initial root position : "
,
self
.
q_init
[
0
:
3
])
print
(
"final root position : "
,
self
.
q_goal
[
0
:
3
])
print
(
"initial root position : "
,
self
.
q_init
[:
3
])
print
(
"final root position : "
,
self
.
q_goal
[:
3
])
self
.
ps
.
setInitialConfig
(
self
.
q_init
)
self
.
ps
.
addGoalConfig
(
self
.
q_goal
)
...
...
@@ -73,7 +74,6 @@ class PathPlanner(AnymalPathPlanner):
success
=
self
.
ps
.
client
.
problem
.
prepareSolveStepByStep
()
if
not
success
:
print
(
"planning failed."
)
import
sys
sys
.
exit
(
1
)
self
.
ps
.
client
.
problem
.
finishSolveStepByStep
()
self
.
display_path
()
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/talos_bauzil_with_stairs_path.py
View file @
6fc60fcd
from
hpp.corbaserver.rbprm.tools.sample_root_config
import
generate_random_conf_without_orientation
from
hpp.corbaserver.rbprm.scenarios.talos_path_planner
import
TalosPathPlanner
from
talos_rbprm.talos_abstract
import
Robot
import
random
class
PathPlanner
(
TalosPathPlanner
):
...
...
@@ -50,7 +51,6 @@ class PathPlanner(TalosPathPlanner):
"""
randomly sample initial and goal configuration :
"""
import
random
random
.
seed
()
# sample an initial and goal zone : either floor, right platform or left platform. With more weight on the floor
# 0,1,2 are floor, 3 is platform10 4 is platform15
...
...
@@ -123,7 +123,7 @@ class PathPlanner(TalosPathPlanner):
self
.
q_init
=
self
.
ps
.
configAtParam
(
pathId
,
0
)
self
.
q_goal
=
self
.
ps
.
configAtParam
(
pathId
,
self
.
ps
.
pathLength
(
pathId
))
self
.
display_path
()
#self.play_path()
#
self.play_path()
self
.
hide_rom
()
...
...
src/hpp/corbaserver/rbprm/scenarios/memmo/talos_circle_oriented_path.py
View file @
6fc60fcd
from
hpp.corbaserver.rbprm.scenarios.talos_path_planner
import
TalosPathPlanner
from
pinocchio
import
Quaternion
import
numpy
as
np
import
random
import
sys
class
PathPlanner
(
TalosPathPlanner
):
...
...
@@ -22,17 +24,16 @@ class PathPlanner(TalosPathPlanner):
"""
randomly sample initial and goal configuration :
"""
self
.
q_init
[
0
:
3
]
=
[
0
,
0
,
1.
]
self
.
q_init
[:
3
]
=
[
0
,
0
,
1.
]
self
.
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
self
.
q_init
[
-
6
]
=
self
.
v_init
import
random
random
.
seed
()
alpha
=
random
.
uniform
(
0.
,
2.
*
np
.
pi
)
#alpha = 4.
print
(
"Test on a circle, alpha = "
,
alpha
)
self
.
q_goal
=
self
.
q_init
[::]
self
.
q_goal
[
0
:
3
]
=
[
self
.
radius
*
np
.
sin
(
alpha
),
-
self
.
radius
*
np
.
cos
(
alpha
),
1.
]
self
.
q_goal
[:
3
]
=
[
self
.
radius
*
np
.
sin
(
alpha
),
-
self
.
radius
*
np
.
cos
(
alpha
),
1.
]
# set final orientation to be along the circle :
vx
=
np
.
matrix
([
1
,
0
,
0
]).
T
v_goal
=
np
.
matrix
([
self
.
q_goal
[
0
],
self
.
q_goal
[
1
],
0
]).
T
...
...
@@ -61,12 +62,11 @@ class PathPlanner(TalosPathPlanner):
success
=
self
.
ps
.
client
.
problem
.
prepareSolveStepByStep
()
if
not
success
:
print
(
"planning failed."
)
import
sys
sys
.
exit
(
1