diff --git a/python/ergodic_control_SMC_1D copy.py b/python/ergodic_control_SMC_1D copy.py deleted file mode 100644 index 9f2692041dcf79a68c8bf7ef55fc8c298c08842e..0000000000000000000000000000000000000000 --- a/python/ergodic_control_SMC_1D copy.py +++ /dev/null @@ -1,140 +0,0 @@ -""" -2D ergodic control formulated as Spectral Multiscale Coverage (SMC) objective, -with a spatial distribution described as a mixture of Gaussians. - -Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch> -Written by Cem Bilaloglu <cem.bilaloglu@idiap.ch> and -Sylvain Calinon <https://calinon.ch> - -This file is part of RCFS <https://rcfs.ch> -License: GPL-3.0-only -""" - -import numpy as np -import matplotlib.pyplot as plt - - -# Parameters -# =============================== -nbData = 2000 # Number of datapoints -nbFct = 10 # Number of basis functions -nbGaussian = 1 # Number of Gaussians to represent the spatial distribution -dt = 1e-2 # Time step -xlim = [0, 1] # Domain limit for each dimension (considered to be 1 for each dimension in this implementation) -L = (xlim[1] - xlim[0]) * 2 # Size of [-xlim(2),xlim(2)] -om = 2 * np.pi / L # omega -u_max = 1e-0 # Maximum speed allowed - -# Initial point -x0 = 0.1 - -# Desired spatial distribution represented as a mixture of Gaussians (GMM) -# gaussian centers -Mu = np.array([ - 0.7, -]) - -# Gaussian covariances -Sigma = np.ones((1, 1, nbGaussian)) * 0.01 - -# Mixing coefficients -Priors = np.ones(nbGaussian) / nbGaussian - - -# Compute Fourier series coefficients w_hat of desired spatial distribution -# =============================== -rg = np.arange(nbFct, dtype=float).reshape((nbFct, 1)) -kk = rg * om -Lambda = (rg**2 + 1) ** -1 # Weighting vector (Eq.(15) - -# Explicit description of w_hat by exploiting the Fourier transform -# properties of Gaussians (optimized version by exploiting symmetries) -w_hat = np.zeros((nbFct, 1)) -for j in range(nbGaussian): - w_hat = w_hat + Priors[j] * np.cos(kk * Mu[j]) * np.exp(-.5 * kk**2 * Sigma[:,:,j]) # Eq.(22) - -w_hat = w_hat / L - - -# Fourier basis functions (for a discretized map) -# =============================== -nbRes = 200 -xm = np.linspace(xlim[0], xlim[1], nbRes).reshape((1, nbRes)) # Spatial range for 1D -phim = np.cos(kk @ xm) * 2 # Fourier basis functions -phim[1:,:] = phim[1:,:] * 2 - -# Desired spatial distribution -g = w_hat.T @ phim - - -# Ergodic control -# =============================== -x = x0 # Initial position - -wt = np.zeros((nbFct, 1)) -r_x = np.zeros((nbData)) -r_g = np.zeros((nbRes, nbData)) -# r_w = np.zeros((nbFct, nbData)) -# r_e = np.zeros((nbData)) - -for t in range(nbData): - r_x[t] = x - - # Fourier basis functions and derivatives for each dimension - # (only cosine part on [0,L/2] is computed since the signal - # is even and real by construction) - phi = np.cos(x * kk) # Eq.(18) - - # Gradient of basis functions - dphi = -np.sin(x * kk) * kk - - # wt/t are the Fourier series coefficients along trajectory (Eq.(17)) - wt = wt + phi / L - - # Controller with constrained velocity norm - u = -dphi.T @ (Lambda * (wt / (t + 1) - w_hat)) # Eq.(24) - u = u * u_max / (np.linalg.norm(u) + 1e-2) # Velocity command - - # Update of position - x = x + (u * dt) - - # Log data - r_x[t] = x - r_g[:,t] = (wt / (t+1)).T @ phim # Reconstructed spatial distribution (for visualization) - - -# Plot -# =============================== -def gdf(x, mu, sigma): - return 1. / (np.sqrt(2. * np.pi) * sigma) * np.exp(-np.power((x - mu) / sigma, 2.) / 2) - - -fig, ax = plt.subplots(4, 1, figsize=(16, 12), gridspec_kw={'height_ratios': [3, 3, 1, 1]}) - -xx = xm.reshape((nbRes)) -for j in range(nbGaussian): - yy = gdf(xx, Mu[j], Sigma[0,0,j] * 4) - ax[0].plot(xx, yy) - ax[0].fill_between(xx, yy, alpha=0.2) - -ax[0].set_xlim(xlim[0], xlim[1]) -ax[0].set_ylim(0.0) -ax[0].title.set_text('gaussians') -ax[0].set_yticks([]) - -ax[1].plot(r_x[:], np.arange(nbData), linestyle="-", color="black") -ax[1].plot(r_x[0], [0], marker=".", color="black", markersize=10) -ax[1].set_xlim(xlim[0], xlim[1]) -ax[1].set_ylim(0, nbData) -ax[1].title.set_text('trajectory') -ax[1].set_ylabel('$t$') - -ax[2].set_title(r"$w$") -ax[2].imshow(np.reshape(wt / nbData, [1, nbFct]), cmap="gray_r") -ax[2].set_yticks([]) - -ax[3].set_title(r"$\hat{w}$") -ax[3].imshow(np.reshape(w_hat / nbData, [1, nbFct]), cmap="gray_r") -ax[3].set_yticks([]) - -plt.show()