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
aef9ad1f
Commit
aef9ad1f
authored
Oct 24, 2019
by
Pierre Fernbach
Committed by
Pierre Fernbach
Jan 17, 2020
Browse files
[script] update ANYmal scripts
parent
f5a74947
Changes
10
Hide whitespace changes
Inline
Side-by-side
script/scenarios/sandbox/ANYmal/anymal_flatGround.py
View file @
aef9ad1f
...
...
@@ -31,7 +31,8 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
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_ref
=
fullBody
.
referenceConfig
[::]
+
[
0
]
*
6
#q_init = fullBody.referenceConfig_asymetric[::]+[0]*6
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
...
...
@@ -40,7 +41,9 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
print
"Generate limb DB ..."
tStart
=
time
.
time
()
fullBody
.
loadAllLimbs
(
"fixedStep04"
,
"ReferenceConfiguration"
,
nbSamples
=
100000
,
disableEffectorCollision
=
False
)
dict_heuristic
=
{
fullBody
.
rLegId
:
"fixedStep04"
,
fullBody
.
lLegId
:
"fixedStep04"
,
fullBody
.
rArmId
:
"static"
,
fullBody
.
lArmId
:
"static"
}
#fullBody.loadAllLimbs("static","ReferenceConfiguration",nbSamples=50000,disableEffectorCollision=False)
fullBody
.
loadAllLimbs
(
dict_heuristic
,
"ReferenceConfiguration"
,
nbSamples
=
100000
,
disableEffectorCollision
=
False
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
...
...
@@ -50,7 +53,7 @@ fullBody.setReferenceConfig(q_ref)
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
]
q_goal
=
q_
ref
[::];
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
0
:
7
]
dir_init
=
[
0
,
0
,
0
]
acc_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.
)[
tp
.
indexECS
+
3
:
tp
.
indexECS
+
6
]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
))[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
...
...
@@ -63,6 +66,7 @@ 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] = fullBody.referenceConfig_asymetric[2]
q_init
[
2
]
=
q_ref
[
2
]
q_goal
[
2
]
=
q_ref
[
2
]
...
...
script/scenarios/sandbox/ANYmal/anymal_modular_palet_flat.py
0 → 100644
View file @
aef9ad1f
from
hpp.corbaserver.rbprm.anymal
import
Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
import
scenarios.sandbox.ANYmal.anymal_modular_palet_flat_path
as
tp
#Robot.urdfSuffix += "_safeFeet"
statusFilename
=
tp
.
statusFilename
pId
=
tp
.
pid
f
=
open
(
statusFilename
,
"a"
)
if
tp
.
ps
.
numberPaths
()
>
0
:
print
"Path planning OK."
f
.
write
(
"Planning_success: True"
+
"
\n
"
)
f
.
close
()
else
:
print
"Error during path planning"
f
.
write
(
"Planning_success: False"
+
"
\n
"
)
f
.
close
()
import
sys
sys
.
exit
(
1
)
fullBody
=
Robot
()
# Set the bounds for the root
rootBounds
=
tp
.
rootBounds
[::]
rootBounds
[
0
]
-=
0.2
rootBounds
[
1
]
+=
0.2
rootBounds
[
2
]
-=
0.5
rootBounds
[
3
]
+=
0.5
rootBounds
[
4
]
-=
0.2
rootBounds
[
5
]
+=
0.2
fullBody
.
setJointBounds
(
"root_joint"
,
rootBounds
)
fullBody
.
setConstrainedJointsBounds
()
# 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
,
-
tp
.
vMax
,
tp
.
vMax
,
-
tp
.
aMax
,
tp
.
aMax
,
-
tp
.
aMax
,
tp
.
aMax
,
-
tp
.
aMaxZ
,
tp
.
aMaxZ
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
setParameter
(
"Kinodynamic/velocityBound"
,
tp
.
vMax
)
ps
.
setParameter
(
"Kinodynamic/accelerationBound"
,
tp
.
aMax
)
ps
.
setParameter
(
"FullBody/frictionCoefficient"
,
tp
.
mu
)
#load the viewer
try
:
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
except
Exception
:
print
"No viewer started !"
class
FakeViewer
():
def
__init__
(
self
):
return
def
__call__
(
self
,
q
):
return
def
addLandmark
(
self
,
a
,
b
):
return
v
=
FakeViewer
()
# 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.usePosturalTaskContactCreation(True)
fullBody
.
setCurrentConfig
(
q_init
)
print
"Generate limb DB ..."
tStart
=
time
.
time
()
# generate databases :
fullBody
.
loadAllLimbs
(
"static"
,
"ReferenceConfiguration"
,
nbSamples
=
100000
,
disableEffectorCollision
=
False
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
v
.
addLandmark
(
'anymal/base_0'
,
0.2
)
#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
]
vel_init
=
[
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.13
q_goal
[
2
]
=
q_ref
[
2
]
+
0.15
normals
=
[[
0.
,
0.
,
1.
]
for
_
in
range
(
4
)]
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
v
(
q_init
)
fullBody
.
setStartState
(
q_init
,
Robot
.
limbs_names
,
normals
)
fullBody
.
setEndState
(
q_goal
,
Robot
.
limbs_names
,
normals
)
fullBody
.
setCurrentConfig
(
q_goal
)
v
(
q_goal
)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.001
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
testReachability
=
True
,
quasiStatic
=
True
)
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)
if
len
(
configs
)
<
2
:
cg_success
=
False
print
"Error during contact generation."
else
:
cg_success
=
True
print
"Contact generation Done."
if
abs
(
configs
[
-
1
][
0
]
-
tp
.
q_goal
[
0
])
<
0.01
and
abs
(
configs
[
-
1
][
1
]
-
tp
.
q_goal
[
1
])
<
0.01
and
(
len
(
fullBody
.
getContactsVariations
(
len
(
configs
)
-
2
,
len
(
configs
)
-
1
))
==
1
):
print
"Contact generation successful."
cg_reach_goal
=
True
else
:
print
"Contact generation failed to reach the goal."
cg_reach_goal
=
False
f
=
open
(
statusFilename
,
"a"
)
f
.
write
(
"cg_success: "
+
str
(
cg_success
)
+
"
\n
"
)
f
.
write
(
"cg_reach_goal: "
+
str
(
cg_reach_goal
)
+
"
\n
"
)
f
.
close
()
if
(
not
cg_success
):
import
sys
sys
.
exit
(
1
)
#beginId = 2
# put back original bounds for wholebody methods
fullBody
.
resetJointsBounds
()
#displayContactSequence(v,configs)
script/scenarios/sandbox/ANYmal/anymal_modular_palet_flat_path.py
0 → 100644
View file @
aef9ad1f
from
hpp.corbaserver.rbprm.anymal_abstract
import
Robot
Robot
.
urdfName
+=
"_large"
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
from
pinocchio
import
Quaternion
import
numpy
as
np
import
time
import
math
statusFilename
=
"infos.log"
REF_Z_VALUE
=
0.47
Z_VALUE_LOGS
=
REF_Z_VALUE
+
0.13
Z_VALUE_PALET
=
REF_Z_VALUE
+
0.165
Z_VALUE_WOOD
=
REF_Z_VALUE
+
0.15
Y_VALUE
=
-
0.13
vInit
=
0.01
vMax
=
0.5
# linear velocity bound for the root
aMax
=
0.5
# linear acceleration bound for the root
aMaxZ
=
5.
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
rootBounds
=
[
-
1.201
,
1.11
,
Y_VALUE
-
0.001
,
Y_VALUE
+
0.001
,
Z_VALUE_LOGS
-
0.001
,
Z_VALUE_PALET
+
0.06
]
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
rootBounds
)
# 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
.
urdfNameRom
)
for
rom
in
rbprmBuilder
.
urdfNameRom
:
rbprmBuilder
.
setAffordanceFilter
(
rom
,
[
'Support'
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder
.
boundSO3
([
-
0.01
,
0.01
,
-
0.01
,
0.01
,
-
0.01
,
0.01
])
# 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
,
-
2.
,
2.
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
-
aMaxZ
,
aMaxZ
])
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
(
"Kinodynamic/verticalAccelerationBound"
,
aMaxZ
)
#ps.setParameter("Kinodynamic/synchronizeVerticalAxis",False)
#ps.setParameter("Kinodynamic/forceYawOrientation",True)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.01
)
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
])
afftool
.
setMinimumArea
(
'Support'
,
0.03
)
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"ori/modular_palet_flat"
,
"planning"
,
vf
,
reduceSizes
=
[
0.11
,
0
,
0
])
#afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "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)
v
.
addLandmark
(
v
.
sceneName
,
1
)
# 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"
)
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
#ps.addPathOptimizer ("RandomShortcut")
# init to beginning of palet
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
2
]
=
Z_VALUE_LOGS
q_init
[
0
:
2
]
=
[
-
1.2
,
Y_VALUE
]
q_init
[
-
6
]
=
vInit
v
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
]
=
-
0.55
q_goal
[
2
]
=
Z_VALUE_PALET
+
0.03
q_goal
[
3
:
7
]
=
[
0
,
-
0.0499792
,
0
,
0.9987503
]
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
pidBegin
=
ps
.
numberPaths
()
-
1
## middle part on palet
q_init
=
q_goal
[::]
q_goal
[
3
:
7
]
=
[
0
,
0.0499792
,
0
,
0.9987503
]
q_goal
[
0
]
=
0.55
ps
.
resetGoalConfigs
()
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
pidMiddle
=
ps
.
numberPaths
()
-
1
## last part on wood floor
q_init
=
q_goal
[::]
q_goal
[
3
:
7
]
=
[
0
,
0
,
0
,
1.
]
q_goal
[
0
]
=
1.1
q_goal
[
2
]
=
Z_VALUE_WOOD
ps
.
resetGoalConfigs
()
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
pidLast
=
ps
.
numberPaths
()
-
1
# concatenate paths :
pid
=
pidBegin
ps
.
concatenatePath
(
pid
,
pidMiddle
)
ps
.
concatenatePath
(
pid
,
pidLast
)
"""
for i in range(5):
print "optimize pass : ",i
ps.optimizePath(pid)
pid = ps.numberPaths()-1
"""
try
:
# display solution :
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
pp
.
dt
=
0.1
pp
.
displayPath
(
pid
)
#pp.displayVelocityPath(0) #
v
.
client
.
gui
.
setVisibility
(
"path_"
+
str
(
pid
)
+
"_root"
,
"ALWAYS_ON_TOP"
)
pp
.
dt
=
0.01
pp
(
pid
)
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
)
script/scenarios/sandbox/ANYmal/anymal_modular_palet_low.py
View file @
aef9ad1f
...
...
@@ -3,7 +3,7 @@ from hpp.gepetto import Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
import
scenarios.sandbox.ANYmal.anymal_modular_palet_low_path
as
tp
import
scenarios.sandbox.ANYmal.anymal_modular_palet_low_
obstacles_
path
as
tp
#Robot.urdfSuffix += "_safeFeet"
statusFilename
=
tp
.
statusFilename
...
...
@@ -32,7 +32,7 @@ rootBounds[3] += 0.5
rootBounds
[
4
]
-=
0.2
rootBounds
[
5
]
+=
0.2
fullBody
.
setJointBounds
(
"root_joint"
,
rootBounds
)
fullBody
.
set
Very
ConstrainedJointsBounds
()
fullBody
.
setConstrainedJointsBounds
()
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody
.
client
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
...
...
@@ -85,7 +85,7 @@ 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
]
vel_init
=
[
0
,
0
,
0
]
robTreshold
=
5
robTreshold
=
3
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
vel_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
...
...
@@ -93,8 +93,8 @@ q_goal[configSize:configSize+3] = vel_goal[::]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
q_init
[
2
]
=
q_ref
[
2
]
q_goal
[
2
]
=
q_ref
[
2
]
q_init
[
2
]
=
q_ref
[
2
]
+
0.13
q_goal
[
2
]
=
q_ref
[
2
]
+
0.15
normals
=
[[
0.
,
0.
,
1.
]
for
_
in
range
(
4
)]
fullBody
.
setStaticStability
(
True
)
...
...
script/scenarios/sandbox/ANYmal/anymal_modular_palet_low_obstacles_path.py
0 → 100644
View file @
aef9ad1f
from
hpp.corbaserver.rbprm.anymal_abstract
import
Robot
Robot
.
urdfName
+=
"_large"
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
ProblemSolver
from
pinocchio
import
Quaternion
import
numpy
as
np
import
time
import
math
statusFilename
=
"infos.log"
REF_Z_VALUE
=
0.47
Z_VALUE_LOGS
=
REF_Z_VALUE
+
0.13
Z_VALUE_PALET
=
REF_Z_VALUE
+
0.165
Z_VALUE_WOOD
=
REF_Z_VALUE
+
0.15
Y_VALUE
=
-
0.13
vInit
=
0.2
vMax
=
0.5
# linear velocity bound for the root
aMax
=
0.8
# linear acceleration bound for the root
aMaxZ
=
5.
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
rootBounds
=
[
-
1.201
,
1.11
,
Y_VALUE
-
0.3
,
Y_VALUE
+
0.3
,
Z_VALUE_LOGS
-
0.001
,
Z_VALUE_PALET
+
0.06
]
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
rootBounds
)
# 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
.
urdfNameRom
)
for
rom
in
rbprmBuilder
.
urdfNameRom
:
rbprmBuilder
.
setAffordanceFilter
(
rom
,
[
'Support'
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder
.
boundSO3
([
-
0.3
,
0.3
,
-
0.01
,
0.01
,
-
0.01
,
0.01
])
# 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
,
-
2.
,
2.
,
-
aMax
,
aMax
,
-
aMax
,
aMax
,
-
aMaxZ
,
aMaxZ
])
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
(
"Kinodynamic/verticalAccelerationBound"
,
aMaxZ
)
#ps.setParameter("Kinodynamic/synchronizeVerticalAxis",False)
ps
.
setParameter
(
"Kinodynamic/forceYawOrientation"
,
True
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
ps
.
setParameter
(
"PathOptimization/RandomShortcut/NumberOfLoops"
,
500
)
# 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
.
setMinimumArea
(
'Support'
,
0.03
)
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"ori/modular_palet_low_obstacles"
,
"planning"
,
vf
,
reduceSizes
=
[
0.1
,
0
,
0
])
#afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "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)
v
.
addLandmark
(
v
.
sceneName
,
1
)
# 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"
)
ps
.
addPathOptimizer
(
"RandomShortcutDynamic"
)
#ps.addPathOptimizer ("RandomShortcut")
# init to beginning of palet
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
2
]
=
Z_VALUE_LOGS
q_init
[
0
:
2
]
=
[
-
1.2
,
Y_VALUE
]
q_init
[
-
6
]
=
vInit
v
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
]
=
-
0.55
q_goal
[
2
]
=
Z_VALUE_PALET
+
0.03
q_goal
[
3
:
7
]
=
[
0
,
-
0.0499792
,
0
,
0.9987503
]
q_goal
[
-
6
]
=
vInit
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
pidBegin
=
ps
.
numberPaths
()
-
1
## middle part on palet
q_init
=
q_goal
[::]
q_goal
[
3
:
7
]
=
[
0
,
0.0499792
,
0
,
0.9987503
]
q_goal
[
0
]
=
0.55
q_goal
[
1
]
=
-
Y_VALUE
q_goal
[
-
6
]
=
vInit
ps
.
resetGoalConfigs
()
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
[
q_init
[
0
],
q_goal
[
0
],
-
0.2
,
0.2
,
q_init
[
2
],
q_init
[
2
]])
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
#v.solveAndDisplay('rm',2,0.001)
pidMiddle
=
ps
.
numberPaths
()
-
1
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
rootBounds
)
## last part on wood floor
q_init
=
q_goal
[::]
q_goal
[
1
]
=
Y_VALUE
q_goal
[
3
:
7
]
=
[
0
,
0
,
0
,
1.
]
q_goal
[
0
]
=
1.1
q_goal
[
2
]
=
Z_VALUE_WOOD
q_goal
[
-
6
]
=
vInit
ps
.
resetGoalConfigs
()
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
pidLast
=
ps
.
numberPaths
()
-
1
# concatenate paths :
pid
=
pidBegin
ps
.
concatenatePath
(
pid
,
pidMiddle
)
ps
.
concatenatePath
(
pid
,
pidLast
)
"""
ps.optimizePath(pid)
pid = ps.numberPaths()-1
"""
try
:
# display solution :
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
v
)
pp
.
dt
=
0.1
pp
.
displayPath
(
pid
)
#pp.displayVelocityPath(0) #
v
.
client
.
gui
.
setVisibility
(
"path_"
+
str
(
pid
)
+
"_root"
,
"ALWAYS_ON_TOP"
)
pp
.
dt
=
0.01
pp
(
pid
)
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
)