svPathsLeft[k][ctr] = np.array(pt).flatten()
ibRhoR = np.matrix(Ark)*ibRho[i][j].value
pt = x_WS[i][j].value+(np.array(ibRhoR[k]).flatten()[0]*vp[i][j].T+heights[k]*v2).T
svPathsRight[k][ctr] = np.array(pt).flatten()
ctr = ctr+1
ctr=0
for i in range(0,N_walkablesurfaces):
for j in range(0,len(x_WS[i])):
[k,h1,h2,h3] = Harray[minimaIter]
thetaV[ctr] = htoq(k,h1,h2,h3)[1]
ctr = ctr+1
svLeftFname= "data/path/xpathL.dat"
svRightFname = "data/path/xpathR.dat"
svMiddleFname = "data/path/xpathM.dat"
svQValuesFname = "data/path/xpathQ.dat"
pickle.dump( svPathsLeft, open( svLeftFname, "wb" ) )
pickle.dump( svPathsRight, open( svRightFname, "wb" ) )
pickle.dump( svPathsMiddle, open( svMiddleFname, "wb" ) )
pickle.dump( thetaV, open( svQValuesFname, "wb" ) )
plot.lines(svPathsLeft,'-or')
plot.lines(svPathsRight,'-om')
plot.lines(svPathsMiddle,'-ok')
plot.set_view(90,0)
plot.showEnvironment()
else:
print "problem not feasible"
