Uploading python version of demo_OC_LQT_nullspace01.m

python/LQT_nullspace.py

+'''
+    Linear Quadratic tracker applied on a via point example
+
+    Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
+    Written by Jeremy Maceiras <jeremy.maceiras@idiap.ch>,
+    Sylvain Calinon <https://calinon.ch>
+
+    This file is part of RCFS.
+
+    RCFS is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License version 3 as
+    published by the Free Software Foundation.
+
+    RCFS is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with RCFS. If not, see <http://www.gnu.org/licenses/>.
+'''
+
+import numpy as np
+from math import factorial
+import matplotlib.pyplot as plt
+from scipy.linalg import block_diag
+
+# Parameters
+# ===============================
+dt = 1e-1  # Time step length
+nbPoints = 1 # Number of targets
+nbDeriv = 1 # Order of the dynamical system
+nbVarPos = 2 # Number of position variable
+nbData = 50  # Number of datapoints
+rfactor = 1e-4  # Control weight term
+nbRepros = 60 # Number of stochastic reproductions
+nb_var = nbVarPos * nbDeriv # Dimension of state vector
+
+
+# Dynamical System settings (discrete)
+# =====================================
+
+A1d = np.zeros((nbDeriv,nbDeriv))
+B1d = np.zeros((nbDeriv,1))
+
+for i in range(nbDeriv):
+    A1d += np.diag( np.ones(nbDeriv-i) ,i ) * dt**i * 1/factorial(i)
+    B1d[nbDeriv-i-1] = dt**(i+1) * 1/factorial(i+1)
+
+A = np.kron(A1d,np.identity(nbVarPos))
+B = np.kron(B1d,np.identity(nbVarPos))
+
+# Build Sx and Su transfer matrices
+Su = np.zeros((nb_var*nbData,nbVarPos * (nbData-1)))
+Sx = np.kron(np.ones((nbData,1)),np.eye(nb_var,nb_var))
+
+M = B
+for i in range(1, nbData):
+    Sx[i*nb_var:nbData*nb_var,:] = np.dot(Sx[i*nb_var:nbData*nb_var,:], A)
+    Su[nb_var*i:nb_var*i+M.shape[0],0:M.shape[1]] = M
+    M = np.hstack((np.dot(A,M),B)) # [0,nb_state_var-1]
+
+# Cost function settings
+# =====================================
+
+R = np.identity( (nbData-1) * nbVarPos ) * rfactor  # Control cost matrix
+
+t_list = np.stack([nbData-1]) # viapoint time list
+mu_list = np.stack([np.array([20,10.])]) # viapoint list
+Q_list = np.stack([np.eye(nb_var)*1e3]) # viapoint precision list
+
+mus = np.zeros((nbData, nb_var))
+Qs = np.zeros((nbData, nb_var, nb_var))
+for i,t in enumerate(t_list):
+    mus[t] = mu_list[i]
+    Qs[t] = Q_list[i]
+muQ = mus.flatten()
+Q  = block_diag(*Qs)
+
+# Change here off block diagonals of Q
+
+
+# Batch LQR Reproduction
+# =====================================
+
+## Precomputation of basis functions to generate structured stochastic u through Bezier curves
+# Building Bernstein basis functions
+def build_phi_bernstein(nb_data, nb_fct):
+    t = np.linspace(0,1,nb_data)
+    phi = np.zeros((nb_data,nb_fct))
+    for i in range(nb_fct):
+        phi[:,i] = factorial(nb_fct-1) / (factorial(i) * factorial(nb_fct-1-i)) * (1-t)**(nb_fct-1-i) * t**i
+    return phi
+nbRBF = 10
+H = build_phi_bernstein(nbData-1, nbRBF)
+
+
+J = Q @ Su
+N = np.eye(J.shape[-1]) - np.linalg.pinv(J) @ J # nullspace operator of LQT
+
+# Principal Task
+x0 = np.zeros(nb_var)
+u1 = np.linalg.solve(Su.T @ Q @ Su + R,  Su.T @ Q @ (muQ - Sx @ x0))
+x1 = (Sx @ x0 + Su @ u1).reshape((-1,nb_var))
+
+# Secondary Task
+repr_x = np.zeros((nbRepros, nbData, nb_var))
+for n in range(nbRepros):
+    w = np.random.randn(nbRBF, nbVarPos) * 1E1 # Random weights
+    u2 = H @ w # Reconstruction of control signals by a weighted superposition of basis functions
+    u = u1 + N @ u2.flatten()
+    repr_x[n] = np.reshape(Sx @ x0 + Su @ u, (-1, nb_var)) # Reshape data for plotting
+
+# Plotting
+# =========
+plt.figure()
+plt.title("2D Trajectory")
+plt.scatter(x1[0,0],x1[0,1],c='black',s=100)
+for mu in mu_list:
+    plt.scatter(mu[0], mu[1],c='red',s=100)
+
+plt.plot(x1[:,0] , x1[:,1], c='black')
+for i in range(nbRepros):
+    plt.plot(repr_x[i, :, 0], repr_x[i, :, 1], c='blue', alpha=0.1, zorder=0)
+
+plt.axis("off")
+plt.gca().set_aspect('equal', adjustable='box')
+
+fig,axs = plt.subplots(2,1)
+for i,t in enumerate(t_list):
+    axs[0].scatter(t, mu_list[i][0],c='red')
+
+for i in range(nbRepros):
+    axs[0].plot(repr_x[i, :, 0], c='blue', alpha=0.1, zorder=0)
+
+axs[0].plot(x1[:,0], c='black')
+axs[0].set_ylabel("$x_1$")
+axs[0].set_xticks([0,nbData])
+axs[0].set_xticklabels(["0","T"])
+
+for i,t in enumerate(t_list):
+    axs[1].scatter(t, mu_list[i][1],c='red')
+
+for i in range(nbRepros):
+    axs[1].plot(repr_x[i, :, 1], c='blue', alpha=0.1, zorder=0)
+
+axs[1].plot(x1[:,1], c='black')
+axs[1].set_ylabel("$x_2$")
+axs[1].set_xlabel("$t$")
+axs[1].set_xticks([0,nbData])
+axs[1].set_xticklabels(["0","T"])
+
+plt.show()