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
16ea2bf6
Commit
16ea2bf6
authored
Sep 14, 2019
by
Pierre Fernbach
Browse files
[script] update working script for anymal flat-floor / modular
parent
72980efc
Changes
4
Hide whitespace changes
Inline
Side-by-side
script/scenarios/sandbox/ANYmal/anymal_flatGround.py
View file @
16ea2bf6
...
...
@@ -36,11 +36,11 @@ q_init = q_ref[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setCurrentConfig
(
q_init
)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
fullBody
.
usePosturalTaskContactCreation
(
True
)
#
fullBody.usePosturalTaskContactCreation(True)
print
"Generate limb DB ..."
tStart
=
time
.
time
()
fullBody
.
loadAllLimbs
(
"fixedStep04"
,
"ReferenceConfiguration"
)
fullBody
.
loadAllLimbs
(
"fixedStep04"
,
"ReferenceConfiguration"
,
nbSamples
=
100000
,
disableEffectorCollision
=
False
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
...
...
@@ -51,12 +51,12 @@ configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraCo
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
]
dir_init
=
tp
.
ps
.
configAtParam
(
pId
,
0.
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
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
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
2
robTreshold
=
5
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
dir_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
...
...
@@ -66,30 +66,30 @@ q_goal[configSize+3:configSize+6] = [0,0,0]
q_init
[
2
]
=
q_ref
[
2
]
q_goal
[
2
]
=
q_ref
[
2
]
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
)
v
.
addLandmark
(
'anymal/base'
,
0.3
)
v
(
q_init
)
z
=
[
0.
,
0.
,
1
]
# specify the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,
fullBody
.
limbs_names
,
[
z
for
_
in
range
(
4
)])
fullBody
.
setEndState
(
q_goal
,
fullBody
.
limbs_names
,
[
z
for
_
in
range
(
4
)])
print
"Generate contact plan ..."
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.00
2
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
testReachability
=
True
,
quasiStatic
=
True
)
configs
=
fullBody
.
interpolate
(
0.00
1
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
testReachability
=
True
,
quasiStatic
=
True
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done.
"
print
"Contact
sequence compu
ted in "
+
str
(
tInterpolateConfigs
)
+
" s
.
"
print
"Done."
print
"Contact
plan genera
ted in
:
"
+
str
(
tInterpolateConfigs
)
+
" s"
print
"number of configs :"
,
len
(
configs
)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
fullBody
.
resetJointsBounds
()
script/scenarios/sandbox/ANYmal/anymal_flatGround_path.py
View file @
16ea2bf6
...
...
@@ -36,7 +36,7 @@ ps = ProblemSolver( rbprmBuilder )
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
(
"Kinodynamic/force
All
Orientation"
,
True
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootX"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/sizeFootY"
,
0.01
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
...
...
script/scenarios/sandbox/ANYmal/anymal_modular_palet_low.py
View file @
16ea2bf6
...
...
@@ -4,6 +4,7 @@ from tools.display_tools import *
import
time
print
"Plan guide trajectory ..."
import
scenarios.sandbox.ANYmal.anymal_modular_palet_low_path
as
tp
#Robot.urdfSuffix += "_safeFeet"
statusFilename
=
tp
.
statusFilename
pId
=
tp
.
pid
...
...
@@ -68,7 +69,7 @@ print "Generate limb DB ..."
tStart
=
time
.
time
()
# generate databases :
fullBody
.
loadAllLimbs
(
"static"
,
"ReferenceConfiguration"
,
nbSamples
=
100000
)
fullBody
.
loadAllLimbs
(
"static"
,
"ReferenceConfiguration"
,
nbSamples
=
100000
,
disableEffectorCollision
=
False
)
tGenerate
=
time
.
time
()
-
tStart
print
"Done."
print
"Databases generated in : "
+
str
(
tGenerate
)
+
" s"
...
...
@@ -83,8 +84,8 @@ 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
=
2
vel_init
=
[
0
,
0
,
0
]
robTreshold
=
5
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
vel_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
...
...
script/scenarios/sandbox/ANYmal/anymal_modular_palet_low_path.py
View file @
16ea2bf6
...
...
@@ -6,7 +6,7 @@ from pinocchio import Quaternion
import
numpy
as
np
import
time
import
math
statusFilename
=
"
/res/
infos.log"
statusFilename
=
"infos.log"
Z_VALUE
=
0.465
Y_VALUE
=
-
0.1
...
...
@@ -59,7 +59,8 @@ 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"
,
"ori/modular_palet_low"
,
"planning"
,
vf
,
reduceSizes
=
[
0.06
,
0
,
0
])
afftool
.
setMinimumArea
(
'Support'
,
0.03
)
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"ori/modular_palet_low_collision"
,
"planning"
,
vf
,
reduceSizes
=
[
0.1
,
0
,
0
])
#afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "planning", vf)
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
...
...
@@ -72,7 +73,7 @@ except Exception:
return
v
=
FakeViewer
()
#
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
afftool
.
visualiseAffordances
(
'Support'
,
v
,
v
.
color
.
lightBrown
)
#v.addLandmark(v.sceneName,1)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
...
...
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