Notational changes in LQR files and update of kmeans init

demo_testLQR03.m

@@ -44,9 +44,9 @@ for n=1:nbRepros
     r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
   else
     %First call to LQR to get an estimate of the final feedback terms 
-    [~,Sfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
+    [~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
     %Second call to LQR with finite horizon 
-    r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Sfinal);
+    r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal);
   end
 end
 for n=1:nbRepros

init_tensorGMM_kmeans.m

 function model = init_tensorGMM_kmeans(Data, model)
-% Author:   Leonel Rozo, 2014
-%           http://programming-by-demonstration.org/LeonelRozo
-%
+% Initialization of the model with k-means. 
+% Authors:	Sylvain Calinon, Tohid Alizadeh, 2013
+%         http://programming-by-demonstration.org/
 
 diagRegularizationFactor = 1E-4;
 
-%Matricization/flattening of tensor
-DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); 
+DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor
 
-%The function 'kmeans' below is from the Matlab Statistics Toolbox (see note above)
-[Data_id, Centers] = kmeans(DataAll', model.nbStates);
+%k-means clustering
+[Data_id, Mu] = kmeansClustering(DataAll, model.nbStates);
 
-% Setting means and covariance matrices
-Mu = Centers';
-Sigma = zeros(model.nbFrames*model.nbVar, model.nbFrames*model.nbVar, model.nbStates);
-for i = 1 : model.nbStates
+for i=1:model.nbStates
   idtmp = find(Data_id==i);
   model.Priors(i) = length(idtmp);
-  Sigma(:,:,i) = cov(DataAll(:,idtmp)') + eye(size(DataAll,1))*diagRegularizationFactor;
+  Sigma(:,:,i) = cov([DataAll(:,idtmp) DataAll(:,idtmp)]') + eye(size(DataAll,1))*diagRegularizationFactor;
 end
 model.Priors = model.Priors / sum(model.Priors);
 
-%Reshape GMM parameters into a tensor 
-for m = 1 : model.nbFrames
-  for i = 1 : model.nbStates
-    model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i); 
-    model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i); 
+%Reshape GMM parameters into a tensor
+for m=1:model.nbFrames
+  for i=1:model.nbStates
+    model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i);
+    model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i);
   end
 end
 
 
+
+
+

kmeansClustering.m

 function [idList, Mu] = kmeansClustering(Data, nbStates)
 % Initialization of the model with k-means. 
-% Author:	Sylvain Calinon, Tohid Alizadeh, 2013
+% Authors:	Sylvain Calinon, Tohid Alizadeh, 2013
 %         http://programming-by-demonstration.org/
 
 %Criterion to stop the EM iterative update

reproduction_LQR_finiteHorizon.m

-function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Sfinal)
+function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Pfinal)
 % Reproduction with a linear quadratic regulator of finite horizon
 %
 % Authors: Sylvain Calinon and Danilo Bruno, 2014
@@ -28,23 +28,23 @@ B = kron([0; 1], eye(nbVarOut));
 %Initialize Q and R weighting matrices
 Q = zeros(nbVarOut*2,nbVarOut*2,nbData);
 for t=1:nbData
-  Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t)); 
+	Q(1:nbVarOut,1:nbVarOut,t) = inv(r.currSigma(:,:,t)); 
 end
 R = eye(nbVarOut) * rFactor;
 
 if nargin<6
-  %Sfinal = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]
-  Sfinal = B*R*[eye(nbVarOut)*0, eye(nbVarOut)*0]; %final feedback terms (boundary conditions)
+	%Pfinal = B*R*[eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]
+	Pfinal = B*R*[eye(nbVarOut)*0, eye(nbVarOut)*0]; %final feedback terms (boundary conditions)
 end
 
 %Auxiliary variables to minimize the cost function
-S = zeros(nbVarOut*2, nbVarOut*2, nbData);
+P = zeros(nbVarOut*2, nbVarOut*2, nbData);
 d = zeros(nbVarOut*2, nbData); %For optional feedforward term computation
 
 %Feedback term
 L = zeros(nbVarOut, nbVarOut*2, nbData);
-%Compute S_T from the desired final feedback gains L_T,  
-S(:,:,nbData) = Sfinal; 
+%Compute P_T from the desired final feedback gains L_T,  
+P(:,:,nbData) = Pfinal; 
 
 %Compute derivative of target path
 %dTar = diff(r.currTar, 1, 2); %For optional feedforward term computation
@@ -62,14 +62,14 @@ dtar = gradient(tar,1,2)/model.dt;
 
 %Backward integration of the Riccati equation and additional equation
 for t=nbData-1:-1:1
-  S(:,:,t) = S(:,:,t+1) + model.dt * (A'*S(:,:,t+1) + S(:,:,t+1)*A - S(:,:,t+1) * B * (R\B') * S(:,:,t+1) + Q(:,:,t+1));  
-  %Optional feedforward term computation
-	d(:,t) = d(:,t+1) + model.dt * ((A'-S(:,:,t+1)*B*(R\B'))*d(:,t+1) +  S(:,:,t+1)*dtar(:,t+1) - S(:,:,t+1)*A*tar(:,t+1)); 
+	P(:,:,t) = P(:,:,t+1) + model.dt * (A'*P(:,:,t+1) + P(:,:,t+1)*A - P(:,:,t+1)*B*(R\B')*P(:,:,t+1) + Q(:,:,t+1));  
+	%Optional feedforward term computation
+	d(:,t) = d(:,t+1) + model.dt * ((A'-P(:,:,t+1)*B*(R\B'))*d(:,t+1) +  P(:,:,t+1)*dtar(:,t+1) - P(:,:,t+1)*A*tar(:,t+1)); 
 end
-%Computation of the feedback term L in u=-LX+M
+%Computation of the feedback term L and feedforward term M in u=-LX+M
 for t=1:nbData
-  L(:,:,t) = R\B' * S(:,:,t); 
-  M(:,t) = R\B' * d(:,t); %Optional feedforward term computation
+	L(:,:,t) = R\B' * P(:,:,t); 
+	M(:,t) = R\B' * d(:,t); %Optional feedforward term computation
 end
 
 %% Reproduction with varying impedance parameters
@@ -77,26 +77,22 @@ end
 x = currPos;
 dx = zeros(nbVarOut,1);
 for t=1:nbData
-  %Compute acceleration (with only feedback term)
-  %ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx];
-  
-  %Compute acceleration (with both feedback and feedforward terms)
-  ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t);
-  r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
-  r.FF(:,t) = M(:,t);
-  
-  %ddx =  -L * ([x;dx]-tar(:,t)) + M;
-  %r.FB(:,t) = -L * ([x;dx]-tar(:,t));
-  %r.FF(:,t) = M;
-  
-  %Update velocity and position
-  dx = dx + ddx * model.dt;
-  x = x + dx * model.dt;
-  %Log data
-  r.Data(:,t) = [DataIn(:,t); x]; 
-  r.ddxNorm(t) = norm(ddx);
-  r.kpDet(t) = det(L(:,1:nbVarOut,t));
-  r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
+	%Compute acceleration (with only feedback term)
+	%ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx];
+
+	%Compute acceleration (with both feedback and feedforward terms)
+	ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t);
+	%r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
+	%r.FF(:,t) = M(:,t);
+
+	%Update velocity and position
+	dx = dx + ddx * model.dt;
+	x = x + dx * model.dt;
+	%Log data
+	r.Data(:,t) = [DataIn(:,t); x]; 
+	r.ddxNorm(t) = norm(ddx);
+	r.kpDet(t) = det(L(:,1:nbVarOut,t));
+	r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
 end
 
 

reproduction_LQR_infiniteHorizon.m

-function [r,S] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor)
+function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor)
 % Reproduction with a linear quadratic regulator of infinite horizon 
 %
 % Authors: Sylvain Calinon and Danilo Bruno, 2014
@@ -32,14 +32,9 @@ R = eye(nbVarOut) * rFactor;
 %Variables for feedforward term computation (optional for movements with low dynamics)
 %tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
 %dtar = gradient(tar,1,2)/model.dt;
-
 tar = [r.currTar; zeros(nbVarOut,nbData)];
 dtar = gradient(tar,1,2)/model.dt;
 
-%tar = [r.currTar; diff([r.currTar(:,1) r.currTar],1,2)/model.dt];
-%dtar = diff([tar tar(:,1)],1,2)/model.dt;
-
-
   
 %% Reproduction with varying impedance parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -51,16 +46,16 @@ for t=1:nbData
   
   %care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation, 
 	%also available with the control toolbox of GNU Octave
-  S = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q 
+  P = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q 
   
   %Alternatively the function below can be used for an implementation based on Schur decomposition.
-  %S = solveAlgebraicRiccati(A, B/R*B', (Q+Q')/2);
+  %P = solveAlgebraicRiccati(A, B/R*B', (Q+Q')/2);
   
   %Variable for feedforward term computation (optional for movements with low dynamics)
-  d = inv(S*B*(R\B')-A') * (S*dtar(:,t) - S*A*tar(:,t));
+  d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t));
   
   %Feedback term
-  L = R\B'*S;
+  L = R\B'*P;
   %Feedforward term
   M = R\B'*d;
   
@@ -69,11 +64,7 @@ for t=1:nbData
   
   %Compute acceleration (with feedback and feedforward terms)
   ddx =  -L * [x-r.currTar(:,t); dx] + M;
-  r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
-  r.FF(:,t) = M;
-  
-  %ddx =  -L * ([x;dx]-tar(:,t)) + M;
-  %r.FB(:,t) = -L * ([x;dx]-tar(:,t));
+  %r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
   %r.FF(:,t) = M;
   
   %Update velocity and position