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
4357c47d
Commit
4357c47d
authored
Dec 13, 2016
by
Steve Tonneau
Browse files
trying to make darpa scenario work
parent
3f6633d0
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/scenarios/darpa_hyq_interp.py
View file @
4357c47d
...
...
@@ -99,9 +99,11 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
r
(
q_init
)
# computing the contact sequence
configs
=
fullBody
.
interpolate
(
0.12
,
10
,
10
,
True
)
#~ configs = fullBody.interpolate(0.12, 10, 5, True)
#~ configs = fullBody.interpolate(0.08, 10, 10, True)
#~ configs = fullBody.interpolate(0.11, 7, 10, True)
#~ configs = fullBody.interpolate(0.1, 1, 5, True)
#~ configs = fullBody.interpolate(0.1, 1, 10, True)
configs
=
fullBody
.
interpolate
(
0.04
,
1
,
10
,
True
)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
...
...
@@ -139,3 +141,74 @@ def saveAll(name):
#~ fullBody.exportAll(r, trajec, 'darpa_hyq_t_var_04f_andrea');
#~ saveToPinocchio('darpa_hyq_t_var_04f_andrea')
gui
=
r
.
client
.
gui
scene
=
"oddct"
r
.
client
.
gui
.
createScene
(
scene
)
resolution
=
0.03
i
=
0
boxname
=
scene
+
"/b"
+
str
(
i
)
gui
.
addBox
(
boxname
,
resolution
,
resolution
,
resolution
,
[
1
,
1
,
1
,
1
])
gui
.
applyConfiguration
(
boxname
,[
0
,
0
,
0
,
1
,
0
,
0
,
0
])
gui
.
addSceneToWindow
(
scene
,
0
)
gui
.
refresh
()
import
hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
as
cwc_trajectory_helper
import
time
def
applycom
():
global
gui
global
com
c
=
fullBody
.
getCenterOfMass
()
gui
.
applyConfiguration
(
boxname
,[
c
[
0
],
c
[
1
],
0
,
1
,
0
,
0
,
0
])
gui
.
refresh
()
def
go
(
dt_framerate
=
1.
/
24.
):
path_player
=
pp
configs
=
cwc_trajectory_helper
.
trajec
for
q
in
configs
:
start
=
time
.
time
()
pp
.
publisher
.
robotConfig
=
q
pp
.
publisher
.
publishRobots
()
elapsed
=
time
.
time
()
-
start
applycom
()
if
elapsed
<
dt_framerate
:
time
.
sleep
(
dt_framerate
-
elapsed
)
#~ for i in range(9,14):
#~ act(i,verbose=True, use_window=0, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(14,16):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(16,17):
#~ act(i,verbose=True, use_window=2, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(17,19):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(19,22):
#~ act(i,verbose=True, use_window=1, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ for i in range(23,24):
#~ act(i,verbose=True, use_window=2, numOptim=5, optim_effectors=False, draw=False);go()
#~ saveAll("test"+str(i));
#~ 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
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
28
,
30
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
False
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
30
,
42
):
act
(
i
,
verbose
=
True
,
use_window
=
1
,
numOptim
=
5
,
optim_effectors
=
False
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
for
i
in
range
(
32
,
83
):
act
(
i
,
verbose
=
True
,
use_window
=
0
,
numOptim
=
5
,
optim_effectors
=
False
,
draw
=
False
);
go
()
saveAll
(
"test"
+
str
(
i
));
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @
4357c47d
...
...
@@ -96,7 +96,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
use_window
=
max
(
0
,
min
(
use_window
,
(
len
(
states
)
-
1
)
-
(
state_id
+
2
)))
# can't use preview if last state is reached
assert
(
len
(
phase_dt
)
>=
2
+
use_window
*
2
),
"phase_dt does not describe all phases"
constraints
=
[
'cones_constraint'
,
'end_reached_constraint'
,
'end_speed_constraint'
]
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint', 'com_kinematic_constraint']
constraints
=
[
'cones_constraint'
,
'end_reached_constraint'
]
#~ constraints = ['end_reached_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint']
#~ constraints = ['cones_constraint']
...
...
@@ -108,6 +110,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
if
(
use_window
>
0
):
init_waypoint_time
=
int
(
np
.
round
(
t_end_phases
[
-
1
]
/
dt
))
-
1
init_end_com
=
end_com
[:]
constraints
=
[
'cones_constraint'
,
'end_reached_constraint'
,
'end_speed_constraint'
]
for
w
in
range
(
1
,
use_window
+
1
):
waypoint
=
end_com
[:]
waypoint_time
=
int
(
np
.
round
(
t_end_phases
[
-
1
]
/
dt
))
-
1
...
...
@@ -164,7 +167,9 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
print
"init speed"
,
lastspeed
else
:
raise
ValueError
(
"projection failed, this is bad"
)
lastspeed
=
np
.
array
([
0
,
0
,
0
])
#~ lastspeed = np.array([0,0,0])
lastspeed
=
var_final
[
'dc'
][
-
1
]
print
"end speed"
,
lastspeed
return
var_final
,
params
,
timeelapsed
,
cones
...
...
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @
4357c47d
...
...
@@ -162,6 +162,7 @@ trackedEffectors = []):
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
)
...
...
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