Commit 09715469 authored by Pierre Fernbach's avatar Pierre Fernbach Committed by Pierre Fernbach
Browse files

[tools] add script to extract sequence of surfaces from the guide planning

parent c6d103e9
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]
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)
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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment