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
0bcd5c3f
Unverified
Commit
0bcd5c3f
authored
Mar 11, 2020
by
Fernbach Pierre
Committed by
GitHub
Mar 11, 2020
Browse files
Merge pull request #61 from pFernbach/devel
Major refactoring of scripts organization
parents
0bfb5ce4
38c474f9
Changes
533
Hide whitespace changes
Inline
Side-by-side
cmake
@
7eca9ee6
Compare
2de34adf
...
7eca9ee6
Subproject commit
2de34adfce816937d2403cb602261ade0c26f0cd
Subproject commit
7eca9ee6c9d1c4ee20eb82272e94f9d11642053a
script/
scenarios
/__init__.py
→
script/
ANYmal
/__init__.py
View file @
0bcd5c3f
File moved
script/
scenarios/sandbox/
ANYmal/anymal_flatGround.py
→
script/ANYmal/anymal_flatGround.py
View file @
0bcd5c3f
...
...
@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
print
(
"Plan guide trajectory ..."
)
import
anymal_flatGround_path
as
tp
print
"Done."
print
(
"Done."
)
import
time
...
...
@@ -39,14 +39,14 @@ fullBody.setCurrentConfig (q_init)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
#fullBody.usePosturalTaskContactCreation(True)
print
"Generate limb DB ..."
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
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"
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
fullBody
.
setReferenceConfig
(
q_ref
)
#define initial and final configurations :
...
...
@@ -84,13 +84,13 @@ v(q_goal)
v
.
addLandmark
(
'anymal/base'
,
0.3
)
print
"Generate contact plan ..."
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
)
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/sandbox/
ANYmal/anymal_flatGround_path.py
→
script/ANYmal/anymal_flatGround_path.py
View file @
0bcd5c3f
...
...
@@ -84,7 +84,7 @@ ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
print
(
"Guide planning time : "
,
t
)
# display solution :
...
...
script/
scenarios/sandbox/
ANYmal/anymal_modular_palet_flat.py
→
script/ANYmal/anymal_modular_palet_flat.py
View file @
0bcd5c3f
...
...
@@ -2,7 +2,7 @@ from hpp.corbaserver.rbprm.anymal import Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
print
(
"Plan guide trajectory ..."
)
import
scenarios.sandbox.ANYmal.anymal_modular_palet_flat_path
as
tp
#Robot.urdfSuffix += "_safeFeet"
...
...
@@ -10,11 +10,11 @@ statusFilename = tp.statusFilename
pId
=
tp
.
pid
f
=
open
(
statusFilename
,
"a"
)
if
tp
.
ps
.
numberPaths
()
>
0
:
print
"Path planning OK."
print
(
"Path planning OK."
)
f
.
write
(
"Planning_success: True"
+
"
\n
"
)
f
.
close
()
else
:
print
"Error during path planning"
print
(
"Error during path planning"
)
f
.
write
(
"Planning_success: False"
+
"
\n
"
)
f
.
close
()
import
sys
...
...
@@ -45,7 +45,7 @@ ps.setParameter("FullBody/frictionCoefficient",tp.mu)
try
:
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
except
Exception
:
print
"No viewer started !"
print
(
"No viewer started !"
)
class
FakeViewer
():
def
__init__
(
self
):
return
...
...
@@ -65,14 +65,14 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody
.
setCurrentConfig
(
q_init
)
print
"Generate limb DB ..."
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"
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
v
.
addLandmark
(
'anymal/base_0'
,
0.2
)
#define initial and final configurations :
...
...
@@ -108,27 +108,27 @@ fullBody.setCurrentConfig (q_goal)
v
(
q_goal
)
print
"Generate contact plan ..."
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
)
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."
print
(
"Error during contact generation."
)
else
:
cg_success
=
True
print
"Contact generation Done."
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."
print
(
"Contact generation successful."
)
cg_reach_goal
=
True
else
:
print
"Contact generation failed to reach the goal."
print
(
"Contact generation failed to reach the goal."
)
cg_reach_goal
=
False
f
=
open
(
statusFilename
,
"a"
)
...
...
script/
scenarios/sandbox/
ANYmal/anymal_modular_palet_flat_path.py
→
script/ANYmal/anymal_modular_palet_flat_path.py
View file @
0bcd5c3f
...
...
@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planni
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
print
"No viewer started !"
print
(
"No viewer started !"
)
class
FakeViewer
():
def
__init__
(
self
):
return
...
...
@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
print
(
"Guide planning done in "
+
str
(
t
)
)
pidBegin
=
ps
.
numberPaths
()
-
1
...
...
@@ -124,7 +124,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
print
(
"Guide planning done in "
+
str
(
t
)
)
pidMiddle
=
ps
.
numberPaths
()
-
1
...
...
@@ -140,7 +140,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
print
(
"Guide planning done in "
+
str
(
t
)
)
pidLast
=
ps
.
numberPaths
()
-
1
...
...
script/
scenarios/sandbox/
ANYmal/anymal_modular_palet_low.py
→
script/ANYmal/anymal_modular_palet_low.py
View file @
0bcd5c3f
...
...
@@ -2,7 +2,7 @@ from hpp.corbaserver.rbprm.anymal import Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
print
(
"Plan guide trajectory ..."
)
import
scenarios.sandbox.ANYmal.anymal_modular_palet_low_obstacles_path
as
tp
#Robot.urdfSuffix += "_safeFeet"
...
...
@@ -10,11 +10,11 @@ statusFilename = tp.statusFilename
pId
=
tp
.
pid
f
=
open
(
statusFilename
,
"a"
)
if
tp
.
ps
.
numberPaths
()
>
0
:
print
"Path planning OK."
print
(
"Path planning OK."
)
f
.
write
(
"Planning_success: True"
+
"
\n
"
)
f
.
close
()
else
:
print
"Error during path planning"
print
(
"Error during path planning"
)
f
.
write
(
"Planning_success: False"
+
"
\n
"
)
f
.
close
()
import
sys
...
...
@@ -45,7 +45,7 @@ ps.setParameter("FullBody/frictionCoefficient",tp.mu)
try
:
v
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
v
.
client
,
displayCoM
=
True
)
except
Exception
:
print
"No viewer started !"
print
(
"No viewer started !"
)
class
FakeViewer
():
def
__init__
(
self
):
return
...
...
@@ -65,14 +65,14 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody
.
setCurrentConfig
(
q_init
)
print
"Generate limb DB ..."
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"
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
v
.
addLandmark
(
'anymal/base_0'
,
0.2
)
#define initial and final configurations :
...
...
@@ -107,27 +107,27 @@ fullBody.setCurrentConfig (q_goal)
v
(
q_goal
)
print
"Generate contact plan ..."
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
)
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."
print
(
"Error during contact generation."
)
else
:
cg_success
=
True
print
"Contact generation Done."
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."
print
(
"Contact generation successful."
)
cg_reach_goal
=
True
else
:
print
"Contact generation failed to reach the goal."
print
(
"Contact generation failed to reach the goal."
)
cg_reach_goal
=
False
f
=
open
(
statusFilename
,
"a"
)
...
...
script/
scenarios/sandbox/
ANYmal/anymal_modular_palet_low_obstacles_path.py
→
script/ANYmal/anymal_modular_palet_low_obstacles_path.py
View file @
0bcd5c3f
...
...
@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_obstacles"
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
print
"No viewer started !"
print
(
"No viewer started !"
)
class
FakeViewer
():
def
__init__
(
self
):
return
...
...
@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
print
(
"Guide planning done in "
+
str
(
t
)
)
pidBegin
=
ps
.
numberPaths
()
-
1
...
...
@@ -126,7 +126,7 @@ 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
)
print
(
"Guide planning done in "
+
str
(
t
)
)
#v.solveAndDisplay('rm',2,0.001)
pidMiddle
=
ps
.
numberPaths
()
-
1
rbprmBuilder
.
setJointBounds
(
"root_joint"
,
rootBounds
)
...
...
@@ -144,7 +144,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
print
(
"Guide planning done in "
+
str
(
t
)
)
pidLast
=
ps
.
numberPaths
()
-
1
...
...
script/
scenarios/sandbox/
ANYmal/anymal_modular_palet_low_path.py
→
script/ANYmal/anymal_modular_palet_low_path.py
View file @
0bcd5c3f
...
...
@@ -69,7 +69,7 @@ afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low_collision"
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
print
"No viewer started !"
print
(
"No viewer started !"
)
class
FakeViewer
():
def
__init__
(
self
):
return
...
...
@@ -110,7 +110,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
print
(
"Guide planning done in "
+
str
(
t
)
)
pidBegin
=
ps
.
numberPaths
()
-
1
...
...
@@ -124,7 +124,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
print
(
"Guide planning done in "
+
str
(
t
)
)
pidMiddle
=
ps
.
numberPaths
()
-
1
...
...
@@ -140,7 +140,7 @@ ps.addGoalConfig (q_goal)
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning done in "
+
str
(
t
)
print
(
"Guide planning done in "
+
str
(
t
)
)
pidLast
=
ps
.
numberPaths
()
-
1
...
...
script/
scenarios/sandbox/
ANYmal/anymal_plinth.py
→
script/ANYmal/anymal_plinth.py
View file @
0bcd5c3f
...
...
@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
print
(
"Plan guide trajectory ..."
)
import
anymal_plinth_path
as
tp
print
"Done."
print
(
"Done."
)
import
time
...
...
@@ -43,12 +43,12 @@ fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody
.
usePosturalTaskContactCreation
(
True
)
dict_heuristic
=
{
fullBody
.
rLegId
:
"static"
,
fullBody
.
lLegId
:
"static"
,
fullBody
.
rArmId
:
"fixedStep04"
,
fullBody
.
lArmId
:
"fixedStep04"
}
print
"Generate limb DB ..."
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
fullBody
.
loadAllLimbs
(
dict_heuristic
,
"ReferenceConfiguration"
,
nbSamples
=
50000
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
fullBody
.
setReferenceConfig
(
q_ref
)
#define initial and final configurations :
...
...
@@ -86,13 +86,13 @@ fullBody.setStartState(q_init,fullBody.limbs_names)
fullBody
.
setEndState
(
q_goal
,
fullBody
.
limbs_names
)
print
"Generate contact plan ..."
print
(
"Generate contact plan ..."
)
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.002
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
testReachability
=
True
,
quasiStatic
=
True
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done. "
print
"Contact sequence computed in "
+
str
(
tInterpolateConfigs
)
+
" s."
print
"number of configs :"
,
len
(
configs
)
print
(
"Done. "
)
print
(
"Contact sequence computed in "
+
str
(
tInterpolateConfigs
)
+
" s."
)
print
(
"number of configs :"
,
len
(
configs
)
)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
...
...
script/
scenarios/sandbox/
ANYmal/anymal_plinth_path.py
→
script/ANYmal/anymal_plinth_path.py
View file @
0bcd5c3f
...
...
@@ -87,7 +87,7 @@ ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
print
(
"Guide planning time : "
,
t
)
#v.solveAndDisplay('rm',2,radiusSphere=0.01)
...
...
script/
scenarios/sandbox/
ANYmal/anymal_slalom1.py
→
script/ANYmal/anymal_slalom1.py
View file @
0bcd5c3f
...
...
@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
print
(
"Plan guide trajectory ..."
)
import
anymal_slalom1_path
as
tp
print
"Done."
print
(
"Done."
)
import
time
...
...
@@ -39,14 +39,14 @@ fullBody.setCurrentConfig (q_init)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
#fullBody.usePosturalTaskContactCreation(True)
print
"Generate limb DB ..."
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
#dict_heuristic = {fullBody.rLegId:"fixedStep04", fullBody.lLegId:"fixedStep04", fullBody.rArmId:"static", fullBody.lArmId:"static"}
fullBody
.
loadAllLimbs
(
"static"
,
"ReferenceConfiguration"
,
nbSamples
=
100000
,
disableEffectorCollision
=
False
)
#fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
fullBody
.
setReferenceConfig
(
q_ref
)
#define initial and final configurations :
...
...
@@ -84,13 +84,13 @@ v(q_goal)
v
.
addLandmark
(
'anymal/base'
,
0.3
)
print
"Generate contact plan ..."
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
)
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/sandbox/
ANYmal/anymal_slalom1_path.py
→
script/ANYmal/anymal_slalom1_path.py
View file @
0bcd5c3f
...
...
@@ -83,11 +83,11 @@ ps.addPathOptimizer ("RandomShortcutDynamic")
#ps.addPathOptimizer ("RandomShortcut")
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
print
(
"Guide planning time : "
,
t
)
pid
=
ps
.
numberPaths
()
-
1
for
i
in
range
(
5
):
print
"Optimization pass "
,
i
print
(
"Optimization pass "
,
i
)
ps
.
optimizePath
(
pid
)
pid
=
ps
.
numberPaths
()
-
1
...
...
script/
scenarios/sandbox/
ANYmal/anymal_slalom2.py
→
script/ANYmal/anymal_slalom2.py
View file @
0bcd5c3f
...
...
@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot
from
hpp.gepetto
import
Viewer
from
tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
print
(
"Plan guide trajectory ..."
)
import
anymal_slalom2_path
as
tp
print
"Done."
print
(
"Done."
)
import
time
...
...
@@ -39,14 +39,14 @@ fullBody.setCurrentConfig (q_init)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
#fullBody.usePosturalTaskContactCreation(True)
print
"Generate limb DB ..."
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
#dict_heuristic = {fullBody.rLegId:"fixedStep04", fullBody.lLegId:"fixedStep04", fullBody.rArmId:"static", fullBody.lArmId:"static"}
fullBody
.
loadAllLimbs
(
"static"
,
"ReferenceConfiguration"
,
nbSamples
=
100000
,
disableEffectorCollision
=
False
)
#fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=100000,disableEffectorCollision=False)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
fullBody
.
setReferenceConfig
(
q_ref
)
#define initial and final configurations :
...
...
@@ -84,13 +84,13 @@ v(q_goal)
v
.
addLandmark
(
'anymal/base'
,
0.3
)
print
"Generate contact plan ..."
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
)
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/sandbox/
ANYmal/anymal_slalom2_path.py
→
script/ANYmal/anymal_slalom2_path.py
View file @
0bcd5c3f
...
...
@@ -85,13 +85,13 @@ ps.addPathOptimizer ("RandomShortcutDynamic")
#ps.addPathOptimizer ("RandomShortcut")
# Solve the planning problem :
t
=
ps
.
solve
()
print
"Guide planning time : "
,
t
print
(
"Guide planning time : "
,
t
)
#v.solveAndDisplay('rm',2,0.001)
#v.client.gui.removeFromGroup("rm",v.sceneName)
pid
=
ps
.
numberPaths
()
-
1
for
i
in
range
(
5
):
print
"Optimization pass "
,
i
print
(
"Optimization pass "
,
i
)
ps
.
optimizePath
(
pid
)
pid
=
ps
.
numberPaths
()
-
1
...
...
script/
scenarios/sandbox/
ANYmal/anymal_slalom_debris.py
→
script/ANYmal/anymal_slalom_debris.py
View file @
0bcd5c3f
...
...
@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal import Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
print
(
"Plan guide trajectory ..."
)
import
anymal_slalom_debris_path
as
tp
print
"Done."
print
(
"Done."
)
import
time
...
...
@@ -38,12 +38,12 @@ fullBody.setCurrentConfig (q_init)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
#fullBody.usePosturalTaskContactCreation(True)
print
"Generate limb DB ..."
print
(
"Generate limb DB ..."
)
tStart
=
time
.
time
()
fullBody
.
loadAllLimbs
(
"fixedStep04"
,
"ReferenceConfiguration"
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
print
(
"Done."
)
print
(
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
)
fullBody
.
setReferenceConfig
(
q_ref
)
#define initial and final configurations :
...
...
@@ -81,13 +81,13 @@ fullBody.setStartState(q_init,fullBody.limbs_names, [[0.,0.,1.] for _ in range(4
fullBody
.
setEndState
(
q_goal
,
fullBody
.
limbs_names
,
[[
0.
,
0.
,
1.
]
for
_
in
range
(
4
)])
print
"Generate contact plan ..."
print
(
"Generate contact plan ..."
)
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.002
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
testReachability
=
True
,
quasiStatic
=
True
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done. "
print
"Contact sequence computed in "
+
str
(
tInterpolateConfigs
)
+
" s."
print
"number of configs :"
,
len
(
configs
)
print
(
"Done. "
)
print
(
"Contact sequence computed in "
+
str
(
tInterpolateConfigs
)
+
" s."
)
print
(
"number of configs :"
,
len
(
configs
)
)
fullBody
.
resetJointsBounds
()
...
...
script/
scenarios/sandbox/
ANYmal/anymal_slalom_debris_path.py
→
script/ANYmal/anymal_slalom_debris_path.py
View file @
0bcd5c3f
...
...
@@ -53,7 +53,7 @@ vf = ViewerFactory (ps)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
afftool
=
AffordanceTool
()
afftool
.
setAffordanceConfig
(
'Support'
,
[
0.5
,
0.03
,
0.00005
])
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"multicontact/slalom_debris"
,
"planning"
,
vf
,
reduceSizes
=
[
0.1
,
0
,
0
]
)
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"multicontact/slalom_debris"
,
"planning"
,
vf
)
v
=
vf
.
createViewer
(
displayArrows
=
True
)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
#v.addLandmark(v.sceneName,1)
...
...
@@ -86,7 +86,7 @@ ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t
=
ps
.
solve
()
#v.solveAndDisplay('rm',2,radiusSphere=0.01)
print
"Guide planning time : "
,
t
print
(
"Guide planning time : "
,
t
)
# display solution :
...
...
script/
scenarios/sandbox/
ANYmal/darpa.py
→
script/ANYmal/darpa.py
View file @
0bcd5c3f
...
...
@@ -2,9 +2,9 @@ from hpp.corbaserver.rbprm.anymal_contact6D import Robot
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver.rbprm.tools.display_tools
import
*
import
time
print
"Plan guide trajectory ..."
print
(
"Plan guide trajectory ..."
)