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
e666fab0
Commit
e666fab0
authored
Nov 21, 2016
by
Steve Tonneau
Browse files
debugging python ocp
parent
ddb8acf2
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @
e666fab0
...
...
@@ -86,6 +86,7 @@ def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, li
COMConstraints
.
append
(
__get_com_constraint
(
fullBody
,
state_id
,
states
[
state_id
],
limbsCOMConstraints
,
True
))
if
(
len
(
p
)
>
len
(
COMConstraints
)):
COMConstraints
.
append
(
__get_com_constraint
(
fullBody
,
state_id
+
1
,
states
[
state_id
+
1
],
limbsCOMConstraints
))
print
'num cones '
,
len
(
cones
)
return
p
,
N
,
init_com
,
end_com
,
t_end_phases
,
cones
,
COMConstraints
...
...
@@ -95,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']
constraints
=
[
'end_reached_constraint'
]
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_constraint']
param_constraints
=
[]
mass
=
fullBody
.
getMass
()
...
...
@@ -152,12 +155,20 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
else
:
if
norm
(
np
.
array
(
var_final
[
'c'
][
-
1
])
-
np
.
array
(
__get_com
(
fullBody
,
states
[
state_id
+
1
])))
>
0.00001
:
print
"error in com desired: obtained "
,
__get_com
(
fullBody
,
states
[
state_id
+
1
]),
var_final
[
'c'
][
-
1
]
print
"trying to update com target... "
,
__get_com
(
fullBody
,
states
[
state_id
+
1
]),
var_final
[
'c'
][
-
1
]
if
(
fullBody
.
projectStateToCOM
(
state_id
+
1
,
(
var_final
[
'c'
][
-
1
]).
tolist
())):
#~ print "PROJECTED", init_end_com, var_final['c'][-1]
states
[
state_id
+
1
]
=
fullBody
.
getConfigAtState
(
state_id
+
1
)
#updating config from python side)
lastspeed
=
var_final
[
'dc'
][
-
1
]
print
"init speed"
,
lastspeed
else
:
raise
ValueError
(
"projection failed, this is bad"
)
lastspeed
=
np
.
array
([
0
,
0
,
0
])
return
var_final
,
params
,
timeelapsed
return
var_final
,
params
,
timeelapsed
,
cones
def
draw_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
=
False
,
mu
=
1
,
dt
=
0.2
,
phase_dt
=
[
0.4
,
1
],
reduce_ineq
=
True
,
verbose
=
False
,
limbsCOMConstraints
=
None
,
use_window
=
0
):
var_final
,
params
,
elapsed
=
gen_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
verbose
,
limbsCOMConstraints
,
False
,
use_window
=
use_window
)
var_final
,
params
,
elapsed
,
cones
=
gen_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
verbose
,
limbsCOMConstraints
,
False
,
use_window
=
use_window
)
p
,
N
=
fullBody
.
computeContactPoints
(
state_id
)
print
"p"
,
p
fig
=
plt
.
figure
()
...
...
@@ -230,7 +241,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
plt
.
show
()
plt
.
savefig
(
'/tmp/dL'
+
str
(
state_id
)
+
'.png'
)
return
var_final
,
params
,
elapsed
return
var_final
,
params
,
elapsed
,
cones
def
__cVarPerPhase
(
var
,
dt
,
t
,
final_state
,
addValue
):
varVals
=
addValue
+
[
v
.
tolist
()
for
v
in
final_state
[
var
]]
...
...
@@ -267,20 +278,20 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
#now compute com trajectorirs
comTrajIds
=
[
fullBody
.
generateComTraj
(
comPosPerPhase
[
i
],
comVelPerPhase
[
i
],
comAccPerPhase
[
i
],
dt
)
for
i
in
range
(
0
,
3
)]
return
comTrajIds
,
res
[
2
],
final_state
#res[2] is timeelapsed
return
comTrajIds
,
res
[
2
],
final_state
,
res
[
-
1
]
#res[2] is timeelapsed
, res[-1] is the cones
def
solve_com_RRT
(
fullBody
,
states
,
state_id
,
computeCones
=
False
,
mu
=
1
,
dt
=
0.1
,
phase_dt
=
[
0.4
,
1
],
reduce_ineq
=
True
,
num_optims
=
0
,
draw
=
False
,
verbose
=
False
,
limbsCOMConstraints
=
None
,
profile
=
False
,
use_window
=
0
,
trackedEffectors
=
[]):
comPosPerPhase
,
timeElapsed
,
final_state
=
__optim__threading_ok
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
comPosPerPhase
,
timeElapsed
,
final_state
,
cones
=
__optim__threading_ok
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
num_optims
,
draw
,
verbose
,
limbsCOMConstraints
,
profile
,
use_window
)
print
"done. generating state trajectory "
,
state_id
paths_ids
=
[
int
(
el
)
for
el
in
fullBody
.
comRRTFromPos
(
state_id
,
comPosPerPhase
[
0
],
comPosPerPhase
[
1
],
comPosPerPhase
[
2
],
num_optims
)]
print
"done. computing final trajectory to display "
,
state_id
,
"path ids "
,
paths_ids
[
-
1
],
" ,"
,
paths_ids
[:
-
1
]
return
paths_ids
[
-
1
],
paths_ids
[:
-
1
],
timeElapsed
,
final_state
return
paths_ids
[
-
1
],
paths_ids
[:
-
1
],
timeElapsed
,
final_state
,
cones
def
solve_effector_RRT
(
fullBody
,
states
,
state_id
,
computeCones
=
False
,
mu
=
1
,
dt
=
0.1
,
phase_dt
=
[
0.4
,
1
],
reduce_ineq
=
True
,
num_optims
=
0
,
draw
=
False
,
verbose
=
False
,
limbsCOMConstraints
=
None
,
profile
=
False
,
use_window
=
0
,
trackedEffectors
=
[]):
comPosPerPhase
,
timeElapsed
,
final_state
=
__optim__threading_ok
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
comPosPerPhase
,
timeElapsed
,
final_state
,
cones
=
__optim__threading_ok
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
num_optims
,
draw
,
verbose
,
limbsCOMConstraints
,
profile
,
use_window
)
print
"done. generating state trajectory "
,
state_id
if
(
len
(
trackedEffectors
)
==
0
):
...
...
@@ -290,5 +301,5 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
refPathId
=
trackedEffectors
[
0
];
path_start
=
trackedEffectors
[
1
];
path_to
=
trackedEffectors
[
2
];
effectorstracked
=
trackedEffectors
[
3
]
paths_ids
=
[
int
(
el
)
for
el
in
fullBody
.
effectorRRTFromPath
(
state_id
,
refPathId
,
path_start
,
path_to
,
comPosPerPhase
[
0
],
comPosPerPhase
[
1
],
comPosPerPhase
[
2
],
num_optims
,
effectorstracked
)]
print
"done. computing final trajectory to display "
,
state_id
,
"path ids "
,
paths_ids
[
-
1
],
" ,"
,
paths_ids
[:
-
1
]
return
paths_ids
[
-
1
],
paths_ids
[:
-
1
],
timeElapsed
,
final_state
return
paths_ids
[
-
1
],
paths_ids
[:
-
1
],
timeElapsed
,
final_state
,
cones
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @
e666fab0
...
...
@@ -9,12 +9,13 @@ from numpy import append, array
res
=
[]
trajec
=
[]
trajec_mil
=
[]
#~
contacts = []
contacts
=
[]
pos
=
[]
normals
=
[]
pEffs
=
[]
coms
=
[]
errorid
=
[]
cones_saved
=
[]
def
displayComPath
(
pp
,
pathId
,
color
=
[
0.
,
0.75
,
0.15
,
0.9
])
:
pathPos
=
[]
...
...
@@ -32,13 +33,14 @@ def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
pp
.
publisher
.
client
.
gui
.
refresh
()
def
genPandNperFrame
(
fullBody
,
stateid
,
limbsCOMConstraints
,
pp
,
path_ids
,
times
,
dt_framerate
=
1.
/
24.
):
def
genPandN
andCones
perFrame
(
fullBody
,
stateid
,
limbsCOMConstraints
,
cones
,
pp
,
path_ids
,
times
,
dt_framerate
=
1.
/
24.
):
p
,
N
=
fullBody
.
computeContactPointsPerLimb
(
stateid
,
limbsCOMConstraints
.
keys
(),
limbsCOMConstraints
)
freeEffectors
=
[
[
limbsCOMConstraints
[
limb
][
'effector'
]
for
limb
in
limbsCOMConstraints
.
keys
()
if
not
p
[
i
].
has_key
(
limbsCOMConstraints
[
limb
][
'effector'
])]
for
i
in
range
(
len
(
p
))]
config_size
=
len
(
fullBody
.
getCurrentConfig
())
interpassed
=
False
pRes
=
[]
nRes
=
[]
cRes
=
[]
for
idx
,
path_id
in
enumerate
(
path_ids
):
length
=
pp
.
client
.
problem
.
pathLength
(
path_id
)
num_frames_required
=
times
[
idx
]
/
dt_framerate
...
...
@@ -48,7 +50,8 @@ def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times
dt_finals
=
[
dt
*
i
for
i
in
range
(
int
(
round
(
num_frames_required
)))]
pRes
+=
[
p
[
idx
]
for
t
in
dt_finals
]
nRes
+=
[
N
[
idx
]
for
t
in
dt_finals
]
return
pRes
,
nRes
,
freeEffectors
cRes
+=
[
cones
[
idx
]
for
t
in
dt_finals
]
return
pRes
,
nRes
,
freeEffectors
,
cRes
def
__getPos
(
effector
,
fullBody
,
config
):
...
...
@@ -148,9 +151,9 @@ trackedEffectors = []):
else
:
comC
=
None
if
(
optim_effectors
):
pid
,
trajectory
,
timeelapsed
,
final_state
=
solve_effector_RRT
(
fullBody
,
configs
,
i
,
True
,
friction
,
dt
,
times
,
False
,
optim
,
draw
,
verbose
,
comC
,
False
,
use_window
=
use_window
,
trackedEffectors
=
trackedEffectors
)
pid
,
trajectory
,
timeelapsed
,
final_state
,
cones
=
solve_effector_RRT
(
fullBody
,
configs
,
i
,
True
,
friction
,
dt
,
times
,
False
,
optim
,
draw
,
verbose
,
comC
,
False
,
use_window
=
use_window
,
trackedEffectors
=
trackedEffectors
)
else
:
pid
,
trajectory
,
timeelapsed
,
final_state
=
solve_com_RRT
(
fullBody
,
configs
,
i
,
True
,
friction
,
dt
,
times
,
False
,
optim
,
draw
,
verbose
,
comC
,
False
,
use_window
=
use_window
,
trackedEffectors
=
trackedEffectors
)
pid
,
trajectory
,
timeelapsed
,
final_state
,
cones
=
solve_com_RRT
(
fullBody
,
configs
,
i
,
True
,
friction
,
dt
,
times
,
False
,
optim
,
draw
,
verbose
,
comC
,
False
,
use_window
=
use_window
,
trackedEffectors
=
trackedEffectors
)
displayComPath
(
pp
,
pid
)
#~ pp(pid)
global
res
...
...
@@ -159,15 +162,16 @@ trackedEffectors = []):
global
trajec_mil
frame_rate
=
1.
/
24.
#~ frame_rate_andrea = 1./1000.
frame_rate_andrea
=
1.
/
1000.
#~ if(len(trajec) > 0):
#~ frame_rate = 1./25.
#~ frame_rate_andrea = 1./1001.
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)
new_traj_andrea
=
gen_trajectory_to_play
(
fullBody
,
pp
,
trajectory
,
time_per_path
,
frame_rate_andrea
)
#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
#~
Ps, Ns, freeEffectorsPerPhase = genPandNperFrame(fullBody, i, limbsCOMConstraints, 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)
Ps
,
Ns
,
freeEffectorsPerPhase
,
Ks
=
genPandN
andCones
perFrame
(
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
)
#~ if(len(trajec) > 0):
#~ new_traj = new_traj[1:]
#~ new_traj_andrea = new_traj_andrea[1:]
...
...
@@ -176,19 +180,21 @@ trackedEffectors = []):
#~ com = com[1:]
#~ NPeffs = NPeffs[1:]
trajec
=
trajec
+
new_traj
#~
trajec_mil += new_traj_andrea
trajec_mil
+=
new_traj_andrea
#~ global contacts
#~ contacts += new_contacts
#~ 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)
#~ assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(coms) == len(pEffs))
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"
...
...
@@ -261,7 +267,8 @@ trackedEffectors = []):
#~ stat_data["num_errors"] += 1
#~ errorid += [i]
#~ fail+=1
return
fail
#~ return fail
return
final_state
,
cones
def
step_profile
(
fullBody
,
configs
,
i
,
optim
,
limbsCOMConstraints
,
friction
=
0.5
,
optim_effectors
=
True
,
time_scale
=
20.
,
useCOMConstraints
=
False
):
global
errorid
...
...
@@ -353,6 +360,7 @@ from pickle import dump
def
compressData
(
data_array
,
filename
):
qs
=
[
data
[
'q'
][:]
for
data
in
data_array
]
C
=
[
data
[
'C'
][:]
for
data
in
data_array
]
cones
=
[
data
[
'cone'
][:]
for
data
in
data_array
]
a
=
{}
frameswitches
=
[]
for
i
in
range
(
0
,
len
(
pos
)):
...
...
@@ -364,6 +372,7 @@ def compressData(data_array, filename):
res
=
{}
res
[
'Q'
]
=
[
data
[
'q'
][:]
for
data
in
data_array
]
res
[
'C'
]
=
[
data
[
'C'
][:]
for
data
in
data_array
]
res
[
'cones'
]
=
[
data
[
'cone'
][:]
for
data
in
data_array
]
res
[
'fly'
]
=
pEffs
res
[
'frameswitches'
]
=
frameswitches
f1
=
open
(
filename
+
"_compressed"
,
'w+'
)
...
...
@@ -379,7 +388,7 @@ def saveToPinocchio(filename):
quat_end
=
q
[
4
:
7
]
q
[
6
]
=
q
[
3
]
q
[
3
:
6
]
=
quat_end
data
=
{
'q'
:
q
,
'P'
:
pos
[
i
],
'N'
:
normals
[
i
],
'C'
:
coms
[
i
],
'pEffs'
:
pEffs
[
i
]}
data
=
{
'q'
:
q
,
'P'
:
pos
[
i
],
'N'
:
normals
[
i
],
'C'
:
coms
[
i
],
'pEffs'
:
pEffs
[
i
]
,
'cone'
:
cones_saved
[
i
]
}
res
+=
[
data
]
f1
=
open
(
filename
,
'w+'
)
dump
(
res
,
f1
)
...
...
@@ -396,10 +405,12 @@ def clean():
global
normals
global
pEffs
global
coms
global
cones_saved
cones_saved
=
[]
res
=
[]
trajec
=
[]
trajec_mil
=
[]
contacts
=
[]
#~
contacts = []
errorid
=
[]
pos
=
[]
normals
=
[]
...
...
@@ -440,6 +451,7 @@ def profile(fullBody, configs, i_start, i_end, limbsCOMConstraints, friction =
write_stats
(
filename
)
def
saveAllData
(
fullBody
,
r
,
name
):
global
trajec
fullBody
.
exportAll
(
r
,
trajec
,
name
)
return
saveToPinocchio
(
name
)
...
...
src/rbprmbuilder.impl.cc
View file @
e666fab0
...
...
@@ -1198,6 +1198,7 @@ namespace hpp {
success
=
true
;
return
intermediary
;
}
std
::
cout
<<
"no contact break during intermediary phase "
<<
std
::
endl
;
return
firstState
;
}
...
...
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