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"
