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
Humanoid Path Planner
hpp-rbprm-corba
Commits
211fc7b7
Commit
211fc7b7
authored
Mar 03, 2020
by
Pierre Fernbach
Browse files
[Script] refactor hrp2/talos scripts in demos to use templates
parent
2d0a5781
Changes
13
Hide whitespace changes
Inline
Side-by-side
script/scenarios/demos/hrp2_flatGround.py
View file @
211fc7b7
from
hpp.corbaserver.rbprm.hrp2
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
(
"Plan guide trajectory ..."
)
from
.
import
hrp2_flatGround_path
as
tp
print
(
"Done."
)
import
time
from
scenarios.demos.hrp2_flatGround_path
import
PathPlanner
from
scenarios.hrp2_contact_generator
import
HRP2ContactGenerator
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
Robot
()
class
ContactGenerator
(
HRP2ContactGenerator
):
# Set the bounds for the root
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.5
,
0.8
])
fullBody
.
setJointBounds
(
"LLEG_JOINT3"
,[
0.4
,
2.61799
])
# increase min bounds on knee, required to stay in the 'comfort zone' of the stabilizer
fullBody
.
setJointBounds
(
"RLEG_JOINT3"
,[
0.4
,
2.61799
])
# TODO : fix rot y legs
# 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
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
fullBody
.
setCurrentConfig
(
q_init
)
v
(
q_ref
)
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
# generate databases :
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
()
self
.
interpolate
()
nbSamples
=
100000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
''
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep06"
,
0.01
,
limbOffset
=
fullBody
.
rLegLimbOffset
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
,
kinematicConstraintsMin
=
0.4
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
''
,
fullBody
.
lLegOffset
,
fullBody
.
lLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep06"
,
0.01
,
limbOffset
=
fullBody
.
lLegLimbOffset
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
,
kinematicConstraintsMin
=
0.4
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
"""
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.rArmOffset,fullBody.rArmNormal, fullBody.rArmx, fullBody.rArmy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.rArmKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.rArmId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.lArmOffset,fullBody.lArmNormal, fullBody.lArmx, fullBody.lArmy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.lArmKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.lArmId, "ReferenceConfiguration", True)
"""
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
tGenerate
=
time
.
time
()
-
tStart
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
#define initial and final configurations :
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
0
:
7
]
# use this to get the correct orientation
q_goal
=
q_init
[::];
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
dir_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
tp
.
indexECS
+
3
:
tp
.
indexECS
+
6
]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
0.01
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
3
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
dir_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
q_goal
[
configSize
:
configSize
+
3
]
=
dir_goal
[::]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
q_init
[
2
]
=
q_ref
[
2
]
q_goal
[
2
]
=
q_ref
[
2
]
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
v
(
q_init
)
fullBody
.
setCurrentConfig
(
q_goal
)
v
(
q_goal
)
v
.
addLandmark
(
'hrp2_14/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
=
1.
,
filterStates
=
True
,
testReachability
=
False
,
quasiStatic
=
False
)
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/hrp2_flatGround_path.py
View file @
211fc7b7
from
hpp.corbaserver.rbprm.hrp2_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
import
time
from
scenarios.hrp2_path_planner
import
HRP2PathPlanner
class
PathPlanner
(
HRP2PathPlanner
):
def
run
(
self
):
self
.
init_problem
()
vMax
=
0.2
# linear velocity bound for the root
aMax
=
0.1
# 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
rbprmBuilder
=
Robot
()
# Define bounds for the root : bounding box of the scenario
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.5
,
0.8
]
)
self
.
q_init
[
0
:
2
]
=
[
0
,
0
]
self
.
q_goal
[
0
]
=
1.5
self
.
init_viewer
(
"multicontact/ground"
,
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
# self.play_path()
self
.
hide_rom
(
)
# 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
([
Robot
.
rLegId
,
Robot
.
lLegId
])
rbprmBuilder
.
setAffordanceFilter
(
Robot
.
rLegId
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
Robot
.
lLegId
,
[
'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"
,
Robot
.
legX
*
2.
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
Robot
.
legY
*
2.
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
# 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
)
v
=
vf
.
createViewer
(
displayArrows
=
True
)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
q_init
[
0
:
3
]
=
[
0
,
0
,
0.6
]
v
(
q_init
)
ps
.
setInitialConfig
(
q_init
)
# set goal config
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
]
=
1.5
v
(
q_goal
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
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
)
# 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)
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
script/scenarios/demos/hrp2_plateformes.py
View file @
211fc7b7
from
hpp.corbaserver.rbprm.hrp2
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
(
"Plan guide trajectory ..."
)
from
.
import
hrp2_plateformes_path
as
tp
print
(
"Done."
)
import
time
from
scenarios.demos.hrp2_plateformes_path
import
PathPlanner
from
scenarios.hrp2_contact_generator
import
HRP2ContactGenerator
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
Robot
()
class
ContactGenerator
(
HRP2ContactGenerator
):
# Set the bounds for the root
fullBody
.
setJointBounds
(
"root_joint"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.65
,
0.9
])
#fullBody.setConstrainedJointsBounds()
#fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2])
#fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1])
# 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
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
#fullBody.usePosturalTaskContactCreation(True)
def
run
(
self
):
self
.
root_translation_bounds
=
[
-
5
,
5
,
-
1.5
,
1.5
,
0.65
,
0.9
]
self
.
robustness
=
0.
self
.
quasi_static
=
False
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
# generate databases :
self
.
load_fullbody
(
)
self
.
set_joints_bounds
()
self
.
set_reference
(
True
)
nbSamples
=
100000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep1"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
,
kinematicConstraintsMin
=
0.3
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rLegId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep1"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
,
kinematicConstraintsMin
=
0.3
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
self
.
fullbody
.
minDist
=
0.3
self
.
load_limbs
(
"fixedStep1"
)
self
.
init_problem
()
self
.
init_viewer
()
self
.
compute_configs_from_guide
()
# force root height to be at exactly on the first platform
self
.
q_init
[
2
]
=
self
.
q_ref
[
2
]
+
0.16
self
.
q_goal
[
2
]
=
self
.
q_ref
[
2
]
+
0.16
self
.
interpolate
()
tGenerate
=
time
.
time
()
-
tStart
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
#define initial and final configurations :
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
0.
)[
0
:
7
]
# use this to get the correct orientation
q_goal
=
q_init
[::];
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
vel_init
=
tp
.
ps
.
configAtParam
(
pId
,
0
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_init
=
tp
.
ps
.
configAtParam
(
pId
,
0
)[
tp
.
indexECS
+
3
:
tp
.
indexECS
+
6
]
vel_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
0.
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
vel_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
q_goal
[
configSize
:
configSize
+
3
]
=
vel_goal
[::]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
q_init
[
2
]
=
q_ref
[
2
]
+
0.16
q_goal
[
2
]
=
q_ref
[
2
]
+
0.16
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
v
(
q_init
)
fullBody
.
setCurrentConfig
(
q_goal
)
v
(
q_goal
)
v
.
addLandmark
(
'hrp2_14/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
=
robTreshold
,
filterStates
=
True
,
testReachability
=
False
)
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)
#fullBody.resetJointsBounds()
if
__name__
==
"__main__"
:
cg
=
ContactGenerator
()
cg
.
run
()
script/scenarios/demos/hrp2_plateformes_path.py
View file @
211fc7b7
from
hpp.corbaserver.rbprm.hrp2_abstract
import
Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
import
time
vMax
=
0.3
# linear velocity bound for the root
aMax
=
0.1
# 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"
,
[
-
5
,
5
,
-
1.5
,
1.5
,
0.70
,
0.8
])
# 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
([])
rbprmBuilder
.
setAffordanceFilter
(
Robot
.
rLegId
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
Robot
.
lLegId
,
[
'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/plateforme_surfaces"
,
"planning"
,
vf
,
reduceSizes
=
[
0.14
,
0
,
0
])
v
=
vf
.
createViewer
(
displayArrows
=
True
)
afftool
.
visualiseAffordances
(
'Support'
,
v
,
v
.
color
.
lightBrown
)
# Setting initial configuration
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
q_init
[
0
:
3
]
=
[
0.12
,
0.25
,
0.75
]
v
(
q_init
)
ps
.
setInitialConfig
(
q_init
)
# set goal config
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
]
=
1.08
v
(
q_goal
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
ps
.
selectConfigurationShooter
(
"RbprmShooter"
)
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
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
()
ps
.
client
.
problem
.
finishSolveStepByStep
()
# 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)
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
from
scenarios.hrp2_path_planner
import
HRP2PathPlanner
class
PathPlanner
(
HRP2PathPlanner
):
def
run
(
self
):
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
.
init_viewer
(
"multicontact/plateforme_surfaces"
,
reduce_sizes
=
[
0.12
,
0
,
0
],
visualize_affordances
=
[
"Support"
])
self
.
init_planner
()
self
.
solve
()
self
.
display_path
()
# self.play_path()
self
.
hide_rom
()
if
__name__
==
"__main__"
:
planner
=
PathPlanner
()
planner
.
run
()
script/scenarios/demos/talos_flatGround.py
View file @
211fc7b7
...
...
@@ -14,9 +14,6 @@ class ContactGenerator(TalosContactGenerator):
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
()
...
...
script/scenarios/demos/talos_navBauzil.py
View file @
211fc7b7
from
talos_rbprm.talos
import
Robot
# select the robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
(
"Plan guide trajectory ..."
)
from
.
import
talos_navBauzil_path
as
tp
# load the guide planning script
print
(
"Done."
)
import
time
Robot
.
urdfSuffix
+=
"_safeFeet"
from
scenarios.demos.talos_navBauzil_path
import
PathPlanner
from
scenarios.talos_contact_generator
import
TalosContactGenerator
class
ContactGenerator
(
TalosContactGenerator
):
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
Robot
()
# Set the bounds for the root, take slightly larger bounding box than during root planning
root_bounds
=
tp
.
root_bounds
[::]
root_bounds
[
0
]
-=
0.2
root_bounds
[
1
]
+=
0.2
root_bounds
[
2
]
-=
0.2
root_bounds
[
3
]
+=
0.2
root_bounds
[
-
1
]
=
1.2
root_bounds
[
-
2
]
=
0.8
fullBody
.
setJointBounds
(
"root_joint"
,
root_bounds
)
# 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_elbowsUp
[::]
+
[
0
,
0
,
0
,
0
,
0
,
0
]
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
fullBody
.
usePosturalTaskContactCreation
(
True
)
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
# generate databases :
nbSamples
=
100000
def
run
(
self
):
self
.
load_fullbody
()
# set reference configuration with the arms bended to avoid collisions with the plateform
self
.
q_ref
=
self
.
fullbody
.
referenceConfig_elbowsUp
[::]
+
[
0
]
*
self
.
path_planner
.
extra_dof
self
.
set_joints_bounds
()
self
.
set_reference
(
True
)
self
.
load_limbs
(
"fixedStep06"
)
self
.
init_problem
()
self
.
init_viewer
()
self
.
compute_configs_from_guide
()
self
.
interpolate
()
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep06"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
,
kinematicConstraintsMin
=
0.85
)
#fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep06"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
,
kinematicConstraintsMin
=
0.85
)
#fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
tGenerate
=
time
.
time
()
-
tStart
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
#define initial and final configurations :
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
robot
.
getDimensionExtraConfigSpace
()
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
0
:
7
]
# use this to get the correct orientation
q_goal
=
q_init
[::];
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
dir_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.01
)[
tp
.
indexECS
+
3
:
tp
.
indexECS
+
6
]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
0.01
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
3
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
dir_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
q_goal
[
configSize
:
configSize
+
3
]
=
dir_goal
[::]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]