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
97d59ab5
Commit
97d59ab5
authored
Mar 20, 2018
by
Pierre Fernbach
Browse files
updates scripts of IROS18
parent
b3129bc4
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py
View file @
97d59ab5
...
...
@@ -50,7 +50,7 @@ rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.09
;
rLegy
=
0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep1"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep1"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.3
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
...
...
@@ -60,7 +60,7 @@ lLegLimbOffset=[0,0,0.035]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.09
;
lLegy
=
0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep1"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
)
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep1"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.3
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
fullBody
.
setReferenceConfig
(
q_ref
)
...
...
@@ -240,3 +240,85 @@ displayOneStepConstraints(r)
#player.interpolate(2,len(configs)-1)
"""
""" # tests front line
s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
s02,success = StateHelper.addNewContact(s0,rLegId,[0.2,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s02.sId)
s04,success = StateHelper.addNewContact(s0,rLegId,[0.4,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s04.sId)
s06,success = StateHelper.addNewContact(s0,rLegId,[0.6,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s06.sId)
s07,success = StateHelper.addNewContact(s0,rLegId,[0.7,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s07.sId)
s075,success = StateHelper.addNewContact(s0,rLegId,[0.75,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s07.sId)
s08,success = StateHelper.addNewContact(s0,rLegId,[0.8,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s08.sId)
s085,success = StateHelper.addNewContact(s0,rLegId,[0.85,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s09.sId)
"""
"""#test backline
s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
s02,success = StateHelper.addNewContact(s0,rLegId,[-0.2,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s02.sId)
s04,success = StateHelper.addNewContact(s0,rLegId,[-0.4,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s04.sId)
s06,success = StateHelper.addNewContact(s0,rLegId,[-0.6,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s06.sId)
s07,success = StateHelper.addNewContact(s0,rLegId,[-0.7,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s07.sId)
s07i,success = StateHelper.removeContact(s0,rLegId)
fullBody.isReachableFromState(s0.sId,s07i.sId)
fullBody.isReachableFromState(s07i.sId,s07.sId)
s075,success = StateHelper.addNewContact(s0,rLegId,[-0.75,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s075.sId)
s075i,success = StateHelper.removeContact(s0,rLegId)
fullBody.isReachableFromState(s0.sId,s075i.sId)
fullBody.isReachableFromState(s075i.sId,s075.sId)
s08,success = StateHelper.addNewContact(s0,rLegId,[-0.8,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s08.sId)
s085,success = StateHelper.addNewContact(s0,rLegId,[-0.85,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s09.sId)
"""
# test right / diag
s0
=
State
(
fullBody
,
q
=
q_init
,
limbsIncontact
=
[
rLegId
,
lLegId
])
s02
,
success
=
StateHelper
.
addNewContact
(
s0
,
rLegId
,[
0.2
,
0
-
0.1
,
0.01
],[
0
,
0
,
1
])
assert
(
success
)
fullBody
.
isReachableFromState
(
s0
.
sId
,
s02
.
sId
)
s04
,
success
=
StateHelper
.
addNewContact
(
s0
,
rLegId
,[
0.4
,
0
-
0.1
,
0.01
],[
0
,
0
,
1
])
assert
(
success
)
fullBody
.
isReachableFromState
(
s0
.
sId
,
s04
.
sId
)
s06
,
success
=
StateHelper
.
addNewContact
(
s0
,
rLegId
,[
0.6
,
0
-
0.1
,
0.01
],[
0
,
0
,
1
])
assert
(
success
)
fullBody
.
isReachableFromState
(
s0
.
sId
,
s06
.
sId
)
s07
,
success
=
StateHelper
.
addNewContact
(
s0
,
rLegId
,[
0.7
,
0
-
0.1
,
0.01
],[
0
,
0
,
1
])
assert
(
success
)
script/dynamic/plateform_hrp2_interp_testTransition.py
View file @
97d59ab5
...
...
@@ -167,8 +167,12 @@ r(smid.q())
sf2
=
State
(
fullBody
,
q
=
q_goal
,
limbsIncontact
=
[
lLegId
,
rLegId
])
"""
fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,True)
fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,timings=[0.4,0.2,0.4])
fullBody.isDynamicallyReachableFromState(si.sId,smid.sId,timings=[0.8,0.6,0.8])
fullBody.isDynamicallyReachableFromState(smid2.sId,sf.sId,timings=[0.8,0.6,0.8])
pid
=
fullBody
.
isDynamicallyReachableFromState
(
smid
.
sId
,
smid2
.
sId
,
True
)
import disp_bezier
pp.dt = 0.00001
disp_bezier.showPath(r,pp,pid)
...
...
@@ -185,6 +189,8 @@ for i in range(1,4):
r.client.gui.writeNodeFile('s',path+'_S.stl')
"""
"""
com = fullBody.getCenterOfMass()
com[1] = 0
...
...
script/dynamic/sideWall_hyq_interpKino_intersection.py
View file @
97d59ab5
...
...
@@ -128,6 +128,30 @@ configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 1, filterStat
r
(
configs
[
-
1
])
tInterpolateConfigs
=
time
.
time
()
-
tStart
pid
=
fullBody
.
isDynamicallyReachableFromState
(
17
,
18
,
True
)
import
disp_bezier
pp
.
dt
=
0.0001
disp_bezier
.
showPath
(
r
,
pp
,
pid
)
x
=
[
2.47985
,
-
0.25492
,
0.962874
]
createSphere
(
's'
,
r
)
moveSphere
(
's'
,
r
,
x
)
displayBezierConstraints
(
r
)
path
=
"/local/dev_hpp/screenBlender/iros2018/polytopes/hyq/path"
for
i
in
range
(
1
,
4
):
r
.
client
.
gui
.
writeNodeFile
(
'path_'
+
str
(
int
(
pid
[
i
]))
+
'_root'
,
path
+
str
(
i
-
1
)
+
'.obj'
)
r
.
client
.
gui
.
writeNodeFile
(
's'
,
path
+
'_S.stl'
)
noCOQP
=
0
for
i
in
range
(
len
(
configs
)
-
2
):
...
...
@@ -137,7 +161,6 @@ for i in range(len(configs)-2):
f
=
open
(
"/local/fernbac/bench_iros18/success/log_successSideWall.log"
,
"a"
)
f
.
write
(
"num states : "
+
str
(
len
(
configs
))
+
"
\n
"
)
if
noCOQP
>
0
:
...
...
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