### add files for contact generation

parent 52c7a74f
 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 removeNone (l): return [i for i in l if len(i) is not 0] 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]
 from numpy import arange, array from narrow_convex_hull import getSurfaceExtremumPoints, removeDuplicates, normal, area from tools.display_tools import displaySurfaceFromPoints from pinocchio import XYZQUATToSe3 import numpy as np 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 def removeNull (l): ll = [] for el in l: if el != []: ll.append(el) return ll # change the format into an array def listToArray (seqs): nseq = []; nseqs= [] for seq in seqs: nseq = [] for surface in seq: if surface != []: 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_rot = config[3:7] R.append(XYZQUATToSe3([0,0,0] + q_rot).rotation) 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 return removeNull(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 getSurfacesFromPathContinuous(rbprmBuilder, ps, surfaces_dict, pId, viewer = None, phaseStepSize = 1., useIntersection = False): pathLength = ps.pathLength(pId) # length of the path discretizationStepSize = 0.4 # 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 = phaseStepSize 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 += discretizationStepSize # 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] #  because the last vector contain the normal of the surface if useIntersection and area(surface) > MAX_SURFACE : if len(step_contacts) == len(intersections): # in case of the error with contact detection if name in step_contacts : intersection = intersections[step_contacts.index(name)] phase_surfaces.append(intersection) else: phase_surfaces.append(surface) else : phase_surfaces.append(surface) phase_surfaces = sorted(phase_surfaces) # why is this step required ? without out the lp can fail 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 += phaseStepSize if current_phase_end >= pathLength: current_phase_end = pathLength # end for all the guide path seqs = listToArray(seqs) # convert from list to array, we cannot do this before because sorted() require list # get rotation matrix of the root at each discretization step configs = [] for t in arange (0, pathLength, phaseStepSize) : configs.append(ps.configAtParam(pId, t)) R = getRotationMatrixFromConfigs(configs) return R,seqs """ LARGE_STEP_SIZE = 1.3 SMALL_STEP_SIZE = 1.0 def getSurfacesFromPathContinuous(rbprmBuilder, ps, surfaces_dict, pId, viewer = None, phaseStepSize = 1., useIntersection = False): pathLength = ps.pathLength(pId) # length of the path discretizationStepSize = 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 = phaseStepSize end = False i = 0 phase_contacts_names = [] configs = [] while not end: # for all the path phase_contacts_prev = phase_contacts_names phase_contacts_names = [] configs.append(ps.configAtParam(pId, t)) 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 += discretizationStepSize # end current phase # phase_contacts_names = sorted(phase_contacts_names) # is this step needed? # # if the previous phase and the current phase has different surface candidates, decrease the phase step # if phase_contacts_prev != phase_contacts_names: # # print "different surface candidates" # phaseStepSize = SMALL_STEP_SIZE # else: # # print "same surface candidates" # phaseStepSize = LARGE_STEP_SIZE # get all the surfaces from the names and add it to seqs: if useIntersection : intersections = getContactsIntersections(rbprmBuilder,i,q) if viewer: for intersection in intersections: displaySurfaceFromPoints(viewer,intersection,[0,0,1,1]) phase_surfaces = [] for name in phase_contacts_names: surface = surfaces_dict[name] #  because the last vector contain the normal of the surface if useIntersection and area(surface) > MAX_SURFACE : if name in step_contacts : intersection = intersections[step_contacts.index(name)] phase_surfaces.append(intersection) else : phase_surfaces.append(surface) phase_surfaces = sorted(phase_surfaces) # why is this step required ? without out the lp can fail 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 += phaseStepSize if current_phase_end >= pathLength: current_phase_end = pathLength # end for all the guide path seqs = listToArray(seqs) # convert from list to array, we cannot do this before because sorted() require list # get rotation matrix of the root at each discretization step # configs = [] # for t in arange (0, pathLength, phaseStepSize) : # 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!
Please register or to comment