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
Humanoid Path Planner
hpp-rbprm-corba
Commits
1ac429b8
Commit
1ac429b8
authored
Oct 15, 2019
by
Pierre Fernbach
Committed by
Pierre Fernbach
Jan 17, 2020
Browse files
[tools] surfaces_from_path : check if the intersection is not empty before adding it
parent
2170f8f2
Changes
2
Hide whitespace changes
Inline
Side-by-side
script/tools/surfaces_from_path.py
View file @
1ac429b8
...
...
@@ -43,11 +43,16 @@ def getAllSurfacesDict (afftool) :
# get rotation matrices form configs
def
getRotationMatrixFromConfigs
(
configs
)
:
eigenpy
.
switchToNumpyMatrix
()
R
=
[]
for
config
in
configs
:
q
=
[
0
,
0
,
0
]
+
config
[
3
:
7
]
#print "q = ",q
R
.
append
(
np
.
array
(
XYZQUATToSe3
(
q
).
rotation
))
print
"q = "
,
q
placement
=
XYZQUATToSe3
(
q
)
print
"placement = "
,
placement
rot
=
placement
.
rotation
print
"rot = "
,
rot
R
.
append
(
np
.
array
(
rot
))
print
"R in getRotationMatrixFromConfigs : "
,
R
return
R
...
...
@@ -112,9 +117,10 @@ def getSurfacesFromGuideContinuous(rbprmBuilder,ps,afftool,pId,viewer = None,ste
if
useIntersection
and
area
(
surface
)
>
MAX_SURFACE
:
if
name
in
step_contacts
:
intersection
=
intersections
[
step_contacts
.
index
(
name
)]
phase_surfaces
.
append
(
intersection
)
if
viewer
:
displaySurfaceFromPoints
(
viewer
,
intersection
,[
0
,
0
,
1
,
1
])
if
len
(
intersection
)
>
3
:
phase_surfaces
.
append
(
intersection
)
if
viewer
:
displaySurfaceFromPoints
(
viewer
,
intersection
,[
0
,
0
,
1
,
1
])
else
:
phase_surfaces
.
append
(
surface
)
# [0] because the last vector contain the normal of the surface
#print "There was "+str(len(phase_contacts_names))+" surfaces in contact during this phase."
...
...
src/hpp/corbaserver/rbprm/tools/display_tools.py
View file @
1ac429b8
...
...
@@ -48,6 +48,8 @@ def addVector(viewer, rbprmBuilder, color, v, name=None):
def
displaySurfaceFromPoints
(
viewer
,
p_list
,
color
=
[
0
,
0
,
0
,
1
],
name
=
None
):
if
len
(
p_list
)
<
2
:
return
gui
=
viewer
.
client
.
gui
if
name
is
None
:
i
=
0
...
...
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