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
70ea14c6
Commit
70ea14c6
authored
Mar 04, 2020
by
Pierre Fernbach
Browse files
[Scripts] refactor memmo/talos_*_path scripts
parent
211fc7b7
Changes
15
Hide whitespace changes
Inline
Side-by-side
script/scenarios/abstract_path_planner.py
View file @
70ea14c6
...
...
@@ -113,24 +113,32 @@ class AbstractPathPlanner:
def
addLandmark
(
self
,
a
,
b
):
return
self
.
v
=
FakeViewer
()
self
.
pp
=
PathPlayer
(
self
.
v
)
try
:
self
.
pp
=
PathPlayer
(
self
.
v
)
except
Exception
:
pass
for
aff_type
in
visualize_affordances
:
self
.
afftool
.
visualiseAffordances
(
aff_type
,
self
.
v
,
self
.
v
.
color
.
lightBrown
)
def
init_planner
(
self
,
kinodynamic
=
True
):
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)
"""
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"
)
if
optimize
:
if
kinodynamic
:
self
.
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
else
:
self
.
ps
.
addPathOptimizer
(
"RandomShortcut"
)
def
solve
(
self
):
...
...
@@ -160,10 +168,11 @@ 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.1)
"""
if
path_id
<
0
:
path_id
=
self
.
ps
.
numberPaths
()
-
1
self
.
pp
.
dt
=
dt
self
.
pp
.
displayVelocityPath
(
path_id
)
if
self
.
pp
is
not
None
:
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
):
"""
...
...
@@ -171,10 +180,11 @@ 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)
"""
if
path_id
<
0
:
path_id
=
self
.
ps
.
numberPaths
()
-
1
self
.
pp
.
dt
=
dt
self
.
pp
(
path_id
)
if
self
.
pp
is
not
None
:
if
path_id
<
0
:
path_id
=
self
.
ps
.
numberPaths
()
-
1
self
.
pp
.
dt
=
dt
self
.
pp
(
path_id
)
def
hide_rom
(
self
):
"""
...
...
script/scenarios/demos/talos_stairs10_path.py
View file @
70ea14c6
...
...
@@ -17,6 +17,8 @@ class PathPlanner(TalosPathPlanner):
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
self
.
ps
.
setParameter
(
"Kinodynamic/synchronizeVerticalAxis"
,
True
)
self
.
ps
.
setParameter
(
"Kinodynamic/verticalAccelerationBound"
,
3.
)
def
compute_extra_config_bounds
(
self
):
# bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
...
...
script/scenarios/memmo/talos_bauzil_with_stairs_path.py
0 → 100644
View file @
70ea14c6
from
tools.sample_root_config
import
generate_random_conf_without_orientation
from
scenarios.talos_path_planner
import
TalosPathPlanner
from
talos_rbprm.talos_abstract
import
Robot
class
PathPlanner
(
TalosPathPlanner
):
Z_FLOOR
=
Robot
.
ref_height
Z_PLATFORM
=
Robot
.
ref_height
+
0.6
BOUNDS_ALL
=
[
-
2.6
,
4.7
,
-
1.5
,
3.3
,
Z_FLOOR
,
Z_PLATFORM
]
BOUNDS_ALL_PLAT
=
[
-
2.1
,
2.4
,
-
0.1
,
1.6
,
Z_PLATFORM
,
Z_PLATFORM
]
BOUNDS_FLOOR
=
[
-
2.6
,
4.7
,
-
1.5
,
3.3
,
Z_FLOOR
,
Z_FLOOR
]
BOUNDS_PLAT_10
=
[
1.9
,
2.4
,
-
0.1
,
1.6
,
Z_PLATFORM
,
Z_PLATFORM
]
BOUNDS_PLAT_15
=
[
-
2.1
,
-
1.5
,
-
0.1
,
0.5
,
Z_PLATFORM
,
Z_PLATFORM
]
def
__init__
(
self
):
super
().
__init__
()
self
.
status_filename
=
"/res/infos.log"
self
.
root_translation_bounds
=
self
.
BOUNDS_ALL
self
.
v_max
=
0.3
self
.
a_max
=
0.2
def
load_rbprm
(
self
):
# select model with conservative collision geometry for trunk
Robot
.
urdfName
+=
"_large_reducedROM"
# select conservative ROM for feet
Robot
.
urdfNameRom
+=
[
'talos_lleg_rom_reduced'
,
'talos_rleg_rom_reduced'
]
self
.
rbprmBuilder
=
Robot
()
self
.
rbprmBuilder
.
setReferenceEndEffector
(
'talos_lleg_rom_reduced'
,
self
.
rbprmBuilder
.
ref_EE_lLeg
)
self
.
rbprmBuilder
.
setReferenceEndEffector
(
'talos_rleg_rom_reduced'
,
self
.
rbprmBuilder
.
ref_EE_rLeg
)
def
compute_extra_config_bounds
(
self
):
# bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
self
.
extra_dof_bounds
=
[
-
self
.
v_max
,
self
.
v_max
,
-
self
.
v_max
,
self
.
v_max
,
-
2
,
2
,
-
self
.
a_max
,
self
.
a_max
,
-
self
.
a_max
,
self
.
a_max
,
-
3
,
3
]
def
init_problem
(
self
):
super
().
init_problem
()
# greatly increase the number of loops of the random shortcut
self
.
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
500
)
# force the base orientation to follow the direction of motion along the Z axis
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
self
.
ps
.
setParameter
(
"Kinodynamic/synchronizeVerticalAxis"
,
True
)
self
.
ps
.
setParameter
(
"Kinodynamic/verticalAccelerationBound"
,
3.
)
self
.
ps
.
setMaxIterPathPlanning
(
50000
)
def
set_random_configs
(
self
):
"""
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
WEIGHT_FLOOR
=
3
plat_id_init
=
random
.
randint
(
0
,
WEIGHT_FLOOR
+
1
)
plat_id_goal
=
random
.
randint
(
0
,
WEIGHT_FLOOR
+
1
)
while
plat_id_goal
>=
WEIGHT_FLOOR
and
plat_id_goal
==
plat_id_init
:
# cannot have init and end on the same platform (too easy)
plat_id_goal
=
random
.
randint
(
0
,
WEIGHT_FLOOR
+
1
)
print
((
"platform id init : "
,
plat_id_init
))
print
((
"platform id goal : "
,
plat_id_goal
))
#plat_id_init = 3
#plat_id_goal = 0
if
plat_id_init
<
WEIGHT_FLOOR
:
print
(
"init of flat floor"
)
init_bounds
=
self
.
BOUNDS_FLOOR
[::]
elif
plat_id_init
==
WEIGHT_FLOOR
:
print
(
"init on 10cm platform"
)
init_bounds
=
self
.
BOUNDS_PLAT_10
[::]
elif
plat_id_init
==
WEIGHT_FLOOR
+
1
:
print
(
"init on 15cm platform"
)
init_bounds
=
self
.
BOUNDS_PLAT_15
[::]
else
:
print
(
"Unknown case"
)
if
plat_id_goal
<
WEIGHT_FLOOR
:
print
(
"goal of flat floor"
)
goal_bounds
=
self
.
BOUNDS_FLOOR
[::]
elif
plat_id_goal
==
WEIGHT_FLOOR
:
print
(
"goal on 10cm platform"
)
goal_bounds
=
self
.
BOUNDS_PLAT_10
[::]
elif
plat_id_goal
==
WEIGHT_FLOOR
+
1
:
print
(
"goal on 15cm platform"
)
goal_bounds
=
self
.
BOUNDS_PLAT_15
[::]
else
:
print
(
"Unknown case"
)
root_bounds
=
[
0
]
*
6
for
i
in
range
(
3
):
root_bounds
[
i
*
2
]
=
min
(
init_bounds
[
i
*
2
],
goal_bounds
[
i
*
2
])
root_bounds
[
i
*
2
+
1
]
=
max
(
init_bounds
[
i
*
2
+
1
],
goal_bounds
[
i
*
2
+
1
])
self
.
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
root_bounds
)
self
.
q_init
=
generate_random_conf_without_orientation
(
self
.
rbprmBuilder
,
init_bounds
,
self
.
v
)
self
.
q_goal
=
generate_random_conf_without_orientation
(
self
.
rbprmBuilder
,
goal_bounds
,
self
.
v
)
print
((
"init config : "
,
self
.
q_init
))
print
((
"goal config : "
,
self
.
q_goal
))
# write problem in files :
f
=
open
(
self
.
status_filename
,
"w"
)
f
.
write
(
"q_init= "
+
str
(
self
.
q_init
)
+
"
\n
"
)
f
.
write
(
"q_goal= "
+
str
(
self
.
q_goal
)
+
"
\n
"
)
f
.
close
()
def
run
(
self
):
self
.
init_problem
()
self
.
init_viewer
(
"multicontact/bauzil_stairs"
,
visualize_affordances
=
[
"Support"
],
reduce_sizes
=
[
0.12
,
0.
,
0.
])
self
.
set_random_configs
()
self
.
init_planner
()
self
.
solve
()
print
(
"Original path length : "
,
self
.
ps
.
pathLength
(
self
.
ps
.
numberPaths
()
-
1
))
# optimize several time the path
for
i
in
range
(
5
):
print
((
"Optimize path, "
+
str
(
i
+
1
)
+
"/5 ... "
))
self
.
ps
.
optimizePath
(
self
.
ps
.
numberPaths
()
-
1
)
print
((
"New path length : "
,
self
.
ps
.
pathLength
(
self
.
ps
.
numberPaths
()
-
1
)))
# remove the very beginning and end of the path to avoid orientation discontinuities
self
.
ps
.
extractPath
(
self
.
ps
.
numberPaths
()
-
1
,
0.05
,
self
.
ps
.
pathLength
(
self
.
ps
.
numberPaths
()
-
1
)
-
0.05
)
pathId
=
self
.
ps
.
numberPaths
()
-
1
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
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
script/scenarios/memmo/talos_circle_oriented_path.py
View file @
70ea14c6
from
talos_rbprm.talos_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
import
numpy
as
np
from
scenarios.talos_path_planner
import
TalosPathPlanner
from
pinocchio
import
Quaternion
import
time
statusFilename
=
"/res/infos.log"
vMax
=
0.5
# linear velocity bound for the root
vInit
=
0.05
# initial / final velocity to fix the direction
vGoal
=
0.01
aMax
=
0.05
# linear acceleration bound for the root
extraDof
=
6
mu
=
0.5
# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Robot
()
# Define bounds for the root : bounding box of the scenario
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
-
2
,
2
,
-
2
,
2
,
1.
,
1.
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder
.
setFilter
([
'talos_lleg_rom'
,
'talos_rleg_rom'
])
rbprmBuilder
.
setAffordanceFilter
(
'talos_lleg_rom'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'talos_rleg_rom'
,
[
'Support'
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder
.
boundSO3
([
-
1.7
,
1.7
,
-
0.1
,
0.1
,
-
0.1
,
0.1
])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder
.
client
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
vMax
,
vMax
,
-
vMax
,
vMax
,
0
,
0
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
0
,
0
])
indexECS
=
rbprmBuilder
.
getConfigSize
()
-
rbprmBuilder
.
client
.
robot
.
getDimensionExtraConfigSpace
()
# Creating an instance of HPP problem solver
ps
=
ProblemSolver
(
rbprmBuilder
)
# define parameters used by various methods :
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.2
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.12
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
0.5
)
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
# initialize the viewer :
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
# load the module to analyse the environnement and compute the possible contact surfaces
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"multicontact/ground"
,
"planning"
,
vf
)
#load the viewer
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
print
(
"No viewer started !"
)
class
FakeViewer
():
sceneName
=
""
def
__init__
(
self
):
return
def
__call__
(
self
,
q
):
return
def
addLandmark
(
self
,
a
,
b
):
return
v
=
FakeViewer
()
v
.
addLandmark
(
v
.
sceneName
,
0.5
)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
0
,
0
,
1.
]
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
q_init
[
-
6
]
=
vInit
# sample random position on a circle of radius 2m
radius
=
1.
import
random
random
.
seed
()
#alpha = random.uniform(0.,2.*np.pi)
alpha
=
random
.
uniform
(
0.
,
2.
*
np
.
pi
)
print
(
"Test on a circle, alpha = "
,
alpha
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
radius
*
np
.
sin
(
alpha
),
-
radius
*
np
.
cos
(
alpha
),
1.
]
# set final orientation to be along the circle :
vx
=
np
.
matrix
([
1
,
0
,
0
]).
T
v_goal
=
np
.
matrix
([
q_goal
[
0
],
q_goal
[
1
],
0
]).
T
quat
=
Quaternion
.
FromTwoVectors
(
vx
,
v_goal
)
q_goal
[
3
:
7
]
=
quat
.
coeffs
().
tolist
()
# set final velocity to fix the orientation :
q_goal
[
-
6
]
=
vGoal
*
np
.
sin
(
alpha
)
q_goal
[
-
5
]
=
-
vGoal
*
np
.
cos
(
alpha
)
v
(
q_goal
)
print
(
"initial root position : "
,
q_init
)
print
(
"final root position : "
,
q_goal
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# write problem in files :
f
=
open
(
statusFilename
,
"w"
)
f
.
write
(
"q_init= "
+
str
(
q_init
)
+
"
\n
"
)
f
.
write
(
"q_goal= "
+
str
(
q_goal
)
+
"
\n
"
)
f
.
close
()
# Choosing RBPRM shooter and path validation methods.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
ps
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
# Choosing kinodynamic methods :
ps
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
ps
.
selectDistance
(
"Kinodynamic"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
# Solve the planning problem :
success
=
ps
.
client
.
problem
.
prepareSolveStepByStep
()
if
not
success
:
print
(
"planning failed."
)
import
sys
sys
.
exit
(
1
)
ps
.
client
.
problem
.
finishSolveStepByStep
()
try
:
# display solution :
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
pp
.
dt
=
0.1
pp
.
displayVelocityPath
(
0
)
#v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp
.
dt
=
0.01
#pp(0)
except
Exception
:
pass
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
import
numpy
as
np
class
PathPlanner
(
TalosPathPlanner
):
def
__init__
(
self
):
super
().
__init__
()
self
.
status_filename
=
"/res/infos.log"
self
.
v_max
=
0.5
self
.
a_max
=
0.05
self
.
v_init
=
0.05
self
.
v_goal
=
0.01
self
.
radius
=
1.
self
.
root_translation_bounds
=
[
-
2
,
2
,
-
2
,
2
,
1.
,
1.
]
def
init_problem
(
self
):
super
().
init_problem
()
self
.
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
def
set_random_configs
(
self
):
"""
randomly sample initial and goal configuration :
"""
self
.
q_init
[
0
:
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.
]
# 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
quat
=
Quaternion
.
FromTwoVectors
(
vx
,
v_goal
)
self
.
q_goal
[
3
:
7
]
=
quat
.
coeffs
().
tolist
()
# set final velocity to fix the orientation :
self
.
q_goal
[
-
6
]
=
self
.
v_goal
*
np
.
sin
(
alpha
)
self
.
q_goal
[
-
5
]
=
-
self
.
v_goal
*
np
.
cos
(
alpha
)
self
.
v
(
self
.
q_goal
)
print
(
"initial root position : "
,
self
.
q_init
)
print
(
"final root position : "
,
self
.
q_goal
)
self
.
ps
.
setInitialConfig
(
self
.
q_init
)
self
.
ps
.
addGoalConfig
(
self
.
q_goal
)
# write problem in files :
f
=
open
(
self
.
status_filename
,
"w"
)
f
.
write
(
"q_init= "
+
str
(
self
.
q_init
)
+
"
\n
"
)
f
.
write
(
"q_goal= "
+
str
(
self
.
q_goal
)
+
"
\n
"
)
f
.
close
()
def
run
(
self
):
self
.
init_problem
()
self
.
init_viewer
(
"multicontact/ground"
,
visualize_affordances
=
[
"Support"
])
self
.
set_random_configs
()
self
.
init_planner
(
optimize
=
False
)
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
()
#self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
script/scenarios/memmo/talos_circle_path.py
View file @
70ea14c6
from
talos_rbprm.talos_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
from
scenarios.talos_path_planner
import
TalosPathPlanner
import
numpy
as
np
import
time
statusFilename
=
"/res/infos.log"
vMax
=
0.5
# linear velocity bound for the root
aMax
=
0.5
# linear acceleration bound for the root
extraDof
=
6
mu
=
0.5
# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Robot
()
# Define bounds for the root : bounding box of the scenario
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
-
2
,
2
,
-
2
,
2
,
0.95
,
1.05
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder
.
setFilter
([
'talos_lleg_rom'
,
'talos_rleg_rom'
])
rbprmBuilder
.
setAffordanceFilter
(
'talos_lleg_rom'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'talos_rleg_rom'
,
[
'Support'
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder
.
boundSO3
([
-
1.7
,
1.7
,
-
0.1
,
0.1
,
-
0.1
,
0.1
])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder
.
client
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder
.
client
.
robot
.
setExtraConfigSpaceBounds
([
-
vMax
,
vMax
,
-
vMax
,
vMax
,
0
,
0
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
0
,
0
])
indexECS
=
rbprmBuilder
.
getConfigSize
()
-
rbprmBuilder
.
client
.
robot
.
getDimensionExtraConfigSpace
()
# Creating an instance of HPP problem solver
ps
=
ProblemSolver
(
rbprmBuilder
)
# define parameters used by various methods :
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
aMax
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.2
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.12
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
0.5
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
# initialize the viewer :
from
hpp.gepetto
import
ViewerFactory
vf
=
ViewerFactory
(
ps
)
# load the module to analyse the environnement and compute the possible contact surfaces
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"multicontact/ground"
,
"planning"
,
vf
)
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
print
(
"No viewer started !"
)
class
FakeViewer
():
def
__init__
(
self
):
return
def
__call__
(
self
,
q
):
return
v
=
FakeViewer
()
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
0
,
0
,
1.
]
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
# sample random position on a circle of radius 2m
radius
=
0.3
import
random
random
.
seed
()
alpha
=
random
.
uniform
(
0.
,
2.
*
np
.
pi
)
print
(
"Test on a circle, alpha = "
,
alpha
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
radius
*
np
.
sin
(
alpha
),
-
radius
*
np
.
cos
(
alpha
),
1.
]
print
(
"initial root position : "
,
q_init
[
0
:
3
])
print
(
"final root position : "
,
q_goal
[
0
:
3
])
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# write problem in files :
f
=
open
(
statusFilename
,
"w"
)
f
.
write
(
"q_init= "
+
str
(
q_init
)
+
"
\n
"
)
f
.
write
(
"q_goal= "
+
str
(
q_goal
)
+
"
\n
"
)
f
.
close
()
# Choosing RBPRM shooter and path validation methods.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
ps
.
selectPathValidation
(
"RbprmPathValidation"
,
0.05
)
# Choosing kinodynamic methods :
ps
.
selectSteeringMethod
(
"RBPRMKinodynamic"
)
ps
.
selectDistance
(
"Kinodynamic"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
(
"Guide planning time : "
,
t
)
try
:
# display solution :
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
pp
.
dt
=
0.1
pp
.
displayVelocityPath
(
0
)
#v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp
.
dt
=
0.01
#pp(0)
except
Exception
:
pass
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
class
PathPlanner
(
TalosPathPlanner
):
def
__init__
(
self
):
super
().
__init__
()
self
.
status_filename
=
"/res/infos.log"
self
.
v_max
=
0.5
self
.
a_max
=
0.5
self
.
radius
=
0.3
def
set_random_configs
(
self
):
"""
randomly sample initial and goal configuration :
"""
self
.
q_init
[
0
:
3
]
=
[
0
,
0
,
1.
]
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
),
1.
]
print
(
"initial root position : "
,
self
.
q_init
[
0
:
3
])
print
(
"final root position : "
,
self
.
q_goal
[
0
:
3
])
self
.
ps
.
setInitialConfig
(
self
.
q_init
)
self
.
ps
.
addGoalConfig
(
self
.
q_goal
)
# write problem in files :
f
=
open
(
self
.
status_filename
,
"w"
)