Merge branch 'example/LQT_nullspace' into 'master'

Merge branch example/lqt nullspace on master

See merge request !9

README.md

@@ -16,6 +16,7 @@ RCFS is a collection of source codes to study robotics through simple 2D example
 | LQT | Linear quadratic tracking (LQT) applied to a viapoint task (batch formulation) | ✅ | ✅ |  |  |
 | LQT_tennisServe | LQT in a ballistic task mimicking a bimanual tennis serve problem (batch formulation) | ✅ |  |  |  |
 | LQT_recursive | LQT applied to a viapoint task with a recursive formulation based on augmented state space to find a controller) | ✅ | ✅ |  |  |
+| LQT_nullspace | Batch LQT with nullspace formulation | ✅ | ✅ |  |  |
 | LQT_recursive_LS | LQT applied to a viapoint task with a recursive formulation based on least squares and an augmented state space to find a controller | ✅ | ✅ |  |  |
 | LQT_recursive_LS_multiAgents | LQT applied to a multi-agent system with recursive formulation based on least squares and augmented state, by using a precision matrix with nonzero offdiagonal elements to find a controller in which the two agents coordinate their movements to find an optimal meeting point | ✅ | ✅ |  |  |
 | LQT_CP | LQT with control primitives applied to a viapoint task (batch formulation) | ✅ | ✅ |  |  |
@@ -43,7 +44,6 @@ Additional reading material can be be found as [video lectures](https://tube.swi
 
 | Filename | Main responsible |
 |----------|------------------|
-| Example similar to demo_OC_LQT_nullspace_minimal01.m | Hakan |
 | Example similar to demo_OC_LQT_Lagrangian01.m (inclusion of constraints) | ??? |
 
 

matlab/LQT_nullspace.m

+%%    Batch LQT with nullspace formulation
+%%
+%%    Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
+%%    Written by 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/>.
+
+function LQT_nullspace
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+param.nbData = 50; %Number of datapoints
+param.nbRepros = 60; %Number of stochastic reproductions
+param.nbPoints = 1; %Number of keypoints
+param.nbVar = 2; %Dimension of state vector
+param.dt = 1E-1; %Time step duration
+param.rfactor = 1E-4; %param.dt^nbDeriv;	%Control cost in LQR
+
+
+R = speye((param.nbData-1)*param.nbVar) * param.rfactor;
+x0 = zeros(param.nbVar,1); %Initial point
+
+
+%% Dynamical System settings
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+Su = [zeros(param.nbVar, param.nbVar*(param.nbData-1)); kron(tril(ones(param.nbData-1)), eye(param.nbVar)*param.dt)];
+Sx = kron(ones(param.nbData,1), eye(param.nbVar));
+
+
+%% Task setting
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+tl = linspace(1,param.nbData,param.nbPoints+1);
+tl = round(tl(2:end)); %[param.nbData/2, param.nbData];
+%Mu = rand(param.nbVarPos,param.nbPoints) - 0.5; 
+Mu = [20; 10];
+
+Sigma = repmat(eye(param.nbVar)*1E-3, [1,1,param.nbPoints]);
+MuQ = zeros(param.nbVar*param.nbData,1); 
+Q = zeros(param.nbVar*param.nbData);
+for t=1:length(tl)
+    id(:,t) = [1:param.nbVar] + (tl(t)-1) * param.nbVar;
+    Q(id(:,t), id(:,t)) = inv(Sigma(:,:,t));
+    MuQ(id(:,t)) = Mu(:,t);
+end
+
+
+%% Batch LQR reproduction
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%Precomputation of basis functions to generate control commands u (here, with Bernstein basis functions)
+nbRBF = 10;
+H = buildPhiBernstein(param.nbData-1,nbRBF);
+
+%Reproduction with nullspace planning
+[V,D] = eig(Q);
+U = V * D.^.5;
+J = U' * Su; %Jacobian
+
+%Left pseudoinverse solution
+pinvJ = (J' * J + R) \ J'; %Left pseudoinverse
+N = speye((param.nbData-1)*param.nbVar) - pinvJ * J; %Nullspace projection matrix
+u1 = pinvJ * U' * (MuQ - Sx * x0); %Principal task (least squares solution)
+x = reshape(Sx*x0+Su*u1, param.nbVar, param.nbData); %Reshape data for plotting
+
+%General solutions
+for n=1:param.nbRepros
+    w = randn(param.nbVar,nbRBF) * 1E1; %Random weights	
+    u2 = w * H'; %Reconstruction of control signals by a weighted superposition of basis functions
+    u = u1 + N * u2(:);
+    r(n).x = reshape(Sx*x0+Su*u, param.nbVar, param.nbData); %Reshape data for plotting
+end
+
+
+%% Plot 2D
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+for n=1:param.nbRepros
+    plot(r(n).x(1,:), r(n).x(2,:), '-','linewidth',1,'color',.9-[.7 .7 .7].*rand(1));
+end
+plot(x(1,:), x(2,:), '-','linewidth',2,'color',[.8 0 0]);
+plot(x(1,1), x(2,1), 'o','linewidth',2,'markersize',8,'color',[.8 0 0]);
+plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',30,'color',[.8 0 0]);
+axis equal; 
+%print('-dpng','graphs/LQT_nullspace02.png');
+
+pause(10);
+close all;
+end
+
+%Building Bernstein basis functions
+function phi = buildPhiBernstein(nbData, nbFct)
+	t = linspace(0, 1, nbData);
+	phi = zeros(nbData, nbFct);
+	for i=1:nbFct
+		phi(:,i) = factorial(nbFct-1) ./ (factorial(i-1) .* factorial(nbFct-i)) .* (1-t).^(nbFct-i) .* t.^(i-1);
+	end
+end
+

python/LQT_nullspace.py

+'''
+    Batch LQT with nullspace formulation
+
+    Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
+    Written by Hakan Girgin <hakan.girgin@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
+
+## 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
+
+# Parameters
+# ===============================
+param = lambda: None # Lazy way to define an empty class in python
+param.dt = 1e-1  # Time step length
+param.nbPoints = 1 # Number of targets
+param.nbDeriv = 1 # Order of the dynamical system
+param.nbVarPos = 2 # Number of position variable
+param.nbData = 50  # Number of datapoints
+param.rfactor = 1e-4  # Control weight term
+param.nbRepros = 60 # Number of stochastic reproductions
+param.nbVar = param.nbVarPos * param.nbDeriv # Dimension of state vector
+
+
+# Dynamical System settings (discrete)
+# =====================================
+
+A1d = np.zeros((param.nbDeriv,param.nbDeriv))
+B1d = np.zeros((param.nbDeriv,1))
+
+for i in range(param.nbDeriv):
+    A1d += np.diag( np.ones(param.nbDeriv-i) ,i ) * param.dt**i * 1/factorial(i)
+    B1d[param.nbDeriv-i-1] = param.dt**(i+1) * 1/factorial(i+1)
+
+A = np.kron(A1d,np.identity(param.nbVarPos))
+B = np.kron(B1d,np.identity(param.nbVarPos))
+
+# Build Sx and Su transfer matrices
+Su = np.zeros((param.nbVar*param.nbData,param.nbVarPos * (param.nbData-1)))
+Sx = np.kron(np.ones((param.nbData,1)),np.eye(param.nbVar,param.nbVar))
+
+M = B
+for i in range(1, param.nbData):
+    Sx[i*param.nbVar:param.nbData*param.nbVar,:] = np.dot(Sx[i*param.nbVar:param.nbData*param.nbVar,:], A)
+    Su[param.nbVar*i:param.nbVar*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( (param.nbData-1) * param.nbVarPos ) * param.rfactor  # Control cost matrix
+
+t_list = np.stack([param.nbData-1]) # viapoint time list
+mu_list = np.stack([np.array([20,10.])]) # viapoint list
+Q_list = np.stack([np.eye(param.nbVar)*1e3]) # viapoint precision list
+
+mus = np.zeros((param.nbData, param.nbVar))
+Qs = np.zeros((param.nbData, param.nbVar, param.nbVar))
+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
+# =====================================
+
+nbRBF = 10
+H = build_phi_bernstein(param.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(param.nbVar)
+u1 = np.linalg.solve(Su.T @ Q @ Su + R,  Su.T @ Q @ (muQ - Sx @ x0))
+x1 = (Sx @ x0 + Su @ u1).reshape((-1,param.nbVar))
+
+# Secondary Task
+repr_x = np.zeros((param.nbRepros, param.nbData, param.nbVar))
+for n in range(param.nbRepros):
+    w = np.random.randn(nbRBF, param.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, param.nbVar)) # 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(param.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(param.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,param.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(param.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,param.nbData])
+axs[1].set_xticklabels(["0","T"])
+
+plt.show()