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
3ae78599
Commit
3ae78599
authored
Jul 24, 2019
by
Joseph Mirabel
Browse files
Fix compatibility with Python 3.
parent
8d158892
Changes
8
Hide whitespace changes
Inline
Side-by-side
src/hpp/corbaserver/rbprm/__init__.py
View file @
3ae78599
from
client
import
Client
from
.
client
import
Client
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
3ae78599
...
...
@@ -407,7 +407,7 @@ class FullBody (Robot):
Ns
.
append
({})
if
(
len
(
P
[
i
])
>
0
):
targetName
=
limb
if
(
dicEffector
.
has_key
(
limb
)
):
if
(
limb
in
dicEffector
):
targetName
=
dicEffector
[
limb
][
'effector'
]
Ps
[
i
][
targetName
]
=
P
[
i
]
Ns
[
i
][
targetName
]
=
N
[
i
]
...
...
@@ -829,7 +829,7 @@ class FullBody (Robot):
# \param viewer gepetto viewer instance
def
draw
(
self
,
configuration
,
viewer
):
viewer
(
configuration
)
for
limb
,
groupid
in
self
.
octrees
.
iter
items
():
for
limb
,
groupid
in
self
.
octrees
.
items
():
transform
=
self
.
clientRbprm
.
rbprm
.
getOctreeTransform
(
limb
,
configuration
)
viewer
.
client
.
gui
.
applyConfiguration
(
groupid
,
transform
)
viewer
.
client
.
gui
.
refresh
()
...
...
src/hpp/corbaserver/rbprm/state_alg.py
View file @
3ae78599
...
...
@@ -16,6 +16,7 @@
# hpp-manipulation-corba. If not, see
# <http://www.gnu.org/licenses/>.
from
__future__
import
print_function
from
hpp.corbaserver.rbprm.rbprmstate
import
State
from
lp_find_point
import
find_valid_c_cwc
,
find_valid_c_cwc_qp
,
lp_ineq_4D
from
hpp.corbaserver.rbprm.tools.com_constraints
import
*
...
...
@@ -53,7 +54,7 @@ def isContactReachable(state, limbName, p, n, limbsCOMConstraints):
res_ineq
=
[
np
.
vstack
([
new_ineq
[
0
],
active_ineq
[
0
]]),
np
.
hstack
([
new_ineq
[
1
],
active_ineq
[
1
]])]
success
,
status_ok
,
res
=
lp_ineq_4D
(
res_ineq
[
0
],
-
res_ineq
[
1
])
if
not
success
:
print
"In isContactReachable no stability, Lp failed (should not happen) "
,
status_ok
print
(
"In isContactReachable no stability, Lp failed (should not happen) "
,
status_ok
)
return
False
,
[
-
1
,
-
1
,
-
1
]
return
(
res
[
3
]
>=
0
),
res
[
0
:
3
]
...
...
@@ -134,8 +135,8 @@ def projectToFeasibleCom(state, ddc =[0.,0.,0.], max_num_samples = 10, friction
ps
=
state
.
getContactPosAndNormals
()
p
=
ps
[
0
][
0
]
N
=
ps
[
1
][
0
]
print
"p"
,
p
print
"N"
,
N
print
(
"p"
,
p
)
print
(
"N"
,
N
)
#~ try:
H
=
compute_CWC
(
p
,
N
,
state
.
fullBody
.
client
.
basic
.
robot
.
getMass
(),
mu
=
friction
,
simplify_cones
=
False
)
c_ref
=
state
.
getCenterOfMass
()
...
...
@@ -150,12 +151,12 @@ def projectToFeasibleCom(state, ddc =[0.,0.,0.], max_num_samples = 10, friction
x
[
2
]
+=
0.35
for
i
in
range
(
10
):
if
state
.
fullBody
.
projectStateToCOM
(
state
.
sId
,
x
,
max_num_samples
):
print
"success after "
+
str
(
i
)
+
" trials"
print
(
"success after "
+
str
(
i
)
+
" trials"
)
return
True
else
:
x
[
2
]
-=
0.05
else
:
print
"qp failed"
print
(
"qp failed"
)
return
False
;
def
isContactCreated
(
s1
,
s2
):
...
...
src/hpp/corbaserver/rbprm/tools/com_constraints.py
View file @
3ae78599
from
__future__
import
print_function
import
matplotlib
#~ matplotlib.use('Agg')
import
matplotlib.pyplot
as
plt
from
mpl_toolkits.mplot3d
import
Axes3D
from
cwc
import
cone_optimization
from
obj_to_constraints
import
ineq_from_file
,
rotate_inequalities
from
.
obj_to_constraints
import
ineq_from_file
,
rotate_inequalities
import
numpy
as
np
import
math
from
numpy.linalg
import
norm
...
...
@@ -45,8 +46,8 @@ def get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = Fa
As
=
[];
bs
=
[]
fullBody
.
setCurrentConfig
(
config
)
contacts
=
[]
for
i
,
v
in
limbsCOMConstraints
.
iter
items
():
if
not
constraintsComLoaded
.
has_key
(
i
)
:
for
i
,
v
in
limbsCOMConstraints
.
items
():
if
i
not
in
constraintsComLoaded
:
constraintsComLoaded
[
i
]
=
ineq_from_file
(
ineqPath
+
v
[
'file'
])
contact
=
(
interm
and
fullBody
.
isLimbInContactIntermediary
(
i
,
state
))
or
(
not
interm
and
fullBody
.
isLimbInContact
(
i
,
state
))
if
(
contact
):
...
...
@@ -65,12 +66,12 @@ def get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = Fa
if
(
len
(
As
)
>
0
):
return
[
np
.
vstack
(
As
),
np
.
hstack
(
bs
)]
else
:
print
"Warning: no active contact in get_com_constraint"
print
(
"Warning: no active contact in get_com_constraint"
)
return
[
np
.
zeros
([
3
,
3
]),
np
.
zeros
(
3
)]
def
get_com_constraint_at_transform
(
pos_quat
,
limb
,
limbsCOMConstraints
):
global
constraintsLoaded
if
not
constraintsComLoaded
.
has_key
(
limb
)
:
if
limb
not
in
constraintsComLoaded
:
constraintsComLoaded
[
limb
]
=
ineq_from_file
(
ineqPath
+
limbsCOMConstraints
[
limb
][
'file'
])
tr
=
quaternion_matrix
(
pos_quat
[
3
:
7
])
tr
[:
3
,
3
]
=
np
.
array
(
pos_quat
[
0
:
3
])
...
...
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @
3ae78599
from
__future__
import
print_function
import
matplotlib
#~ matplotlib.use('Agg')
import
matplotlib.pyplot
as
plt
from
mpl_toolkits.mplot3d
import
Axes3D
from
cwc
import
cone_optimization
from
obj_to_constraints
import
ineq_from_file
,
rotate_inequalities
from
.
obj_to_constraints
import
ineq_from_file
,
rotate_inequalities
import
numpy
as
np
import
math
from
numpy.linalg
import
norm
from
com_constraints
import
get_com_constraint
from
.
com_constraints
import
get_com_constraint
import
time
import
hpp.corbaserver.rbprm.data.com_inequalities
as
ine
...
...
@@ -43,7 +44,7 @@ lastspeed = np.array([0,0,0])
def
compute_state_info
(
fullBody
,
states
,
state_id
,
phase_dt
,
mu
,
computeCones
,
limbsCOMConstraints
,
pathId
=
0
):
print
"phase dt in compute_state_info:"
,
phase_dt
print
(
"phase dt in compute_state_info:"
,
phase_dt
)
init_com
=
__get_com
(
fullBody
,
states
[
state_id
])
end_com
=
__get_com
(
fullBody
,
states
[
state_id
+
1
])
p
,
N
=
fullBody
.
computeContactPoints
(
state_id
)
...
...
@@ -70,7 +71,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
)
print
(
'num cones '
,
len
(
cones
)
)
return
p
,
N
,
init_com
,
end_com
,
t_end_phases
,
cones
,
COMConstraints
def
compute_initial_guess
(
fullBody
,
t_end_phases
,
pathId
,
state_id
,
dt
=
0.01
):
...
...
@@ -117,8 +118,8 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
t_end_phases
+=
[
t_end_phases
[
-
1
]
+
t
for
t
in
t_end_phases1
[
1
:]]
initial_guess
=
compute_initial_guess
(
fullBody
,
t_end_phases
,
pathId
,
state_id
,
dt
)
if
(
not
profile
):
print
"num cones "
,
len
(
cones
)
print
"end_phases"
,
t_end_phases
print
(
"num cones "
,
len
(
cones
)
)
print
(
"end_phases"
,
t_end_phases
)
timeelapsed
=
0
if
(
profile
):
...
...
@@ -129,10 +130,10 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
else
:
init_vel
=
[
0
,
0
,
0
]
end_vel
=
[
0
,
0
,
0
]
print
"init_vel ="
print
init_vel
print
"end_vel = "
print
end_vel
print
(
"init_vel ="
)
print
(
init_vel
)
print
(
"end_vel = "
)
print
(
end_vel
)
var_final
,
params
=
cone_optimization
(
p
,
N
,
[
init_com
+
init_vel
,
end_com
+
end_vel
],
t_end_phases
[
1
:],
dt
,
cones
,
COMConstraints
,
mu
,
mass
,
9.81
,
reduce_ineq
,
verbose
,
constraints
,
param_constraints
,
initial_guess
=
initial_guess
)
#~ print "end_com ", end_com , "computed end come", var_final['c'][-1], var_final['c_end']
...
...
@@ -148,20 +149,20 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
var_final
[
'dc'
]
=
var_final
[
'dc'
][:
init_waypoint_time
+
1
]
var_final
[
'ddc'
]
=
var_final
[
'ddc'
][:
init_waypoint_time
+
1
]
params
[
"t_init_phases"
]
=
params
[
"t_init_phases"
][:
4
]
print
"trying to project on com (from, to) "
,
init_end_com
,
var_final
[
'c'
][
-
1
]
print
(
"trying to project on com (from, to) "
,
init_end_com
,
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'
][
init_waypoint_time
]
print
"init speed"
,
lastspeed
print
(
"init speed"
,
lastspeed
)
else
:
use_window
=
0
print
"reached com is not good, restarting problem with window = "
,
use_window
print
(
"reached com is not good, restarting problem with window = "
,
use_window
)
return
gen_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
verbose
,
limbsCOMConstraints
,
profile
,
use_window
=
use_window
,
pathId
=
pathId
)
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
"restarting problem with windows = "
,
use_window
+
1
print
(
"error in com desired: obtained "
,
__get_com
(
fullBody
,
states
[
state_id
+
1
]),
var_final
[
'c'
][
-
1
]
)
print
(
"restarting problem with windows = "
,
use_window
+
1
)
return
gen_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
verbose
,
limbsCOMConstraints
,
profile
,
use_window
+
1
,
use_velocity
,
pathId
)
lastspeed
=
np
.
array
([
0
,
0
,
0
])
...
...
@@ -194,8 +195,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
#plt.show()
plt
.
savefig
(
'/tmp/figCWC/c'
+
str
(
state_id
)
+
'.png'
)
print
"plotting speed "
print
"end target "
,
params
[
'x_end'
]
print
(
"plotting speed "
)
print
(
"end target "
,
params
[
'x_end'
]
)
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
)
if
(
use_window
>
0
):
...
...
@@ -213,7 +214,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
#~ plt.show()
plt
.
savefig
(
'/tmp/figCWC/dc'
+
str
(
state_id
)
+
'.png'
)
print
"plotting acceleration "
print
(
"plotting acceleration "
)
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
)
if
(
use_window
>
0
):
...
...
@@ -229,7 +230,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
#~ plt.show()
plt
.
savefig
(
'/tmp/figCWC/ddc'
+
str
(
state_id
)
+
'.png'
)
print
"plotting Dl "
print
(
"plotting Dl "
)
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
)
points
=
var_final
[
'dL'
]
...
...
@@ -244,7 +245,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
def
__cVarPerPhase
(
var
,
dt
,
t
,
final_state
,
addValue
):
varVals
=
addValue
+
[
v
.
tolist
()
for
v
in
final_state
[
var
]]
print
"cVarPerPhase : t = "
,
t
print
(
"cVarPerPhase : t = "
,
t
)
varPerPhase
=
[[
varVals
[(
int
)(
round
(
t_id
/
dt
))
]
for
t_id
in
np
.
arange
(
t
[
index
],
t
[
index
+
1
]
-
_EPS
,
dt
)]
for
index
,
_
in
enumerate
(
t
[:
-
1
])
]
#print "varperPhase ="
#print varPerPhase
...
...
@@ -264,15 +265,15 @@ def __cVarPerPhase(var, dt, t, final_state, addValue):
def
__optim__threading_ok
(
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
,
use_velocity
=
False
,
pathId
=
0
):
print
"callgin gen "
,
state_id
print
(
"callgin gen "
,
state_id
)
if
(
draw
):
res
=
draw_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
verbose
,
limbsCOMConstraints
,
use_window
,
use_velocity
,
pathId
)
else
:
res
=
gen_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
verbose
,
limbsCOMConstraints
,
profile
,
use_window
,
use_velocity
,
pathId
)
alpha
=
res
[
1
][
'alpha'
]
print
"t in optim_threading :"
,
res
[
1
][
"t_init_phases"
]
print
(
"t in optim_threading :"
,
res
[
1
][
"t_init_phases"
]
)
t
=
[
ti
*
alpha
for
ti
in
res
[
1
][
"t_init_phases"
]]
print
"t after alpha in optim_threading :"
,
t
print
(
"t after alpha in optim_threading :"
,
t
)
dt
=
res
[
1
][
"dt"
]
*
alpha
final_state
=
res
[
0
]
c0
=
res
[
1
][
"x_init"
][
0
:
3
]
...
...
@@ -289,22 +290,22 @@ def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =
reduce_ineq
=
True
,
num_optims
=
0
,
draw
=
False
,
verbose
=
False
,
limbsCOMConstraints
=
None
,
profile
=
False
,
use_window
=
0
,
trackedEffectors
=
[],
use_velocity
=
False
,
pathId
=
0
):
comPosPerPhase
,
timeElapsed
,
final_state
=
__optim__threading_ok
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
num_optims
,
draw
,
verbose
,
limbsCOMConstraints
,
profile
,
use_window
,
use_velocity
,
pathId
)
print
"done. generating state trajectory "
,
state_id
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
]
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
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
=
[],
use_velocity
=
False
,
pathId
=
0
):
comPosPerPhase
,
timeElapsed
,
final_state
=
__optim__threading_ok
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
num_optims
,
draw
,
verbose
,
limbsCOMConstraints
,
profile
,
use_window
,
use_velocity
,
pathId
)
print
"done. generating state trajectory "
,
state_id
print
(
"done. generating state trajectory "
,
state_id
)
if
(
len
(
trackedEffectors
)
==
0
):
paths_ids
=
[
int
(
el
)
for
el
in
fullBody
.
effectorRRT
(
state_id
,
comPosPerPhase
[
0
],
comPosPerPhase
[
1
],
comPosPerPhase
[
2
],
num_optims
)]
else
:
print
"handling extra effector constraints"
print
(
"handling extra effector constraints"
)
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
]
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
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @
3ae78599
from
__future__
import
print_function
from
hpp.corbaserver.rbprm.tools.cwc_trajectory
import
*
from
hpp.corbaserver.rbprm.tools.path_to_trajectory
import
*
from
cwc
import
OptimError
,
cone_optimization
...
...
@@ -35,7 +35,7 @@ def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
def
genPandNperFrame
(
fullBody
,
stateid
,
limbsCOMConstraints
,
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
))]
freeEffectors
=
[
[
limbsCOMConstraints
[
limb
][
'effector'
]
for
limb
in
limbsCOMConstraints
.
keys
()
if
limbsCOMConstraints
[
limb
][
'effector'
]
not
in
p
[
i
]
]
for
i
in
range
(
len
(
p
))]
config_size
=
len
(
fullBody
.
getCurrentConfig
())
interpassed
=
False
pRes
=
[]
...
...
@@ -127,9 +127,9 @@ def __getTimes(fullBody, configs, i, time_scale):
def
__getTimes
(
fullBody
,
configs
,
i
,
time_scale
,
use_window
=
0
):
t
=
fullBody
.
getTimeAtState
(
i
+
1
)
-
fullBody
.
getTimeAtState
(
i
)
dt
=
0.02
print
"t = "
,
t
print
(
"t = "
,
t
)
t
=
time_scale
*
t
print
"after scale, t = "
,
t
print
(
"after scale, t = "
,
t
)
trunk_distance
=
np
.
linalg
.
norm
(
np
.
array
(
configs
[
i
+
1
][
0
:
3
])
-
np
.
array
(
configs
[
i
][
0
:
3
]))
distance
=
max
(
fullBody
.
getEffectorDistance
(
i
,
i
+
1
),
trunk_distance
)
# TODO : si t = 0, hardcoded ...
...
...
@@ -146,7 +146,7 @@ def __getTimes(fullBody, configs, i, time_scale,use_window=0):
"""
times
[
1
]
=
t
-
2
*
times
[
0
]
times
[
1
]
=
float
((
int
)(
math
.
floor
(
times
[
1
]
*
100.
)))
/
100.
print
"times : "
,
times
print
(
"times : "
,
times
)
return
times
,
dt
,
distance
,
use_window
...
...
@@ -154,30 +154,30 @@ 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
=
[],
use_velocity
=
False
,
pathId
=
0
):
print
"##########################################"
print
(
"##########################################"
)
global
errorid
global
stat_data
fail
=
0
#~ try:
print
"Use window = "
,
use_window
print
(
"Use window = "
,
use_window
)
if
(
True
):
times
=
[];
dt
=
1000
;
times
,
dt
,
distance
,
use_window
=
__getTimes
(
fullBody
,
configs
,
i
,
time_scale
,
use_window
)
print
"Use window = "
,
use_window
print
(
"Use window = "
,
use_window
)
if
distance
==
0
:
use_window
=
use_window
+
1
use_window
=
max
(
0
,
min
(
use_window
,
(
len
(
configs
)
-
1
)
-
(
i
+
2
)))
# can't use preview if last state is reached
w
=
0
while
w
<
(
use_window
+
1
):
times2
,
dt2
,
dist2
,
use_window
=
__getTimes
(
fullBody
,
configs
,
i
+
w
,
time_scale
,
use_window
)
print
"Use window = "
,
use_window
print
(
"Use window = "
,
use_window
)
times
+=
times2
dt
=
min
(
dt
,
dt2
)
w
=
w
+
1
time_per_path
=
[
times
[
0
]]
+
[
times
[
1
]]
+
[
times
[
0
]]
print
'time per path'
,
times
,
time_per_path
print
'dt'
,
dt
print
(
'time per path'
,
times
,
time_per_path
)
print
(
'dt'
,
dt
)
if
(
distance
>
0.0001
):
stat_data
[
"num_trials"
]
+=
1
if
(
useCOMConstraints
):
...
...
@@ -200,9 +200,9 @@ trackedEffectors = [],use_velocity=False,pathId = 0):
#~ if(len(trajec) > 0):
#~ frame_rate = 1./25.
#~ frame_rate_andrea = 1./1001.
print
"first traj :"
print
(
"first traj :"
)
new_traj
=
gen_trajectory_to_play
(
fullBody
,
pp
,
trajectory
,
time_per_path
,
frame_rate
)
print
"traj Andrea : "
print
(
"traj 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
)
...
...
@@ -231,7 +231,7 @@ trackedEffectors = [],use_velocity=False,pathId = 0):
#assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(coms) == len(pEffs))
stat_data
[
"num_success"
]
+=
1
else
:
print
"TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
print
(
"TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
)
#~ except hpperr as e:
#~ print "hpperr failed at id " + str(i) , e.strerror
#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
...
...
@@ -335,27 +335,27 @@ def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0
__update_cwc_time
(
timeelapsed
)
stat_data
[
"num_success"
]
+=
1
else
:
print
"TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
print
(
"TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
)
except
hpperr
as
e
:
print
"hpperr failed at id "
+
str
(
i
)
,
e
.
strerror
print
(
"hpperr failed at id "
+
str
(
i
)
,
e
.
strerror
)
stat_data
[
"error_com_proj"
]
+=
1
stat_data
[
"num_errors"
]
+=
1
errorid
+=
[
i
]
fail
+=
1
except
OptimError
as
e
:
print
"OptimError failed at id "
+
str
(
i
)
,
e
.
strerror
print
(
"OptimError failed at id "
+
str
(
i
)
,
e
.
strerror
)
stat_data
[
"error_optim_fail"
]
+=
1
stat_data
[
"num_errors"
]
+=
1
errorid
+=
[
i
]
fail
+=
1
except
ValueError
as
e
:
print
"ValueError failed at id "
+
str
(
i
)
,
e
print
(
"ValueError failed at id "
+
str
(
i
)
,
e
)
stat_data
[
"error_unknown"
]
+=
1
stat_data
[
"num_errors"
]
+=
1
errorid
+=
[
i
]
fail
+=
1
except
IndexError
as
e
:
print
"IndexError failed at id "
+
str
(
i
)
,
e
print
(
"IndexError failed at id "
+
str
(
i
)
,
e
)
stat_data
[
"error_unknown"
]
+=
1
stat_data
[
"num_errors"
]
+=
1
errorid
+=
[
i
]
...
...
@@ -363,7 +363,7 @@ def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0
except
Exception
as
e
:
stat_data
[
"error_unknown"
]
+=
1
stat_data
[
"num_errors"
]
+=
1
print
e
print
(
e
)
errorid
+=
[
i
]
fail
+=
1
except
:
...
...
@@ -472,7 +472,7 @@ def write_stats(filename):
def
profile
(
fullBody
,
configs
,
i_start
,
i_end
,
limbsCOMConstraints
,
friction
=
0.5
,
optim_effectors
=
False
,
time_scale
=
20.
,
useCOMConstraints
=
False
,
filename
=
"log.txt"
):
global
stat_data
if
(
i_end
>
len
(
configs
)
-
1
):
print
"ERROR: i_end < len_configs "
,
i_end
,
len
(
configs
)
-
1
print
(
"ERROR: i_end < len_configs "
,
i_end
,
len
(
configs
)
-
1
)
return
# no point in trying optim, path was not fully computed
for
i
in
range
(
i_start
,
i_end
):
step_profile
(
fullBody
,
configs
,
i
,
0
,
limbsCOMConstraints
,
friction
,
optim_effectors
,
time_scale
,
useCOMConstraints
)
...
...
src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
View file @
3ae78599
__24fps
=
1.
/
24.
__EPS
=
0.00001
from
__future__
import
print_function
from
numpy.linalg
import
norm
def
__linear_interpolation
(
p0
,
p1
,
dist_p0_p1
,
val
):
...
...
@@ -29,7 +30,7 @@ def __find_q_t(robot, path_player, path_id, t):
q
=
pp
.
client
.
problem
.
configAtParam
(
path_id
,
u
)
current_t
=
q
[
-
1
]
if
(
a
>=
b
):
print
"ERROR, a > b, t does not exist"
print
(
"ERROR, a > b, t does not exist"
)
if
abs
(
current_t
-
t
)
<
__EPS
:
#print "last config q = ",q[-1]
return
q
[:
-
1
]
...
...
@@ -50,7 +51,7 @@ def linear_interpolate_path(robot, path_player, path_id, total_time, dt_framerat
def
follow_trajectory_path
(
robot
,
path_player
,
path_id
,
total_time
,
dt_framerate
=
__24fps
):
pp
=
path_player
print
"total_times in follow path : "
,
total_time
print
(
"total_times in follow path : "
,
total_time
)
length
=
pp
.
client
.
problem
.
pathLength
(
path_id
)
num_frames_required
=
total_time
/
dt_framerate
dt
=
float
(
length
)
/
num_frames_required
...
...
@@ -60,12 +61,12 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
return
[
__find_q_t
(
robot
,
path_player
,
path_id
,
t
)
for
t
in
dt_finals
]
def
gen_trajectory_to_play
(
robot
,
path_player
,
path_ids
,
total_time_per_paths
,
dt_framerate
=
__24fps
):
print
"gen_trajectory : dt = "
,
dt_framerate
print
(
"gen_trajectory : dt = "
,
dt_framerate
)
config_size
=
len
(
robot
.
getCurrentConfig
())
res
=
[]
pp
=
path_player
activeid
=
0
print
"path_ids = "
,
path_ids
print
(
"path_ids = "
,
path_ids
)
for
i
,
path_id
in
enumerate
(
path_ids
):
config_size_path
=
len
(
path_player
.
client
.
problem
.
configAtParam
(
int
(
path_id
),
0
))
if
(
config_size_path
>
config_size
):
...
...
src/hpp/corbaserver/rbprm/tools/time_out.py
View file @
3ae78599
from
__future__
import
print_function
import
signal
# Register an handler for the timeout
...
...
@@ -19,17 +20,17 @@ if __name__ == '__main__':
def
loop
(
dt1
,
dt2
):
import
time
for
i
in
range
(
dt1
+
dt2
):
print
"sec"
print
(
"sec"
)
time
.
sleep
(
1
)
try
:
run_time_out
(
loop
,
3
,
3
,
2
)
except
Exception
,
exc
:
print
exc
print
"exception caught, ok"
except
Exception
as
exc
:
print
(
exc
)
print
(
"exception caught, ok"
)
try
:
run_time_out
(
loop
,
6
,
3
,
2
)
except
Exception
,
exc
:
print
exc
print
"exception should NOT becaught"
except
Exception
as
exc
:
print
(
exc
)
print
(
"exception should NOT becaught"
)
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