### [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) p2 = array(points) p3 = array(points) 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]*(s[i+1] - s[i-1])) i = len(s) -1 area += abs(s[i]*(s - s[i-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 ; uy = u ; uz = u 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 cosx = np.dot(surface,[0,0,1]) axisx = np.cross(surface,[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)] 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] 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+el) # 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).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)) 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] 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]]) # 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!