print "forming connection between ",pcurrent,"and",pnext
W=Wsurfaces_decomposed[pcurrent]
Wnext=Wsurfaces_decomposed[pnext]
Wstack = Wsurface_box_vstack[pcurrent]
WstackNext = Wsurface_box_vstack[pnext]
#### create foot connector
boxW = W.createBox(0,ROBOT_FOOT_HEIGHT,DeltaSide=0.02)
boxWnext = Wnext.createBox(0,ROBOT_FOOT_HEIGHT)
binter = boxW.intersectWithPolytope(Wnext)
ap = Wnext.ap
bp = Wnext.bp
connectorWtoWnext = WalkableSurface(ap,bp,binter.A, binter.b, Wnext.iObject)
plot.walkableSurface( \
connectorWtoWnext.getVertexRepresentation(),\
fcolor=[1,0,0,0.4],\
thickness=0.01)
connector.append(connectorWtoWnext)
### create upper body connectors
upperBodyConnectorStack = []
for j in range(0,XspaceDimension):
if j < len(Wstack):
Winter = Wstack[j][0].intersectWithPolytope(WstackNext[j][0])
upperBodyConnectorStack.append(Winter)
else:
## create helper box for higher dimensions (the small
## boxes which contain only 1 point
Whelper = connectorWtoWnext.createTinyHelperBox(heights[j],heights[j]+VSTACK_DELTA)
upperBodyConnectorStack.append(Whelper)
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"
for j in range(0,len(vstack)):
hstack = vstack[j]
#print " layer",j,"is decomposed into",len(hstack),"boxes"
for k in range(0,len(hstack)):
V = hstack[k].getVertexRepresentation()
middle = True
minDistance = 0.0
