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

script/tools/narrow_convex_hull.py

+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

+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

+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
+