From 5270b272c6d0e24f5161895131c95865a6e74996 Mon Sep 17 00:00:00 2001
From: jmaceiras <jeremy.maceiras@idiap.ch>
Date: Wed, 11 May 2022 14:59:23 +0200
Subject: [PATCH] [matlab] Added LQR_infHor example
---
matlab/LQR_infHor.m | 175 ++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 175 insertions(+)
create mode 100644 matlab/LQR_infHor.m
diff --git a/matlab/LQR_infHor.m b/matlab/LQR_infHor.m
new file mode 100644
index 0000000..4468647
--- /dev/null
+++ b/matlab/LQR_infHor.m
@@ -0,0 +1,175 @@
+%% Point-mass LQR with infinite horizon
+%%
+%% 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 LQR_infHor
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+param.nbData = 200; %Number of datapoints
+param.nbRepros = 4; %Number of reproductions
+
+param.param.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+param.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
+param.nbVar = param.param.nbVarPos * param.nbDeriv; %Dimension of state vector in the tangent space
+param.dt = 1E-2; %Time step duration
+param.rfactor = 4E-2; %Control cost in LQR
+
+%Control cost matrix
+R = eye(param.param.nbVarPos) * param.rfactor;
+
+%Target and desired covariance
+param.Mu = [randn(param.param.nbVarPos,1); zeros(param.param.nbVarPos*(param.nbDeriv-1),1)];
+
+[Ar,~] = qr(randn(param.param.nbVarPos));
+xCov = Ar * diag(rand(param.param.nbVarPos,1)) * Ar' * 1E-1
+
+%% Discrete dynamical System settings
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+A1d = zeros(param.nbDeriv);
+for i=0:param.nbDeriv-1
+ A1d = A1d + diag(ones(param.nbDeriv-i,1),i) * param.dt^i * 1/factorial(i); %Discrete 1D
+end
+B1d = zeros(param.nbDeriv,1);
+for i=1:param.nbDeriv
+ B1d(param.nbDeriv-i+1) = param.dt^i * 1/factorial(i); %Discrete 1D
+end
+A = kron(A1d, eye(param.param.nbVarPos)); %Discrete nD
+B = kron(B1d, eye(param.param.nbVarPos)); %Discrete nD
+
+
+%% discrete LQR with infinite horizon
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+Q = blkdiag(inv(xCov), zeros(param.param.nbVarPos*(param.nbDeriv-1))); %Precision matrix
+P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), (Q+Q')/2);
+L = (B' * P * B + R) \ B' * P * A; %Feedback gain (discrete version)
+
+%Test ratio between kp and kv
+if param.nbDeriv>1
+ kp = eigs(L(:,1:param.param.nbVarPos));
+ kv = eigs(L(:,param.param.nbVarPos+1:end));
+ ratio = kv ./ (2 * kp.^.5)
+end
+
+for n=1:param.nbRepros
+ x = [ones(param.param.nbVarPos,1)+randn(param.param.nbVarPos,1)*5E-1; zeros(param.param.nbVarPos*(param.nbDeriv-1),1)];
+ for t=1:param.nbData
+ r(n).Data(:,t) = x;
+ u = L * (param.Mu - x); %Compute acceleration (with only feedback terms)
+ x = A * x + B * u;
+ end
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+figure('position',[10,10,650,650]); hold on; axis off; grid off;
+plotGMM(param.Mu(1:2), xCov(1:2,1:2), [.8 0 0], .3);
+for n=1:param.nbRepros
+ plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',1,'color',[0 0 0]);
+end
+%plot(param.Mu(1,1), param.Mu(2,1), 'r.','markersize',80);
+axis equal;
+%print('-dpng','graphs/demo_MPC_infHor01.png');
+
+
+%% Timeline plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+labList = {'x_1','x_2','dx_1','dx_2','ddx_1','ddx_2'};
+figure('position',[720 10 600 650],'color',[1 1 1]);
+for j=1:param.nbVar
+subplot(param.nbVar+1,1,j); hold on;
+for n=1:param.nbRepros
+ plot(r(n).Data(j,:), '-','linewidth',1,'color',[0 0 0]);
+end
+if j<7
+ ylabel(labList{j},'fontsize',14,'interpreter','latex');
+end
+end
+%Speed profile
+if param.nbDeriv>1
+subplot(param.nbVar+1,1,param.nbVar+1); hold on;
+for n=1:param.nbRepros
+ sp = sqrt(r(n).Data(3,:).^2 + r(n).Data(4,:).^2);
+ plot(sp, '-','linewidth',1,'color',[0 0 0]);
+end
+ylabel('|dx|','fontsize',14);
+xlabel('t','fontsize',14);
+end
+%print('-dpng','graphs/demo_LQR_infHor01.png');
+
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+% Solve Algebraic Ricatty discrete equation
+function X = solveAlgebraicRiccati_eig_discrete(A, G, Q)
+ n = size(A,1);
+
+ %Symplectic matrix (see https://en.wikipedia.org/wiki/Algebraic_Riccati_equation)
+ %Z = [A+B*(R\B')/A'*Q, -B*(R\B')/A'; -A'\Q, A'^-1];
+ Z = [A+G/A'*Q, -G/A'; -A'\Q, inv(A')];
+
+
+ %Since Z is symplectic, if it does not have any eigenvalues on the unit circle,
+ %then exactly half of its eigenvalues are inside the unit circle.
+ [V,D] = eig(Z);
+ U = [];
+ for j=1:2*n
+ if norm(D(j,j)) < 1 %inside unit circle
+ U = [U V(:,j)];
+ end
+ end
+
+ X = real(U(n+1:end,:) / U(1:n,:));
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+% Plot a 2D Mixture of Gaussians
+function [h, X] = plotGMM(Mu, Sigma, color, valAlpha)
+ nbStates = size(Mu,2);
+ nbDrawingSeg = 100;
+ darkcolor = color * .7; %max(color-0.5,0);
+ t = linspace(-pi, pi, nbDrawingSeg);
+ if nargin<4
+ valAlpha = 1;
+ end
+
+ h = [];
+ X = zeros(2,nbDrawingSeg,nbStates);
+ for i=1:nbStates
+ [V,D] = eig(Sigma(:,:,i));
+ R = real(V*D.^.5);
+ X(:,:,i) = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg);
+ if nargin>3 %Plot with alpha transparency
+ h = [h patch(X(1,:,i), X(2,:,i), color, 'lineWidth', 1, 'EdgeColor', color, 'facealpha', valAlpha,'edgealpha', valAlpha)];
+ h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 10, 'color', darkcolor)];
+ else %Plot without transparency
+ h = [h patch(X(1,:,i), X(2,:,i), color, 'lineWidth', 1, 'EdgeColor', darkcolor)];
+ h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 10, 'color', darkcolor)];
+ end
+ end
+end
+
+
+
--
GitLab