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
3030735b
Commit
3030735b
authored
Sep 03, 2020
by
Pierre Fernbach
Browse files
[Tools] surface from path: add optionnal argument to consider all limbs at each steps
parent
e68c9c73
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/hpp/corbaserver/rbprm/tools/surfaces_from_path.py
View file @
3030735b
...
...
@@ -62,18 +62,33 @@ def getRotationMatrixFromConfigs(configs):
#print "R in getRotationMatrixFromConfigs : ",R
return
R
def
getALlContactsNames
(
rbprmBuilder
,
q
):
step_contacts
=
[]
for
limb
in
rbprmBuilder
.
urdfNameRom
:
step_contacts
+=
rbprmBuilder
.
clientRbprm
.
rbprm
.
getCollidingObstacleAtConfig
(
q
,
limb
)
return
step_contacts
# get contacted surface names at configuration
def
getContactsNames
(
rbprmBuilder
,
i
,
q
):
def
getContactsNames
(
rbprmBuilder
,
i
,
q
,
use_all_limbs
=
False
):
if
use_all_limbs
:
return
getALlContactsNames
(
rbprmBuilder
,
q
)
if
i
%
2
==
LF
:
# left leg
step_contacts
=
rbprmBuilder
.
clientRbprm
.
rbprm
.
getCollidingObstacleAtConfig
(
q
,
rbprmBuilder
.
lLegId
)
elif
i
%
2
==
RF
:
# right leg
step_contacts
=
rbprmBuilder
.
clientRbprm
.
rbprm
.
getCollidingObstacleAtConfig
(
q
,
rbprmBuilder
.
rLegId
)
return
step_contacts
def
getALlContactsIntersections
(
rbprmBuilder
,
q
):
intersections
=
[]
for
limb
in
rbprmBuilder
.
urdfNameRom
:
intersections
+=
rbprmBuilder
.
getContactSurfacesAtConfig
(
q
,
limb
)
return
intersections
# get intersections with the rom and surface at configuration
def
getContactsIntersections
(
rbprmBuilder
,
i
,
q
):
def
getContactsIntersections
(
rbprmBuilder
,
i
,
q
,
use_all_limbs
=
False
):
if
use_all_limbs
:
return
getALlContactsIntersections
(
rbprmBuilder
,
q
)
if
i
%
2
==
LF
:
# left leg
intersections
=
rbprmBuilder
.
getContactSurfacesAtConfig
(
q
,
rbprmBuilder
.
lLegId
)
elif
i
%
2
==
RF
:
# right leg
...
...
@@ -118,7 +133,8 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
useIntersection
=
False
,
mergeCandidates
=
False
,
max_yaw
=
0.5
,
max_surface_area
=
MAX_SURFACE
):
max_surface_area
=
MAX_SURFACE
,
use_all_limbs
=
False
):
pathLength
=
ps
.
pathLength
(
pId
)
#length of the path
discretizationStep
=
0.01
# step at which we check the colliding surfaces
#print "path length = ",pathLength
...
...
@@ -152,24 +168,42 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,
t
+=
0.0001
q
=
ps
.
configAtParam
(
pId
,
t
)
#print " t in getSurfacesFromGuideContinuous : ",t
step_contacts
=
getContactsNames
(
rbprmBuilder
,
i
,
q
)
step_contacts
=
getContactsNames
(
rbprmBuilder
,
i
,
q
,
use_all_limbs
)
for
contact_name
in
step_contacts
:
if
not
contact_name
in
phase_contacts_names
:
phase_contacts_names
.
append
(
contact_name
)
# end current phase
# get all the surfaces from the names and add it to seqs:
if
useIntersection
:
intersections
=
getContactsIntersections
(
rbprmBuilder
,
i
,
q
)
phase_surfaces
=
[]
if
useIntersection
:
intersections
=
getContactsIntersections
(
rbprmBuilder
,
i
,
q
,
use_all_limbs
)
# First add intersection for the surfaces in contact at the last discretization step
for
k
,
name
in
enumerate
(
step_contacts
):
surface
=
surfaces_dict
[
name
][
0
]
# [0] because the last vector contain the normal of the surface
if
area
(
surface
)
>
max_surface_area
:
intersection
=
intersections
[
k
]
if
len
(
intersection
)
>
3
:
phase_surfaces
.
append
(
intersection
)
else
:
phase_surfaces
.
append
(
surfaces_dict
[
name
][
0
])
# then add possible small surfaces encountered during the discretization of the path
for
name
in
phase_contacts_names
:
if
name
not
in
step_contacts
:
phase_surfaces
.
append
(
surfaces_dict
[
name
][
0
])
else
:
# add all the surfaces encountered during the path
for
name
in
phase_contacts_names
:
phase_surfaces
.
append
(
surfaces_dict
[
name
][
0
])
for
name
in
phase_contacts_names
:
surface
=
surfaces_dict
[
name
][
0
]
surface
=
surfaces_dict
[
name
][
0
]
# [0] because the last vector contain the normal of the surface
if
useIntersection
and
area
(
surface
)
>
max_surface_area
:
if
name
in
step_contacts
:
intersection
=
intersections
[
step_contacts
.
index
(
name
)]
if
len
(
intersection
)
>
3
:
phase_surfaces
.
append
(
intersection
)
else
:
phase_surfaces
.
append
(
surface
)
# [0] because the last vector contain the normal of the surface
phase_surfaces
.
append
(
surface
)
#print "There was "+str(len(phase_contacts_names))+" surfaces in contact during this phase."
phase_surfaces
=
sorted
(
phase_surfaces
)
# why is this step required ? without out the lp can fail
phase_surfaces_array
=
[]
# convert from list to array, we cannot do this before because sorted() require list
...
...
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