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
09715469
Commit
09715469
authored
Oct 14, 2019
by
Pierre Fernbach
Committed by
Pierre Fernbach
Jan 17, 2020
Browse files
[tools] add script to extract sequence of surfaces from the guide planning
parent
c6d103e9
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/tools/narrow_convex_hull.py
0 → 100644
View file @
09715469
import
numpy
as
np
from
numpy
import
arange
,
array
,
append
,
cross
,
dot
,
zeros
from
numpy.linalg
import
norm
from
scipy.spatial
import
ConvexHull
def
normal
(
points
):
p1
=
array
(
points
[
0
])
p2
=
array
(
points
[
1
])
p3
=
array
(
points
[
2
])
normal
=
cross
((
p2
-
p1
),(
p3
-
p1
))
normal
/=
norm
(
normal
)
return
normal
.
tolist
()
def
area
(
s
):
area
=
0
for
i
in
range
(
1
,
len
(
s
)
-
1
):
area
+=
abs
(
s
[
i
][
0
]
*
(
s
[
i
+
1
][
1
]
-
s
[
i
-
1
][
1
]))
i
=
len
(
s
)
-
1
area
+=
abs
(
s
[
i
][
0
]
*
(
s
[
0
][
1
]
-
s
[
i
-
1
][
1
]))
return
area
*
0.5
def
cutList2D
(
l
):
return
[
el
[:
2
]
for
el
in
l
]
def
roundPoints
(
points
,
precision
):
return
[[
round
(
x
,
precision
)
for
x
in
p
]
for
p
in
points
]
def
removeDuplicates
(
points
):
pList
=
[]
for
p
in
points
:
if
p
not
in
pList
:
pList
.
append
(
p
)
return
pList
def
computeAxisAngleRotation
(
u
,
c
):
ux
=
u
[
0
]
;
uy
=
u
[
1
]
;
uz
=
u
[
2
]
s
=
np
.
sqrt
(
1
-
c
*
c
)
return
[[
c
+
ux
*
ux
*
(
1
-
c
),
ux
*
uy
*
(
1
-
c
)
-
uz
*
s
,
ux
*
uz
*
(
1
-
c
)
+
uy
*
s
],
[
uy
*
ux
*
(
1
-
c
)
+
uz
*
s
,
c
+
uy
*
uy
*
(
1
-
c
),
uy
*
uz
*
(
1
-
c
)
-
ux
*
s
],
[
uz
*
ux
*
(
1
-
c
)
-
uy
*
s
,
uz
*
uy
*
(
1
-
c
)
+
ux
*
s
,
c
+
uz
*
uz
*
(
1
-
c
)]]
def
getSurfaceRotation
(
surface
):
n
=
surface
[
1
]
cosx
=
np
.
dot
(
surface
[
1
],[
0
,
0
,
1
])
axisx
=
np
.
cross
(
surface
[
1
],[
0
,
0
,
1
])
;
n_axisx
=
norm
(
axisx
)
if
n_axisx
>
0
:
axisx
/=
n_axisx
return
computeAxisAngleRotation
(
axisx
,
cosx
)
def
getPtsRotation
(
points
):
return
getSurfaceRotation
((
points
,
normal
(
points
)))
def
getSurfaceTranslation
(
surface
):
return
[
sum
(
x
)
/
len
(
x
)
for
x
in
zip
(
*
surface
[
0
])]
def
getPtsTranslation
(
points
):
return
getSurfaceTranslation
((
points
,
normal
(
points
)))
def
allignSurface
(
surface
):
R
=
getSurfaceRotation
(
surface
)
t
=
getSurfaceTranslation
(
surface
)
translatedPts
=
[(
array
(
p
)
-
array
(
t
)).
tolist
()
for
p
in
surface
[
0
]]
rotatedPts
=
[
np
.
dot
(
R
,
p
).
tolist
()
for
p
in
translatedPts
]
return
[(
array
(
p
)
+
array
(
t
)).
tolist
()
for
p
in
rotatedPts
]
def
allignPoints
(
points
):
return
allignSurface
((
points
,
normal
(
points
)))
def
pointsTransform
(
points
,
R
,
t
):
translatedPts
=
[(
array
(
pt
)
-
array
(
t
)).
tolist
()
for
pt
in
points
]
rotatedPts
=
[
np
.
dot
(
R
,
pt
).
tolist
()
for
pt
in
translatedPts
]
return
[(
array
(
pt
)
+
array
(
t
)).
tolist
()
for
pt
in
rotatedPts
]
def
getSurfaceExtremumPoints
(
el
):
pts
=
removeDuplicates
(
el
[
0
]
+
el
[
1
])
# This might only works in rectangular shape? with triangular mesh*2
apts
=
allignPoints
(
pts
)
hull
=
ConvexHull
(
cutList2D
(
apts
))
return
[
pts
[
idx
]
for
idx
in
hull
.
vertices
]
script/tools/plot_surfaces.py
0 → 100644
View file @
09715469
import
matplotlib.pyplot
as
plt
from
mpl_toolkits.mplot3d
import
Axes3D
import
numpy
as
np
COLORS
=
[
'r'
,
'g'
,
'b'
,
'm'
,
'y'
,
'c'
]
def
plotSurface
(
points
,
ax
,
plt
,
color_id
=
-
1
):
xs
=
np
.
append
(
points
[
0
,:],
points
[
0
,
0
]).
tolist
()
ys
=
np
.
append
(
points
[
1
,:],
points
[
1
,
0
]).
tolist
()
zs
=
(
np
.
append
(
points
[
2
,:],
points
[
2
,
0
])
-
np
.
ones
(
len
(
xs
))
*
0.005
*
color_id
).
tolist
()
if
color_id
==
-
1
:
ax
.
plot
(
xs
,
ys
,
zs
,
'gray'
)
else
:
ax
.
plot
(
xs
,
ys
,
zs
,
COLORS
[
color_id
])
plt
.
draw
()
# draw the scene, all the surfaces
def
drawScene
(
all_surfaces
,
ax
=
None
):
if
ax
is
None
:
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
,
projection
=
"3d"
)
for
surface
in
all_surfaces
:
plotSurface
(
np
.
array
(
surface
[
0
]).
T
,
ax
,
plt
,
-
1
)
return
ax
# draw contact surfaces, same color for each pahse
def
drawContacts
(
surfaces
,
ax
=
None
):
color_id
=
0
if
ax
is
None
:
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
,
projection
=
"3d"
)
for
surfaces_phase
in
surfaces
:
for
surface
in
surfaces_phase
:
plotSurface
(
surface
,
ax
,
plt
,
color_id
)
color_id
+=
1
if
color_id
>=
len
(
COLORS
):
color_id
=
0
plt
.
show
()
return
ax
def
draw
(
surfaces
,
all_surfaces
=
None
):
ax
=
None
if
all_surfaces
:
ax
=
drawScene
(
all_surfaces
)
drawContacts
(
surfaces
,
ax
)
script/tools/surfaces_from_path.py
0 → 100644
View file @
09715469
from
numpy
import
arange
,
array
from
pinocchio
import
Quaternion
,
SE3
,
XYZQUATToSe3
from
tools.narrow_convex_hull
import
getSurfaceExtremumPoints
,
removeDuplicates
,
normal
,
area
from
tools.display_tools
import
displaySurfaceFromPoints
import
numpy
as
np
import
eigenpy
eigenpy
.
switchToNumpyMatrix
()
ROBOT_NAME
=
'talos'
MAX_SURFACE
=
0.3
# if a contact surface is greater than this value, the intersection is used instead of the whole surface
LF
=
0
RF
=
1
# change the format into an array
def
listToArray
(
seqs
):
nseq
=
[];
nseqs
=
[]
for
seq
in
seqs
:
nseq
=
[]
for
surface
in
seq
:
nseq
.
append
(
array
(
surface
).
T
)
nseqs
.
append
(
nseq
)
return
nseqs
# get configurations along the path
def
getConfigsFromPath
(
ps
,
pathId
=
0
,
discretisationStepSize
=
1.
)
:
configs
=
[]
pathLength
=
ps
.
pathLength
(
pathId
)
for
s
in
arange
(
0
,
pathLength
,
discretisationStepSize
)
:
configs
.
append
(
ps
.
configAtParam
(
pathId
,
s
))
return
configs
# get all the contact surfaces (pts and normal)
def
getAllSurfaces
(
afftool
)
:
l
=
afftool
.
getAffordancePoints
(
"Support"
)
return
[(
getSurfaceExtremumPoints
(
el
),
normal
(
el
[
0
]))
for
el
in
l
]
# get surface information
def
getAllSurfacesDict
(
afftool
)
:
all_surfaces
=
getAllSurfaces
(
afftool
)
all_names
=
afftool
.
getAffRefObstacles
(
"Support"
)
# id in names and surfaces match
surfaces_dict
=
dict
(
zip
(
all_names
,
all_surfaces
))
# map surface names to surface points
return
surfaces_dict
# get rotation matrices form configs
def
getRotationMatrixFromConfigs
(
configs
)
:
R
=
[]
for
config
in
configs
:
q
=
[
0
,
0
,
0
]
+
config
[
3
:
7
]
#print "q = ",q
R
.
append
(
np
.
array
(
XYZQUATToSe3
(
q
).
rotation
))
print
"R in getRotationMatrixFromConfigs : "
,
R
return
R
# get contacted surface names at configuration
def
getContactsNames
(
rbprmBuilder
,
i
,
q
):
if
i
%
2
==
LF
:
# left leg
step_contacts
=
rbprmBuilder
.
clientRbprm
.
rbprm
.
getCollidingObstacleAtConfig
(
q
,
ROBOT_NAME
+
'_lleg_rom'
)
elif
i
%
2
==
RF
:
# right leg
step_contacts
=
rbprmBuilder
.
clientRbprm
.
rbprm
.
getCollidingObstacleAtConfig
(
q
,
ROBOT_NAME
+
'_rleg_rom'
)
return
step_contacts
# get intersections with the rom and surface at configuration
def
getContactsIntersections
(
rbprmBuilder
,
i
,
q
):
if
i
%
2
==
LF
:
# left leg
intersections
=
rbprmBuilder
.
getContactSurfacesAtConfig
(
q
,
ROBOT_NAME
+
'_lleg_rom'
)
elif
i
%
2
==
RF
:
# right leg
intersections
=
rbprmBuilder
.
getContactSurfacesAtConfig
(
q
,
ROBOT_NAME
+
'_rleg_rom'
)
return
intersections
# merge phases with the next phase
def
getMergedPhases
(
seqs
):
nseqs
=
[]
for
i
,
seq
in
enumerate
(
seqs
):
nseq
=
[]
if
i
==
len
(
seqs
)
-
1
:
nseq
=
seqs
[
i
]
else
:
nseq
=
seqs
[
i
]
+
seqs
[
i
+
1
]
nseq
=
removeDuplicates
(
nseq
)
nseqs
.
append
(
nseq
)
return
nseqs
def
getSurfacesFromGuideContinuous
(
rbprmBuilder
,
ps
,
surfaces_dict
,
pId
,
viewer
=
None
,
step
=
1.
,
useIntersection
=
False
):
pathLength
=
ps
.
pathLength
(
pId
)
#length of the path
discretizationStep
=
0.5
# step at which we check the colliding surfaces
seqs
=
[]
# list of list of surfaces : for each phase contain a list of surfaces. One phase is defined by moving of 'step' along the path
t
=
0.
current_phase_end
=
step
end
=
False
i
=
0
while
not
end
:
# for all the path
phase_contacts_names
=
[]
while
t
<
current_phase_end
:
# get the names of all the surfaces that the rom collide while moving from current_phase_end-step to current_phase_end
q
=
ps
.
configAtParam
(
pId
,
t
)
step_contacts
=
getContactsNames
(
rbprmBuilder
,
i
,
q
)
for
contact_name
in
step_contacts
:
if
not
contact_name
in
phase_contacts_names
:
phase_contacts_names
.
append
(
contact_name
)
t
+=
discretizationStep
# end current phase
# get all the surfaces from the names and add it to seqs:
if
useIntersection
:
intersections
=
getContactsIntersections
(
rbprmBuilder
,
i
,
q
)
phase_surfaces
=
[]
for
name
in
phase_contacts_names
:
surface
=
surfaces_dict
[
name
][
0
]
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
])
else
:
phase_surfaces
.
append
(
surface
)
phase_surfaces
=
sorted
(
phase_surfaces
)
seqs
.
append
(
phase_surfaces
)
# increase values for next phase
t
=
current_phase_end
i
+=
1
if
current_phase_end
==
pathLength
:
end
=
True
current_phase_end
+=
step
if
current_phase_end
>=
pathLength
:
current_phase_end
=
pathLength
# end for all the guide path
seqs
=
listToArray
(
seqs
)
#get rotation matrix of the root at each discretization step
configs
=
[]
for
t
in
arange
(
0
,
pathLength
,
step
)
:
configs
.
append
(
ps
.
configAtParam
(
pId
,
t
))
R
=
getRotationMatrixFromConfigs
(
configs
)
return
R
,
seqs
def
getSurfacesFromPath
(
rbprmBuilder
,
configs
,
surfaces_dict
,
viewer
=
None
,
useIntersection
=
False
,
useMergePhase
=
False
):
seqs
=
[]
# get sequence of surface candidates at each discretization step
for
i
,
q
in
enumerate
(
configs
):
seq
=
[]
intersections
=
getContactsIntersections
(
rbprmBuilder
,
i
,
q
)
# get intersections at config
phase_contacts_names
=
getContactsNames
(
rbprmBuilder
,
i
,
q
)
# get the list of names of the surface in contact at config
for
j
,
intersection
in
enumerate
(
intersections
):
if
useIntersection
and
area
(
intersection
)
>
MAX_SURFACE
:
# append the intersection
seq
.
append
(
intersection
)
else
:
if
len
(
intersections
)
==
len
(
phase_contacts_names
):
# in case getCollidingObstacleAtConfig does not work (because of the rom?)
seq
.
append
(
surfaces_dict
[
phase_contacts_names
[
j
]][
0
])
# append the whole surface
else
:
seq
.
append
(
intersection
)
# append the intersection
if
viewer
:
displaySurfaceFromPoints
(
viewer
,
intersection
,[
0
,
0
,
1
,
1
])
seqs
.
append
(
seq
)
# merge candidates with the previous and the next phase
if
useMergePhase
:
seqs
=
getMergedPhases
(
seqs
)
seqs
=
listToArray
(
seqs
)
R
=
getRotationMatrixFromConfigs
(
configs
)
return
R
,
seqs
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