def update(self, iSim): if self.pause: # NO ANIMATION -- PAUSE self.old_time=time.time() return (self.lines + self.obs_polygon + self.contour + self.centers + self.cent_dyns + self.startPoints + self.endPoints) if not (iSim%10): # Display every tenth loop count print('loop count={} - frame ={}-Simulation time ={}'.format(self.iSim, iSim, np.round(self.dt*self.iSim, 3) )) intersection_obs = obs_common_section(self.obs) dynamic_center_3d(self.obs, intersection_obs) if self.RK4_int: # Runge kutta integration for j in range(self.N_points): self.x_pos[:, self.iSim+1,j] = obs_avoidance_rk4(self.dt, self.x_pos[:,self.iSim,j], self.obs, x0=self.attractorPos, obs_avoidance = obs_avoidance_interpolation_moving) else: # Simple euler integration # Calculate DS for j in range(self.N_points): xd_temp = linearAttractor(self.x_pos[:,self.iSim, j], self.attractorPos) self.xd_ds[:,self.iSim,j] = obs_avoidance_interpolation_moving(self.x_pos[:,self.iSim, j], xd_temp, self.obs) self.x_pos[:,self.iSim+1,:] = self.x_pos[:,self.iSim, :] + self.xd_ds[:,self.iSim, :]*self.dt self.t[self.iSim+1] = (self.iSim+1)*self.dt # Update plots for j in range(self.N_points): self.lines[j].set_xdata(self.x_pos[0,:self.iSim+1,j]) self.lines[j].set_ydata(self.x_pos[1,:self.iSim+1,j]) if self.dim==3: self.lines[j].set_3d_properties(zs=self.x_pos[2,:self.iSim+1,j]) self.endPoints[j].set_xdata(self.x_pos[0,self.iSim+1,j]) self.endPoints[j].set_ydata(self.x_pos[1,self.iSim+1,j]) if self.dim==3: self.endPoints[j].set_3d_properties(zs=self.x_pos[2,self.biSim+1,j]) # ========= Check collision ---------- #collisions = obs_check_collision(self.x_pos[:,self.iSim+1,:], obs) #collPoints = np.array() # if collPoints.shape[0] > 0: # plot(collPoints[0,:], collPoints[1,:], 'rx') # print('Collision detected!!!!') for o in range(len(self.obs)):# update obstacles if moving self.obs[o].update_pos(self.t[self.iSim], self.dt) # Update obstacles self.centers[o].set_xdata(self.obs[o].x0[0]) self.centers[o].set_ydata(self.obs[o].x0[1]) if self.dim==3: self.centers[o].set_3d_properties(zs=obs[o].x0[2]) if hasattr(self.obs[o], 'center_dyn'):# automatic adaptation of center self.cent_dyns[o].set_xdata(self.obs[o].center_dyn[0]) self.cent_dyns[o].set_ydata(self.obs[o].center_dyn[1]) if self.dim==3: self.cent_dyns[o].set_3d_properties(zs=self.obs[o].center_dyn[2]) if self.obs[o].x_end > self.t[self.iSim] or self.iSim<1: # First two rounds or moving if self.dim ==2: # only show safety-contour in 2d, otherwise not easily understandable self.contour[o].set_xdata([self.obs[o].x_obs_sf[ii][0] for ii in range(len(self.obs[o].x_obs_sf))]) self.contour[o].set_ydata([self.obs[o].x_obs_sf[ii][1] for ii in range(len(self.obs[o].x_obs_sf))]) if self.obs[o].x_end > self.t[self.iSim] or self.iSim<2: if self.dim==2: self.obs_polygon[o].xy = self.obs[o].x_obs else: self.obs_polygon[o].xyz = self.obs[o].x_obs self.iSim += 1 # update simulation counter self.check_convergence() # Check convergence # Pause for constant simulation speed self.old_time = self.sleep_const(self.old_time) return (self.lines + self.obs_polygon + self.contour + self.centers + self.cent_dyns + self.startPoints + self.endPoints)
def Simulation_vectorFields( x_range=[0, 10], y_range=[0, 10], point_grid=10, obs=[], sysDyn_init=False, xAttractor=np.array(([0, 0])), saveFigure=False, figName='default', noTicks=True, showLabel=True, figureSize=(16., 13), obs_avoidance_func=obs_avoidance_interpolation_moving, attractingRegion=False, drawVelArrow=False, colorCode=False, streamColor=[0.05, 0.05, 0.7], obstacleColor=[], plotObstacle=True, plotStream=True, figHandle=[], alphaVal=1, dynamicalSystem=linearAttractor, draw_vectorField=True, points_init=[], show_obstacle_number=False): dim = 2 # Numerical hull of ellipsoid for n in range(len(obs)): obs[n].draw_ellipsoid(numPoints=50) # 50 points resolution # Adjust dynamic center intersection_obs = obs_common_section(obs) if len(obs) > 1: dynamic_center_3d(obs, intersection_obs) if len(figHandle): fig_ifd, ax_ifd = figHandle[0], figHandle[1] else: fig_ifd, ax_ifd = plt.subplots(figsize=figureSize) if plotObstacle: obs_polygon = [] obs_polygon_sf = [] for n in range(len(obs)): x_obs_sf = obs[n].x_obs_sf # todo include in obs_draw_ellipsoid plt.plot([x_obs_sf[i][0] for i in range(len(x_obs_sf))], [x_obs_sf[i][1] for i in range(len(x_obs_sf))], 'k--') obs_polygon_sf.append(plt.Polygon(obs[n].x_obs_sf, zorder=1)) obs_polygon.append(plt.Polygon(obs[n].x_obs, zorder=2)) if len(obstacleColor) == len(obs): obs_polygon_sf[n].set_color([1, 1, 1]) obs_polygon[n].set_color(obstacleColor[n]) else: obs_polygon_sf[n].set_color([1, 1, 1]) obs_polygon[n].set_color(np.array([176, 124, 124]) / 255) plt.gca().add_patch(obs_polygon_sf[n]) plt.gca().add_patch(obs_polygon[n]) if show_obstacle_number: ax_ifd.annotate('{}'.format(n + 1), xy=np.array(obs[n].x0) + 0.16, textcoords='data', size=16, weight="bold") ax_ifd.plot(obs[n].x0[0], obs[n].x0[1], 'k.') if hasattr(obs[n], 'center_dyn'): # automatic adaptation of center ax_ifd.plot(obs[n].center_dyn[0], obs[n].center_dyn[1], 'k+', linewidth=18, markeredgewidth=4, markersize=13) if drawVelArrow and np.linalg.norm(obs[n].xd) > 0: col = [0.5, 0, 0.9] fac = 5 # scaling factor of velocity ax_ifd.arrow(obs[n].x0[0], obs[n].x0[1], obs[n].xd[0] / fac, obs[n].xd[1] / fac, head_width=0.3, head_length=0.3, linewidth=10, fc=col, ec=col, alpha=1) plt.gca().set_aspect('equal', adjustable='box') ax_ifd.set_xlim(x_range) ax_ifd.set_ylim(y_range) if noTicks: plt.tick_params(axis='both', which='major', bottom=False, top=False, left=False, right=False, labelbottom=False, labelleft=False) if showLabel: plt.xlabel(r'$\xi_1$', fontsize=16) plt.ylabel(r'$\xi_2$', fontsize=16) plt.tick_params(axis='both', which='major', labelsize=14) plt.tick_params(axis='both', which='minor', labelsize=12) ax_ifd.plot(xAttractor[0], xAttractor[1], 'k*', linewidth=18.0, markersize=18) # Show certain streamlines if np.array(points_init).shape[0]: plot_streamlines(points_init, ax_ifd, obs, xAttractor) if not draw_vectorField: plt.ion() plt.show() return start_time = time.time() # Create meshrgrid of points if type(point_grid) == int: N_x = N_y = point_grid YY, XX = np.mgrid[y_range[0]:y_range[1]:N_y * 1j, x_range[0]:x_range[1]:N_x * 1j] else: N_x = N_y = 1 XX, YY = np.array([[point_grid[0]]]), np.array([[point_grid[1]]]) if attractingRegion: # Forced to attracting Region def obs_avoidance_temp(x, xd, obs): return obs_avoidance_func(x, xd, obs, xAttractor) obs_avoidance = obs_avoidance_temp else: obs_avoidance = obs_avoidance_func xd_init = np.zeros((2, N_x, N_y)) xd_mod = np.zeros((2, N_x, N_y)) for ix in range(N_x): for iy in range(N_y): pos = np.array([XX[ix, iy], YY[ix, iy]]) xd_init[:, ix, iy] = dynamicalSystem(pos, x0=xAttractor) # initial DS xd_mod[:, ix, iy] = obs_avoidance(pos, xd_init[:, ix, iy], obs) # modulataed DS with IFD if sysDyn_init: fig_init, ax_init = plt.subplots(figsize=(5, 2.5)) res_init = ax_init.streamplot(XX, YY, xd_init[0, :, :], xd_init[1, :, :], color=[(0.3, 0.3, 0.3)]) ax_init.plot(xAttractor[0], xAttractor[1], 'k*') plt.gca().set_aspect('equal', adjustable='box') plt.xlim(x_range) plt.ylim(y_range) indOfnoCollision = obs_check_collision_2d(obs, XX, YY) dx1_noColl = np.squeeze(xd_mod[0, :, :]) * indOfnoCollision dx2_noColl = np.squeeze(xd_mod[1, :, :]) * indOfnoCollision end_time = time.time() print('Number of points: {}'.format(point_grid * point_grid)) print('Average time: {} ms'.format( np.round((end_time - start_time) / (N_x * N_y) * 1000), 5)) print('Modulation calulcation total: {} s'.format( np.round(end_time - start_time), 4)) if plotStream: if colorCode: velMag = np.linalg.norm(np.dstack((dx1_noColl, dx2_noColl)), axis=2) / 6 * 100 strm = res_ifd = ax_ifd.streamplot( XX, YY, dx1_noColl, dx2_noColl, color=velMag, cmap='winter', norm=matplotlib.colors.Normalize(vmin=0, vmax=10.)) else: # Normalize normVel = np.sqrt(dx1_noColl**2 + dx2_noColl**2) ind_nonZero = normVel > 0 dx1_noColl[ ind_nonZero] = dx1_noColl[ind_nonZero] / normVel[ind_nonZero] dx2_noColl[ ind_nonZero] = dx2_noColl[ind_nonZero] / normVel[ind_nonZero] res_ifd = ax_ifd.streamplot(XX, YY, dx1_noColl, dx2_noColl, color=streamColor, zorder=0) plt.ion() plt.show() returnFigure = True if returnFigure: return fig_ifd, ax_ifd if saveFigure: plt.savefig('fig/' + figName + '.eps', bbox_inches='tight') return fig_ifd, ax_ifd