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
ecab1d26
Commit
ecab1d26
authored
Nov 08, 2016
by
Steve Tonneau
Browse files
handling fall in prediction
parent
9dc2c37f
Changes
2
Hide whitespace changes
Inline
Side-by-side
script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py
View file @
ecab1d26
...
...
@@ -198,7 +198,7 @@ def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto):
for
k
,
v
in
res
[
"contact_points"
].
iteritems
():
if
k
!=
effector
:
contact_points
[
k
]
=
v
success
,
dc
,
c_final
,
v0
=
scv
.
com
PosA
fter
07s
(
c
,
res
[
"q"
],
contact_points
)
success
,
dc
,
c_final
,
v0
=
scv
.
com
_pos_a
fter
_t
(
c
,
res
[
"q"
],
contact_points
)
effector_data
=
{}
state_id
=
fullBody
.
createState
(
config_gepetto
,
maintained_limbs
)
if
(
success
and
fullBody
.
projectStateToCOM
(
state_id
,
c_final
.
tolist
())):
#all good, all contacts kinematically maintained):
...
...
script/tools/tro_capture_point/sample_com_vel.py
View file @
ecab1d26
...
...
@@ -133,7 +133,7 @@ def gen_com_vel(q_hpp, contacts):
return
(
success
,
v
[:],
invDynForm
.
J_com
*
v
);
def
com
PosA
fter
07s
(
c
,
q_hpp
,
contacts
,
v
=
None
):
def
com
_pos_a
fter
_t
(
c
,
q_hpp
,
contacts
,
v
=
None
,
t
=
0.7
):
q0
=
q_pin
(
q_hpp
)
init
(
q0
);
v0
=
mat_zeros
(
nv
);
...
...
@@ -160,10 +160,11 @@ def comPosAfter07s(c, q_hpp, contacts, v = None):
P
=
np
.
matlib
.
copy
(
invDynForm
.
contact_points
);
N
=
np
.
matlib
.
copy
(
invDynForm
.
contact_normals
);
stab_criterion
=
StabilityCriterion
(
"default"
,
invDynForm
.
x_com
,
dx_com_des
,
P
.
T
,
N
.
T
,
conf
.
mu
[
0
],
np
.
array
([
0
,
0
,
-
9.81
]),
invDynForm
.
M
[
0
,
0
])
res
=
stab_criterion
.
predict_future_state
(
0.7
,
c_init
,
invDynForm
.
J_com
*
v
)
res
=
stab_criterion
.
predict_future_state
(
t
,
c_init
,
invDynForm
.
J_com
*
v
)
#TODO : res.t != 0.7
print
"c "
,
res
.
c
print
"dc "
,
res
.
dc
return
success
,
res
.
dc
,
res
.
c
,
v0
return
success
and
abs
(
res
.
t
-
t
)
<
EPS
,
res
.
dc
,
res
.
c
,
v0
np
.
set_printoptions
(
precision
=
2
,
suppress
=
True
);
...
...
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