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
d655e334
Commit
d655e334
authored
Apr 11, 2018
by
Pierre Fernbach
Browse files
[demo] scripts for hrp-darpa OK
parent
81d90313
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/dynamic/darpa_hrp2_interStatic.py
View file @
d655e334
...
...
@@ -21,7 +21,7 @@ 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
))
...
...
@@ -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
)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
''
,
rLegOffset
,
rLegNormal
,
rLegx
,
rLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
limbOffset
=
rLegLimbOffset
,
kinematicConstraintsMin
=
0.3
)
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
)
fullBody
.
addLimb
(
lLegId
,
lLeg
,
''
,
lLegOffset
,
rLegNormal
,
lLegx
,
lLegy
,
100000
,
"fixedStep06"
,
0.01
,
"_6_DOF"
,
limbOffset
=
lLegLimbOffset
,
kinematicConstraintsMin
=
0.3
)
fullBody
.
runLimbSampleAnalysis
(
lLegId
,
"ReferenceConfiguration"
,
True
)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
...
...
@@ -131,7 +131,7 @@ pp = PathPlayer (fullBody.client.basic, r)
import
fullBodyPlayerHrp2
tStart
=
time
.
time
()
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
Fals
e
)
configs
=
fullBody
.
interpolate
(
0.01
,
pathId
=
pId
,
robustnessTreshold
=
robTreshold
,
filterStates
=
Tru
e
)
tInterpolate
=
time
.
time
()
-
tStart
print
"number of configs : "
,
len
(
configs
)
print
"generated in "
+
str
(
tInterpolate
)
+
" s"
...
...
@@ -143,20 +143,23 @@ player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=
# remove the last config (= user defined q_goal, not consitent with the previous state)
#r(configs[0])
#player.displayContactPlan()
player
.
displayContactPlan
()
"""
from planning.configs.darpa import *
from generate_contact_sequence import *
beginState = 0
endState = len(configs)-1
cs = generateContactSequence(fullBody,configs,beginState, endState,r)
"""
"""
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs.saveAsXML(filename, "ContactSequence")
print "save contact sequence : ",filename
"""
"""
r(q_init)
...
...
@@ -179,7 +182,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_hrp2_path.py
View file @
d655e334
...
...
@@ -90,13 +90,13 @@ 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.5
8
];
r
(
q_init
)
q_init
[
0
:
3
]
=
[
-
0.8
,
0
,
0.5
3
];
r
(
q_init
)
#q_init [0:3] = [-0.5, 0, 0.75]; r (q_init)
rbprmBuilder
.
setCurrentConfig
(
q_init
)
q_goal
=
q_init
[::]
q_goal
[
0
:
3
]
=
[
1.2
,
0
,
0.5
8
];
r
(
q_goal
)
q_goal
[
0
:
3
]
=
[
1.2
,
0
,
0.5
3
];
r
(
q_goal
)
#q_goal [0:3] = [1, 0, 0.75]; r(q_goal)
r
(
q_goal
)
...
...
@@ -107,9 +107,9 @@ r (q_goal)
q1
=
q_init
[::]
q1
[
0
:
3
]
=
[
-
0.
45
,
0
,
0.
8
]
q1
[
0
:
3
]
=
[
-
0.
3
,
0
,
0.
77
]
q2
=
q_goal
[::]
q2
[
0
:
3
]
=
[
0.
9
,
0
,
0.
8
]
q2
[
0
:
3
]
=
[
0.
8
,
0
,
0.
77
]
ps
.
setInitialConfig
(
q1
)
ps
.
addGoalConfig
(
q2
)
...
...
script/tools/effectorRRT_bezier.py
0 → 100644
View file @
d655e334
from
darpa_hrp2_interStatic
import
*
from
disp_bezier
import
*
from
hpp.corbaserver.rbprm.tools.path_to_trajectory
import
*
#fullBody.client.basic.robot.setDimensionExtraConfigSpace(7)
total_paths_ids
=
[]
time_per_paths
=
[]
for
stateId
in
range
(
len
(
configs
)
-
2
):
# generate COM traj between two adjacent states
pidCOM
=
fullBody
.
isDynamicallyReachableFromState
(
stateId
,
stateId
+
1
,
True
)
showPath
(
r
,
pp
,
pidCOM
)
# call effector-rrt with com traj : pid[1:3]
paths_ids
=
fullBody
.
effectorRRTFromPosBetweenState
(
stateId
,
stateId
+
1
,
int
(
pidCOM
[
1
]),
int
(
pidCOM
[
2
]),
int
(
pidCOM
[
3
]),
1
)
total_paths_ids
+=
paths_ids
[:
-
1
]
time_per_paths
+=
[
ps
.
pathLength
(
int
(
pidCOM
[
1
]))]
time_per_paths
+=
[
ps
.
pathLength
(
int
(
pidCOM
[
2
]))]
time_per_paths
+=
[
ps
.
pathLength
(
int
(
pidCOM
[
3
]))]
trajectory
=
gen_trajectory_to_play
(
fullBody
,
pp
,
total_paths_ids
,
time_per_paths
)
play_trajectory
(
fullBody
,
pp
,
trajectory
)
"""
fullBody.client.basic.robot.setDimensionExtraConfigSpace(7)
pp.displayPath(8,jointName=rfoot)
"""
\ No newline at end of file
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