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
6a1df7bf
Commit
6a1df7bf
authored
Feb 02, 2017
by
Steve Tonneau
Browse files
updated demos
parent
916029ca
Changes
7
Hide whitespace changes
Inline
Side-by-side
script/scenarios/darpa_hyq_interp.py
View file @
6a1df7bf
...
...
@@ -58,27 +58,27 @@ lLegId = 'lhleg'
rarmId
=
'rhleg'
larmId
=
'lfleg'
addLimbDb
(
rLegId
,
"static"
)
addLimbDb
(
lLegId
,
"static"
)
addLimbDb
(
rarmId
,
"static"
)
addLimbDb
(
larmId
,
"static"
)
#~
addLimbDb(rLegId, "static")
#~
addLimbDb(lLegId, "static")
#~
addLimbDb(rarmId, "static")
#~
addLimbDb(larmId, "static")
#~
fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
fullBody
.
addLimb
(
rLegId
,
rLeg
,
rfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"jointlimits"
,
0.1
,
cType
)
lLegId
=
'lhleg'
lLeg
=
'lh_haa_joint'
lfoot
=
'lh_foot_joint'
#~
fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
fullBody
.
addLimb
(
lLegId
,
lLeg
,
lfoot
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"jointlimits"
,
0.05
,
cType
)
#~
rarmId
=
'rhleg'
rarm
=
'rh_haa_joint'
rHand
=
'rh_foot_joint'
#~
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"jointlimits"
,
0.05
,
cType
)
#~
larmId
=
'lfleg'
larm
=
'lf_haa_joint'
lHand
=
'lf_foot_joint'
#~
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
offset
,
normal
,
legx
,
legy
,
nbSamples
,
"jointlimits"
,
0.05
,
cType
)
q_0
=
fullBody
.
getCurrentConfig
();
q_init
=
fullBody
.
getCurrentConfig
();
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
]
...
...
@@ -198,17 +198,23 @@ def go(dt_framerate=1./24.):
#~ for i in range(24,26):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
for
i
in
range
(
16
,
28
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
False
,
draw
=
False
);
go
()
for
i
in
range
(
14
,
19
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
19
,
22
):
act
(
i
,
verbose
=
True
,
use_window
=
1
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
22
,
28
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
True
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
28
,
30
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
Fals
e
,
draw
=
False
);
go
()
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
Tru
e
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
30
,
42
):
act
(
i
,
verbose
=
True
,
use_window
=
1
,
numOptim
=
5
,
optim_effectors
=
Fals
e
,
draw
=
False
);
go
()
act
(
i
,
verbose
=
True
,
use_window
=
1
,
numOptim
=
5
,
optim_effectors
=
Tru
e
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
32
,
83
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
Fals
e
,
draw
=
False
);
go
()
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
Tru
e
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
script/scenarios/demos/ground_crouch_hyq_interp.py
View file @
6a1df7bf
...
...
@@ -172,19 +172,7 @@ def contactPlan():
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
for
i
in
range
(
1
,
len
(
configs
)):
r
(
configs
[
i
]);
time
.
sleep
(
0.5
)
def
interpolate
():
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk"
,
"OFF"
)
for
i
in
range
(
7
,
20
):
act
(
i
,
1
,
optim_effectors
=
True
)
def
play
(
frame_rate
=
1.
/
24.
):
play_traj
(
fullBody
,
pp
,
frame_rate
)
time
.
sleep
(
0.5
)
def
a
():
...
...
@@ -207,13 +195,5 @@ def e():
print
"displaying contact plan"
contactPlan
()
def
f
():
print
"computing feasible com trajectory"
interpolate
()
def
g
():
print
"playing feasible trajectory"
play
()
print
"Root path generated in "
+
str
(
tp
.
t
)
+
" ms."
script/scenarios/polaris/poralis_hrp2_interp.py
View file @
6a1df7bf
...
...
@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "man
rarmId
=
'hrp2_rarm_rom'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
-
0.0
5
,
-
0.0
50
,
-
0.05
0
]
rArmOffset
=
[
-
0.0
40
,
-
0.0
1
,
-
0.0
8
5
]
rArmNormal
=
[
1
,
0
,
0
]
rArmx
=
0.024
;
rArmy
=
0.024
fullBody
.
addLimb
(
rarmId
,
rarm
,
rHand
,
rArmOffset
,
rArmNormal
,
rArmx
,
rArmy
,
20000
,
"EFORT"
,
0.05
,
"_6_DOF"
,
True
)
...
...
@@ -51,7 +51,7 @@ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "
larmId
=
'hrp2_larm_rom'
larm
=
'LARM_JOINT0'
lHand
=
'LARM_JOINT5'
lArmOffset
=
[
-
0.0
5
,
-
0.0
50
,
-
0.05
0
]
lArmOffset
=
[
-
0.0
40
,
0.0
1
,
-
0.0
8
5
]
lArmNormal
=
[
1
,
0
,
0
]
lArmx
=
0.024
;
lArmy
=
0.024
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
lArmOffset
,
lArmNormal
,
lArmx
,
lArmy
,
20000
,
"EFORT"
,
0.05
,
"_6_DOF"
,
True
)
...
...
@@ -158,5 +158,5 @@ def saveAll(name):
saveAllData
(
fullBody
,
r
,
name
)
for
i
in
range
(
10
,
25
):
act
(
i
,
numOptim
=
60
,
use_window
=
0
,
friction
=
1
,
optim_effectors
=
True
,
verbose
=
False
,
draw
=
False
);
saveAll
(
'polarisTestAndrea'
)
#~
for i in range(10,25): act(i,numOptim = 60, use_window = 0, friction = 1, optim_effectors = True, verbose = False, draw = False);
#~
saveAll('polarisTestAndrea')
script/scenarios/polaris/poralis_hrp2_interp_2.py
View file @
6a1df7bf
...
...
@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "man
rarmId
=
'hrp2_rarm_rom'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
-
0.05
,
-
0.0
50
,
-
0.05
0
]
rArmOffset
=
[
-
0.0
4
5
,
-
0.0
1
,
-
0.0
8
5
]
rArmNormal
=
[
1
,
0
,
0
]
rArmx
=
0.024
;
rArmy
=
0.024
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
...
...
@@ -51,7 +51,7 @@ rArmx = 0.024; rArmy = 0.024
larmId
=
'hrp2_larm_rom'
larm
=
'LARM_JOINT0'
lHand
=
'LARM_JOINT5'
lArmOffset
=
[
-
0.05
,
-
0.0
50
,
-
0.05
0
]
lArmOffset
=
[
-
0.0
4
5
,
0.0
1
,
-
0.0
8
5
]
lArmNormal
=
[
1
,
0
,
0
]
lArmx
=
0.024
;
lArmy
=
0.024
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
lArmOffset
,
lArmNormal
,
lArmx
,
lArmy
,
20000
,
"EFORT"
,
0.05
,
"_6_DOF"
,
True
)
...
...
script/scenarios/polaris/poralis_hrp2_interp_3.py
View file @
6a1df7bf
...
...
@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "sta
rarmId
=
'hrp2_rarm_rom'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
-
0.05
,
-
0.0
50
,
-
0.05
0
]
rArmOffset
=
[
-
0.0
4
5
,
-
0.0
1
,
-
0.0
8
5
]
rArmNormal
=
[
1
,
0
,
0
]
rArmx
=
0.024
;
rArmy
=
0.024
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
...
...
@@ -51,10 +51,10 @@ rArmx = 0.024; rArmy = 0.024
larmId
=
'hrp2_larm_rom'
larm
=
'LARM_JOINT0'
lHand
=
'LARM_JOINT5'
lArmOffset
=
[
-
0.05
,
-
0.0
50
,
-
0.05
0
]
lArmOffset
=
[
-
0.0
4
5
,
0.0
1
,
-
0.0
8
5
]
lArmNormal
=
[
1
,
0
,
0
]
lArmx
=
0.024
;
lArmy
=
0.024
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
lArmOffset
,
lArmNormal
,
lArmx
,
lArmy
,
20000
,
"
static
"
,
0.05
,
"_6_DOF"
,
True
)
fullBody
.
addLimb
(
larmId
,
larm
,
lHand
,
lArmOffset
,
lArmNormal
,
lArmx
,
lArmy
,
20000
,
"
EFORT
"
,
0.05
,
"_6_DOF"
,
True
)
#~
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"jointLimitsDistance"
,
True
)
...
...
script/scenarios/polaris/poralis_hrp2_interp_4.py
View file @
6a1df7bf
...
...
@@ -43,7 +43,7 @@ fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "sta
rarmId
=
'hrp2_rarm_rom'
rarm
=
'RARM_JOINT0'
rHand
=
'RARM_JOINT5'
rArmOffset
=
[
-
0.05
,
-
0.0
50
,
-
0.05
0
]
rArmOffset
=
[
-
0.0
4
5
,
-
0.0
1
,
-
0.0
8
5
]
rArmNormal
=
[
1
,
0
,
0
]
rArmx
=
0.024
;
rArmy
=
0.024
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", True)
...
...
@@ -51,10 +51,10 @@ rArmx = 0.024; rArmy = 0.024
larmId
=
'hrp2_larm_rom'
larm
=
'LARM_JOINT0'
lHand
=
'LARM_JOINT5'
lArmOffset
=
[
-
0.05
,
-
0.0
50
,
-
0.05
0
]
lArmOffset
=
[
-
0.0
4
5
,
0.0
1
,
-
0.0
8
5
]
lArmNormal
=
[
1
,
0
,
0
]
lArmx
=
0.024
;
lArmy
=
0.024
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "
static
", 0.05, "_6_DOF", True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "
EFORT
", 0.05, "_6_DOF", True)
#~
fullBody
.
runLimbSampleAnalysis
(
rLegId
,
"jointLimitsDistance"
,
True
)
...
...
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @
6a1df7bf
...
...
@@ -126,8 +126,8 @@ def __getTimes(fullBody, configs, i, time_scale):
from
hpp
import
Error
as
hpperr
import
sys
,
time
def
step
(
fullBody
,
configs
,
i
,
optim
,
pp
,
limbsCOMConstraints
,
friction
=
0.5
,
optim_effectors
=
True
,
time_scale
=
20.
,
useCOMConstraints
=
False
,
use_window
=
0
,
verbose
=
False
,
draw
=
False
,
trackedEffectors
=
[]):
def
step
(
fullBody
,
configs
,
i
,
optim
,
pp
,
limbsCOMConstraints
,
friction
=
0.5
,
optim_effectors
=
True
,
time_scale
=
20.
,
useCOMConstraints
=
False
,
use_window
=
0
,
verbose
=
False
,
draw
=
False
,
trackedEffectors
=
[]
,
saveForSim
=
False
,
):
global
errorid
global
stat_data
fail
=
0
...
...
@@ -161,29 +161,29 @@ trackedEffectors = []):
global
trajec
global
trajec_mil
frame_rate
=
1.
/
24.
frame_rate_andrea
=
1.
/
1000.
#~ frame_rate_andrea = 1./10.
new_traj
=
gen_trajectory_to_play
(
fullBody
,
pp
,
trajectory
,
time_per_path
,
frame_rate
)
new_traj_andrea
=
gen_trajectory_to_play
(
fullBody
,
pp
,
trajectory
,
time_per_path
,
frame_rate_andrea
)
Ps
,
Ns
,
freeEffectorsPerPhase
,
Ks
=
genPandNandConesperFrame
(
fullBody
,
i
,
limbsCOMConstraints
,
cones
,
pp
,
trajectory
,
time_per_path
,
frame_rate_andrea
)
NPeffs
=
genPEffperFrame
(
fullBody
,
freeEffectorsPerPhase
,
new_traj_andrea
,
pp
,
time_per_path
,
frame_rate_andrea
)
com
=
genComPerFrame
(
final_state
,
dt
,
frame_rate_andrea
)
trajec
=
trajec
+
new_traj
trajec_mil
+=
new_traj_andrea
#~ global contacts
#~ contacts += new_contacts
global
cones_saved
cones_saved
+=
Ks
global
pos
pos
+=
Ps
global
normals
normals
+=
Ns
global
pEffs
pEffs
+=
NPeffs
global
coms
coms
+=
com
print
len
(
trajec_mil
),
" "
,
len
(
pos
),
" "
,
len
(
normals
),
" "
,
len
(
coms
),
" "
,
len
(
pEffs
),
" "
,
len
(
cones_saved
)
assert
(
len
(
trajec_mil
)
==
len
(
pos
)
and
len
(
normals
)
==
len
(
pos
)
and
len
(
normals
)
==
len
(
coms
)
and
len
(
cones_saved
)
==
len
(
coms
)
and
len
(
coms
)
==
len
(
pEffs
))
if
(
saveForSim
):
frame_rate_sim
=
1.
/
1000.
new_traj_andrea
=
gen_trajectory_to_play
(
fullBody
,
pp
,
trajectory
,
time_per_path
,
frame_rate_sim
)
Ps
,
Ns
,
freeEffectorsPerPhase
,
Ks
=
genPandNandConesperFrame
(
fullBody
,
i
,
limbsCOMConstraints
,
cones
,
pp
,
trajectory
,
time_per_path
,
frame_rate_andrea
)
NPeffs
=
genPEffperFrame
(
fullBody
,
freeEffectorsPerPhase
,
new_traj_andrea
,
pp
,
time_per_path
,
frame_rate_andrea
)
com
=
genComPerFrame
(
final_state
,
dt
,
frame_rate_andrea
)
trajec_mil
+=
new_traj_andrea
#~ global contacts
#~ contacts += new_contacts
global
cones_saved
cones_saved
+=
Ks
global
pos
pos
+=
Ps
global
normals
normals
+=
Ns
global
pEffs
pEffs
+=
NPeffs
global
coms
coms
+=
com
print
len
(
trajec_mil
),
" "
,
len
(
pos
),
" "
,
len
(
normals
),
" "
,
len
(
coms
),
" "
,
len
(
pEffs
),
" "
,
len
(
cones_saved
)
assert
(
len
(
trajec_mil
)
==
len
(
pos
)
and
len
(
normals
)
==
len
(
pos
)
and
len
(
normals
)
==
len
(
coms
)
and
len
(
cones_saved
)
==
len
(
coms
)
and
len
(
coms
)
==
len
(
pEffs
))
stat_data
[
"num_success"
]
+=
1
else
:
print
"TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
...
...
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