spline refined

python/ergodic_control_SMC_1D copy.py

-"""
-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()