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
db5376c8
Commit
db5376c8
authored
Mar 01, 2018
by
Pierre Fernbach
Browse files
[demo] platform polytopes
parent
c42dc451
Changes
9
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
db5376c8
...
...
@@ -288,6 +288,7 @@ install(FILES
data/meshes/siggraph_asia/stairs_lower.stl
data/meshes/flat_hole.stl
data/meshes/cross_gap.stl
data/meshes/cross_gap2.stl
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/meshes
)
...
...
data/urdf/cross_gap.urdf
View file @
db5376c8
...
...
@@ -3,7 +3,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap
2
.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
...
...
@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/cross_gap
2
.stl"/>
</geometry>
</collision>
</link>
...
...
script/dynamic/plateform_hand_hrp2_interp.py
View file @
db5376c8
...
...
@@ -126,8 +126,8 @@ r(q_goal)
r
(
q_init
)
#
fullBody.setStartState(q_init,[lLegId,rLegId])
#
fullBody.setEndState(q_goal,[lLegId,rLegId])
fullBody
.
setStartState
(
q_init
,[
lLegId
,
rLegId
])
fullBody
.
setEndState
(
q_goal
,[
lLegId
,
rLegId
])
#p = fullBody.computeContactPointsAtState(init_sid)
...
...
@@ -146,13 +146,13 @@ pp = PathPlayer (fullBody.client.basic, r)
import
fullBodyPlayerHrp2
"""
tStart
=
time
.
time
()
configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold =
1
, filterStates = False)
configsFull
=
fullBody
.
interpolate
(
0.001
,
pathId
=
pId
,
robustnessTreshold
=
0
,
filterStates
=
False
)
tInterpolateConfigs
=
time
.
time
()
-
tStart
print
"number of configs :"
,
len
(
configsFull
)
"""
"""
q_init[0] += 0.05
createSphere('s',r)
...
...
@@ -189,7 +189,7 @@ r(smid2.q())
sf2,success = StateHelper.removeContact(smid2,larmId)
sf2.projectToCOM([1.3,0,0.82])
"""
"""
com = fullBody.getCenterOfMass()
...
...
script/dynamic/plateform_hand_hrp2_path.py
View file @
db5376c8
...
...
@@ -30,7 +30,7 @@ urdfNameRom = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix
=
""
srdfSuffix
=
""
vMax
=
omniORB
.
any
.
to_any
(
0.3
);
aMax
=
omniORB
.
any
.
to_any
(
0.2
);
aMax
=
omniORB
.
any
.
to_any
(
1
);
#aMax = omniORB.any.to_any(0.3);
extraDof
=
6
mu
=
omniORB
.
any
.
to_any
(
MU
)
...
...
@@ -40,7 +40,7 @@ rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, pa
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.25,2, -0.5, 5.5, 0.6, 1.8])
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1
,
2
,
-
0.5
,
0.5
,
0.5
,
0.8
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT0'
,[
0
,
0
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT1'
,[
-
0.
3
5
,
0.
1
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT1'
,[
-
0.
5
5
,
0.
85
])
rbprmBuilder
.
setJointBounds
(
'HEAD_JOINT0'
,[
0
,
0
])
rbprmBuilder
.
setJointBounds
(
'HEAD_JOINT1'
,[
0
,
0
])
...
...
@@ -79,7 +79,7 @@ afftool.visualiseAffordances('Support', r, r.color.lightBrown)
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
q_init
[
0
:
3
]
=
[
0.3
,
0
,
0.58
];
r
(
q_init
)
q_init
[
8
]
=
0.8
#q_init[3:7] = [0.7071,0,0,0.7071]
#q_init [0:3] = [1, 1, 0.65]
...
...
@@ -89,7 +89,7 @@ q_goal = q_init [::]
q_goal
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
q_goal
[
0
:
3
]
=
[
1.28
,
0
,
0.58
];
r
(
q_goal
)
q_goal
[
8
]
=
0
r
(
q_goal
)
#~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
...
...
@@ -119,8 +119,8 @@ tPlanning = time.time() -tStart
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
rbprmBuilder
.
client
.
basic
,
r
)
pp
.
dt
=
0.03
#
pp.displayVelocityPath(0)
#
r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp
.
displayVelocityPath
(
0
)
r
.
client
.
gui
.
setVisibility
(
"path_0_root"
,
"ALWAYS_ON_TOP"
)
"""
...
...
script/dynamic/plateform_hrp2_interp_testTransition.py
View file @
db5376c8
...
...
@@ -149,7 +149,8 @@ createSphere('s',r)
n
=
[
0
,
0
,
1
]
p
=
[
0
,
0.1
,
0
]
q_init
[
-
6
:
-
3
]
=
[
0.2
,
0
,
0
]
q_goal
[
-
6
:
-
3
]
=
[
0.1
,
0
,
0
]
sf
=
State
(
fullBody
,
q
=
q_goal
,
limbsIncontact
=
[
lLegId
,
rLegId
])
si
=
State
(
fullBody
,
q
=
q_init
,
limbsIncontact
=
[
lLegId
,
rLegId
])
...
...
@@ -164,11 +165,18 @@ assert(success)
r
(
smid
.
q
())
sf2
=
State
(
fullBody
,
q
=
q_goal
,
limbsIncontact
=
[
lLegId
,
rLegId
])
pid
=
fullBody
.
isDynamicallyReachableFromState
(
smid
.
sId
,
smid2
.
sId
,
True
)
import
disp_bezier
disp_bezier
.
showPath
(
r
,
pp
,
pid
)
"""
com = fullBody.getCenterOfMass()
com[1] = 0
"""
"""
pids = []
pids += [fullBody.isDynamicallyReachableFromState(si.sId,smid.sId)]
pids += [fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId)]
...
...
@@ -181,7 +189,7 @@ for pid in pids :
#r.client.gui.setVisibility('path_'+str(pid)+'_root','ALWAYS_ON_TOP')
else:
print "fail."
"""
"""
n = [0,0,1]
...
...
script/dynamic/sideWall_hyq_interpKino_intersection.py
View file @
db5376c8
...
...
@@ -128,11 +128,28 @@ configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 1, filterStat
r
(
configs
[
-
1
])
tInterpolateConfigs
=
time
.
time
()
-
tStart
noCOQP
=
0
for
i
in
range
(
len
(
configs
)
-
2
):
pid
=
fullBody
.
isDynamicallyReachableFromState
(
i
,
i
+
1
)
if
len
(
pid
)
==
0
:
noCOQP
+=
1
f
=
open
(
"/local/fernbac/bench_iros18/success/log_successSideWall.log"
,
"a"
)
f
.
write
(
"num states : "
+
str
(
len
(
configs
))
+
"
\n
"
)
if
noCOQP
>
0
:
f
.
write
(
"fail, with infeasibles transitions "
+
str
(
noCOQP
)
+
"
\n
"
)
else
:
f
.
write
(
"all transition feasibles
\n
"
)
f
.
close
()
"""
print "number of configs =", len(configs)
r(configs[-1])
...
...
@@ -143,6 +160,8 @@ from fullBodyPlayer import Player
player = Player(fullBody,pp,tp,configs,draw=True,optim_effector=False,use_velocity=dynamic,pathId = 1)
player.displayContactPlan()
"""
...
...
@@ -196,7 +215,7 @@ camera = [0.5681925415992737,
r.client.gui.setCameraTransform(0,camera)
"""
"""
# infeasible :
r([1.46,0,0.851192,1,0,0,0,-0.32001,1.18232,-1.5504,-0.305161,-0.139056,1.37011,-0.36127,1.20462,-1.55472,-0.361812,0.407251,0.775043,0.2,0,0.000953452,0,0,0.000381381,])
...
...
@@ -212,6 +231,6 @@ leg = rhLegId
configs=configs[6:8]
cs = generate_contact_sequence_hyq.generateContactSequence(fullBody,configs,6, 7,r)
"""
script/dynamic/stair_bauzil_hrp2_interp_noRamp.py
View file @
db5376c8
...
...
@@ -169,15 +169,27 @@ r(configs[-1])
noCOQP
=
0
for
i
in
range
(
len
(
configs
)
-
1
):
pid
=
fullBody
.
isDynamicallyReachableFromState
(
i
,
i
+
1
)
if
len
(
pid
)
==
0
:
noCOQP
+=
1
f
=
open
(
"/home/pfernbac/Documents/com_ineq_test/log_success.log"
,
"a"
)
f
.
write
(
"num states : "
+
str
(
len
(
configs
))
+
"
\n
"
)
f
=
open
(
"/local/fernbac/bench_iros18/success/log_successStairsNoRamp.log"
,
"a"
)
if
noCOQP
>
0
:
f
.
write
(
"fail, with "
+
str
(
noCOQP
)
+
" infeasibles transitions
\n
"
)
else
:
f
.
write
(
"all transition feasibles
\n
"
)
f
.
close
()
"""
from fullBodyPlayerHrp2 import Player
...
...
script/dynamic/stair_bauzil_hrp2_interp_testTransition.py
View file @
db5376c8
...
...
@@ -159,11 +159,33 @@ from hpp.gepetto import PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
pp
.
dt
=
0.001
configs
=
fullBody
.
interpolate
(
0.00
5
,
pathId
=
0
,
robustnessTreshold
=
0
,
filterStates
=
True
)
configs
=
fullBody
.
interpolate
(
0.00
1
,
pathId
=
0
,
robustnessTreshold
=
1
,
filterStates
=
True
)
print
"number of configs :"
,
len
(
configs
)
r
(
configs
[
-
1
])
noCOQP
=
0
for
i
in
range
(
len
(
configs
)
-
2
):
pid
=
fullBody
.
isDynamicallyReachableFromState
(
i
,
i
+
1
)
if
len
(
pid
)
==
0
:
noCOQP
+=
1
f
=
open
(
"/local/fernbac/bench_iros18/success/log_successStairs.log"
,
"a"
)
f
.
write
(
"num states : "
+
str
(
len
(
configs
))
+
"
\n
"
)
if
noCOQP
>
0
:
f
.
write
(
"fail, with infeasibles transitions "
+
str
(
noCOQP
)
+
"
\n
"
)
else
:
f
.
write
(
"all transition feasibles
\n
"
)
f
.
close
()
"""
f = open("/home/pfernbac/Documents/com_ineq_test/log_success.log","a")
f.write("num states : "+str(len(configs))+"
\n
")
...
...
@@ -172,7 +194,7 @@ f.close()
"""
from fullBodyPlayerHrp2 import Player
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=False,pathId = 0)
...
...
@@ -191,7 +213,7 @@ endState = 14
configs=configs[beginState:endState+1]
#cs = generateContactSequence(fullBody,configs,beginState, endState,r)
cs = generateContactSequenceWithInitGuess(ps,fullBody,configs,beginState, endState,r)
"""
"""
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs.saveAsXML(filename, "ContactSequence")
...
...
script/tools/stat_muscod_qp.py
View file @
db5376c8
...
...
@@ -33,7 +33,7 @@ def callMuscodBetweenTwoState(fullBody,s0,s1,c_qp = [], t_qp = []):
states
=
genStateWithOneStep
(
fullBody
,
limbs
[
0
],
5
00
,
False
)
states
=
genStateWithOneStep
(
fullBody
,
limbs
[
0
],
1
00
,
False
)
name
=
"/local/fernbac/bench_iros18/muscod_qp/one_step"
file_exist
=
True
...
...
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