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
20ae730b
Commit
20ae730b
authored
Nov 25, 2016
by
Steve Tonneau
Browse files
generate also unstable configurations
parent
c2cdb581
Changes
1
Hide whitespace changes
Inline
Side-by-side
script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py
View file @
20ae730b
...
...
@@ -318,18 +318,20 @@ def _boundSO3(q, num_limbs):
return
q
def
_genbalance
(
limbs
):
def
_genbalance
(
limbs
,
unstable
):
for
i
in
range
(
10000
):
q
=
fullBody
.
client
.
basic
.
robot
.
shootRandomConfig
()
q
=
_boundSO3
(
q
,
len
(
limbs
))
if
fullBody
.
isConfigValid
(
q
)[
0
]
and
fullBody
.
isConfigBalanced
(
q
,
limbs
,
5
)
and
__loosely_z_aligned
(
limbs
[
0
],
q
)
and
__loosely_z_aligned
(
limbs
[
1
],
q
):
if
fullBody
.
isConfigValid
(
q
)[
0
]
and
__loosely_z_aligned
(
limbs
[
0
],
q
)
and
__loosely_z_aligned
(
limbs
[
1
],
q
):
if
(
unstable
and
not
fullBody
.
isConfigBalanced
(
q
,
limbs
,
5
))
or
(
not
unstable
and
fullBody
.
isConfigBalanced
(
q
,
limbs
,
5
)):
#~ if fullBody.isConfigValid(q)[0] and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
return
q
return
q
print
"can't generate equilibrium config"
all_qs
=
[]
all_states
=
[]
def
gen
(
limbs
,
num_samples
=
1000
,
coplanar
=
True
,
num_candidates_per_config
=
0
,
num_contact_candidates
=
10
,
q_entries
=
None
,
projectToObstacles
=
False
):
all_data
=
[]
def
gen
(
limbs
,
num_samples
=
1000
,
coplanar
=
True
,
num_candidates_per_config
=
0
,
num_contact_candidates
=
10
,
q_entries
=
None
,
projectToObstacles
=
False
,
unstable
=
False
):
q_0
=
fullBody
.
getCurrentConfig
();
#~ fullBody.getSampleConfig()
qs
=
[];
qs_gepetto
=
[];
states
=
[]
...
...
@@ -343,7 +345,7 @@ def gen(limbs, num_samples = 1000, coplanar = True, num_candidates_per_config =
if
(
coplanar
):
q
=
fullBody
.
generateGroundContact
(
limbs
)
else
:
q
=
_genbalance
(
limbs
)
q
=
_genbalance
(
limbs
,
unstable
)
else
:
q
=
q_entries
[
i
]
q_gep
=
q
[:]
...
...
@@ -366,7 +368,10 @@ def gen(limbs, num_samples = 1000, coplanar = True, num_candidates_per_config =
fname
+=
"configs"
if
(
coplanar
):
fname
+=
"_coplanar"
if
(
unstable
):
fname
+=
"_unstable"
data
[
"samples"
]
=
states
all_data
.
append
(
data
)
from
pickle
import
dump
#~ f1=open("configs_feet_on_ground_static_eq", 'w+')
f1
=
open
(
fname
,
'w+'
)
...
...
@@ -386,14 +391,15 @@ q_init = [
0.0
,
0.0
,
-
0.453785606
,
0.872664626
,
-
0.41887902
,
0.0
,
# RLEG 31-36
];
r
(
q_init
)
limbs
=
[[
lLegId
,
rLegId
],[
lLegId
,
rLegId
,
rarmId
],
[
lLegId
,
rLegId
,
larmId
],
[
lLegId
,
rLegId
,
rarmId
,
larmId
]
]
#~ limbs = [[lLegId,rLegId, rarmId]]
#~ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLegId,rLegId, rarmId, larmId] ]
#~ limbs = [[lLegId,rLegId] ]
limbs
=
[[
lLegId
,
rLegId
,
rarmId
]]
#~ limbs = [[larmId, rarmId]]
gen
(
limbs
[
0
],
10
)
#~
gen(limbs[0], 10
00
)
for
ls
in
limbs
:
gen
(
ls
,
10
,
False
)
gen
(
limbs
[
0
],
10
)
gen
(
ls
,
10
00
,
False
,
unstable
=
True
)
gen
(
limbs
[
0
],
10
00
,
unstable
=
True
)
i
=
0
a
=
None
...
...
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