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
Jason Chemin
hpp-rbprm-corba
Commits
ac3fbf51
Commit
ac3fbf51
authored
Jan 18, 2019
by
Pierre Fernbach
Browse files
[clean] clean and add comment for talos scripts in demos
parent
a5ea58ec
Changes
6
Hide whitespace changes
Inline
Side-by-side
script/scenarios/demos/talos_flatGround.py
View file @
ac3fbf51
...
...
@@ -2,7 +2,6 @@ from hpp.corbaserver.rbprm.talos import Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
import
time
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
print
"Plan guide trajectory ..."
import
talos_flatGround_path
as
tp
print
"Done."
...
...
@@ -11,22 +10,21 @@ import time
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
Robot
()
# 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
)
# load a reference configuration
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
print
"Generate limb DB ..."
...
...
@@ -36,20 +34,15 @@ tStart = time.time()
nbSamples
=
10000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep06"
,
0.01
)
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
)
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"
q_0
=
fullBody
.
getCurrentConfig
();
#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
...
...
@@ -78,17 +71,14 @@ v(q_init)
fullBody
.
setCurrentConfig
(
q_goal
)
v
(
q_goal
)
# specifying the full body configurations as start and goal state of the problem
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
])
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
True
,
testReachability
=
False
)
...
...
script/scenarios/demos/talos_flatGround_path.py
View file @
ac3fbf51
...
...
@@ -6,41 +6,45 @@ import time
vMax
=
0.3
aMax
=
0.1
vMax
=
0.3
# linear velocity bound for the root
aMax
=
0.1
# linear acceleration bound for the root
extraDof
=
6
mu
=
0.5
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.95
,
1.05
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact
...
# 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 and the viewer
# 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
])
...
...
@@ -53,14 +57,14 @@ q_init = rbprmBuilder.getCurrentConfig ();
q_init
[
3
:
7
]
=
[
0
,
0
,
0
,
1
]
q_init
[
0
:
3
]
=
[
0
,
0
,
1.
]
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
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
...
...
@@ -72,7 +76,7 @@ ps.selectSteeringMethod("RBPRMKinodynamic")
ps
.
selectDistance
(
"Kinodynamic"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
...
...
@@ -85,7 +89,7 @@ pp.displayVelocityPath(0)
v
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
pp
(
0
)
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
...
...
script/scenarios/demos/talos_navBauzil.py
View file @
ac3fbf51
...
...
@@ -2,7 +2,6 @@ from hpp.corbaserver.rbprm.talos import Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
import
time
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
print
"Plan guide trajectory ..."
import
talos_navBauzil_path
as
tp
print
"Done."
...
...
@@ -12,7 +11,7 @@ import time
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
...
...
@@ -21,15 +20,16 @@ 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
)
# load a reference configuration
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
,
0
,
0
,
0
,
0
,
0
]
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
...
...
@@ -39,24 +39,20 @@ fullBody.setCurrentConfig (q_init)
print
"Generate limb DB ..."
tStart
=
time
.
time
()
# generate databases :
nbSamples
=
10000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep08"
,
0.01
)
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
,
"fixedStep08"
,
0.01
)
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"
q_0
=
fullBody
.
getCurrentConfig
();
#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
...
...
@@ -73,11 +69,9 @@ 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
)
...
...
@@ -85,17 +79,14 @@ v(q_init)
fullBody
.
setCurrentConfig
(
q_goal
)
v
(
q_goal
)
# specifying the full body configurations as start and goal state of the problem
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
.
lLegId
,
fullBody
.
rLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
lLegId
,
fullBody
.
rLegId
])
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
True
,
testReachability
=
False
)
...
...
script/scenarios/demos/talos_navBauzil_hard.py
View file @
ac3fbf51
...
...
@@ -2,19 +2,16 @@ from hpp.corbaserver.rbprm.talos import Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
import
time
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
print
"Plan guide trajectory ..."
import
talos_navBauzil_hard_path
as
tp
print
"Done."
import
time
##
# Information to retrieve urdf and srdf files.
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
...
...
@@ -23,16 +20,18 @@ 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
)
q_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
# load a reference configuration
q_ref
=
fullBody
.
referenceConfig_legsApart
[::]
+
[
0
]
*
6
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
...
...
@@ -40,23 +39,23 @@ fullBody.setCurrentConfig (q_init)
print
"Generate limb DB ..."
tStart
=
time
.
time
()
# generate databases :
nbSamples
=
1
0000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep0
8
"
,
0.01
)
nbSamples
=
2
0000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep0
6
"
,
0.01
)
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
,
"fixedStep08"
,
0.01
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep06"
,
0.01
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
#In this scenario, the arms are not used to create contact, but they may move to avoid collision.
fullBody
.
addNonContactingLimb
(
fullBody
.
lArmId
,
fullBody
.
larm
,
fullBody
.
lhand
,
5000
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
lArmId
,
"ReferenceConfiguration"
,
True
)
fullBody
.
addNonContactingLimb
(
fullBody
.
rArmId
,
fullBody
.
rarm
,
fullBody
.
rhand
,
5000
)
fullBody
.
runLimbSampleAnalysis
(
fullBody
.
rArmId
,
"ReferenceConfiguration"
,
True
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
q_0
=
fullBody
.
getCurrentConfig
();
#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
...
...
@@ -73,11 +72,9 @@ 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
)
...
...
@@ -85,20 +82,18 @@ v(q_init)
fullBody
.
setCurrentConfig
(
q_goal
)
v
(
q_goal
)
# specifying the full body configurations as start and goal state of the problem
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
.
lLegId
,
fullBody
.
rLegId
])
fullBody
.
setEndState
(
q_goal
,[
fullBody
.
lLegId
,
fullBody
.
rLegId
])
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
Fals
e
)
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
2
,
filterStates
=
Tru
e
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done."
print
"number of configs :"
,
len
(
configs
)
...
...
script/scenarios/demos/talos_navBauzil_hard_path.py
View file @
ac3fbf51
...
...
@@ -5,45 +5,52 @@ from hpp.corbaserver import ProblemSolver
import
time
vMax
=
0.3
aMax
=
0.1
vMax
=
0.3
# linear velocity bound for the root
aMax
=
0.1
# linear acceleration bound for the root
extraDof
=
6
mu
=
0.5
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
root_bounds
=
[
-
2.3
,
4.6
,
-
1.5
,
3.3
,
0.98
,
0.98
]
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
root_bounds
)
#rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
#rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
# As this scenario only consider walking, we fix the DOF of the torso :
rbprmBuilder
.
setJointBounds
(
'torso_1_joint'
,
[
0
,
0
])
rbprmBuilder
.
setJointBounds
(
'torso_2_joint'
,
[
0.006761
,
0.006761
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact
...
# 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
([
-
4.
,
4.
,
-
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 and the viewer
# 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
)
# force the orientation of the trunk to match the direction of the motion
ps
.
setParameter
(
"Kinodynamic/forceOrientation"
,
True
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.2
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.12
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
50
)
# 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
])
...
...
@@ -56,14 +63,13 @@ v.addLandmark(v.sceneName,1)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
0
:
3
]
=
[
-
0.7
,
2
,
0.98
]
v
(
q_init
)
ps
.
setInitialConfig
(
q_init
)
# set goal config
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
0.
,
-
1.
,
0.98
]
v
(
q_goal
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
...
...
@@ -75,7 +81,7 @@ ps.selectSteeringMethod("RBPRMKinodynamic")
ps
.
selectDistance
(
"Kinodynamic"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
...
...
@@ -89,7 +95,7 @@ v.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
pp
.
dt
=
0.01
pp
(
1
)
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
...
...
script/scenarios/demos/talos_navBauzil_path.py
View file @
ac3fbf51
...
...
@@ -6,44 +6,51 @@ import time
vMax
=
0.3
aMax
=
0.1
vMax
=
0.3
# linear velocity bound for the root
aMax
=
0.1
# linear acceleration bound for the root
extraDof
=
6
mu
=
0.5
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
root_bounds
=
[
-
1.5
,
3
,
0.
,
3.3
,
0.98
,
0.98
]
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
root_bounds
)
#rbprmBuilder.setJointBounds ('torso_1_joint', [0,0])
#rbprmBuilder.setJointBounds ('torso_2_joint', [0,0])
# As this scenario only consider walking, we fix the DOF of the torso :
rbprmBuilder
.
setJointBounds
(
'torso_1_joint'
,
[
0
,
0
])
rbprmBuilder
.
setJointBounds
(
'torso_2_joint'
,
[
0.006761
,
0.006761
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact
...
# 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
([
-
4.
,
4.
,
-
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 and the viewer
# 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
)
# force the orientation of the trunk to match the direction of the motion
ps
.
setParameter
(
"Kinodynamic/forceOrientation"
,
True
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.2
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.12
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
100
)
# 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
])
...
...
@@ -57,7 +64,7 @@ q_init = rbprmBuilder.getCurrentConfig ();
q_init
[
0
:
3
]
=
[
-
0.9
,
1.5
,
0.98
]
q_init
[
-
6
:
-
3
]
=
[
0.07
,
0
,
0
]
v
(
q_init
)
ps
.
setInitialConfig
(
q_init
)
# set goal config
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
...
...
@@ -65,7 +72,7 @@ q_goal[0:3] = [2,2.6,0.98]
q_goal
[
-
6
:
-
3
]
=
[
0.1
,
0
,
0
]
v
(
q_goal
)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Choosing RBPRM shooter and path validation methods.
...
...
@@ -77,7 +84,7 @@ ps.selectSteeringMethod("RBPRMKinodynamic")
ps
.
selectDistance
(
"Kinodynamic"
)
ps
.
selectPathPlanner
(
"DynamicPlanner"
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
...
...
@@ -91,7 +98,7 @@ v.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
pp
.
dt
=
0.01
pp
(
1
)
# move the robot out of the view before computing the contacts
q_far
=
q_init
[::]
q_far
[
2
]
=
-
2
v
(
q_far
)
...
...
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