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
ed054fb4
Commit
ed054fb4
authored
Jun 16, 2018
by
Pierre Fernbach
Browse files
[script] add/update lot of scenario scripts
parent
cb4275c6
Changes
30
Hide whitespace changes
Inline
Side-by-side
script/dynamic/darpa_hrp2_interStatic.py
View file @
ed054fb4
...
...
@@ -47,7 +47,7 @@ rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.09
;
rLegy
=
0.05
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.
3
)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.
5
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
...
...
@@ -57,7 +57,7 @@ lLegLimbOffset=[0,0,0.035]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.09
;
lLegy
=
0.05
#fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.
3
)
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.
5
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
...
...
script/dynamic/darpa_hrp2_interStatic_testTransition.py
0 → 100644
View file @
ed054fb4
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
from
hpp.gepetto
import
Viewer
from
tools
import
*
import
darpa_hrp2_path
as
tp
import
time
import
omniORB.any
import
numpy
as
np
from
numpy
import
linalg
as
LA
from
hpp.corbaserver.rbprm.rbprmstate
import
State
,
StateHelper
packageName
=
"hrp2_14_description"
meshPackageName
=
"hrp2_14_description"
rootJointType
=
"freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
urdfSuffix
=
"_reduced"
srdfSuffix
=
""
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1.2
,
1.5
,
-
0.05
,
0.05
,
0.55
,
0.85
])
fullBody
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
fullBody
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
-
0
,
0
,
-
0
,
0
,
-
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
client
.
problem
.
setParameter
(
"aMax"
,
omniORB
.
any
.
to_any
(
tp
.
aMax
))
ps
.
client
.
problem
.
setParameter
(
"vMax"
,
omniORB
.
any
.
to_any
(
tp
.
vMax
))
r
=
tp
.
Viewer
(
ps
,
viewerClient
=
tp
.
r
.
client
,
displayArrows
=
True
,
displayCoM
=
True
)
q_init
=
[
0
,
0
,
0.648702
,
1.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.0
,
0.261799388
,
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
0.261799388
,
-
0.174532925
,
0.0
,
-
0.523598776
,
0.0
,
0.0
,
0.17
,
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
0
,
0
,
0
,
0
,
0
,
0
];
r
(
q_init
)
q_ref
=
q_init
[::]
fullBody
.
setCurrentConfig
(
q_init
)
fullBody
.
setReferenceConfig
(
q_ref
)
#~ AFTER loading obstacles
rLegId
=
'hrp2_rleg_rom'
lLegId
=
'hrp2_lleg_rom'
tStart
=
time
.
time
()
rLeg
=
'RLEG_JOINT0'
rLegOffset
=
[
0
,
0
,
-
0.105
]
rLegLimbOffset
=
[
0
,
0
,
-
0.035
]
#0.035
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.09
;
rLegy
=
0.05
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep08"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj"
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg
=
'LLEG_JOINT0'
lLegOffset
=
[
0
,
0
,
-
0.105
]
lLegLimbOffset
=
[
0
,
0
,
0.035
]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.09
;
lLegy
=
0.05
#fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep08"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj"
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
## Add arms (not used for contact) :
tGenerate
=
time
.
time
()
-
tStart
print
"generate databases in : "
+
str
(
tGenerate
)
+
" s"
"""
fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
tLoad = time.time() - tStart
print "Load databases in : "+str(tLoad)+" s"
"""
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
eps
=
0.0001
configSize
=
fullBody
.
getConfigSize
()
-
fullBody
.
client
.
basic
.
robot
.
getDimensionExtraConfigSpace
()
q_init
=
fullBody
.
getCurrentConfig
();
q_init
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
eps
)[
0
:
7
]
# use this to get the correct orientation
q_goal
=
fullBody
.
getCurrentConfig
();
q_goal
[
0
:
7
]
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
0.0001
)[
0
:
7
]
dir_init
=
tp
.
ps
.
configAtParam
(
pId
,
eps
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_init
=
tp
.
ps
.
configAtParam
(
pId
,
0
)[
tp
.
indexECS
+
3
:
tp
.
indexECS
+
6
]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
eps
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
1
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
dir_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
q_goal
[
configSize
:
configSize
+
3
]
=
dir_goal
[::]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
# Randomly generating a contact configuration at q_init
fullBody
.
setStaticStability
(
True
)
fullBody
.
setCurrentConfig
(
q_init
)
r
(
q_init
)
q_init
=
fullBody
.
generateContacts
(
q_init
,
dir_init
,
acc_init
,
robTreshold
)
r
(
q_init
)
# Randomly generating a contact configuration at q_end
fullBody
.
setCurrentConfig
(
q_goal
)
q_goal
=
fullBody
.
generateContacts
(
q_goal
,
dir_goal
,
acc_goal
,
robTreshold
)
r
(
q_goal
)
# specifying the full body configurations as start and goal state of the problem
r
.
addLandmark
(
'hrp2_14/BODY'
,
0.3
)
r
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
fullBody
.
setStaticStability
(
True
)
# only set it after the init/goal configuration are computed
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
fullBody
.
client
.
basic
,
r
)
import
fullBodyPlayerHrp2
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
testReachability
=
True
,
quasiStatic
=
True
)
tInterpolate
=
time
.
time
()
-
tStart
print
"number of configs : "
,
len
(
configs
)
print
"generated in "
+
str
(
tInterpolate
)
+
" s"
r
(
configs
[
len
(
configs
)
-
2
])
f
=
open
(
"/local/fernbac/bench_iros18/kin_constraint_tog/without_kin_constraints.log"
,
"a"
)
global
num_transition
global
valid_transition
global
length_traj
global
valid_length
global
dt
num_transition
=
0.
valid_transition
=
0.
dt
=
0.001
valid_length
=
0.
length_traj
=
0.
def
check_traj
(
s
,
c0
,
c1
,
reverse
=
False
):
global
length_traj
global
valid_length
global
dt
dist
=
LA
.
norm
(
c1
-
c0
)
if
dist
<
dt
:
return
True
length_traj
+=
dist
current_dist
=
0.
dtu
=
dt
/
dist
successProj_total
=
True
if
not
reverse
:
u
=
0.
while
u
<
1.
:
c
=
c0
*
(
1.
-
u
)
+
c1
*
u
successProj
=
s
.
projectToCOM
(
c
.
transpose
().
tolist
(),
20
)
current_dist
+=
dt
if
successProj
:
valid_length
+=
dt
else
:
print
"projection failed."
successProj_total
=
False
u
+=
dtu
successProj
=
s
.
projectToCOM
(
c1
.
transpose
().
tolist
(),
20
)
if
successProj
:
valid_length
+=
dist
-
current_dist
else
:
print
"projection failed."
successProj_total
=
False
else
:
u
=
1.
while
u
>
0.
:
c
=
c0
*
(
1.
-
u
)
+
c1
*
u
successProj
=
s
.
projectToCOM
(
c
.
transpose
().
tolist
(),
20
)
current_dist
+=
dt
if
successProj
:
valid_length
+=
dt
else
:
print
"projection failed."
successProj_total
=
False
u
-=
dtu
successProj
=
s
.
projectToCOM
(
c0
.
transpose
().
tolist
(),
20
)
if
successProj
:
valid_length
+=
dist
-
current_dist
else
:
print
"projection failed."
successProj_total
=
False
return
successProj_total
def
check_kin_feasibility
(
s0Id
,
s1Id
):
res
=
fullBody
.
isReachableFromState
(
s0Id
,
s1Id
,
True
)
if
not
res
[
0
]:
print
"Error : isReachable returned False !"
raise
Exception
(
'Error : isReachable returned False !'
)
r
(
configs
[
s0Id
])
c0
=
np
.
array
(
fullBody
.
getCenterOfMass
())
c1
=
np
.
array
(
res
[
1
])
c2
=
np
.
array
(
res
[
2
])
r
(
configs
[
s1Id
])
c3
=
np
.
array
(
fullBody
.
getCenterOfMass
())
s0
=
State
(
fullBody
,
sId
=
s0Id
)
s1
=
State
(
fullBody
,
sId
=
s1Id
)
s0_orig
=
State
(
s0
.
fullBody
,
q
=
s0
.
q
(),
limbsIncontact
=
s0
.
getLimbsInContact
())
s1_orig
=
State
(
s1
.
fullBody
,
q
=
s1
.
q
(),
limbsIncontact
=
s1
.
getLimbsInContact
())
moving_limb
=
s0
.
contactsVariations
(
s1
)
smid
,
successMid
=
StateHelper
.
removeContact
(
s0_orig
,
moving_limb
[
0
])
if
not
successMid
:
return
False
successFeasible
=
True
print
"check first part"
successFeasible
=
successFeasible
and
check_traj
(
s0_orig
,
c0
,
c1
)
print
"check mid part"
successFeasible
=
successFeasible
and
check_traj
(
smid
,
c1
,
c2
)
print
"check last part"
successFeasible
=
successFeasible
and
check_traj
(
s1_orig
,
c2
,
c3
,
True
)
return
successFeasible
for
i
in
range
(
len
(
configs
)
-
2
):
print
"check traj between state "
+
str
(
i
)
+
" and "
+
str
(
i
+
1
)
success
=
check_kin_feasibility
(
i
,
i
+
1
)
num_transition
+=
1.
if
success
:
valid_transition
+=
1.
f
.
write
(
"num_transition "
+
str
(
num_transition
)
+
"
\n
"
)
f
.
write
(
"valid_transition "
+
str
(
valid_transition
)
+
"
\n
"
)
if
num_transition
>
0
:
f
.
write
(
"valid_transition_percent "
+
str
(
float
(
valid_transition
/
num_transition
)
*
100.
)
+
" %
\n
"
)
f
.
write
(
"traj_length "
+
str
(
length_traj
)
+
"
\n
"
)
f
.
write
(
"valid_length "
+
str
(
valid_length
)
+
"
\n
"
)
if
length_traj
>
0
:
f
.
write
(
"valid_traj_percent "
+
str
(
float
(
valid_length
/
length_traj
)
*
100.
)
+
" %
\n
"
)
f
.
close
()
#wid = r.client.gui.getWindowID("window_hpp_")
#r.client.gui.attachCameraToNode( 'hrp2_14/BODY_0',wid)
script/dynamic/darpa_hrp2_path.py
View file @
ed054fb4
...
...
@@ -8,7 +8,7 @@ import math
import
omniORB.any
from
configs.darpa
import
*
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
...
...
@@ -28,7 +28,7 @@ class Robot (Parent):
self
.
load
=
load
MU
=
0.5
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
...
...
@@ -91,7 +91,7 @@ r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
q_init
[
0
:
3
]
=
[
-
0.
8
,
0
,
0.53
];
r
(
q_init
)
q_init
[
0
:
3
]
=
[
-
0.
95
,
0
,
0.53
];
r
(
q_init
)
#q_init [0:3] = [-0.5, 0, 0.75]; r (q_init)
rbprmBuilder
.
setCurrentConfig
(
q_init
)
...
...
script/dynamic/darpa_line_hrp2_interStatic.py
View file @
ed054fb4
...
...
@@ -13,15 +13,16 @@ rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
urdfSuffix
=
"_reduced"
urdfSuffix
=
"_reduced_safe"
#srdfSuffix = "_disable_leg_autocol"
srdfSuffix
=
""
pId
=
tp
.
ps
.
numberPaths
()
-
1
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1.2
,
1.5
,
-
0.
05
,
0.
05
,
0.55
,
0.85
])
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1.2
,
1.5
,
-
0.
2
,
0.
2
,
0.55
,
1.
])
fullBody
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
tp
.
extraDof
)
fullBody
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
])
ps
=
tp
.
ProblemSolver
(
fullBody
)
ps
.
client
.
problem
.
setParameter
(
"aMax"
,
omniORB
.
any
.
to_any
(
tp
.
aMax
))
ps
.
client
.
problem
.
setParameter
(
"vMax"
,
omniORB
.
any
.
to_any
(
tp
.
vMax
))
...
...
@@ -33,6 +34,15 @@ q_ref = q_init[::]
fullBody
.
setCurrentConfig
(
q_init
)
fullBody
.
setReferenceConfig
(
q_ref
)
view
=
[
0.4
,
-
0.
,
5.5
,
0.7071067811865475
,
0.
,
0.
,
-
0.7071067811865475
]
r
.
client
.
gui
.
setCameraTransform
(
0
,
view
)
#~ AFTER loading obstacles
...
...
@@ -42,22 +52,23 @@ tStart = time.time()
rLeg
=
'RLEG_JOINT0'
rLegOffset
=
[
0
,
0
,
-
0.
1
05
]
rLegOffset
=
[
0
,
0
,
-
0.0
95
5
]
rLegLimbOffset
=
[
0
,
0
,
-
0.035
]
#0.035
rLegNormal
=
[
0
,
0
,
1
]
rLegx
=
0.09
;
rLegy
=
0.05
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"dynamicWalk"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
)
#fixedStep08
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep1"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj"
,
kinematicConstraintsMin
=
0.
)
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
lLeg
=
'LLEG_JOINT0'
lLegOffset
=
[
0
,
0
,
-
0.
1
05
]
lLegOffset
=
[
0
,
0
,
-
0.0
95
5
]
lLegLimbOffset
=
[
0
,
0
,
0.035
]
lLegNormal
=
[
0
,
0
,
1
]
lLegx
=
0.09
;
lLegy
=
0.05
#fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1,"_6_DOF")
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"
dynamicWalk
"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
)
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"
fixedStep1
"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsPath
=
"package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj"
,
kinematicConstraintsMin
=
0.
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
...
...
@@ -91,7 +102,7 @@ acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
dir_goal
=
tp
.
ps
.
configAtParam
(
pId
,
tp
.
ps
.
pathLength
(
pId
)
-
eps
)[
tp
.
indexECS
:
tp
.
indexECS
+
3
]
acc_goal
=
[
0
,
0
,
0
]
robTreshold
=
1
robTreshold
=
0
# copy extraconfig for start and init configurations
q_init
[
configSize
:
configSize
+
3
]
=
dir_init
[::]
q_init
[
configSize
+
3
:
configSize
+
6
]
=
acc_init
[::]
...
...
@@ -99,10 +110,13 @@ q_goal[configSize:configSize+3] = dir_goal[::]
q_goal
[
configSize
+
3
:
configSize
+
6
]
=
[
0
,
0
,
0
]
q_init
[
2
]
=
q_init
[
2
]
+
0.02
q_goal
[
2
]
=
q_goal
[
2
]
+
0.02
#
q_init[2] = q_init[2]
+
0.02
#
q_goal[2] = q_goal[2]
+
0.02
q_init
[
2
]
=
0.86
q_goal
[
2
]
=
0.86
"""
# Randomly generating a contact configuration at q_init
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
...
...
@@ -114,13 +128,14 @@ r(q_init)
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
r(q_goal)
"""
# specifying the full body configurations as start and goal state of the problem
r
.
addLandmark
(
'hrp2_14/BODY'
,
0.3
)
#
r.addLandmark('hrp2_14/BODY',0.3)
r
(
q_init
)
fullBody
.
setStartState
(
q_init
,[
r
LegId
,
l
LegId
])
fullBody
.
setStartState
(
q_init
,[
l
LegId
,
r
LegId
])
fullBody
.
setEndState
(
q_goal
,[
rLegId
,
lLegId
])
fullBody
.
setStaticStability
(
True
)
# only set it after the init/goal configuration are computed
...
...
@@ -132,26 +147,26 @@ pp = PathPlayer (fullBody.client.basic, r)
import
fullBodyPlayerHrp2
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.1
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
)
tInterpolate
=
time
.
time
()
-
tStart
configs
=
fullBody
.
interpolate
(
0.
0
1
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
True
,
testReachability
=
False
,
quasiStatic
=
False
)
tInterpolate
Configs
=
time
.
time
()
-
tStart
print
"number of configs : "
,
len
(
configs
)
print
"generated in "
+
str
(
tInterpolate
)
+
" s"
r
(
configs
[
len
(
configs
)
-
2
])
print
"generated in "
+
str
(
tInterpolate
Configs
)
+
" s"
r
(
configs
[
-
1
])
player
=
fullBodyPlayerHrp2
.
Player
(
fullBody
,
pp
,
tp
,
configs
,
draw
=
False
,
use_window
=
1
,
optim_effector
=
True
,
use_velocity
=
False
,
pathId
=
pId
)
#
player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=1,optim_effector=True,use_velocity=False,pathId = pId)
# remove the last config (= user defined q_goal, not consitent with the previous state)
#r(configs[0])
#player.displayContactPlan()
beginState
=
2
endState
=
4
beginState
=
0
endState
=
len
(
configs
)
-
1
configs
=
configs
[
beginState
:
endState
+
1
]
#
configs=configs[beginState:endState+1]
from
planning.config
import
*
from
configs.darpa
import
*
from
generate_contact_sequence
import
*
cs
=
generateContactSequence
(
fullBody
,
configs
,
beginState
,
endState
,
r
)
filename
=
OUTPUT_DIR
+
"/"
+
OUTPUT_SEQUENCE_FILE
...
...
@@ -180,7 +195,7 @@ addVector(r,fullBody,r.color.red,vlb)
"""
wid
=
r
.
client
.
gui
.
getWindowID
(
"window_hpp_"
)
#
wid = r.client.gui.getWindowID("window_hpp_")
#r.client.gui.attachCameraToNode( 'hrp2_14/BODY_0',wid)
...
...
script/dynamic/darpa_line_hrp2_path.py
View file @
ed054fb4
...
...
@@ -6,7 +6,7 @@ from hpp.gepetto import Viewer
import
time
import
math
import
omniORB.any
from
planning.config
import
*
from
configs.darpa
import
*
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
...
...
@@ -50,11 +50,11 @@ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
#rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean'])
#rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
#rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
vMax
=
0.
2
;
aMax
=
0.
1
;
vMax
=
0.
1
;
aMax
=
0.
05
;
extraDof
=
6
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1.2
,
1.5
,
-
0.1
,
0.1
,
0.55
,
0.85
])
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
1.2
,
1.5
,
-
0.1
,
0.1
,
0.55
,
1.
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT0'
,[
-
0.05
,
0.05
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT1'
,[
-
0.05
,
0.05
])
# We also bound the rotations of the torso. (z, y, x)
...
...
@@ -76,7 +76,7 @@ ps.client.problem.setParameter("sizeFootX",omniORB.any.to_any(0.24))
ps
.
client
.
problem
.
setParameter
(
"sizeFootY"
,
omniORB
.
any
.
to_any
(
0.14
))
r
=
Viewer
(
ps
,
displayArrows
=
True
)
r
=
Viewer
(
ps
,
displayArrows
=
False
,
displayCoM
=
True
)
from
hpp.corbaserver.affordance.affordance
import
AffordanceTool
...
...
@@ -85,19 +85,21 @@ afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.005])
afftool
.
loadObstacleModel
(
packageName
,
ENV_NAME
,
ENV_PREFIX
,
r
,
reduceSizes
=
[
0.14
,
0
])
#r.loadObstacleModel (packageName, "ground", "planning")
afftool
.
visualiseAffordances
(
'Support'
,
r
,
[
0.25
,
0.5
,
0.5
])
r
.
addLandmark
(
r
.
sceneName
,
1
)
#
r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations
q_init
=
rbprmBuilder
.
getCurrentConfig
();
q_init
[
3
:
7
]
=
[
1
,
0
,
0
,
0
]
q_init
[
0
:
3
]
=
[
-
0.37
,
0
,
0.74
];
r
(
q_init
)
#q_init [0:3] = [-0.5, 0, 0.75]; r (q_init)
#q_init [0:3] =[-0.37, 0, 0.74]; r (q_init)
q_init
[
0
:
3
]
=
[
-
0.71
,
-
0.03
,
0.76
];
r
(
q_init
)
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
0.85
,
0
,
0.74
];
r
(
q_goal
)
#q_goal [0:3] = [1, 0, 0.75]; r(q_goal)
q_goal
[
0
:
3
]
=
[
1.12
,
-
0.03
,
0.76
];
r
(
q_goal
)
#q_goal [0:3] =[0.85, 0, 0.74]; r(q_goal)
r
(
q_goal
)
...
...
script/dynamic/downSlope_hrp2_loadPath.py
0 → 100644
View file @
ed054fb4
from
hpp.corbaserver.rbprm.rbprmbuilder
import
Builder
from
hpp.gepetto
import
Viewer
from
hpp.corbaserver
import
Client
from
hpp.corbaserver.robot
import
Robot
as
Parent
from
hpp.corbaserver.rbprm.problem_solver
import
ProblemSolver
import
omniORB.any
class
Robot
(
Parent
):
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName
=
'hrp2_trunk_flexible'
urdfSuffix
=
""
srdfSuffix
=
""
def
__init__
(
self
,
robotName
,
load
=
True
):
Parent
.
__init__
(
self
,
robotName
,
self
.
rootJointType
,
load
)
self
.
tf_root
=
"base_footprint"
self
.
client
.
basic
=
Client
()
self
.
load
=
load
rootJointType
=
'freeflyer'
packageName
=
'hpp-rbprm-corba'
meshPackageName
=
'hpp-rbprm-corba'
urdfName
=
'hrp2_trunk_flexible'
urdfNameRom
=
[
'hrp2_larm_rom'
,
'hrp2_rarm_rom'
,
'hrp2_lleg_rom'
,
'hrp2_rleg_rom'
]
urdfSuffix
=
""
srdfSuffix
=
""
vMax
=
4
;
aMax
=
6
;
extraDof
=
6
# Creating an instance of the helper class, and loading the robot
rbprmBuilder
=
Builder
()
rbprmBuilder
.
loadModel
(
urdfName
,
urdfNameRom
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1.25,2, -0.5, 5.5, 0.6, 1.8])
rbprmBuilder
.
setJointBounds
(
"base_joint_xyz"
,
[
-
2
,
4
,
0
,
2
,
0.2
,
1.8
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT0'
,[
0
,
0
])
rbprmBuilder
.
setJointBounds
(
'CHEST_JOINT1'
,[
-
0.35
,
0.1
])
rbprmBuilder
.
setJointBounds
(
'HEAD_JOINT0'
,[
0
,
0
])
rbprmBuilder
.
setJointBounds
(
'HEAD_JOINT1'
,[
0
,
0
])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder
.
setFilter
([
'hrp2_lleg_rom'
,
'hrp2_rleg_rom'
])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_lleg_rom'
,
[
'Support'
,])
rbprmBuilder
.
setAffordanceFilter
(
'hrp2_rleg_rom'
,
[
'Support'
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder
.
boundSO3
([
-
0.1
,
0.1
,
-
0.65
,
0.65
,
-
0.2
,
0.2
])
rbprmBuilder
.
client
.
basic
.
robot
.
setDimensionExtraConfigSpace
(
extraDof
)
rbprmBuilder
.
client
.
basic
.
robot
.
setExtraConfigSpaceBounds
([
-
4
,
4
,
-
1
,
1
,
-
2
,
2
,
0
,
0
,
0
,
0
,
0
,
0
])
indexECS
=
rbprmBuilder
.
getConfigSize
()
-
rbprmBuilder
.
client
.
basic
.
robot
.
getDimensionExtraConfigSpace
()
# Creating an instance of HPP problem solver and the viewer