def SocialNavigationGaussian_getPassOnRight(self, persons, h, dibujar): plt.close("all") lx_inf = -6 lx_sup = 8 ly_inf = -6 ly_sup = 8 normals = [] for p in persons: pn = Person(p.x / 1000, p.z / 1000, p.angle, p.vel) pn.draw(2., 1, 4. / 3., pi / 2 - pn.th, drawPersonalSpace=dibujar) pn.draw(2., 1, 4. / 3., pi - pn.th, drawPersonalSpace=dibujar) normals.append(Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th - pi / 2., 2, 1, 4. / 3], elliptical=True)) normals.append(Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th, 2, 0.75, 4. / 3], elliptical=True)) # h = 0.4 # h = prox / 100 resolution = 0.1 limits = [[lx_inf, lx_sup], [ly_inf, ly_sup]] _, z = Normal.makeGrid(normals, h, 2, limits=limits, resolution=resolution) # Las posiciones de las personas tienen que estar en metros grid = GM.filterEdges(z, h) if (dibujar): plt.figure() plt.imshow(grid, extent=[lx_inf, lx_sup, ly_inf, ly_sup], shape=grid.shape, interpolation='none', aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2) plt.xlabel('X') plt.ylabel('Y') plt.axis('equal') np.savetxt('log.txt', grid, fmt='%i') polylines = [] totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf) for pol in totalpuntosorden: polyline = [] for pnt in pol: punto = SNGPoint2D() punto.x = pnt[0] * 1000 punto.z = pnt[1] * 1000 polyline.append(punto) polylines.append(polyline) plt.show() return polylines
def SocialNavigationGaussian_getSocialSpace(self, persons, h, draw): plt.close('all') ##Limites de la representacion lx_inf = -6 lx_sup = 10 ly_inf = -6 ly_sup = 10 ##########################################CLUSTERING################################################## normals = [] for p in persons: pn = Person(p.x / 1000, p.z / 1000, p.angle) pn.draw(3, 1, 1.3, pi / 2 - pn.th, drawPersonalSpace=draw) # Valores originales normals.append( Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th - pi / 2., 3, 1, 1.3], elliptical=True)) # print ("numero de gaussianas",len(normals)) resolution = 0.1 limits = [[lx_inf, lx_sup], [ly_inf, ly_sup]] _, z = Normal.makeGrid(normals, h, 2, limits=limits, resolution=resolution) grid = GM.filterEdges(z, h) ###########################LEO EL GRID Y SEPARO LAS POLILINEAS, DESPUES SE HACE CONVEXHULL#################################### polylines = [] totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf) for pol in totalpuntosorden: polyline = [] for pnt in pol: punto = SNGPoint2D() punto.x = pnt[0] * 1000 punto.z = pnt[1] * 1000 polyline.append(punto) polylines.append(polyline) if (draw): ##DIBUJO ZONA Social _, z = Normal.makeGrid(normals, 0.1, 2, limits=limits, resolution=resolution) grid = GM.filterEdges(z, 0.1) polylines = [] totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf) for pol in totalpuntosorden: polyline = [] for pnt in pol: punto = SNGPoint2D() punto.x = pnt[0] punto.z = pnt[1] polyline.append(punto) polylines.append(polyline) for ps in polylines: # plt.figure() for p in ps: plt.plot(p.x, p.z, "oc-") plt.axis('equal') plt.xlabel('X') plt.ylabel('Y') plt.show() return polylines
def getAllPersonalSpaces(self, persons, represent): personal_spaces = ["intimate", "personal", "social"] # sigma_h, sigma_r, sigma_s, h dict_space_param = { "intimate": [1.3, 1., 1.3, 0.8], "personal": [1.3, 1., 1.3, 0.4], "social": [3., 1., 1.3, 0.1], } dict_space_polylines = { "intimate": [], "personal": [], "social": [], } dict_spaces_to_plot = { "intimate": [], "personal": [], "social": [], } ##Limites de la representacion lx_inf = -6 lx_sup = 10 ly_inf = -6 ly_sup = 10 for space in personal_spaces: normals = [] for p in persons: person = Person(p.x, p.z, p.angle) # print('Pose x', person.x, 'Pose z', person.y, 'Rotacion', person.th) # person.draw(2,1, 4./3.,pi/2 - person.th, drawPersonalSpace=dibujar) #Valores originales person.draw(dict_space_param[space][0], dict_space_param[space][1], dict_space_param[space][2], pi / 2 - person.th, drawPersonalSpace=represent) normals.append( Normal(mu=[[person.x], [person.y]], sigma=[ -person.th - pi / 2., dict_space_param[space][0], dict_space_param[space][1], dict_space_param[space][2] ], elliptical=True)) # print ("numero de gaussianas",len(normals)) resolution = 0.1 limits = [[lx_inf, lx_sup], [ly_inf, ly_sup]] _, z = Normal.makeGrid(normals, dict_space_param[space][3], 2, limits=limits, resolution=resolution) grid = GM.filterEdges(z, dict_space_param[space][3]) totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf) for pol in totalpuntosorden: polyline = [] polyline_to_plt = [] for pnt in pol: punto = SNGPoint2D() punto.x = pnt[0] * 1000 punto.z = pnt[1] * 1000 polyline.append(punto) polyline_to_plt.append([pnt[0], pnt[1]]) dict_space_polylines[space].append(polyline_to_plt) if len(polyline_to_plt) != 0: dict_spaces_to_plot[space].append(polyline_to_plt) if represent: for soc in dict_spaces_to_plot["social"]: x, y = zip(*soc) plt.plot(x, y, color='c', marker='.') for per in dict_spaces_to_plot["personal"]: x, y = zip(*per) plt.plot(x, y, color='m', marker='.') for inti in dict_spaces_to_plot["intimate"]: x, y = zip(*inti) plt.plot(x, y, color='r', marker='.') plt.axis('equal') plt.xlabel('X') plt.ylabel('Y') plt.show() return dict_space_polylines['intimate'], dict_space_polylines[ 'personal'], dict_space_polylines['social']
sigma=[360 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333], elliptical=True), Normal(mu=[[1.88], [1.88]], sigma=[290 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333], elliptical=True), Normal(mu=[[2.48], [1.67]], sigma=[225 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333], elliptical=True), Normal(mu=[[-1.22], [-2.12]], sigma=[360 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333], elliptical=True), Normal(mu=[[-2.12], [-2.12]], sigma=[290 * 3.1415 / 180, 2.0, 1.0, 1.3333333333333333], elliptical=True) ] _, z = Normal.makeGrid(normals, h, 2, limits=limits, resolution=resolution) grid = filterEdges(z, h) np.savetxt('log.txt', grid, fmt='%.4e') plt.figure() plt.imshow(z, shape=grid.shape, interpolation='none', aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2) plt.figure()
def getPersonalSpace(self, persons, h, dibujar): plt.close('all') ##DESCOMENTAR EL FIGUREEE # plt.figure() #ax = fig.add_subplot(111, projection='3d') #fig, ax = plt.subplots() #ax.grid(True) # x = y = np.arange(-3.0, 3.0, 0.05) # X, Y = np.meshgrid(x, y) ##Limites de la representacion lx_inf = -6 lx_sup = 10 ly_inf = -6 ly_sup = 10 """"" ##cambio los limites para los otros valores de sigma lx_inf = 0 lx_sup = 10 ly_inf = 0 ly_sup = 10 """ "" # zs = np.array([fun(x,y) for x,y in zip(np.ravel(X), np.ravel(Y))]) # Z = zs.reshape(X.shape) ##########################################CLUSTERING################################################## normals = [] for p in persons: pn = Person(p.x, p.z, p.angle) #print('Pose x', pn.x, 'Pose z', pn.y, 'Rotacion', pn.th) pn.draw(2., 1, 4. / 3., pi / 2 - pn.th, drawPersonalSpace=dibujar) #normals.append(Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th - pi/2, 2.0, 2.0, 2.0], elliptical=True)) normals.append( Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th - pi / 2., 2, 1, 4. / 3], elliptical=True)) #normals.append(Normal(mu=[[pn.x], [pn.y]], sigma=[-pn.th, 2, 1, 4. / 3], elliptical=True)) #print ("numero de gaussianas",len(normals)) # h = 0.1 # h = prox/100 resolution = 0.1 limits = [[lx_inf, lx_sup], [ly_inf, ly_sup]] _, z = Normal.makeGrid(normals, h, 2, limits=limits, resolution=resolution) grid = GM.filterEdges(z, h) #plt.figure() #plt.imshow(z, shape=grid.shape, interpolation='none', aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2) #plt.figure() #plt.imshow(grid, shape=grid.shape, interpolation='none', aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2) # if (dibujar): # # plt.figure() # plt.imshow(grid, extent=[lx_inf, lx_sup, ly_inf, ly_sup], shape=grid.shape, interpolation='none', aspect='equal', origin='lower', cmap='Greys', vmin=0, vmax=2) # plt.xlabel('X') # plt.ylabel('Y') # plt.axis('equal') np.savetxt('log.txt', grid, fmt='%i') ###########################LEO EL GRID Y SEPARO LAS POLILINEAS, DESPUES SE HACE CONVEXHULL#################################### polylines = [] totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf) for pol in totalpuntosorden: polyline = [] for pnt in pol: punto = SNGPoint2D() punto.x = pnt[0] punto.z = pnt[1] polyline.append(punto) polylines.append(polyline) if (dibujar): ##DIBUJO ZONA Social _, z = Normal.makeGrid(normals, 0.1, 2, limits=limits, resolution=resolution) grid = GM.filterEdges(z, 0.1) polylines = [] totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf) for pol in totalpuntosorden: polyline = [] for pnt in pol: punto = SNGPoint2D() punto.x = pnt[0] punto.z = pnt[1] polyline.append(punto) polylines.append(polyline) for ps in polylines: #plt.figure() for p in ps: plt.plot(p.x, p.z, "oc-") plt.axis('equal') plt.xlabel('X') plt.ylabel('Y') # plt.show() ###DIBUJO ZONA Personal _, z = Normal.makeGrid(normals, 0.4, 2, limits=limits, resolution=resolution) grid = GM.filterEdges(z, 0.4) polylines = [] totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf) for pol in totalpuntosorden: polyline = [] for pnt in pol: punto = SNGPoint2D() punto.x = pnt[0] punto.z = pnt[1] polyline.append(punto) polylines.append(polyline) for ps in polylines: # plt.figure() for p in ps: plt.plot(p.x, p.z, "om-") plt.axis('equal') plt.xlabel('X') plt.ylabel('Y') # plt.show() ###DIBUJO ZONA INTIMA _, z = Normal.makeGrid(normals, 0.8, 2, limits=limits, resolution=resolution) grid = GM.filterEdges(z, 0.8) polylines = [] totalpuntosorden = getPolyline(grid, resolution, lx_inf, ly_inf) for pol in totalpuntosorden: polyline = [] for pnt in pol: punto = SNGPoint2D() punto.x = pnt[0] punto.z = pnt[1] polyline.append(punto) polylines.append(polyline) for ps in polylines: # plt.figure() for p in ps: plt.plot(p.x, p.z, "or-") plt.axis('equal') plt.xlabel('X') plt.ylabel('Y') # plt.show() plt.show() return polylines