### bug fix case 3

parents 24cae4e5 785f14d5
 ... ... @@ -27,14 +27,19 @@ np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)}) ### This solver is called when the sparsity is fixed. It assumes the first contact surface for each phase ### is the one used for contact creation. def solve(pb,surfaces, draw_scene = None, plot = True ): def solve(pb,surfaces, draw_scene = None, plot = True): t1 = clock() A, b, E, e = pl.convertProblemToLp(pb) C = identity(A.shape) c = zeros(A.shape) t2 = clock() res = qp.quadprog_solve_qp(C, c,A,b,E,e) try: res = qp.quadprog_solve_qp(C, c,A,b,E,e) ###### # res = qp.solve_lp(c,A,b,E,e) except: print ("CASE3: turned out to be infeasible") return 3, 3, 3 t3 = clock() print("time to set up problem" , timMs(t1,t2)) ... ... @@ -48,7 +53,8 @@ def solve(pb,surfaces, draw_scene = None, plot = True ): ax = draw_scene(surfaces) pl.plotQPRes(pb, res, ax=ax) return pb, coms, footpos, allfeetpos, res return pb, res, timMs(t1,t3) # return pb, coms, footpos, allfeetpos, res ### Calls the sl1m solver. Brute-forcedly tries to solve non fixed sparsity by handling the combinatorial. ... ... @@ -58,16 +64,25 @@ def solveL1(pb, surfaces, draw_scene = None, plot = True): C = identity(A.shape) * 0.00001 c = pl1.slackSelectionMatrix(pb) res = qp.quadprog_solve_qp(C, c,A,b,E,e) try: res = qp.quadprog_solve_qp(C, c,A,b,E,e) except: return 4, 4, 4 ok = pl1.isSparsityFixed(pb, res) solutionIndices = None solutionComb = None pbs = None if not ok: pbs = pl1.generateAllFixedScenariosWithFixedSparsity(pb, res) t3 = clock() if pbs == 1: print ("CASE1: too big combinatorial") return 1, 1, 1 for (pbComb, comb, indices) in pbs: A, b, E, e = pl1.convertProblemToLp(pbComb, convertSurfaces = False) C = identity(A.shape) * 0.00001 ... ... @@ -100,7 +115,12 @@ def solveL1(pb, surfaces, draw_scene = None, plot = True): for i, idx in enumerate(solutionIndices): pb["phaseData"][idx]["S"] = [surfaces[idx][solutionComb[i]]] return solve(pb,surfaces, draw_scene = draw_scene, plot = True ) return solve (pb, surfaces, draw_scene, plot) print ("CASE2: combinatorials all sparsity not fixed") return 2, 2, 2 # return solve(pb,surfaces, draw_scene = draw_scene, plot = True ) ############### MIXED-INTEGER SOLVER ############### ... ... @@ -162,5 +182,6 @@ def solveMIP(pb, surfaces, MIP = True, draw_scene = None, plot = True): ax = draw_scene(surfaces) pl1.plotQPRes(pb, res, ax=ax) return timMs(t1,t2) # return timMs(t1,t2) return pb, res, timMs(t1,t2)
 ... ... @@ -160,7 +160,7 @@ def plotPoints(ax, wps, color = "b", D3 = True, linewidth=2): y = array(wps)[:,1] if(D3): z = array(wps)[:,2] ax.scatter(x, y, z, c=color, marker='o', linewidth = 5) ax.scatter(x, y, z, color=color, marker='o', linewidth = 5) else: ax.scatter(x,y,color=color, linewidth = linewidth) ... ...
 ... ... @@ -283,30 +283,32 @@ def slackSelectionMatrix(pb): cIdx += phaseVars assert cIdx == nvars return c def num_non_zeros(pb, res): nvars = getTotalNumVariablesAndIneqConstraints(pb) # nvars = getTotalNumVariablesAndIneqConstraints(pb) indices = [] cIdx = 0 wrongsurfaces = [] wrongsurfaces_indices = [] for i, phase in enumerate(pb["phaseData"]): numSurfaces = len(phase["S"]) phaseVars = numVariablesForPhase(phase) if numSurfaces > 1: startIdx = cIdx + DEFAULT_NUM_VARS betas = [res[startIdx+j] for j in range(0,numSurfaces*2,2) ] if array(betas).min() > 0.01: #~ print "wrong ", i, array(betas).min() if array(betas).min() > 0.01: #### # print "wrong ", i, array(betas).min() indices += [i] sorted_surfaces = np.argsort(betas) wrongsurfaces_indices += [sorted_surfaces] #~ print "sorted_surfaces ",sorted_surfaces wrongsurfaces += [[[phase["S"][idx]] for idx in sorted_surfaces] ] #~ print "lens ", len([[phase["S"][idx]] for idx in sorted_surfaces] ) cIdx += phaseVars return indices, wrongsurfaces return indices, wrongsurfaces, wrongsurfaces_indices def isSparsityFixed(pb, res): indices, wrongsurfaces = num_non_zeros(pb, res) indices, wrongsurfaces, wrongsurfaces_indices = num_non_zeros(pb, res) return len(indices) == 0 def genOneComb(pb,indices, surfaces, res): ... ... @@ -318,29 +320,31 @@ def genOneComb(pb,indices, surfaces, res): import itertools import copy def genCombinatorialRec(pb, indices, wrongsurfaces, res): def genCombinatorialRec(pb, indices, wrongsurfaces, wrongsurfaces_indices, res): lenss = [len(surfs) for surfs in wrongsurfaces] all_indexes = [[el for el in range(lens)] for lens in [len(surfs) for surfs in wrongsurfaces]] wrong_combs = [el for el in itertools.product(*wrongsurfaces_indices)] combs = [el for el in itertools.product(*all_indexes)] for comb in combs: for j, comb in enumerate(combs): pb1 = copy.deepcopy(pb) for i, idx in enumerate(indices): pb1["phaseData"][idx]["S"] = wrongsurfaces[i][comb[i]] res += [[pb1, comb, indices]] res += [[pb1, wrong_combs[j], indices]] def generateAllFixedScenariosWithFixedSparsity(pb, res): indices, wrongsurfaces = num_non_zeros(pb, res) indices, wrongsurfaces, wrongsurfaces_indices = num_non_zeros(pb, res) all_len = [len(surfs) for surfs in wrongsurfaces] comb = 1 for el in all_len: comb *= el res = [] if comb >1000: print("problem probably too big ", comb) print ("problem probably too big ", comb) return 1 else: genCombinatorialRec(pb, indices, wrongsurfaces, res) genCombinatorialRec(pb, indices, wrongsurfaces, wrongsurfaces_indices, res) return res ... ... @@ -409,7 +413,7 @@ def plotPoints(ax, wps, color = "b", D3 = True, linewidth=2): y = array(wps)[:,1] if(D3): z = array(wps)[:,2] ax.scatter(x, y, z, c=color, marker='o', linewidth = 5) ax.scatter(x, y, z, color=color, marker='o', linewidth = 5) else: ax.scatter(x,y,color=color, linewidth = linewidth) ... ... @@ -459,14 +463,14 @@ def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show = ax.set_autoscale_on(False) ax.view_init(elev=8.776933438381377, azim=-99.32358055821186) #~ plotPoints(ax, coms, color = "b") plotPoints(ax, coms, color = "b") plotPoints(ax, footpos[RF], color = "r") plotPoints(ax, footpos[LF], color = "g") cx = [c for c in coms] cy = [c for c in coms] cz = [c for c in coms] #~ ax.plot(cx, cy, cz) ax.plot(cx, cy, cz) px = [c for c in allfeetpos] py = [c for c in allfeetpos] pz = [c for c in allfeetpos] ... ...
 ... ... @@ -53,7 +53,12 @@ def solve_least_square(A,b,G=None, h=None, C=None, d=None): #subject to C x = d def solve_lp(q, G=None, h=None, C=None, d=None): res = linprog(q, A_ub=G, b_ub=h, A_eq=C, b_eq=d, bounds=[(-100000.,10000.) for _ in range(q.shape)], method='interior-point', callback=None, options={'presolve': True}) return res # print "success", res['success'] # print "status", res['status'] if res['success']: return res['x'] else: return res['status'] if GLPK_OK: __eps = 0.00001 ... ...
 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(array(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