class PolytopeSet: """Set of polytopes, specified by A,b and optional centroid xyz""" ROBOT_SPHERE_RADIUS = 0.3 #the max sphere embedded in the irreducible volume ROBOT_MAX_SLOPE = 5 # in degrees ROBOT_FOOT_RADIUS = 0.15 # in m def __init__(self): self.A=[] self.b=[] self.xyz=[] self.D=[] self.M=[] self.WD=[] self.W=[] ##walkable surface self.plot = Plotter() ## Vertex enumeration problem: ## brute forcing algorithm: check the solutions to all 3x3 submatrices S of A: Sx=b. ## If x is feasible, then it is a vertex of Ax <= b def getVertices(self, A, b): A = Matrix(A) b = Matrix(b+0.01) M = A.rows N = A.cols vertices = [] for rowlist in combinations(range(M), N): Ap = A.extract(rowlist, range(N)) bp = b.extract(rowlist, [0]) if Ap.det() != 0: xp = np.linalg.solve(Ap,bp) P = np.less_equal(A*xp,b) if P.all(): vertices.append(xp) V = np.zeros((len(vertices),3)) theta = np.zeros((len(vertices),1)) mean = np.zeros((2,1)) for i in range(0,len(vertices)): mean[0] = mean[0]+vertices[i][0] mean[1] = mean[1]+vertices[i][1] mean[0]=mean[0]/len(vertices) mean[1]=mean[1]/len(vertices) for i in range(0,len(vertices)): V[i,0]=vertices[i][0] V[i,1]=vertices[i][1] V[i,2]=vertices[i][2] theta[i] = atan2(V[i,1]-mean[1],V[i,0]-mean[0]) ## sort vertices clockwise order: Iv = np.argsort(theta.T) return V[Iv][0] def sortVertices(self,vertices): mean = np.zeros((2,1)) V = np.zeros((len(vertices),2)) theta = np.zeros((len(vertices),1)) for i in range(0,len(vertices)): mean[0] = mean[0]+vertices[i][0] mean[1] = mean[1]+vertices[i][1] mean[0]=mean[0]/len(vertices) mean[1]=mean[1]/len(vertices) for i in range(0,len(vertices)): V[i,0]=vertices[i][0] V[i,1]=vertices[i][1] theta[i] = atan2(V[i,1]-mean[1],V[i,0]-mean[0]) ## sort vertices clockwise order: Iv = np.argsort(theta.T) return V[Iv][0] def getRotationMatrixAligningHyperplaneAndXYPlane(self, ap, bp): z=np.zeros((3,1)) z[2]=1 y=np.zeros((3,1)) y[1]=1 x=np.zeros((3,1)) x[0]=1 axy = ap - (dot(ap.T,z))*z axy = axy/np.linalg.norm(axy) azy = ap - (dot(ap.T,x))*x azy = azy/np.linalg.norm(azy) ######################### dya = dot(y.T,axy) if dya > 0.01: txy = acos(dya) else: txy = 0 dza = dot(z.T,azy) if dza > 0.01: tzy = acos(dza) else: tzy = 0 ######################### RX = np.zeros((3,3)) RX[0,0]=1 RX[1,1]=cos(txy) RX[1,2]=-sin(txy) RX[2,1]=sin(txy) RX[2,2]=cos(txy) RZ = np.zeros((3,3)) RZ[2,2]=1 RZ[0,0]=cos(tzy) RZ[0,1]=-sin(tzy) RZ[1,0]=sin(tzy) RZ[1,1]=cos(tzy) R = dot(RX,RZ) return R def projectPointOntoHyperplane(self, v, a, b): a=a[0] return v - (dot(v,a) - b)*a def distancePointHyperplane(self, v, a, b): a=a[0] vprime = v - (dot(v,a) - b)*a return np.linalg.norm(vprime-v) def projectPointOntoPolytope(self, v, Ai, bi): xob = Variable(3) objective = Minimize(sum_squares(xob - v)) constraints = [np.matrix(Ai)*xob <= bi] prob = Problem(objective, constraints) prob.solve() return xob.value def distancePolytopePolytope(self, Ai, bi, Aj, bj): xob = Variable(3) yob = Variable(3) objective = Minimize(sum_squares(xob - yob )) constraints = [np.matrix(Ai)*xob <= bi,np.matrix(Aj)*yob <= bj] prob = Problem(objective, constraints) return sqrt(abs(prob.solve())).value def distanceWalkableSurfacePolytope(self, Wi, Ai, bi): xob = Variable(3) yob = Variable(3) objective = Minimize(sum_squares(xob - yob )) AsurfaceX = Wi[0] bsurfaceX = Wi[1] ApolyX = Wi[2] bpolyX = Wi[3] constraints = [] constraints.append(np.matrix(ApolyX)*xob <= bpolyX) constraints.append(np.matrix(AsurfaceX)*xob == bsurfaceX) constraints.append(np.matrix(Ai)*yob <= bi) prob = Problem(objective, constraints) return sqrt(abs(prob.solve())).value def distanceWalkableSurfaceWalkableSurface(self, Wi, Wj): xob = Variable(3) yob = Variable(3) objective = Minimize(sum_squares(xob - yob )) AsurfaceX = Wi[0] bsurfaceX = Wi[1] ApolyX = Wi[2] bpolyX = Wi[3] AsurfaceY = Wj[0] bsurfaceY = Wj[1] ApolyY = Wj[2] bpolyY = Wj[3] constraints = [] constraints.append(np.matrix(ApolyX)*xob <= bpolyX) constraints.append(np.matrix(AsurfaceX)*xob == bsurfaceX) constraints.append(np.matrix(ApolyY)*yob <= bpolyY) constraints.append(np.matrix(AsurfaceY)*yob == bsurfaceY) prob = Problem(objective, constraints) return sqrt(abs(prob.solve())).value def distanceWalkableSurfaceHyperplane(self, Wi, ai, bi): xob = Variable(3) yob = Variable(3) objective = Minimize(sum_squares(xob - yob )) AsurfaceX = Wi[0] bsurfaceX = Wi[1] ApolyX = Wi[2] bpolyX = Wi[3] constraints = [] constraints.append(np.matrix(ApolyX)*xob <= bpolyX) constraints.append(np.matrix(AsurfaceX)*xob == bsurfaceX) #constraints.append(ai[0]*yob[0]+ai[1]*yob[1]+ai[2]*yob[2]== bi) constraints.append(np.matrix(ai)*yob == bi) prob = Problem(objective, constraints) d = sqrt(abs(prob.solve())).value xx = None if xob is not None: xx = np.zeros((3,1)) x=xob.value xx[0]=x[0] xx[1]=x[1] xx[2]=x[2] return [d,xx] def computeDistanceMatrix(self): N = self.N self.M=np.zeros((N,N)) self.D=np.zeros((N,N)) for i in range(0,N): for j in range(i+1,N): self.D[i,j] = self.distancePolytopePolytope(self.A[i],self.b[i],self.A[j],self.b[j]) self.D[j,i] = self.D[i,j] if self.D[i,j] < self.ROBOT_SPHERE_RADIUS: self.M[i,j] = 1 self.M[j,i] = 1 else: self.M[i,j] = 0 self.M[j,i] = 0 print i,"/",N self.D=np.around(self.D,3) print self.D def distanceWalkableSurfaceMatrix(self): if not self.W: print "No walkable surfaces! Did you call getWalkableSurfaces first?" exit N = len(self.W) self.WD = np.zeros((N,N)) self.WM = np.zeros((N,N)) for i in range(0,N): self.WM[i,i]=1 for j in range(i+1,N): self.WD[i,j]=self.WD[j,i]=self.distanceWalkableSurfaceWalkableSurface(self.W[i],self.W[j]) if self.WD[i,j] < self.ROBOT_FOOT_RADIUS: self.WM[i,j]=self.WM[j,i]=1 self.WD=np.around(self.WD,3) print self.WD print self.WM def createWalkableSimplicialComplex(self): C2candidates=[] N = len(self.W) for i in range(0,N): for j in range(i+1,N): if self.WM[i,j]==1: for k in range(j+1,N): if self.WM[i,k]+self.WM[j,k]==2: C2candidates.append([i,j,k]) C0=[] C1=[] for i in range(0,N): C0.append([i]) for j in range(i+1,N): if self.WM[i,j]==1: C1.append([i,j]) C2=[] for p in range(0,len(C2candidates)): [i,j,k]=C2candidates[p] xob = Variable(3) yob = Variable(3) zob = Variable(3) objective = Minimize(sum_squares(xob-yob)+sum_squares(xob-yob)+sum_squares(yob-zob)) constraints = [A[i]*xob <= b[i],A[j]*yob <= b[j],A[k]*zob <= b[k]] prob = Problem(objective, constraints) dist = sqrt(abs(prob.solve())).value if dist < ROBOT_SPHERE_RADIUS: C2.append([i,j,k]) print "2-cells ",p,"/",len(C2candidates) print C0 print C1 print C2 self.WC0 = C0 self.WC1 = C1 self.WC2 = C2 np.save("WC0.simcomplex",C0) np.save("WC1.simcomplex",C1) np.save("WC2.simcomplex",C2) def computeProjectableObjectCandidates(self, surfaceElement): if surfaceElement >= len(self.W): print "exceeds number of walkable surfaces!" exit W = self.W[surfaceElement] Wobj = np.zeros((self.N)) for i in range(0,self.N): Wobj[i] = self.distanceWalkableSurfacePolytope(W, self.A[i], self.b[i]) H = [] for i in range(0,len(W[2])): h = Halfspace(W[2][i], W[3][i]) H.append(h) hi = HalfspaceIntersection(H, self.projectPointOntoHyperplane(W[4], W[0],W[1]) ) print hi.vertices print np.around(Wobj,3) def fromWalkableSurfaceComputeBoxElement(self, surfaceElement): RobotFootHeight = 0.1 ##introduce some offset to remove the objects which are adjacent if surfaceElement >= len(self.W): print "exceeds number of walkable surfaces!" exit W = self.W[surfaceElement] ap = W[0] objectBelongingToWalkableSurface=W[5] apclean = np.zeros((3,1)) apclean[0]=ap[0][0] apclean[1]=ap[0][1] apclean[2]=ap[0][2] bp = W[1] Rxy = self.getRotationMatrixAligningHyperplaneAndXYPlane(apclean,bp) print Rxy ###################################################### ## create box above S_i^p ###################################################### A_box =[] b_box =[] ##surface hyperplane, but opposite direction A_box.append(-ap) b_box.append(-bp) ##distance from surface hyperplane, pointing outside A_box.append(ap) b_box.append(bp+RobotFootHeight) print "ROBOTFOOTHEIGHT", bp+RobotFootHeight, " <<<" for j in range(0,len(W[2])): aj = W[2][j] bj = W[3][j] if np.dot(ap,aj) >0.99: ##hard alignment, either ##parallel or equal => discard continue [value, x0] = self.distanceWalkableSurfaceHyperplane(W,aj,bj) if value < 0.0001: #project hyperplane ajp = aj - (np.dot(ap,aj) - bp)*ap bjp = dot(x0.T,np.array(ajp).T) A_box.append(ajp) b_box.append(bjp) A_clean = np.zeros((len(A_box),3)) b_clean = np.zeros((len(b_box),1)) for j in range(0,len(A_box)): A_clean[j,0] = A_box[j][0][0] A_clean[j,1] = A_box[j][0][1] A_clean[j,2] = A_box[j][0][2] print b_box[j] b_clean[j] = b_box[j] A_box = A_clean b_box = b_clean ############################################################# ## compute distance between box and objects in the scene ############################################################# N = len(self.A) WD = [] print "-----------------------------------------------" print "Distance between Box over walkable surface and" print "object in the environment" print "-----------------------------------------------" proj_objects=[] for i in range(0,N): if i==objectBelongingToWalkableSurface: continue A_obj = self.A[i] b_obj = self.b[i] ##clean b_obj b_clean = np.zeros((len(b_obj),1)) for j in range(0,len(b_obj)): b_clean[j] = b_obj[j] b_obj = b_clean d=self.distancePolytopePolytope(A_obj,b_obj,A_box,b_box) if d < 0.0001: WD.append(d) N_obj=len(A_obj) N_box=len(A_box) ##A intersection object box A_iob A_iob = np.zeros((N_obj+N_box,3)) b_iob = np.zeros((N_obj+N_box,1)) for j in range(0,N_obj): A_iob[j,:]=A_obj[j] b_iob[j]=b_obj[j] for j in range(0,N_box): A_iob[j+N_obj,:]=A_box[j] b_iob[j+N_obj] = b_box[j] print "------------------------------------" print "Object ",i," distance ",d v_obj = self.getVertices(A_obj,b_obj) self.plot.polytopeFromVertices(v_obj) v_iob = self.getVertices(A_iob,b_iob-0.001) self.plot.polytopeFromVertices(v_iob) v_iob_prime = np.zeros((len(v_iob),3)) for j in range(0,len(v_iob)): v_prime = self.projectPointOntoHyperplane(v_iob[j], ap, bp) v_iob_prime[j] = np.dot(Rxy,v_prime) print v_iob_prime[j][0],v_iob_prime[j][1],v_iob_prime[j][2] proj_objects.append(v_iob_prime) print "-----------------------------------------------" print "Number of objects which have to be projected: ",len(WD) print "-----------------------------------------------" print np.around(WD,3) ####################################################### ## write to file for convex decomposition ####################################################### v_box = self.getVertices(A_box,b_box) v_on_surface = np.zeros((len(v_box),1)) segmentCtr=0 verticesCtr=0 verticesToWrite=[] segmentsToWrite=[] ## get box vertices for j in range(0,len(v_box)): d = self.distancePointHyperplane(v_box[j],ap,bp) v_on_surface[j] = False if d <= 0.02: v_on_surface[j] = True v_box = self.getVertices(A_box,b_box) v_box_prime = [] for j in range(0,len(v_box)): if v_on_surface[j]: v_box_prime.append(v_box[j]) firstVertex = verticesCtr polygonBoxV = [] for j in range(0,len(v_box_prime)): ## use only x,y component, since we will do polygonal ## decomposition x = np.around(v_box_prime[j][0],2) y = np.around(v_box_prime[j][1],2) verticesToWrite.append([verticesCtr,x,y]) if j==len(v_box_prime)-1: segmentsToWrite.append([segmentCtr,verticesCtr,firstVertex]) else: segmentsToWrite.append([segmentCtr,verticesCtr,verticesCtr+1]) segmentCtr=segmentCtr+1 verticesCtr=verticesCtr+1 polygonBoxV.append((x,y)) # get vertices of objects objectSegments=[] objectSegmentsNumber=0 polygonObjArray = [] meanProjObjects=np.zeros((len(proj_objects),2)) for j in range(0,len(proj_objects)): vp = proj_objects[j] nonDoubleCtr=0 lastNonDouble=0 nonDouble = np.zeros((len(vp),1)) for k in range(0,len(vp)): xk = np.around(vp[k][0],2) yk = np.around(vp[k][1],2) meanProjObjects[j][0]=meanProjObjects[j][0]+xk/len(vp) meanProjObjects[j][1]=meanProjObjects[j][1]+yk/len(vp) doubleV=False for l in range(0,k)[::-1]: xl = np.around(vp[l][0],2) yl = np.around(vp[l][1],2) dlk = sqrt((xk-xl)**2+(yk-yl)**2).value if dlk <= 0.012: doubleV=True nonDouble[k]=False if not doubleV: nonDoubleCtr=nonDoubleCtr+1 nonDouble[k]=True lastNonDouble=k firstVertex=verticesCtr polygonObjV=[] for k in range(0,len(vp)): if nonDouble[k]: xk = np.around(vp[k][0],2) yk = np.around(vp[k][1],2) polygonObjV.append((xk,yk)) verticesToWrite.append([verticesCtr,xk,yk]) if k==lastNonDouble: segmentsToWrite.append([segmentCtr,verticesCtr,firstVertex]) else: segmentsToWrite.append([segmentCtr,verticesCtr,verticesCtr+1]) verticesCtr=verticesCtr+1 segmentCtr=segmentCtr+1 polygonObjArray.append(polygonObjV) ####################################################### ## Create Polygons ####################################################### pbox = Polygon.Polygon( polygonBoxV ) qbox = pbox p = [] for j in range(0,len(polygonObjArray)): pobj = Polygon.Polygon( polygonObjArray[j] ) qbox = qbox - pobj p.append(pobj) print qbox qdecomp = qbox.triStrip() print qdecomp for j in range(0,len(qdecomp)): qdecomp[j]=self.sortVertices(qdecomp[j]) self.plot.polytopeFromPolygonVertices(qdecomp[j]) Polygon.IO.writeSVG("poly.img", qdecomp) self.plot.polytopeFromVertices(v_box) ####################################################### def getWalkableSurfaces(self): self.W = [] ## iterate over all objects and extract information if it is a ## walkable surface ##gravity vector vg = np.array((0,0,1)) coneD = float(np.sqrt((2-2*math.cos(self.ROBOT_MAX_SLOPE*math.pi/180.0)))) ctrW = 0 print "-----------------------------------------------" print "Walkable surfaces" print "-----------------------------------------------" for i in range(0,self.N): ##iterate over all polytopes for j in range(0,len(self.A[i])): ## iterate over all surface patches and check the ## two conditions on walkability A = copy(self.A[i]) b = copy(self.b[i]) K = len(A) a = np.matrix(A[j]) if np.linalg.norm(a-vg) <= coneD: ## second condition: check if we can put a foot ## inside the surface R = Variable(1) x = Variable(3) constraints = [] for k in range(0,j)+range(j+1,K): aprime = A[k] - np.dot(A[k],A[j])*A[j] anorm = np.linalg.norm(aprime) if anorm>0.001: ## not parallel hyperplanes aprime = aprime/anorm v = np.dot(A[k],aprime) constraints.append(A[k][0]*x[0]+ A[k][1]*x[1]+A[k][2]*x[2] + R*v <= b[k]) constraints.append( A[j][0]*x[0]+ A[j][1]*x[1]+A[j][2]*x[2]== b[j]) constraints.append(R>=0) objective = Maximize(R) self.prob = Problem(objective, constraints) solver_output = self.prob.solve(solver=ECOS) radius = self.prob.value if radius >= self.ROBOT_FOOT_RADIUS: print ctrW,": radius on surface: ",radius ##surface is walkable self.W.append([np.array(a),b[j],self.A[i],self.b[i],self.xyz[i],i]) ctrW = ctrW + 1 print "-----------------------------------------------" def fromURDF(self,urdf_fname): soup = BeautifulSoup(open(urdf_fname)) links = soup.robot.findAll("collision") K=[] for i in range(0,len(links)): L=links[i] st=L.geometry.box["size"] size = re.split(' ',st) sx = float(size[0]) sy = float(size[1]) sz = float(size[2]) pos=L.origin["xyz"] pos = re.split(' ',pos) ori=L.origin["rpy"] ori = re.split(' ',ori) x = float(pos[0]) y = float(pos[1]) z = float(pos[2]) ro = float(ori[0]) po = float(ori[1]) yo = float(ori[2]) ## prune small boxes (DEBUG MODE) if DEBUG: if sx+sy > 0.5: K.append([sx,sy,sz,x,y,z,ro,po,yo]) else: K.append([sx,sy,sz,x,y,z,ro,po,yo]) self.N = len(K) self.A=[] self.b=[] self.xyz=[] for i in range(0,self.N): v=np.abs(K[i][6])+np.abs(K[i][7])+np.abs(K[i][8]) if v>0.001: print "please do not rotate any boxes in URDF -- not handled atm" exit [sx,sy,sz,x,y,z] = K[i][0:6] p1 = [x+sx/2, y+sy/2, z+sz/2] p2 = [x+sx/2, y+sy/2, z-sz/2] p3 = [x+sx/2, y-sy/2, z+sz/2] p4 = [x+sx/2, y-sy/2, z-sz/2] p5 = [x-sx/2, y+sy/2, z+sz/2] p6 = [x-sx/2, y+sy/2, z-sz/2] p7 = [x-sx/2, y-sy/2, z+sz/2] p8 = [x-sx/2, y-sy/2, z-sz/2] hull = ConvexHull([p1,p2,p3,p4,p5,p6,p7,p8]) E=hull.equations[0::2] Ah = np.array(E[0:,0:3]) bh = np.array(-E[0:,3]) ###normalize for at in range(0,len(Ah)): normA = np.linalg.norm(Ah[at]) Ah[at] = Ah[at]/normA bh[at] = bh[at]/normA self.A.append(Ah) self.b.append(bh) self.xyz.append([x,y,z]) np.save("xyz.simcomplex",self.xyz)
print "plotting workspace swept volume approximation" Ark = Aleftrightconv[minimaIter] rhoR = np.matrix(Ark)*rho.value plot.point(x_goal.value,color=(0,0,0,0.9)) plot.point(x_start.value) maxNonzeroDim = np.max(np.nonzero(Ark)[0]) firstIS = None lastIS = None ## plot intersection environment boxes for i in range(0,N_walkablesurfaces-1): if x_connection[i].value is not None: for k in range(0,maxNonzeroDim): W = upperBodyConnector[i][k] plot.polytopeFromVertices(\ W.getVertexRepresentation(),\ fcolor=colorScene) ### plot paths on each WS ### build one path for each dimension: svPathPoints = 0 for i in range(0,N_walkablesurfaces): svPathPoints = svPathPoints + len(x_WS[i]) svPathsLeft = np.zeros((maxNonzeroDim, svPathPoints, 3)) svPathsRight = np.zeros((maxNonzeroDim, svPathPoints, 3)) svPathsMiddle = np.zeros((maxNonzeroDim, svPathPoints, 3)) thetaV = np.zeros((svPathPoints, 5)) for k in range(0,maxNonzeroDim): ctr = 0 for i in range(0,N_walkablesurfaces):
plot=Plotter() env_fname = "wall.urdf" pobjects = URDFtoPolytopes(env_fname) for i in range(0,len(pobjects)): V = pobjects[i].getVertexRepresentation() middle = True for j in range(0,len(V)): if abs(V[j][1])>0.3: middle=False if middle: plot.polytopeFromVertices(\ pobjects[i].getVertexRepresentation(),\ fcolor=colorScene) N_w = len(Wsurfaces_decomposed) for i in range(0,N_w): V = Wsurfaces_decomposed[i].getVertexRepresentation() - 0.025*np.array((0,0,1)) #V = Wsurfaces_decomposed[i].getVertexRepresentation() plot.walkableSurface( \ V,\ fcolor=colorWalkableSurface, thickness=0.05) ############################################################################### ### print summary of vstacks on each surface ############################################################################### for i in range(0,len(Wsurface_box_vstack)): vstack = Wsurface_box_vstack[i] print "WS",i,"has",len(vstack),"layers"