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
5d2a9e8b
Commit
5d2a9e8b
authored
Jul 24, 2019
by
Pierre Fernbach
Browse files
[script] update python script for talos_platform_random
parent
b4569bdf
Changes
2
Hide whitespace changes
Inline
Side-by-side
script/scenarios/memmo/talos_platform_random.py
View file @
5d2a9e8b
...
...
@@ -26,6 +26,7 @@ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vM
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
)
...
...
@@ -46,8 +47,8 @@ except Exception:
q_ref
=
fullBody
.
referenceConfig_legsApart
[::]
+
[
0
]
*
6
q_init
=
q_ref
[::]
fullBody
.
setReferenceConfig
(
q_ref
)
fullBody
.
setPostureWeights
(
fullBody
.
postureWeights
[::]
+
[
0
]
*
6
)
fullBody
.
usePosturalTaskContactCreation
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
...
...
@@ -56,9 +57,9 @@ tStart = time.time()
# generate databases :
nbSamples
=
100000
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep0
6
"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
,
kinematicConstraintsMin
=
0.85
)
fullBody
.
addLimb
(
fullBody
.
rLegId
,
fullBody
.
rleg
,
fullBody
.
rfoot
,
fullBody
.
rLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
rLegx
,
fullBody
.
rLegy
,
nbSamples
,
"fixedStep0
8
"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
rLegKinematicConstraints
,
kinematicConstraintsMin
=
0.85
)
#fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep0
6
"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
,
kinematicConstraintsMin
=
0.85
)
fullBody
.
addLimb
(
fullBody
.
lLegId
,
fullBody
.
lleg
,
fullBody
.
lfoot
,
fullBody
.
lLegOffset
,
fullBody
.
rLegNormal
,
fullBody
.
lLegx
,
fullBody
.
lLegy
,
nbSamples
,
"fixedStep0
8
"
,
0.01
,
kinematicConstraintsPath
=
fullBody
.
lLegKinematicConstraints
,
kinematicConstraintsMin
=
0.85
)
#fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
...
...
@@ -83,19 +84,6 @@ q_init[configSize+3:configSize+6] = acc_init[::]
q_goal
[
configSize
:
configSize
+
3
]
=
vel_goal
[::]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
'''
state_id = fullBody.generateStateInContact(q_init, vel_init,robustnessThreshold=5)
assert fullBody.rLegId in fullBody.getAllLimbsInContact(state_id), "Right leg not in contact in initial state"
assert fullBody.lLegId in fullBody.getAllLimbsInContact(state_id), "Left leg not in contact in initial state"
q_init = fullBody.getConfigAtState(state_id)
v(q_init)
state_id = fullBody.generateStateInContact(q_goal, vel_goal,robustnessThreshold=5)
assert fullBody.rLegId in fullBody.getAllLimbsInContact(state_id), "Right leg not in contact in final state"
assert fullBody.lLegId in fullBody.getAllLimbsInContact(state_id), "Left leg not in contact in final state"
q_goal = fullBody.getConfigAtState(state_id)
v(q_goal)
'''
fullBody
.
setStaticStability
(
True
)
...
...
@@ -106,7 +94,7 @@ q_init[2]=q_ref[2]
q_goal
[
2
]
=
q_ref
[
2
]
# define init gait according the direction of motion, try to move first the leg on the outside of the turn :
# define init gait according
to
the direction of motion, try to move first the leg on the outside of the turn :
if
q_goal
[
0
]
>
q_init
[
0
]
:
#go toward x positif
if
q_goal
[
1
]
>
q_init
[
1
]:
# turn left
gait
=
[
fullBody
.
rLegId
,
fullBody
.
lLegId
]
...
...
@@ -118,11 +106,14 @@ else : # go toward x negatif
else
:
# turn left
gait
=
[
fullBody
.
rLegId
,
fullBody
.
lLegId
]
v
(
q_init
)
fullBody
.
setStartState
(
q_init
,
gait
)
v
(
q_goal
)
fullBody
.
setEndState
(
q_goal
,
gait
)
v
(
q_init
)
print
"Generate contact plan ..."
tStart
=
time
.
time
()
v
(
q_init
)
configs
=
fullBody
.
interpolate
(
0.005
,
pathId
=
pId
,
robustnessTreshold
=
1
,
filterStates
=
True
,
testReachability
=
True
,
quasiStatic
=
True
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"Done."
...
...
script/scenarios/memmo/talos_platform_random_path.py
View file @
5d2a9e8b
...
...
@@ -12,7 +12,7 @@ vGoal = 0.01
vMax
=
0.5
# linear velocity bound for the root
aMax
=
0.5
# linear acceleration bound for the root
extraDof
=
6
mu
=
0.
5
# coefficient of friction
mu
=
0.
3
# 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
()
...
...
@@ -40,7 +40,7 @@ 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
)
ps
.
setParameter
(
"DynamicPlanner/friction"
,
mu
)
# sample only configuration with null velocity and acceleration :
ps
.
setParameter
(
"ConfigurationShooter/sampleExtraDOF"
,
False
)
...
...
@@ -52,7 +52,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/plateforme_not_flat"
,
"planning"
,
vf
,
reduceSizes
=
[
0.1
6
,
0
,
0
])
afftool
.
loadObstacleModel
(
"hpp_environments"
,
"multicontact/plateforme_not_flat"
,
"planning"
,
vf
,
reduceSizes
=
[
0.1
,
0
,
0
])
try
:
v
=
vf
.
createViewer
(
displayArrows
=
True
)
except
Exception
:
...
...
@@ -86,8 +86,8 @@ end_plateform_id = random.randint(init_plateform_id+1,4)
# end_plateform_id+=1
# set x position from the plateform choosen :
x_init
=
0.16
+
0.92
*
init_plateform_id
x_goal
=
0.16
+
0.92
*
end_plateform_id
x_init
=
0.16
+
0.92
5
*
init_plateform_id
x_goal
=
0.16
+
0.92
5
*
end_plateform_id
# uniformly sample y position
y_init
=
random
.
uniform
(
Y_BOUNDS
[
0
],
Y_BOUNDS
[
1
])
...
...
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