diff --git a/demos/demo_Bezier01.m b/demos/demo_Bezier01.m
index 15d803a7c68d9f2a897ca5d254e34452963b34bf..14a98078bd098178626e081aa8ad685eeca1da81 100644
--- a/demos/demo_Bezier01.m
+++ b/demos/demo_Bezier01.m
@@ -50,19 +50,20 @@ x = p * B; %Reconstruction of signal
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,800,1200]);
+figure('position',[10,10,800,800]);
subplot(2,1,1); hold on; axis off;
-plot(p(1,:), p(2,:), 'r.','markersize',20);
+plot(p(1,:), p(2,:), '.','markersize',30,'color',[.6 .6 .6]);
if nbDeg==3
- plot(p(1,[1:2]), p(2,[1:2]), 'r-','linewidth',2);
- plot(p(1,[3:4]), p(2,[3:4]), 'r-','linewidth',2);
+ plot(p(1,[1:2]), p(2,[1:2]), '-','linewidth',2,'color',[.6 .6 .6]);
+ plot(p(1,[3:4]), p(2,[3:4]), '-','linewidth',2,'color',[.6 .6 .6]);
end
-plot(x(1,:), x(2,:), 'k-','linewidth',3);
+plot(x(1,:), x(2,:), '-','linewidth',3,'color',[0 0 0]);
subplot(2,1,2); hold on;
for i=0:nbDeg
plot(t, B(i+1,:),'linewidth',3);
end
-xlabel('t','fontsize',16); ylabel('b_i','fontsize',16);
+set(gca,'xtick',[0,1],'ytick',[0,1],'fontsize',16);
+xlabel('t','fontsize',18); ylabel('b_i','fontsize',18);
pause;
-close all;
+close all;
\ No newline at end of file
diff --git a/demos/demo_Bezier02.m b/demos/demo_Bezier02.m
index 3cd7d1473428fb116e8115d5b353d52deed846b2..cf550c0f4c37fdb6caba18f112e904cefe85e10f 100644
--- a/demos/demo_Bezier02.m
+++ b/demos/demo_Bezier02.m
@@ -56,30 +56,32 @@ x2 = phi * w; %Reconstruction
%Alternative computation through vectorization (useful for extending the approach to proMP representations including trajectory covariance)
Psi = kron(phi, eye(nbVar));
-w = pinv(Psi) * x(:); %Control points
-x2Vec = Psi * w; %Reconstruction
+w2 = pinv(Psi) * x(:); %Control points
+x2Vec = Psi * w2; %Reconstruction
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,800,1200]);
subplot(2,1,1); hold on; axis off;
-plot(x(:,1), x(:,2), '-','linewidth',2,'color',[.7 .7 .7]);
+% plot(w(:,1), w(:,2), '.','markersize',30,'color',[.6 .6 .6]);
+plot(x(1,:), x(2,:), '-','linewidth',6,'color',[.7 .7 .7]);
plot(x2(:,1), x2(:,2), 'k-','linewidth',2);
plot(x2Vec(1:2:end), x2Vec(2:2:end), 'r--','linewidth',2);
% plot(w(1,:), w(2,:), 'r.','markersize',20);
subplot(2,1,2); hold on;
for i=0:nbDeg
- plot(t,phi(:,i+1));
+ plot(t, phi(:,i+1), '-','linewidth',3);
end
-xlabel('t'); ylabel('b_i');
+xlabel('t','fontsize',18); ylabel('b_i','fontsize',18);
+set(gca,'xtick',[0,1],'ytick',[0,1],'fontsize',16);
-%Plot covariance
-figure('position',[1640 10 800 800]); hold on;
-colormap(flipud(gray));
-pcolor(abs(Psi*Psi'));
-set(gca,'xtick',[1,nbData*nbVar],'ytick',[1,nbData*nbVar]);
-axis square; axis([1 nbData*nbVar 1 nbData*nbVar]); shading flat;
+% %Plot covariance
+% figure('position',[1640 10 800 800]); hold on;
+% colormap(flipud(gray));
+% pcolor(abs(Psi*Psi'));
+% set(gca,'xtick',[1,nbData*nbVar],'ytick',[1,nbData*nbVar]);
+% axis square; axis([1 nbData*nbVar 1 nbData*nbVar]); shading flat;
pause;
-close all;
+close all;
\ No newline at end of file
diff --git a/demos/demo_Bezier_illustr01.m b/demos/demo_Bezier_illustr01.m
index f742934eedd5fa326e84b292655f317277043de3..783f8c0f6aa2e42199a4dc793ca0243846338e4d 100644
--- a/demos/demo_Bezier_illustr01.m
+++ b/demos/demo_Bezier_illustr01.m
@@ -78,4 +78,4 @@ end %nb
% print('-dpng','graphs/demo_Bezier_illustr01.png');
pause;
-close all;
+close all;
\ No newline at end of file
diff --git a/demos/demo_Bezier_illustr02.m b/demos/demo_Bezier_illustr02.m
index f140a92a50682e191a004e0fb9445dfcabd8e1ee..9d6b23e9357a2f342fa929d9376092cb52da7de2 100644
--- a/demos/demo_Bezier_illustr02.m
+++ b/demos/demo_Bezier_illustr02.m
@@ -163,4 +163,4 @@ axis equal;
% % print('-dpng','graphs/demo_Bezier_highOrder02.png');
pause;
-close all;
+close all;
\ No newline at end of file
diff --git a/demos/demo_GPR_closedShape02.m b/demos/demo_GPR_closedShape02.m
index ac6642463ae0390305070d9d7f738914cfd2ef58..ec3f34f0ea3e307aed6636527a19375c5554360e 100644
--- a/demos/demo_GPR_closedShape02.m
+++ b/demos/demo_GPR_closedShape02.m
@@ -19,10 +19,10 @@ function demo_GPR_closedShape02
% year = "2007"
% }
%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
+% Copyright (c) 2019 y==1ap Research Institute, http://y==1ap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+% This file is part of PbDlib, http://www.y==1ap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
@@ -39,15 +39,11 @@ function demo_GPR_closedShape02
% addpath('./m_fcts/');
-
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbDataRepro = 50^2; %Number of datapoints in a reproduction
-nbVarX = 2; %Dimension of x
-% nbVarY = 1; %Dimension of y
-% nbRepros = 6; %Number of randomly sampled reproductions
-% SigmaY = eye(nbVarY);
-p(1)=1E0; p(2)=1E-5; %Thin-plate covariance function parameters
+% nbVarX = 2; %Dimension of x
+p = [1E0, 1E-5]; %Thin-plate covariance function parameters
%% Generate data
@@ -60,71 +56,90 @@ p(1)=1E0; p(2)=1E-5; %Thin-plate covariance function parameters
% y = [zeros(1,nbData-5), 1, -1, -1, -1, -1]; %0, 1 and -1 represent border, interior, and exterior points
%mean(x,2)+[-1;0], mean(x,2)+[1;0], mean(x,2)+[0;-1], mean(x,2)+[0;1]
-x0 = [3 3 2 2 0 -1 -1 -3 -3;
- 2 1 0 -1 -2 -2 -1 2 1];
-x = [x0, mean(x0,2), [-3;3], [-2;-3], [4;-2], [-3;3], [4;2], [2;-2] [4;4]] * 1E-1;
-y = [zeros(1,size(x0,2)), 1, -1, -1, -1, -1, -1, -1, -1]; %0, 1 and -1 represent border, interior, and exterior points
-Data = [x; y];
+% x0 = [3 3 2 2 0 -1 -1 -3 -3;
+% 2 1 0 -1 -2 -2 -1 2 1];
+% x = [x0, mean(x0,2), [-3;3], [-2;-3], [4;-2], [-3;3], [4;2], [2;-2] [4;4]] * 1E-1;
+% y = [zeros(1,size(x0,2)), 1, -1, -1, -1, -1, -1, -1, -1]; %0, 1 and -1 represent border, interior, and exterior points
+
+% x = [-.3, -.1, .1; 0, 0, 0];
+% y = [-1, 0, 1]; %-1, 0 and 1 represent exterior, border and interior points
+
+% x = rand(2,15) - .5;
+% y = randi(3,1,15) - 2; %-1, 0 and 1 represent exterior, border and interior points
+
+% x = [-0.3, -0.1, 0.1, 0.0, 0.4, 0.4, 0.3, 0.0, -0.3, 0.0, 0.2, 0.0, 0.4; ...
+% 0.0, 0.0, 0.0, 0.4, -0.4, 0.4, 0.1, 0.2, 0.3, -0.4, -0.2, -0.3, -0.2];
+% y = [-1, 0, 1, -1, -1, -1, 0, 0, -1, -1, 0, 0, 0];
+
+% x = [-0.3, -0.2, 0.1, -0.2, 0.2, 0.3, 0.4; ...
+% 0.0, 0.0, 0.0, 0.4, -0.2, 0.2, -0.4];
+% y = [-1, 0, 1, -1, 0 0, -1];
+
+x = [-0.4500 -0.0006 0.0829 -0.3480 -0.1983; ...
+ -0.4500 -0.2000 -0.3920 -0.3451 -0.0423];
+y = [-1 1 0 0 0];
%% Reproduction with GPR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Mean computation
[Xm, Ym] = meshgrid(linspace(-.5,.5,nbDataRepro^.5), linspace(-.5,.5,nbDataRepro^.5));
xs = [Xm(:)'; Ym(:)'];
p(1) = max(max(pdist2(xs', xs'))); %Refine thin-plate covariance function parameter
-rc = 4E-1;
-S = eye(nbVarX) * rc.^-2;
-MuS = .5 * rc * diag(1 - xs' * S * xs)';
-Mu = .5 * rc * diag(1 - x' * S * x)';
+%Circle as geometric prior
+rc = 4E-1; %Radius of circle
+xc = [.05; .05]; %Location of circle
+S = eye(2) * rc^-2;
+MuS = .5 * rc * diag(1 - (xs-repmat(xc,1,nbDataRepro))' * S * (xs-repmat(xc,1,nbDataRepro)))';
+Mu = .5 * rc * diag(1 - (x-repmat(xc,1,size(x,2)))' * S * (x-repmat(xc,1,size(x,2))))';
K = covFct(x, x, p, 1); %Inclusion of noise on the inputs for the computation of K
[Ks, dKs] = covFct(xs, x, p);
-% r(1).Data = [xs; (Ks / K * y')']; %GPR with Mu=0
-r(1).Data = [xs; MuS + (Ks / K * (y - Mu)')'];
-r(1).dData = [xs; (dKs(:,:,1) / K * (y - Mu)')'; (dKs(:,:,2) / K * (y - Mu)')']; %Gradients
-a = min(r(1).Data(3,:),0); %Amplitude
-for t=1:size(r(1).dData,2)
- r(1).dData(3:4,t) = - exp(a(t)) * r(1).dData(3:4,t) / norm(r(1).dData(3:4,t)); %Vector moving away from contour, with stronger influence when close to the border
-end
+% ry = (Ks / K * y')'; %GPR with Mu=0
+ry = MuS + (Ks / K * (y - Mu)')';
+rdy = [(dKs(:,:,1) / K * (y - Mu)')'; (dKs(:,:,2) / K * (y - Mu)')']; %Gradients
+
+%Redefine gradient
+% a = min(ry,0); %Amplitude
+% for t=1:size(rdy,2)
+% rdy(:,t) = - exp(a(t)) * rdy(:,t) / norm(rdy(:,t)); %Vector moving away from contour, with stronger influence when close to the border
+% end
-% %Uncertainty evaluation
-% Kss = covFct(xs, xs, p);
-% S = Kss - Ks / K * Ks';
-% r(1).SigmaOut = zeros(nbVarY, nbVarY, nbData);
+%Uncertainty evaluation
+Kss = covFct(xs, xs, p);
+S = Kss - Ks / K * Ks';
+% nbVarY = 1; %Dimension of y
+% SigmaY = eye(nbVarY);
+% SigmaOut = zeros(nbVarY, nbVarY, nbDataRepro);
% for t=1:nbDataRepro
-% r(1).SigmaOut(:,:,t) = SigmaY * S(t,t);
+% SigmaOut(:,:,t) = SigmaY * S(t,t);
% end
% %Generate stochastic samples from the prior
+% nbRepros = 6; %Number of randomly sampled reproductions
% [V,D] = eig(Kss);
% for n=2:nbRepros/2
% yp = real(V*D^.5) * randn(nbDataRepro, nbVarY) * SigmaY .* 2E-1;
-% r(n).Data = [xs; yp'];
+% r(n).y = yp';
% end
%
% %Generate stochastic samples from the posterior
% [V,D] = eig(S);
% for n=nbRepros/2+1:nbRepros
-% ys = real(V*D^.5) * randn(nbDataRepro, nbVarY) * SigmaY .* 0.5 + r(1).Data(nbVarX+1:end,:)';
-% r(n).Data = [xs; ys'];
+% ys = real(V*D^.5) * randn(nbDataRepro, nbVarY) * SigmaY .* 0.5 + ry(nbVarX+1:end,:)';
+% r(n).y = ys';
% end
-% %% Spatial plots
-% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-idi = (Data(3,:)==1);
-idb = (Data(3,:)==0);
-ide = (Data(3,:)==-1);
-
-figure('position',[10 10 2300 1300],'color',[1,1,1]); hold on; axis off; %'PaperPosition',[0 0 14 5]
+%% Spatial plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10 10 1200 800],'color',[1,1,1]); hold on; axis off; %'PaperPosition',[0 0 14 5]
set(0,'DefaultAxesLooseInset',[0,0,0,0]);
set(gca,'LooseInset',[0,0,0,0]);
-colormap(repmat(linspace(1,0,64),3,1)');
+colormap(repmat(linspace(1,.4,64),3,1)');
%Plot center of GP (acting as prior if points on the contours are missing)
-subplot(2,3,1); hold on; axis off; rotate3d on; title('Prior (circular contour)','fontsize',20);
+subplot(2,5,1); hold on; axis off; rotate3d on; title('Prior (circular contour)','fontsize',16);
% for n=2:nbRepros/2
% coltmp = [.5 .5 .5] + [.5 .5 .5].*rand(1);
% mesh(Xm, Ym, reshape(r(n).Data(3,:), nbDataRepro^.5, nbDataRepro^.5), 'facealpha',.4,'edgealpha',.4,'facecolor',coltmp,'edgecolor',coltmp); %Prior samples
@@ -132,87 +147,106 @@ subplot(2,3,1); hold on; axis off; rotate3d on; title('Prior (circular contour)'
mesh(Xm, Ym, reshape(MuS, nbDataRepro^.5, nbDataRepro^.5), 'facealpha',.8,'edgealpha',.8,'edgecolor',[.7 .7 .7]);
mesh(Xm, Ym, zeros(nbDataRepro^.5, nbDataRepro^.5), 'facealpha',.3,'edgealpha',.6,'facecolor',[0 0 0],'edgecolor','none');
tl = linspace(0,2*pi,100);
-plot(cos(tl)*rc, sin(tl)*rc, 'k-','linewidth',2);
+plot3(xc(1)+cos(tl)*rc, xc(2)+sin(tl)*rc, zeros(1,100), '-','linewidth',2,'color',[0 0 0]);
view(3); axis vis3d;
%Plot posterior distribution (3D)
-subplot(2,3,4); hold on; axis off; rotate3d on; title('Posterior (3d plot)','fontsize',20);
+subplot(2,5,6); hold on; axis off; rotate3d on; title('Posterior','fontsize',16);
% for n=nbRepros/2+1:nbRepros
% coltmp = [.5 .5 .5] + [.5 .5 .5].*rand(1);
% mesh(Xm, Ym, reshape(r(n).Data(3,:), nbDataRepro.^.5, nbDataRepro.^.5), 'facealpha',.4,'edgealpha',.4,'facecolor',coltmp,'edgecolor',coltmp); %posterior samples
% end
-mesh(Xm, Ym, reshape(r(1).Data(3,:), nbDataRepro^.5, nbDataRepro^.5), 'facealpha',.8,'edgealpha',.8,'edgecolor',[.7 .7 .7]);
+mesh(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), 'facealpha',.8,'edgealpha',.8,'edgecolor',[.7 .7 .7]);
mesh(Xm, Ym, zeros(nbDataRepro^.5, nbDataRepro^.5), 'facealpha',.3,'edgealpha',.6,'facecolor',[0 0 0],'edgecolor','none');
-contour(Xm, Ym, reshape(r(1).Data(3,:), nbDataRepro^.5, nbDataRepro^.5), [0,0], 'linewidth',2,'color',[0 0 0]);
-plot3(Data(1,idi), Data(2,idi), Data(3,idi)+.04, '.','markersize',18,'color',[.8 0 0]); %Interior points
-plot3(Data(1,idb), Data(2,idb), Data(3,idb)+.04, '.','markersize',18,'color',[.8 .4 0]); %Border points
-plot3(Data(1,ide), Data(2,ide), Data(3,ide)+.04, '.','markersize',18,'color',[0 .6 0]); %Exterior points
+contour(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), [0,0], 'linewidth',2,'color',[0 0 0]);
+plot3(x(1,y==1), x(2,y==1), y(y==1)+.04, '.','markersize',18,'color',[.8 0 0]); %Interior points
+plot3(x(1,y==0), x(2,y==0), y(y==0)+.04, '.','markersize',18,'color',[.8 .4 0]); %Border points
+plot3(x(1,y==-1), x(2,y==-1), y(y==-1)+.04, '.','markersize',18,'color',[0 .6 0]); %Exterior points
view(3); axis vis3d;
%Plot posterior distribution (2D)
-subplot(2,3,[2,3,5,6]); hold on; axis off; title('Posterior (2d plot)','fontsize',20);
-surface(Xm, Ym, reshape(r(1).Data(3,:), nbDataRepro^.5, nbDataRepro^.5)-2, 'FaceColor','interp','EdgeColor','interp');
-quiver(r(1).dData(1,:), r(1).dData(2,:), r(1).dData(3,:), r(1).dData(4,:),'color',[0 0 .8]);
-h(1) = plot(Data(1,idi), Data(2,idi), '.','markersize',28,'color',[.8 0 0]); %Interior points
-h(2) = plot(Data(1,idb), Data(2,idb), '.','markersize',28,'color',[.8 .4 0]); %Border points
-h(3) = plot(Data(1,ide), Data(2,ide), '.','markersize',28,'color',[0 .6 0]); %Exterior points
-contour(Xm, Ym, reshape(r(1).Data(3,:), nbDataRepro.^.5, nbDataRepro.^.5), [0,0], 'linewidth',2,'color',[0 0 0]);
-[c, h(4)] = contour(Xm, Ym, reshape(r(1).Data(3,:), nbDataRepro.^.5, nbDataRepro.^.5), [0,0], 'linewidth',2,'color',[0 0 0]);
-% h(4) = plot3(c(1,2:end), c(2,2:end), ones(1,size(c,2)-1), '-','linewidth',2,'color',[0 0 0]);
-% contour(Xm, Ym, reshape(r(1).Data(3,:)+2E0.*diag(S).^.5', nbDataRepro.^.5, nbDataRepro.^.5), [0,0], 'linewidth',1,'color',[.6 .6 .6]);
-% contour(Xm, Ym, reshape(r(1).Data(3,:)-2E0.*diag(S).^.5', nbDataRepro.^.5, nbDataRepro.^.5), [0,0], 'linewidth',1,'color',[.6 .6 .6]);
+subplot(2,5,[2,3,7,8]); hold on; axis off; title('Distance to contour and gradient','fontsize',16);
+mshbrd = [-.5 -.5 .5 .5 -.5; -.5 .5 .5 -.5 -.5];
+plot(mshbrd(1,:), mshbrd(2,:), '-','linewidth',1,'color',[0 0 0]);
+surface(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5)-max(ry), 'FaceColor','interp','EdgeColor','interp');
+quiver(xs(1,:), xs(2,:), rdy(1,:), rdy(2,:),'color',[.2 .2 .2]);
+% quiver(xs(1,ry>0), xs(2,ry>0), rdy(1,ry>0), rdy(2,ry>0), 'color',[.8 .2 .2]);
+% quiver(xs(1,ry<0), xs(2,ry<0), rdy(1,ry<0), rdy(2,ry<0), 'color',[.2 .7 .2]);
+h(1) = plot(x(1,y==1), x(2,y==1), '.','markersize',28,'color',[.8 0 0]); %Interior points
+h(2) = plot(x(1,y==0), x(2,y==0), '.','markersize',28,'color',[.8 .4 0]); %Border points
+h(3) = plot(x(1,y==-1), x(2,y==-1), '.','markersize',28,'color',[0 .6 0]); %Exterior points
+h(4) = plot(xc(1)+cos(tl)*rc, xc(2)+sin(tl)*rc, '--','linewidth',2,'color',[0 0 0]);
+[~,h(5)] = contour(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), [0,0], 'linewidth',2,'color',[0 0 0]);
+% contour(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), [-.4,-.4], 'linewidth',2,'color',[.2 .2 .2]);
+% contour(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), [-.8,-.8], 'linewidth',2,'color',[.4 .4 .4]);
+% contour(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), [-1.2,-1.2], 'linewidth',2,'color',[.6 .6 .6]);
+% patch(c(1,2:end), c(2,2:end), ones(1,size(c,2)-1), [.8 0 0], 'edgecolor','none','facealpha', .1);
+% contour(Xm, Ym, reshape(ry(3,:)+2E0.*diag(S).^.5', nbDataRepro.^.5, nbDataRepro.^.5), [0,0], 'linewidth',1,'color',[.6 .6 .6]);
+% contour(Xm, Ym, reshape(ry(3,:)-2E0.*diag(S).^.5', nbDataRepro.^.5, nbDataRepro.^.5), [0,0], 'linewidth',1,'color',[.6 .6 .6]);
axis tight; axis equal; axis([-.5 .5 -.5 .5]); axis ij;
-legend(h,{'Demonstrated interior points (y=1)','Demonstrated contour points (y=0)','Demonstrated exterior points (y=-1)','Estimated contour'},'fontsize',20);
+legend(h,{'Demonstrated interior points (y=1)','Demonstrated contour points (y=0)','Demonstrated exterior points (y=-1)','Prior for contour estimation','Estimated contour'},'fontsize',16,'location','southwest');
% legend('boxoff');
-% print('-dpng','graphs/GPR_GPIS01.png');
+%Plot uncertainty (2D)
+subplot(2,5,[4,5,9,10]); hold on; axis off; title('Uncertainty','fontsize',16);
+plot(mshbrd(1,:), mshbrd(2,:), '-','linewidth',1,'color',[0 0 0]);
+% surface(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5)-max(ry), 'FaceColor','interp','EdgeColor','interp');
+surface(Xm, Ym, reshape(diag(S), nbDataRepro^.5, nbDataRepro^.5)-max(S(:)), 'FaceColor','interp','EdgeColor','interp');
+plot(x(1,y==1), x(2,y==1), '.','markersize',28,'color',[.8 0 0]); %Interior points
+plot(x(1,y==0), x(2,y==0), '.','markersize',28,'color',[.8 .4 0]); %Border points
+plot(x(1,y==-1), x(2,y==-1), '.','markersize',28,'color',[0 .6 0]); %Exterior points
+contour(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), [0,0], 'linewidth',2,'color',[0 0 0]);
+plot(xc(1)+cos(tl)*rc, xc(2)+sin(tl)*rc, '--','linewidth',2,'color',[0 0 0]);
+axis tight; axis equal; axis([-.5 .5 -.5 .5]); axis ij;
+
+% print('-dpng','graphs/GPIS01.png');
pause;
close all;
end
+%%%%%%%%%%%%%%%%%%%%%%%%%
function d = substr(x1, x2)
d = x1 - x2;
end
+%%%%%%%%%%%%%%%%%%%%%%%%%
function [K, dK] = covFct(x1, x2, p, flag_noiseObs)
if nargin<4
flag_noiseObs = 0;
end
- %Thin plate covariance function (for 3D implicit shape)
- K = 12^-1 * (2 * pdist2(x1',x2').^3 - 3 * p(1) * pdist2(x1',x2').^2 + p(1)^3);
- dK(:,:,1) = 12^-1 * (6 * pdist2(x1',x2') .* substr(x1(1,:)',x2(1,:)) - 6 * p(1) * substr(x1(1,:)',x2(1,:))); %Derivatives along x1
- dK(:,:,2) = 12^-1 * (6 * pdist2(x1',x2') .* substr(x1(2,:)',x2(2,:)) - 6 * p(1) * substr(x1(2,:)',x2(2,:))); %Derivatives along x2
-% for i=1:size(x1,2)
-% for j=1:size(x2,2)
-% e = x1(:,i) - x2(:,j);
-% % K(i,j) = 12^-1 * (2 * pdist2(x1(:,i)',x2(:,j)')^3 - 3 * p(1) * pdist2(x1(:,i)',x2(:,j)')^2 + p(1)^3);
-% % K(i,j) = 12^-1 * (2 * norm(e)^3 - 3 * p(1) * e'*e + p(1)^3);
-% K(i,j) = 12^-1 * (2 * (e'*e)^1.5 - 3 * p(1) * e'*e + p(1)^3); %Kernel (slow one by one computation)
-% dK(i,j,:) = 12^-1 * (6 * (e'*e)^.5 * e - 6 * p(1) * e); %Derivatives (slow one by one computation)
-% end
-% end
+% %Thin plate covariance function (for 3D implicit shape)
+% K = 12^-1 * (2 * pdist2(x1',x2').^3 - 3 * p(1) * pdist2(x1',x2').^2 + p(1)^3); %Kernel
+% dK(:,:,1) = 12^-1 * (6 * pdist2(x1',x2') .* substr(x1(1,:)',x2(1,:)) - 6 * p(1) * substr(x1(1,:)',x2(1,:))); %Derivatives along x1
+% dK(:,:,2) = 12^-1 * (6 * pdist2(x1',x2') .* substr(x1(2,:)',x2(2,:)) - 6 * p(1) * substr(x1(2,:)',x2(2,:))); %Derivatives along x2
+% % for i=1:size(x1,2)
+% % for j=1:size(x2,2)
+% % e = x1(:,i) - x2(:,j);
+% % % K(i,j) = 12^-1 * (2 * pdist2(x1(:,i)',x2(:,j)')^3 - 3 * p(1) * pdist2(x1(:,i)',x2(:,j)')^2 + p(1)^3);
+% % % K(i,j) = 12^-1 * (2 * norm(e)^3 - 3 * p(1) * e'*e + p(1)^3);
+% % K(i,j) = 12^-1 * (2 * (e'*e)^1.5 - 3 * p(1) * e'*e + p(1)^3); %Kernel (slow one by one computation)
+% % dK(i,j,:) = 12^-1 * (6 * (e'*e)^.5 * e - 6 * p(1) * e); %Derivatives (slow one by one computation)
+% % end
+% % end
% %Thin plate covariance function (for 2D implicit shape -> does not seem to work)
% K = 2 * pdist2(x1',x2').^2 .* log(pdist2(x1',x2')) - (1 + 2*log(p(1))) .* pdist2(x1',x2').^2 + p(1)^2;
-% %RBF covariance function
-% K = 1E-1 * exp(-1E-1^-1 * pdist2(x1',x2').^2);
-% % size(x1)
-% % size(x2)
-% % dK(:,:,1) = 1E-1 * exp(-1E-1^-1 * pdist2(x1',x2').^2) .* repmat(x1(1,:)-x2(1,:), size(x1,2), 1); %Derivative
-% % dK(:,:,2) = 1E-1 * exp(-1E-1^-1 * pdist2(x1',x2').^2) .* repmat(x1(2,:)-x2(2,:), size(x1,2), 1); %Derivative
+ %RBF covariance function
+ p = [5E-2^-1, 1E-4, 1E-2];
+ K = p(3) * exp(-p(1) * pdist2(x1', x2').^2); %Kernel
+ dK(:,:,1) = -p(1) * p(3) * exp(-p(1) * pdist2(x1', x2').^2) .* substr(x1(1,:)',x2(1,:)); %Derivatives along x1
+ dK(:,:,2) = -p(1) * p(3) * exp(-p(1) * pdist2(x1', x2').^2) .* substr(x1(2,:)',x2(2,:)); %Derivatives along x2
% for i=1:size(x1,2)
% for j=1:size(x2,2)
-% dK(i,j,:) = 1E-1 * exp(-1E-1^-1 * pdist2(x1(:,i)',x2(:,j)').^2) * (x1(:,i) - x2(:,j));
-% % e = x1(:,i) - x2(:,j);
-% % dK(i,j,:) = 1E-1 * exp(-1E-1^-1 * e'*e) * e;
+% K(i,j) = p(3) * exp(-p(1) * pdist2(x1(:,i)', x2(:,j)').^2); %Kernel (slow one by one computation)
+% dK(i,j,:) = -p(1) * p(3) * exp(-p(1) * pdist2(x1(:,i)', x2(:,j)').^2) * (x1(:,i) - x2(:,j)); %Derivatives (slow one by one computation)
% end
% end
if flag_noiseObs==1
- K = K + p(2) * eye(size(x1,2),size(x2,2)); %Consideration of noisy observation y
+ K = K + p(2) * eye(size(x1,2),size(x2,2)); %Consy==-1ration of noisy observation y
end
end
\ No newline at end of file
diff --git a/demos/demo_ID01.m b/demos/demo_ID01.m
index 9f9fc4cd0743d8041adec76a711fa391be4953d0..05cd79651d1e770e751bf33901ff4b17f9363ea2 100644
--- a/demos/demo_ID01.m
+++ b/demos/demo_ID01.m
@@ -32,29 +32,31 @@ kV = (2*kP)^.5; %Damping in task space
% kV = 10; %Damping in task space
kPq = 10; %Stiffness in joint space
kVq = 1; %Damping in joint space
-dt = 0.01; %Time step
-nbData = 30;
-
+dt = 1E-2; %Time step
+nbData = 100;
%% Create robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbDOFs = 5; %Number of articulations
-armLength = 0.3; %Length of each link
+nbDOFs = 2; %Number of articulations
+
+model.l = 0.3; %Length of each link
+model.m = 1.7; %Mass
+model.g = 9.81; %Acceleration due to gravity
+
for k=1:nbDOFs
- lnk(k).L = Link([0 0 armLength 0], 'standard'); %Links with dh = [THETA D A ALPHA SIGMA OFFSET]
- lnk(k).L.m = 1; %Mass
- lnk(k).L.r = [-armLength/2 0 0]; %Center of mass
- %lnk(k).L.I = zeros(3); %Inertia
- lnk(k).L.I = eye(3)*1E-2; %Inertia
+ lnk(k).L = Link([0 0 model.l 0], 'standard'); %Links with dh = [THETA D A ALPHA SIGMA OFFSET]
+ lnk(k).L.m = model.m; %Mass
+ lnk(k).L.r = [-model.l/2 0 0]; %Link COG (center of rod)
+ lnk(k).L.I = [0; 0; model.m * model.l^2 / 12]; %Inertia at link COG (rod of length L and mass m, https://en.wikipedia.org/wiki/List_of_moments_of_inertia)
lnk(k).L.Jm = 0; %1E6;
lnk(k).L.G = 0;
lnk(k).L.B = 0;
- %lnk(k).L.G = 1E-5;
strTmp(k) = lnk(k).L;
lnk(k).rob = SerialLink(strTmp);
- lnk(k).rob.gravity = [0 9.81 0]; %for 2D robot
+ lnk(k).rob.gravity = [0 model.g 0]; %for 2D robot
end
robot = lnk(end).rob;
+
%Initial pose
q0(:,1) = ones(nbDOFs,1)*pi/4;
Htmp = robot.fkine(q0);
@@ -72,33 +74,49 @@ dx = zeros(2,1);
dq = zeros(nbDOFs,1);
for t=1:nbData
r.q(:,t) = q; %Log data
+
Htmp = robot.fkine(q);
x = Htmp.t(1:2);
J = robot.jacob0(q,'trans');
J = J(1:2,:);
- tauG = robot.gravload(q')'; %Compute gravity compensation term
+
+ uG = robot.gravload(q')'; %Compute torque due to gravity
+% uG = cos(q) * model.l * model.m * model.g / 2; %Compute torque due to gravity
+
%Dynamically consistent nullspace (Jinv=pinv(J) would not work)
- JI = robot.inertia(q'); %Joint inertia
- CI = J/JI*J' + eye(2)*1E-12; %Cartesian inertia
- W = inv(JI);
+ M = robot.inertia(q') %Joint inertia
+% M = model.m * model.l^2 / 3; %Inertia at link frame origin (rod of length L and mass m, https://en.wikipedia.org/wiki/List_of_moments_of_inertia)
+
+ Mx = inv(J / M * J' + eye(2)*1E-12); %Cartesian inertia
+ W = inv(M);
%W = eye(nbDOFs);
- Jinv = W * J' / (J*W*J'); %Generalized inverse (weighted pseudoinverse)
+ Jinv = W * J' / (J*W*J' + eye(2)*1E-12); %Generalized inverse (weighted pseudoinverse)
N = eye(nbDOFs) - J'*Jinv'; %Nullspace projection matrix (dynamically consistent)
%N = (eye(nbDOFs) - Jinv*J)'; %Same as the line above but with Roy Featherston's paper notation
%N = eye(nbDOFs) - pinv(J)*J; %Nullspace projection matrix (for standard least squares solution)
%N = eye(nbDOFs) - J'*pinv(J)'; %Same as the line above (because of symmetry)
%Computed torque control command
-% tau = J'/ CI * (ddxh + kP*(xh-x) + kV*(dxh-dx)) + N * JI*(kPq*(qh-q)-kVq*dq) + tauG;
-% tau = J'/ CI * (kP*(xh-x)-kV*dx) + N * (kPq*(qh-q)-kVq*dq) + tauG;
-% tau = J' * (kP*(xh-x)-kV*dx) + N * (kPq*(qh-q)-kVq*dq) + tauG;
- tau = N * (kPq*(qh-q)-kVq*dq) + tauG;
+% u = J'* Mx * (ddxh + kP*(xh - x) + kV*(dxh - dx)) + N * M *(kPq * (qh - q) - kVq * dq) + uG;
+% u = J'* Mx * (kP * (xh - x) - kV * dx) + N * (kPq * (qh - q) - kVq * dq) + uG;
+% u = J' * (kP * (xh - x) - kV * dx) + N * (kPq * (qh - q) - kVq * dq) + uG;
+% u = N * (kPq * (qh - q) - kVq * dq) + uG;
+% u = uG;
+ u = rand(nbDOFs,1) * 1E-1 + uG;
+
+% robot.inertia(q')
+% uG = robot.gravload(q')'
+
+% ddq = robot.accel(q', dq', u'); %Compute acceleration
+ ddq = accel(q, u, model);
+ pause
+% ddq = (u - uG) / M; %Compute acceleration
- ddq = robot.accel(q', dq', tau');
dq = dq + ddq*dt;
- dx = J*dq;
q = q + dq*dt;
+
+ dx = J * dq;
end
@@ -106,11 +124,34 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,10,1800,1200],'color',[1 1 1]); hold on; axis off;
for t=round(linspace(1,nbData,10))
- plotArm(r.q(:,t), ones(nbDOFs,1)*armLength, [0;0;t*0.1], .02, [.7 .7 .7]);
+ plotArm(r.q(:,t), ones(nbDOFs,1)*model.l, [0;0;t*0.1], .02, [.7 .7 .7]-.7*t/nbData);
end
plot(xh(1),xh(2),'r+');
-axis([-.2 1 -.2 1]); axis equal; set(gca,'xtick',[],'ytick',[]);
+axis equal; %axis([-.2 1 -.2 1]);
%print('-dpng','demoID01.png');
pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Rollout
+function ddq = accel(q, u, model)
+ for n=1:size(q,1)
+ M = model.m * model.l^2 / 3 %Inertia at link frame origin (rod of length L and mass m, https://en.wikipedia.org/wiki/List_of_moments_of_inertia)
+ uG = cos(q(n)) * model.l * model.m * model.g / 2; %Compute torque due to gravity
+ ddq(n) = (u(n) - uG) / M;
+ end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with numerical computation
+function J = jacob0(q, model)
+ e = 1E-6;
+ J = zeros(2,size(q,1));
+ for n=1:size(q,1)
+ qtmp = q;
+ qtmp(n) = qtmp(n) + e;
+ J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_IK01.m b/demos/demo_IK01.m
index e93d44c6d2d470882fd65974daa7a3166fb1fb7c..03a4d2aa379dd50381c8a68907473d56d644c9ec 100644
--- a/demos/demo_IK01.m
+++ b/demos/demo_IK01.m
@@ -1,111 +1,104 @@
-function demo_IK01
-% Inverse kinematics with nullspace.
-%
-% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
-% First run 'startup_rvc' from the robotics toolbox.
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon, http://calinon.ch/
-%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
-%
-% PbDlib 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.
-%
-% PbDlib 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 PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
-addpath('./m_fcts/');
-disp('This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).');
-
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-dt = 0.01; %Time step
-nbData = 100;
-
-
-%% Robot parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbDOFs = 3; %Number of articulations
-armLength = 0.6; %Length of each link
-L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
-robot = SerialLink(repmat(L1,nbDOFs,1));
-%Initial pose
-q(:,1) = [pi/2 pi/2 pi/3]; %ones(1,nbDOFs) .* pi/3;
-
-
-%% IK with nullspace
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-dxh = [0; 0];
-%dxh = 0;
-
-%dqh = ones(nbDOFs,1)*1;
-%dqh = [-1; zeros(nbDOFs-1,1)];
-
-for t=1:nbData
- r.q(:,t) = q; %Log data
-
-% Htmp = robot.fkine(q);
-% r.x(:,t) = Htmp.t(1:2);
-
- J = robot.jacob0(q,'trans');
- J = J(1:2,:);
- %J = J(2,:);
-
-% pinvJ = pinv(J);
-% pinvJ = J' / ( J * J');
-% pinvJ = (J' * J) \ J';
- [U,S,V] = svd(J);
- S(S>0) = S(S>0).^-1;
- pinvJ = V*S'*U';
-
- %Kx = J*Kq*J'; % Stiffness at the end-effector
- %robot.maniplty(q) %Manipulability index
-
-% %Nullspace projection matrix
-% N = eye(nbDOFs) - pinvJ*J;
-% %N = eye(nbDOFs) - J'*pinv(J)'
-
-
-% %Alternative way of computing the nullspace projection matrix, see e.g. wiki page on svd
-% %(https://en.wikipedia.org/wiki/Singular-value_decomposition) or
-% %http://math.stackexchange.com/questions/421813/projection-matrix-onto-null-space
-% %[U,S,V] = svd(J);
- sp = (sum(S,1)<1E-1); %Span of zero rows
- N = V(:,sp) * V(:,sp)';
-
-% [U,S,V] = svd(pinvJ);
-% sp = (sum(S,2)<1E-1); %Span of zero rows
-% N = U(:,sp) * U(:,sp)'; %N = U * [0 0 0; 0 0 0; 0 0 1] * U'
-% % d = zeros(nbDOFs,1);
-% % d(sp) = 1;
-% % N = U * diag(d) * U';
-
-% dqh = [5*(pi/4-q(1)); zeros(nbDOFs-1,1)];
- dqh = ones(nbDOFs,1);
-
- dq = pinvJ * dxh + N * dqh;
- q = q + dq * dt;
-end
-
-%% Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,10,1000,650],'color',[1 1 1]); hold on; axis off;
-%plotopt = robot.plot({'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes'});
-for t=round(linspace(1,nbData,10))
- colTmp = [1,1,1] - [.7,.7,.7] * t/nbData;
- plotArm(r.q(:,t), ones(nbDOFs,1)*armLength, [0;0;t*0.1], .02, colTmp);
- %robot.plot(r.q(:,t)', plotopt);
-end
-axis equal; axis tight;
-
-%print('-dpng','graphs/demoIK01.png');
-pause;
-close all;
\ No newline at end of file
+function demo_IK01
+% Basic forward and inverse kinematics for a planar robot, with numerical Jacobian computation.
+%
+% If this code is useful for your research, please cite the related publication:
+% @article{Silverio19TRO,
+% author="Silv\'erio, J. and Calinon, S. and Rozo, L. and Caldwell, D. G.",
+% title="Learning Task Priorities from Demonstrations",
+% journal="{IEEE} Trans. on Robotics",
+% year="2019",
+% volume="35",
+% number="1",
+% pages="78--94",
+% doi="10.1109/TRO.2018.2878355"
+% }
+%
+% Copyright (c) 2020 Idiap Research Institute, http://idiap.ch/
+% Written by Sylvain Calinon, http://calinon.ch/
+%
+% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+%
+% PbDlib 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.
+%
+% PbDlib 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 PbDlib. If not, see <http://www.gnu.org/licenses/>.
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+dt = 1E-2; %Time step
+nbData = 100;
+
+
+%% Robot parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbDOFs = 2; %Number of articulations
+model.l = 0.6; %Length of each link
+q(:,1) = [pi/2; pi/2; pi/3]; %Initial pose
+% q = rand(model.nbDOFs,1);
+
+
+%% IK
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+dxh = [.1; 0];
+for t=1:nbData-1
+ J = jacob0(q(:,t), model); %Jacobian
+ dq = pinv(J) * dxh;
+ q(:,t+1) = q(:,t) + dq * dt;
+ x(:,t+1) = fkine(q(:,t+1), model); %FK for a planar robot
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,10,800,650],'color',[1 1 1]); hold on; axis off;
+for t=floor(linspace(1,nbData,2))
+ colTmp = [.7,.7,.7] - [.7,.7,.7] * t/nbData;
+ plotArm(q(:,t), ones(model.nbDOFs,1)*model.l, [0;0;-2+t/nbData], .03, colTmp);
+ plot(x(1,t), x(2,t), '.','markersize',50,'color',[.8 0 0]);
+end
+plot(x(1,:), x(2,:), '.','markersize',20,'color',[0 0 0]);
+axis equal; axis tight;
+
+%print('-dpng','graphs/demoIK01.png');
+pause;
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics
+function [x, Tf] = fkine(q, model)
+ Tf = eye(4);
+ T = repmat(Tf, [1,1,size(q,1)]);
+ for n=1:size(q,1)
+ c = cos(q(n));
+ s = sin(q(n));
+ T(:,:,n) = [c, -s, 0, model.l * c; ...
+ s, c, 0, model.l * s; ...
+ 0, 0, 1, 0;
+ 0, 0, 0, 1]; %Homogeneous matrix
+ Tf = Tf * T(:,:,n);
+ end
+ x = Tf(1:2,end);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with numerical computation
+function J = jacob0(q, model)
+ e = 1E-6;
+ J = zeros(2,size(q,1));
+ for n=1:size(q,1)
+ qtmp = q;
+ qtmp(n) = qtmp(n) + e;
+ J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_IK02.m b/demos/demo_IK02.m
index 50ebc1ab410e284bd54fd525459ea140f231a450..d1198fac12c7d14129ae34d77b9c384cc7039128 100644
--- a/demos/demo_IK02.m
+++ b/demos/demo_IK02.m
@@ -1,8 +1,5 @@
function demo_IK02
-% Inverse kinematics with two arms and nullspace.
-%
-% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
-% First run 'startup_rvc' from the robotics toolbox.
+% Inverse kinematics with nullspace projection operator.
%
% If this code is useful for your research, please cite the related publication:
% @article{Silverio19TRO,
@@ -15,8 +12,11 @@ function demo_IK02
% pages="78--94",
% doi="10.1109/TRO.2018.2878355"
% }
-%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
+%
+% The commented parts of this demo require the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
+% First run 'startup_rvc' from the robotics toolbox if you uncomment these parts.
+%
+% Copyright (c) 2020 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
@@ -34,92 +34,119 @@ function demo_IK02
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
-disp('This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 0.01; %Time step
-nbData = 500; %Numer of points in a trajectory
+nbData = 100;
-%% Create robot
+%% Robot parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Robot parameters
-nbDOFs = 5; %Number of articulations
-armLength = 0.6; %Length of each link
-L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
-arm = SerialLink(repmat(L1,3,1));
+nbDOFs = 3; %Number of articulations
+model.l = 0.6; %Length of each link
+% L1 = Link('d', 0, 'a', model.l, 'alpha', 0);
+% robot = SerialLink(repmat(L1,nbDOFs,1));
-%Initial pose
-q0(:,1) = [pi/2 pi/2 pi/3 -pi/2 -pi/3];
-q = q0;
+q(:,1) = [pi/2 pi/2 pi/3]; %Initial pose
%% IK with nullspace
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-r=[];
-for t=1:nbData
- r.q(:,t)=q; %Log data
-
- Htmp = arm.fkine(q([1,4:5]));
- r.rx(:,t) = Htmp.t(1:2);
- Htmp = arm.fkine(q(1:3));
- r.lx(:,t) = Htmp.t(1:2);
+dxh = [0; 0];
+%dxh = 0;
- if t==1
- rxh = [linspace(r.rx(1,1),r.rx(1,1)+.3,nbData); linspace(r.rx(2,1),r.rx(2,1)+1.2,nbData)];
- lxh = [linspace(r.lx(1,1),r.lx(1,1)-.3,nbData); linspace(r.lx(2,1),r.lx(2,1)+1.2,nbData)];
- end
+%dqh = ones(nbDOFs,1)*1;
+%dqh = [-1; zeros(nbDOFs-1,1)];
- rJ = arm.jacob0(q([1,4:5]),'trans');
- rJ = rJ(1:2,:);
+for t=1:nbData
+ r.q(:,t) = q; %Log data
+% Htmp = robot.fkine(q);
+% r.x(:,t) = Htmp.t(1:2);
+ r.x(:,t) = fkine(q, model); %Log data
- lJ = arm.jacob0(q(1:3),'trans');
- lJ = lJ(1:2,:);
-
- J = lJ;
- J(3:4,[1,4:5]) = rJ;
+% J = robot.jacob0(q,'trans');
+% J = J(1:2,:);
+ J = jacob0(q, model);
+
+% pinvJ = pinv(J);
+% pinvJ = J' / ( J * J');
+% pinvJ = (J' * J) \ J';
+ [U,S,V] = svd(J);
+ S(S>0) = S(S>0).^-1;
+ pinvJ = V*S'*U';
- Ja = J(1:2,:);
- Jb = J(3:4,:);
+ %Kx = J*Kq*J'; % Stiffness at the end-effector
+ %robot.maniplty(q) %Manipulability index
-% Na = eye(nbDOFs) - pinv(Ja) * Ja; %Nullspace projection matrix
- Nb = eye(nbDOFs) - pinv(Jb) * Jb; %Nullspace projection matrix
+% %Nullspace projection matrix
+% N = eye(nbDOFs) - pinvJ*J;
+% %N = eye(nbDOFs) - J'*pinv(J)'
+
+
+% %Alternative way of computing the nullspace projection matrix, see e.g. wiki page on svd
+% %(https://en.wikipedia.org/wiki/Singular-value_decomposition) or
+% %http://math.stackexchange.com/questions/421813/projection-matrix-onto-null-space
+% %[U,S,V] = svd(J);
+ sp = (sum(S,1)<1E-1); %Span of zero rows
+ N = V(:,sp) * V(:,sp)';
- ldx = 100 * (lxh(:,t) - r.lx(:,t));
- rdx = 100 * (rxh(:,t) - r.rx(:,t));
+% [U,S,V] = svd(pinvJ);
+% sp = (sum(S,2)<1E-1); %Span of zero rows
+% N = U(:,sp) * U(:,sp)'; %N = U * [0 0 0; 0 0 0; 0 0 1] * U'
+% % d = zeros(nbDOFs,1);
+% % d(sp) = 1;
+% % N = U * diag(d) * U';
- %Prioritized bimanual tracking task
- Ja_mod = Ja * Nb;
- pinvJa_mod = Ja_mod' / (Ja_mod * Ja_mod' + eye(2) .* 1E-1);
- dqb = pinv(Jb) * rdx;
- dq = dqb + Nb * pinvJa_mod * (ldx - Ja * dqb);
-% dq = dqb + Nb * pinv(Ja) * ldx; %Approximation
+% dqh = [5*(pi/4-q(1)); zeros(nbDOFs-1,1)];
+ dqh = ones(nbDOFs,1);
- q = q + dq*dt;
+ dq = pinvJ * dxh + N * dqh;
+ q = q + dq * dt;
end
-
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,2300,1200],'color',[1 1 1]); hold on; axis off;
-listT = round(linspace(1,nbData,10));
-i=0;
-for t=listT
- i=i+1;
- colTmp = [.9,.9,.9] - [.7,.7,.7] * i/length(listT);
- plotArm(r.q(1:3,t), ones(3,1)*armLength, [0;0;i], .03, colTmp); %left arm
- plotArm(r.q([1,4:5],t), ones(3,1)*armLength, [0;0;i+0.1], .03, colTmp); %right arm
+figure('position',[20,10,1000,650],'color',[1 1 1]); hold on; axis off;
+%plotopt = robot.plot({'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes'});
+for t=round(linspace(1,nbData,10))
+ %robot.plot(r.q(:,t)', plotopt);
+ colTmp = [1,1,1] - [.7,.7,.7] * t/nbData;
+ plotArm(r.q(:,t), ones(nbDOFs,1)*model.l, [0;0;t*0.1], .02, colTmp);
end
-plot3(rxh(1,:), rxh(2,:), ones(1,nbData)*20, '-','linewidth',2,'color',[.8 0 0]);
-plot3(lxh(1,:), lxh(2,:), ones(1,nbData)*20, '-','linewidth',2,'color',[.8 0 0]);
-plot3(rxh(1,listT), rxh(2,listT), ones(1,10)*20, '.','markersize',25,'color',[.8 0 0]);
-plot3(lxh(1,listT), lxh(2,listT), ones(1,10)*20, '.','markersize',25,'color',[.8 0 0]);
-
-%axis([-.2 1 -.2 1]);
axis equal; axis tight;
-% print('-dpng','graphs/demoIK02.png');
+%print('-dpng','graphs/demoIK02.png');
pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics
+function [x, Tf] = fkine(q, model)
+ Tf = eye(4);
+ T = repmat(Tf, [1,1,size(q,1)]);
+ for n=1:size(q,1)
+ c = cos(q(n));
+ s = sin(q(n));
+ T(:,:,n) = [c, -s, 0, model.l * c; ...
+ s, c, 0, model.l * s; ...
+ 0, 0, 1, 0;
+ 0, 0, 0, 1]; %Homogeneous matrix
+ Tf = Tf * T(:,:,n);
+ end
+ x = Tf(1:2,end);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with numerical computation
+function J = jacob0(q, model)
+ e = 1E-6;
+ J = zeros(2,size(q,1));
+ for n=1:size(q,1)
+ qtmp = q;
+ qtmp(n) = qtmp(n) + e;
+ J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_IK_nullspaceAsProduct01.m b/demos/demo_IK_nullspaceAsProduct01.m
index 5737f985436fdb6da54d9079365067e060d12c2a..5c9d461fed329cf8b2520997fa0eb7bf4267595d 100644
--- a/demos/demo_IK_nullspaceAsProduct01.m
+++ b/demos/demo_IK_nullspaceAsProduct01.m
@@ -1,5 +1,6 @@
function demo_IK_nullspaceAsProduct01
% 3-level nullspace control formulated as product of Gaussians (PoG).
+% (see also demo_IK_weighted01.m)
%
% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox.
diff --git a/demos/demo_IK_quat01.m b/demos/demo_IK_quat01.m
index cb911e77fb4296757fc210fa12a6aa03b1b78f34..2de809f311467f8574a1775c192827a2258e8494 100644
--- a/demos/demo_IK_quat01.m
+++ b/demos/demo_IK_quat01.m
@@ -1,11 +1,12 @@
function demo_IK_quat01
% Inverse kinematics for orientation data represented as unit quaternions.
+% (see also demo_IK_orient01.m)
%
-% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
-% First run 'startup_rvc' from the robotics toolbox.
+% Copyright (c) 2020 Idiap Research Institute, http://idiap.ch/
+% Written by Sylvain Calinon, http://calinon.ch/
%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by João Silvério and Sylvain Calinon
+% The commented parts of this demo require the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
+% First run 'startup_rvc' from the robotics toolbox if you uncomment these parts.
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
@@ -22,34 +23,28 @@ function demo_IK_quat01
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
-disp('This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 0.01; %Time step
-nbData = 100;
+nbData = 100; %Number of points in a trajectory
+Kp = 1E-1; %Tracking gain
+nbDOFs = 5; %Number of articulations
+model.l = 0.2; %Length of links
-%% Create robot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Robot parameters (based on mdl_twolink.m model)
-nbDOFs = 5; %Nb of degrees of freedom
-armLength = 0.2;
-L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
-robot = SerialLink(repmat(L1,nbDOFs,1));
-%Initial pose
-% q0(:,1) = [pi/2 pi/2 pi/3];
-q0(:,1) = [pi/5 pi/5 pi/5 pi/8 pi/8];
+q0 = rand(nbDOFs,1) * pi/2; %Initial pose
+% q0 = [pi/5; pi/5; pi/5; pi/8; pi/8]; %Initial pose
-%Set desired pose (pointing vertically)
-% Htmp = robot.fkine([pi/2 pi/2 pi/2]);
-Htmp = robot.fkine([pi/5 pi/5 pi/5 pi/5 pi/5]);
+% L1 = Link('d', 0, 'a', model.l, 'alpha', 0);
+% robot = SerialLink(repmat(L1,nbDOFs,1));
+% %Set desired pose (pointing vertically)
+% Htmp = robot.fkine(ones(1,nbDOFs)*pi/5);
+% Qh = UnitQuaternion(Htmp) % Q denotes quaternion, q denotes joint position
+% % Qh = Quaternion(-pi/4, [0 0 1]); % -> Quaternion(angle,vector)
-Qh = UnitQuaternion(Htmp); % Q denotes quaternion, q denotes joint position
-% Qh = Quaternion(-pi/4, [0 0 1]); % -> Quaternion(angle,vector)
-% Qh = Quaternion(-3*pi/4, [0 0 1]);
-% Qh = Quaternion(-rand*pi/4, [0 0 1]);
+[~, Qh] = fkine(ones(nbDOFs,1)*pi/5, model);
%% IK
@@ -57,26 +52,102 @@ Qh = UnitQuaternion(Htmp); % Q denotes quaternion, q denotes joint position
q = q0;
for t=1:nbData
r.q(:,t) = q; %Log data
- Htmp = robot.fkine(q);
- Q = UnitQuaternion(Htmp);
- J = robot.jacob0(q,'rot');
- dx = angDiffFromQuat(Qh,Q) / dt;
- dq = pinv(J) * dx;
+
+% Htmp = robot.fkine(q);
+% Q = UnitQuaternion(Htmp);
+% J = robot.jacob0(q,'rot');
+% dx = angDiffFromQuat(Qh, Q) / dt
+%
+% u = logmap(Qh.double', Q.double');
+% dx = 4 * omegaConversionMatrix(Q.double') * u / dt
+
+ [~, Q] = fkine(q, model);
+ %Keep quaternion with closest difference
+ if(Qh'*Q<0)
+ Q = -Q;
+ end
+
+ dx = 4 * omegaConversionMatrix(Q) * logmap(Qh, Q) / dt;
+
+ J = [zeros(2,nbDOFs); ones(1, nbDOFs)]; %Jacobian for orientation data (corresponds to a constant matrix for planar robots)
+% [~, J] = jacob0(q, model);
+
+ dq = pinv(J) * Kp * dx;
q = q + dq * dt;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,10,1000,650]); hold on;
+figure('position',[20,10,1000,650]); hold on; axis off;
%plotopt = robot.plot({'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes'});
-for t=round(linspace(1,nbData,10))
- colTmp = [1,1,1] - [.7,.7,.7] * t/nbData;
- plotArm(r.q(:,t), ones(nbDOFs,1)*armLength, [0;0;t*0.1], .02, colTmp);
+for t=round(linspace(1,nbData,2))
+ colTmp = [.8,.8,.8] - [.7,.7,.7] * t/nbData;
+ plotArm(r.q(:,t), ones(nbDOFs,1)*model.l, [0;0;t*0.1], .02, colTmp);
%robot.plot(r.q(:,t)', plotopt);
end
-axis([-.2 1 -0.4 1]); axis equal; set(gca,'xtick',[],'ytick',[]);
+axis([-.2 1 -0.4 1]); axis equal;
%print('-dpng','graphs/demoIK_quat02.png');
pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+function x = expmap(u, x0)
+ theta = sqrt(sum(u.^2,1)); %norm(u,'fro')
+ x = real(repmat(x0,[1,size(u,2)]) .* repmat(cos(theta),[size(u,1),1]) + u .* repmat(sin(theta)./theta,[size(u,1),1]));
+ x(:,theta<1e-16) = repmat(x0,[1,sum(theta<1e-16)]);
+end
+
+function u = logmap(x, x0)
+ theta = acos(x0'*x); %acos(trace(x0'*x))
+ u = (x - repmat(x0,[1,size(x,2)]) .* repmat(cos(theta),[size(x,1),1])) .* repmat(theta./sin(theta),[size(x,1),1]);
+ u(:,theta<1e-16) = 0;
+end
+
+function G = omegaConversionMatrix(q)
+%See Basile Graf (2017) "Quaternions And Dynamics" for definition of E and G
+ G = [-q(2) q(1) -q(4) q(3); ...
+ -q(3) q(4) q(1) -q(2); ...
+ -q(4) -q(3) q(2) q(1)]; %E
+% G = [-q(2) q(1) q(4) -q(3); ...
+% -q(3) -q(4) q(1) q(2); ...
+% -q(4) q(3) -q(2) q(1)];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics
+function [xp, xo, Tf] = fkine(q, model)
+ Tf = eye(4);
+ T = repmat(Tf, [1,1,size(q,1)]);
+ for n=1:size(q,1)
+ c = cos(q(n));
+ s = sin(q(n));
+ T(:,:,n) = [c, -s, 0, model.l * c; ...
+ s, c, 0, model.l * s; ...
+ 0, 0, 1, 0;
+ 0, 0, 0, 1]; %Homogeneous matrix
+ Tf = Tf * T(:,:,n);
+ end
+ xp = Tf(1:2,end); %Position
+ xo = atan2(Tf(2,1), Tf(1,1)); %Orientation (as single Euler angle for planar robot)
+ xo = [cos(xo/2); [0;0;1] * sin(xo/2)]; %Orientation (as unit quaternion)
+% xo = rotm2quat(Tf(1:3,1:3)); %Orientation (as unit quaternion)
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with numerical computation
+function [Jp, Jo] = jacob0(q, model)
+ e = 1E-4;
+ Jp = zeros(2, size(q,1)); %Jacobian for position data
+ Jo = [zeros(2, size(q,1)); ones(1, size(q,1))]; %Jacobian for orientation data (corresponds to a vector with 1 elements for planar robots)
+ for n=1:size(q,1)
+ qtmp = q;
+ qtmp(n) = qtmp(n) + e;
+ Jp(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
+% [xp2, xo2] = fkine(qtmp, model);
+% [xp1, xo1] = fkine(q, model);
+% Jp(:,n) = (xp2 - xp1) / e;
+% Jo(:,n) = (xo2 - xo1) / e;
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_IK_quat02.m b/demos/demo_IK_quat02.m
index 92b08725b0f96d68e99eb56992966a2348be7572..92054799421db5e944e4295e0790e386c42f2830 100644
--- a/demos/demo_IK_quat02.m
+++ b/demos/demo_IK_quat02.m
@@ -28,9 +28,9 @@ disp('This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordp
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-dt = 0.01; %Time step
+dt = 1E-2; %Time step
nbData = 100; %Number of datapoints
-Kp = 2; %Tracking gain
+Kp = 2E-1; %Tracking gain
%% Create robot
@@ -38,12 +38,16 @@ Kp = 2; %Tracking gain
%Robot parameters (built from function that contains WAM D-H parameters)
%[robot,L] = initWAMstructure('WAM');
[robot, link] = initiCubstructure('iCub');
-nbDOFs = robot.n;
+nbDOFs = robot.n; %Number of articulations
%Initial pose
-q0(:,1) = zeros(1,nbDOFs);
+q0 = zeros(nbDOFs,1);
+
%Set desired orientation
-Qh = UnitQuaternion([0 0 1 0]); % Aligned with the basis frame
+% Qh = UnitQuaternion([0 0 1 0]); % Aligned with the basis frame
+qh = ones(nbDOFs,1) * 2E-1;
+Htmp = robot.fkine(qh);
+Qh = UnitQuaternion(Htmp);
%% IK
@@ -53,37 +57,57 @@ for t=1:nbData
r.q(:,t) = q; %Log data
Htmp = robot.fkine(q);
Q = UnitQuaternion(Htmp);
- Jw = robot.jacob0(q,'rot');
-
- %The new Jacobian is built from the geometric Jacobian to allow the
- %tracking of quaternion velocities in Cartesian space
- J = 0.5 * QuatToMatrix(Q) * [zeros(1,nbDOFs);Jw];
-
- %Compute the quaternion derivative
- %-> The quaternion derivative at time t is given by:
- %dq(t) = (1/2) * q(t) * w(t), where w(t) is the angular velocity
+ %Keep quaternion with closest difference
+ if(Qh.inner(Q)<0)
+ Q = Quaternion(-Q.double);
+ end
- w = Kp*2*quaternionLog(Qh*Q.inv); % first compute angular velocity
- dQh = 0.5*Q*UnitQuaternion([0 w]); % quaternion derivative
+ Jw = robot.jacob0(q,'rot');
- r.Q(t,:) = Q.double; %Log the quaternion at each instant
+% %The new Jacobian is built from the geometric Jacobian to allow the tracking of quaternion velocities in Cartesian space
+% J = 0.5 * QuatToMatrix(Q) * [zeros(1,nbDOFs); Jw];
+%
+% %Compute the quaternion derivative
+% %-> The quaternion derivative at time t is given by:
+% %dq(t) = (1/2) * q(t) * w(t), where w(t) is the angular velocity
+%
+% w = Kp * 2 * quaternionLog(Qh * Q.inv); % first compute angular velocity
+% dQh = 0.5 * Q * UnitQuaternion([0 w]); % quaternion derivative
+%
+% r.Q(t,:) = Q.double; %Log the quaternion at each instant
+%
+% dq = pinv(J) * dQh.double'; % now with a quaternion derivative reference
+% %N = eye(nbDOFs) - pinv(J)*J; %Nullspace
+% %dq = pinv(J) * dQh.double' + N * [1; 0; 0; 0; 0];
+% %dq = N * [-1; -1; 0; 0; 1];
- dq = pinv(J) * dQh.double'; % now with a quaternion derivative reference
- %N = eye(nbDOFs) - pinv(J)*J; %Nullspace
- %dq = pinv(J) * dQh.double' + N * [1; 0; 0; 0; 0];
- %dq = N * [-1; -1; 0; 0; 1];
+
+ %The two functions below return the same dx
+ dx = 4 * omegaConversionMatrix(Q.double') * logmap(Qh.double', Q.double') / dt;
+% dx = angDiffFromQuat(Qh.double', Q.double') / dt;
+ dq = pinv(Jw) * dx * Kp;
q = q + dq * dt;
end
+Qh.double'
+Q.double'
%% Plots
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,10,1000,650]); hold on; rotate3d on;
+plot3Dframe(eye(3)*5E-2, zeros(3,1));
% %Plot animation of the movement (with robotics toolbox functionalities)
% plotopt = robot.plot({'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes'});
% robot.plot(r.q'); %'floorlevel',-0.5
+for i=1:nbDOFs
+ T = link(i).fkine(qh(1:i));
+ posTmp(:,i+1) = T.t(1:3,end);
+end
+plot3(posTmp(1,:),posTmp(2,:),posTmp(3,:), '-','linewidth',2,'color',[.8 0 0]);
+plot3(posTmp(1,:),posTmp(2,:),posTmp(3,:), '.','markersize',18,'color',[.8 0 0]);
+
%Plot animation of the movement (with standard plot functions)
posTmp(:,1) = zeros(3,1);
@@ -97,7 +121,6 @@ for t=round(linspace(1,nbData,10))
plot3(posTmp(1,:),posTmp(2,:),posTmp(3,:), '.','markersize',18,'color',colTmp);
% plot3Dframe(T.t(1:3,1:3)*1E-1, T.t(1:3,end), min(eye(3)+colTmp(1,1),1));
end
-plot3Dframe(eye(3)*2E-1, zeros(3,1));
set(gca,'xtick',[],'ytick',[],'ztick',[]);
xlabel('x_1'); ylabel('x_2'); zlabel('x_3');
view(3); axis equal; axis vis3d;
@@ -110,4 +133,27 @@ view(3); axis equal; axis vis3d;
% xlabel('t');
pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+function x = expmap(u, x0)
+ theta = sqrt(sum(u.^2,1)); %norm(u,'fro')
+ x = real(repmat(x0,[1,size(u,2)]) .* repmat(cos(theta), [size(u,1),1]) + u .* repmat(sin(theta)./theta, [size(u,1),1]));
+ x(:,theta<1e-16) = repmat(x0, [1,sum(theta<1e-16)]);
+end
+
+function u = logmap(x, x0)
+ theta = acos(x0'*x); %acos(trace(x0'*x))
+ u = (x - repmat(x0, [1,size(x,2)]) .* repmat(cos(theta), [size(x,1),1])) .* repmat(theta./sin(theta), [size(x,1),1]);
+ u(:,theta<1e-16) = 0;
+end
+
+function G = omegaConversionMatrix(q)
+%See Basile Graf (2017) "Quaternions And Dynamics" for definition of E and G
+ G = [-q(2) q(1) -q(4) q(3); ...
+ -q(3) q(4) q(1) -q(2); ...
+ -q(4) -q(3) q(2) q(1)]; %E
+% G = [-q(2) q(1) q(4) -q(3); ...
+% -q(3) -q(4) q(1) q(2); ...
+% -q(4) q(3) -q(2) q(1)];
+end
\ No newline at end of file
diff --git a/demos/demo_IK_weighted01.m b/demos/demo_IK_weighted01.m
index 119574c89b7347a1b6e544c62176cf28dc053dcd..f53c447c7531b7d95b92c87e4c1859d64a5f9760 100644
--- a/demos/demo_IK_weighted01.m
+++ b/demos/demo_IK_weighted01.m
@@ -1,5 +1,6 @@
function demo_IK_weighted01
% Inverse kinematics with nullspace control, by considering weights in both joint space and in task space.
+% (see also demo_IK_nullspaceAsProduct01.m)
%
% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox.
@@ -48,10 +49,12 @@ Wq = diag([1E-44, ones(1,nbDOFs-1)]); %Weight in configuration space (-> we don'
%% Create robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-armLength = 0.3;
-L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
+model.l = 0.3;
+
+L1 = Link('d', 0, 'a', model.l, 'alpha', 0);
robot = SerialLink(repmat(L1,nbDOFs,1));
robot2 = SerialLink(repmat(L1,3,1));
+
q0(:,1) = [pi/4; zeros(nbDOFs-1,1)]; %Initial pose
@@ -66,14 +69,19 @@ xh = [-0.2; 0.8];
xh2 = [0; 0.4];
for t=1:nbData
r(1).q(:,t) = q; %Log data
- J = robot.jacob0(q,'trans');
- J = J(1:2,:);
- J2 = [J(1:2,1:3), zeros(2,nbDOFs-3)];
+
Htmp = robot.fkine(q);
x = Htmp.t(1:2);
Htmp2 = robot2.fkine(q(1:3));
x2 = Htmp2.t(1:2);
+ J = robot.jacob0(q,'trans');
+ J = J(1:2,:);
+
+% x = fkine(q, model);
+% x2 = fkine(q(1:3), model);
+% J = jacob0(q, model);
+ J2 = [J(1:2,1:3), zeros(2, nbDOFs-3)];
Jw = Vx * J * Vq;
pinvJw = pinv(Jw);
N = eye(nbDOFs) - pinvJw * Jw;
@@ -98,22 +106,22 @@ end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,10,1000,650],'color',[1 1 1]); hold on; axis off;
+figure('position',[20,10,2000,1300],'color',[1 1 1]); hold on; axis off;
h(1) = plot3(xh(1), xh(2), 11, '.','markersize',25,'color',[.4 1 .4]);
h(2) = plot3(xh2(1), xh2(2), 11, '.','markersize',25,'color',[.4 .4 1]);
-htmp = plotArm(q0, ones(nbDOFs,1)*armLength, [0;0;1], .02, [.8,.8,.8]);
+htmp = plotArm(q0, ones(nbDOFs,1)*model.l, [0;0;1], .02, [.8,.8,.8]);
h(3) = htmp(1);
ii = 2;
for t=nbData:nbData %round(linspace(1,nbData,2))
colTmp = [1,1,1] - [.7,.7,.7] * t/nbData;
- htmp = plotArm(r(1).q(:,t), ones(nbDOFs,1)*armLength, [0;0;ii], .02, colTmp);
+ htmp = plotArm(r(1).q(:,t), ones(nbDOFs,1)*model.l, [0;0;ii], .02, colTmp);
ii = ii + 1;
end
h(4) = htmp(1);
h(5) = plot3([xh(1),xh(1)], [xh(2)-.3,xh(2)+.3], [10,10], '-','linewidth',2,'color',[.8 0 0]);
-plotArm(q0(1), armLength, [0;0;10], .02, [.8,0,0]);
+plotArm(q0(1), model.l, [0;0;10], .02, [.8,0,0]);
axis equal; axis tight;
-legend(h,{'Primary task','Secondary task','Initial pose','Weighted IK','Weights'},'location','southeast','fontsize',18);
+legend(h,{'Primary task (tracking with endeffector point)','Secondary task (tracking with other point on the kinematic chain)','Initial pose','Weighted IK solution','Weights (in joint space and task space)'},'location','southoutside','fontsize',12);
% figure; hold on;
% plot(r(2).q(1,:),'k-');
@@ -122,4 +130,34 @@ legend(h,{'Primary task','Secondary task','Initial pose','Weighted IK','Weights'
% print('-dpng','graphs/demoIK_weighted01.png');
pause;
-close all;
\ No newline at end of file
+close all;
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics
+function [x, Tf] = fkine(q, model)
+ Tf = eye(4);
+ T = repmat(Tf, [1,1,size(q,1)]);
+ for n=1:size(q,1)
+ c = cos(q(n));
+ s = sin(q(n));
+ T(:,:,n) = [c, -s, 0, model.l * c; ...
+ s, c, 0, model.l * s; ...
+ 0, 0, 1, 0;
+ 0, 0, 0, 1]; %Homogeneous matrix
+ Tf = Tf * T(:,:,n);
+ end
+ x = Tf(1:2,end);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with numerical computation
+function J = jacob0(q, model)
+ e = 1E-3;
+ J = zeros(2,size(q,1));
+ for n=1:size(q,1)
+ qtmp = q;
+ qtmp(n) = qtmp(n) + e;
+ J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
+ end
+end
\ No newline at end of file
diff --git a/demos/demo_MPC01.m b/demos/demo_MPC01.m
index f9eb7de2a5a20b013da0ed10f42be4521867269c..a6bac4861b914c654c953fbbafe2465cbd7354e8 100644
--- a/demos/demo_MPC01.m
+++ b/demos/demo_MPC01.m
@@ -74,7 +74,7 @@ end
%% Task setting
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-tl = linspace(1,nbData,nbPoints+1);
+tl = linspace(1, nbData, nbPoints+1);
tl = round(tl(2:end));
MuQ = zeros(nbVar*nbData,1);
Q = zeros(nbVar*nbData);
@@ -97,7 +97,7 @@ xSigma = Su * uSigma * Su';
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1000 1000]); hold on; axis off;
+figure('position',[10 10 800 800]); hold on; axis off;
for t=1:nbData
plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2]).*1E-6, [.2 .2 .2], .1);
end
diff --git a/demos/demo_MPC02.m b/demos/demo_MPC02.m
index 2f8957728b2d555ef51b55877f45727bd4d5c79d..36135c3a0a909dc19ed98e68b6f8f8dc31619b72 100644
--- a/demos/demo_MPC02.m
+++ b/demos/demo_MPC02.m
@@ -356,7 +356,7 @@ axis equal;
%% Timeline plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$','$\ddot{x}_1$','$\ddot{x}_2$'};
-figure('position',[620 10 1000 1290],'color',[1 1 1]);
+figure('position',[620 10 1000 850],'color',[1 1 1]);
for j=1:model.nbVar
subplot(model.nbVar+model.nbVarPos+1,1,j); hold on;
limAxes = [1, nbData, min(r(1).x(j,:))-8E0, max(r(1).x(j,:))+8E0];
diff --git a/demos/demo_MPC03.m b/demos/demo_MPC03.m
index d03c75c2d2418d92eb54fb61ce8d3bfd08cc2bd7..0c7fb581194044b1aeef132e961fec844e47024b 100644
--- a/demos/demo_MPC03.m
+++ b/demos/demo_MPC03.m
@@ -40,11 +40,11 @@ nbRepros = 1; %Number of reproductions in new situations
nbNewRepros = 0; %Number of stochastically generated reproductions
nbData = 200; %Number of datapoints
-model.nbStates = 8; %Number of Gaussians in the GMM
+model.nbStates = 6; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 1; %Number of static & dynamic features (nbDeriv=1 for just x)
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
-model.dt = 0.01; %Time step duration
+model.dt = 1E-2; %Time step duration
model.rfactor = 1E-3; %Control cost in LQR
%Control cost matrix
@@ -100,7 +100,8 @@ end
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-load('data/2Dletters/E.mat');
+demos = [];
+load('data/2Dletters/S.mat');
Data=[];
for n=1:nbSamples
s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
@@ -229,7 +230,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1200 1200],'color',[1 1 1]); hold on; axis off;
+figure('position',[10 10 800 800],'color',[1 1 1]); hold on; axis off;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [.8 .8 .8]);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','lineWidth',1,'color',[.7 .7 .7]);
@@ -296,7 +297,7 @@ for n=1:nbNewRepros
end
%Plot
labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$'};
-figure('position',[1210 10 1000 1200],'color',[1 1 1]);
+figure('position',[810 10 800 800],'color',[1 1 1]);
for j=1:4
subplot(5,1,j); hold on;
if j<=model.nbVarPos
diff --git a/demos/demo_MPC_fullQ02.m b/demos/demo_MPC_fullQ02.m
index ab40a2ad710bcc788e7169566a8ecaefcdbe3e58..7e4f57773f0048db6d562b780d85df2f5dcea567 100644
--- a/demos/demo_MPC_fullQ02.m
+++ b/demos/demo_MPC_fullQ02.m
@@ -107,12 +107,12 @@ t1 = nbData/2;
t2 = t1+50;
idm(1:2) = [1:2] + (t1-1) * nbVar;
idm(3:4) = [3:4] + (t2-1) * nbVar;
-% MuQ(idm(1:2)) = [1; 1]; %Proposed meeting point for the two agents (optional)
-% MuQ(idm(3:4)) = MuQ(idm(1:2));
Q(idm(1:2), idm(1:2)) = eye(2)*1E0;
Q(idm(3:4), idm(3:4)) = eye(2)*1E0;
Q(idm(1:2), idm(3:4)) = -eye(2)*1E0;
Q(idm(3:4), idm(1:2)) = -eye(2)*1E0;
+MuQ(idm(1:2)) = [0; 0];
+MuQ(idm(3:4)) = MuQ(idm(1:2)) + [.05; 0]; %Meeting point with desired offset between the two agents
% %Constraining the two agents to meet at a given angle at the end of the motion
@@ -163,11 +163,11 @@ xSigma = Su * uSigma * Su';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Generate structured stochastic u through Bezier curves
nbRBF = 18;
-H = zeros(nbRBF,nbData-1);
-tl = linspace(0,1,nbData-1);
+H = zeros(nbRBF, nbData-1);
+tl = linspace(0, 1, nbData-1);
nbDeg = nbRBF - 1;
for i=0:nbDeg
- H(i+1,:) = factorial(nbDeg) ./ (factorial(i) .* factorial(nbDeg-i)) .* (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
+ H(i+1,:) = factorial(nbDeg) / (factorial(i) * factorial(nbDeg-i)) * (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
end
%Nullspace planning
@@ -175,6 +175,8 @@ end
U = V * D.^.5;
N = eye((nbData-1)*nbVarPos) - Su' * U / (U' * (Su * Su') * U + Rx) * U' * Su; %Nullspace projection matrix
up = Su' * U / (U' * (Su * Su') * U + Rx) * U' * (MuQ - Sx * x0); %Principal task
+% up = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - Sx * x0));
+
for n=1:nbRepros
w = randn(nbVarPos,nbRBF) .* 1E0; %Random weights
un = w * H; %Reconstruction of signals by weighted superposition of basis functions
@@ -197,6 +199,7 @@ plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), 'o','linewidth',2,'markersize',8,'color',[0 0 0]);
% plot(rx(1,tlm), rx(2,tlm), '.','markersize',35,'color',[0 .6 0]); %meeting point
plot(rx(1,t1), rx(2,t1), '.','markersize',35,'color',[0 .6 0]); %meeting point
+plot(rx(1,t1), rx(2,t1), '.','markersize',35,'color',[0 .6 0]); %meeting point
plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',35,'color',[0 0 0]);
%Agent 2
% for t=1:nbData
@@ -222,6 +225,7 @@ for n=1:nbRepros
plot(r(n).x(1,:), r(n).x(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(r(n).x(1,1), r(n).x(2,1), 'o','linewidth',2,'markersize',8,'color',[0 0 0]);
% plot(r(n).x(1,tlm), r(n).x(2,tlm), '.','markersize',35,'color',[0 .6 0]); %meeting point
+ plot(r(n).x(1,t1), r(n).x(2,t1), '.','markersize',35,'color',[0 .6 0]); %meeting point
plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',35,'color',[0 0 0]);
%Agent 2
plot(r(n).x(3,:), r(n).x(4,:), '-','linewidth',2,'color',[.8 0 0]);
diff --git a/demos/demo_MPC_iterativeLQR01.m b/demos/demo_MPC_iterativeLQR01.m
index f762004ea43d33c3b082ec71a4ca087fd68244f4..1bdddf87b42f44b72cc11b672ad5e82e0916d26e 100644
--- a/demos/demo_MPC_iterativeLQR01.m
+++ b/demos/demo_MPC_iterativeLQR01.m
@@ -1,5 +1,5 @@
function demo_MPC_iterativeLQR01
-% Iterative computation of linear quadratic tracking (with feedback and feedforward terms).
+% Recursive computation of linear quadratic tracking (with feedback and feedforward terms).
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
diff --git a/demos/demo_MPC_iterativeLQR02.m b/demos/demo_MPC_iterativeLQR02.m
index 222fde594dc815ca0cfb10ee71cfc38d46b3c5ad..ac4505e1211da57e23d89c220a5a82b43ebbed09 100644
--- a/demos/demo_MPC_iterativeLQR02.m
+++ b/demos/demo_MPC_iterativeLQR02.m
@@ -1,5 +1,5 @@
function demo_MPC_iterativeLQR02
-% Iterative computation of linear quadratic tracking (with feedback and feedforward terms),
+% Recursive computation of linear quadratic tracking (with feedback and feedforward terms),
% by relying on a GMM encoding of position and velocity data, including comparison with batch LQT.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC_iterativeLQR03.m b/demos/demo_MPC_iterativeLQR03.m
index 7cfd74619e26fc4826046e7a9a2bd1a3c4584252..f8fc44416de4d9723b9f8263696572f02f179f5e 100644
--- a/demos/demo_MPC_iterativeLQR03.m
+++ b/demos/demo_MPC_iterativeLQR03.m
@@ -1,5 +1,5 @@
function demo_MPC_iterativeLQR03
-% Iterative computation of linear quadratic tracking (with feedback and feedforward terms),
+% Recursive computation of linear quadratic tracking (with feedback and feedforward terms),
% by relying on a GMM encoding of only position data.
%
% If this code is useful for your research, please cite the related publication:
diff --git a/demos/demo_MPC_iterativeLQR04.m b/demos/demo_MPC_iterativeLQR04.m
index 8cbcfbe621ec71cf4e18ec77d983aa2a75315d6f..6f90751747d8093d6a2c7ec7e1e244593e7eb1ea 100644
--- a/demos/demo_MPC_iterativeLQR04.m
+++ b/demos/demo_MPC_iterativeLQR04.m
@@ -1,5 +1,5 @@
function demo_MPC_iterativeLQR04
-% Control of a spring attached to a point with iterative linear quadratic tracking (with feedback and feedforward terms).
+% Control of a spring attached to a point with linear quadratic tracking (with feedback and feedforward terms using the recursive formulation).
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
diff --git a/demos/demo_MPC_iterativeLQR_augmSigma01.m b/demos/demo_MPC_iterativeLQR_augmSigma01.m
index f5b305ee43a39b417a1c7f927f072280c9869f94..a17f034107cf19210f8874ad375a8e73a82996fb 100644
--- a/demos/demo_MPC_iterativeLQR_augmSigma01.m
+++ b/demos/demo_MPC_iterativeLQR_augmSigma01.m
@@ -1,5 +1,5 @@
function demo_MPC_iterativeLQR_augmSigma01
-% Iterative LQR with augmented covariance to transform the tracking problem to a regulation problem.
+% Recursive LQR with augmented covariance to transform the tracking problem to a regulation problem.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
@@ -85,6 +85,7 @@ B = [B0; zeros(1,model.nbVarPos)]; %Augmented B
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
load('data/2Dletters/S.mat');
Data=[];
for n=1:nbSamples
@@ -129,7 +130,7 @@ for i=1:model.nbStates
end
-%% Iterative LQR with augmented state space
+%% LQR with recursive computation and augmented state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Set list of states according to first demonstration (alternatively, an HSMM can be used)
[~,qList] = max(H(:,1:nbData),[],1); %works also for nbStates=1
@@ -140,24 +141,23 @@ for t=nbData-1:-1:1
Q = inv(model.Sigma(:,:,qList(t)));
P(:,:,t) = Q - A' * (P(:,:,t+1) * B / (B' * P(:,:,t+1) * B + R) * B' * P(:,:,t+1) - P(:,:,t+1)) * A;
end
-%Reproduction with only feedback (FB) terms on augmented state
+%Reproduction with only feedback (FB) on augmented state
for n=1:nbRepros
if n==1
- X = [Data(:,1); 1];
+ x = [Data(:,1); 1];
else
- X = [Data(:,1); 1] + [randn(model.nbVarPos,1)*2E0; zeros(model.nbVar-model.nbVarPos,1)];
+ x = [Data(:,1); 1] + [randn(model.nbVarPos,1)*2E0; zeros(model.nbVar-model.nbVarPos,1)];
end
- r(n).X0 = X;
+ r(n).x0 = x;
for t=1:nbData
- r(n).Data(:,t) = X; %Log data
+ r(n).x(:,t) = x; %Log data
K = (B' * P(:,:,t) * B + R) \ B' * P(:,:,t) * A; %FB term
- u = -K * X; %Acceleration command with FB terms on augmented state (resulting in FB and FF terms)
- X = A * X + B * u; %Update of state vector
+ u = -K * x; %Acceleration command with FB on augmented state (resulting in FB and FF terms)
+ x = A * x + B * u; %Update of state vector
end
end
-
-%% Iterative linear quadratic tracking (for comparison)
+%% Linear quadratic tracking with recursive computation (for comparison)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P = zeros(model0.nbVar,model0.nbVar,nbData);
P(:,:,end) = inv(model0.Sigma(:,:,qList(nbData)));
@@ -169,13 +169,13 @@ for t=nbData-1:-1:1
end
%Reproduction with feedback (FB) and feedforward (FF) terms
for n=1:nbRepros
- X = r(n).X0(1:end-1);
+ x = r(n).x0(1:end-1);
for t=1:nbData
- r2(n).Data(:,t) = X; %Log data
+ r2(n).x(:,t) = x; %Log data
K = (B0' * P(:,:,t) * B0 + R) \ B0' * P(:,:,t) * A0; %FB term
M = -(B0' * P(:,:,t) * B0 + R) \ B0' * (P(:,:,t) * (A0 * model0.Mu(:,qList(t)) - model0.Mu(:,qList(t))) + d(:,t)); %FF term
- u = K * (model0.Mu(:,qList(t)) - X) + M; %Acceleration command with FB and FF terms
- X = A0 * X + B0 * u; %Update of state vector
+ u = K * (model0.Mu(:,qList(t)) - x) + M; %Acceleration command with FB and FF terms
+ x = A0 * x + B0 * u; %Update of state vector
end
end
@@ -183,14 +183,14 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot position
-figure('position',[10 10 1000 1000],'color',[1 1 1]); axis off; hold on;
+figure('position',[10 10 800 800],'color',[1 1 1]); axis off; hold on;
plotGMM(model0.Mu(1:2,:), model0.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
end
for n=1:nbRepros
- h(1) = plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]);
- h(2) = plot(r2(n).Data(1,:), r2(n).Data(2,:), '--','linewidth',2,'color',[0 .8 0]);
+ h(1) = plot(r(n).x(1,:), r(n).x(2,:), '-','linewidth',2,'color',[.8 0 0]);
+ h(2) = plot(r2(n).x(1,:), r2(n).x(2,:), '--','linewidth',2,'color',[0 .8 0]);
end
axis equal;
legend(h,'LQR with augmented state space',' LQT with FB and FF terms');
@@ -202,14 +202,14 @@ legend(h,'LQR with augmented state space',' LQT with FB and FF terms');
% plot(Data(3,(n-1)*nbData+1:n*nbData), Data(4,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
% end
% for n=1:nbRepros
-% plot(r(n).Data(3,:), r(n).Data(4,:), '-','linewidth',2,'color',[.8 0 0]);
-% plot(r(n).Data(3,1), r(n).Data(4,1), '.','markersize',18,'color',[.6 0 0]);
-% plot(r2(n).Data(3,:), r2(n).Data(4,:), '--','linewidth',2,'color',[0 .8 0]);
+% plot(r(n).x(3,:), r(n).x(4,:), '-','linewidth',2,'color',[.8 0 0]);
+% plot(r(n).x(3,1), r(n).x(4,1), '.','markersize',18,'color',[.6 0 0]);
+% plot(r2(n).x(3,:), r2(n).x(4,:), '--','linewidth',2,'color',[0 .8 0]);
% end
% plot(0,0,'k+');
% axis equal;
% xlabel('dx_1'); ylabel('dx_2');
-%print('-dpng','graphs/demo_iterativeLQR_augmSigma01.png');
+% print('-dpng','graphs/demo_iterativeLQR_augmSigma01.png');
pause;
close all;
\ No newline at end of file
diff --git a/demos/demo_MPC_iterativeLQR_augmSigma_online01.m b/demos/demo_MPC_iterativeLQR_augmSigma_online01.m
index 031596f9dbff8a322285f4f04b84caae1372d9e7..ffdb1e2489ba1f44609486cff42ae736418df72c 100644
--- a/demos/demo_MPC_iterativeLQR_augmSigma_online01.m
+++ b/demos/demo_MPC_iterativeLQR_augmSigma_online01.m
@@ -1,5 +1,5 @@
function demo_MPC_iterativeLQR_augmSigma_online01
-% Iterative LQR with augmented covariance to transform the tracking problem to a regulation problem, recomputed in an online manner.
+% Recursive LQR with augmented covariance to transform the tracking problem to a regulation problem, recomputed in an online manner.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
diff --git a/demos/demo_MPC_robot01.m b/demos/demo_MPC_robot01.m
index adf4402b578c446cbda5eda857dc56f10b301da9..ba1b8c9a8403d84b6fb039699736eb12b7f7a325 100644
--- a/demos/demo_MPC_robot01.m
+++ b/demos/demo_MPC_robot01.m
@@ -1,9 +1,6 @@
function demo_MPC_robot01
% Robot manipulator tracking task with MPC, by relinearization of the system plant at each time step.
%
-% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
-% First run 'startup_rvc' from the robotics toolbox.
-%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
% author="Calinon, S. and Lee, D.",
@@ -16,6 +13,9 @@ function demo_MPC_robot01
% doi="10.1007/978-94-007-6046-2_68"
% }
%
+% The commented parts of this demo require the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
+% First run 'startup_rvc' from the robotics toolbox if you uncomment these parts.
+%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
@@ -34,7 +34,6 @@ function demo_MPC_robot01
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
-disp('This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).');
%% Parameters
@@ -56,49 +55,52 @@ model.rfactor = 1E-2; %Control cost in LQR
R = eye(model.nbVarU) * model.rfactor;
R = kron(eye(nbD-1),R);
+%Robot description
+model.l = 0.6;
-%% Robot description
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-armLength = 0.6;
-L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
-robot = SerialLink(repmat(L1,nbDOFs,1));
+% L1 = Link('d', 0, 'a', model.l, 'alpha', 0);
+% robot = SerialLink(repmat(L1,nbDOFs,1));
%% Task description
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.Mu = ones(model.nbVarPos,1) .* .7;
-model.Sigma = eye(model.nbVarPos) .* 1E-3;
+model.Mu = ones(model.nbVarPos,1) * .7;
+model.Sigma = eye(model.nbVarPos) * 1E-3;
MuQ = kron(ones(nbD,1), model.Mu);
Q = kron(eye(nbD), inv(model.Sigma));
%% Online batch LQR with relinearization of the system plant
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-q = ones(nbDOFs,1) .* 0.1;
-Htmp = robot.fkine(q);
-x = Htmp.t(1:model.nbVarPos);
+q = ones(nbDOFs,1) * 0.1;
+% Htmp = robot.fkine(q);
+% x = Htmp.t(1:model.nbVarPos);
+x = fkine(q, model);
+
for t=1:nbData
- [Su, Sx] = computeTransferMatrices(q, model, robot, nbD); %Linearization of system plant
+% [Su, Sx] = computeTransferMatrices(q, model, robot, nbD); %Linearization of system plant
+ [Su, Sx] = computeTransferMatrices(q, model, nbD); %Linearization of system plant
u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x); %Control trajectory
%Log data
r(1).q(:,t) = q;
r(1).x(:,t) = x;
r(1).u(:,t) = u(1:model.nbVarU);
%Update state
- q = q + u(1:model.nbVarU) .* model.dt;
- Htmp = robot.fkine(q); %Forward kinematics
- x = Htmp.t(1:model.nbVarPos);
+ q = q + u(1:model.nbVarU) * model.dt;
+% Htmp = robot.fkine(q); %Forward kinematics
+% x = Htmp.t(1:model.nbVarPos);
+ x = fkine(q, model);
end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 700 700],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
for t=round(linspace(1,nbData,2))
colTmp = [.9,.9,.9] - [.7,.7,.7] * t/nbData;
- plotArm(r(1).q(:,t), ones(nbDOFs,1)*armLength, [0;0;-10+t*0.1], .06, colTmp);
+ plotArm(r(1).q(:,t), ones(nbDOFs,1)*model.l, [0;0;-10+t*0.1], .06, colTmp);
end
-% plotArm(model.Mu, ones(nbDOFs,1)*armLength, [0;0;10], .02, [.8 0 0]);
+% plotArm(model.Mu, ones(nbDOFs,1)*model.l, [0;0;10], .02, [.8 0 0]);
plotGMM(model.Mu, model.Sigma, [.8 0 0], .5);
plot(model.Mu(1,:), model.Mu(2,:), '.','markersize',20,'color',[.8 0 0]);
@@ -112,7 +114,7 @@ axis equal;
%% Timeline plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$'};
-figure('position',[720 10 1000 1300],'color',[1 1 1]);
+figure('position',[830 10 800 800],'color',[1 1 1]);
for j=1:model.nbVarX
subplot(model.nbVarX+model.nbVarU,1,j); hold on;
plot([1,nbData],[model.Mu(j) model.Mu(j)],':','color',[.5 .5 .5]);
@@ -156,10 +158,41 @@ function [Su, Sx] = transferMatrices(A, B, nbD)
end
%%%%%%%%%%%%%%%%%%%%%%
-function [Su, Sx, A, B] = computeTransferMatrices(q, model, robot, nbD)
- J = robot.jacob0(q,'trans');
+% function [Su, Sx, A, B] = computeTransferMatrices(q, model, robot, nbD)
+function [Su, Sx, A, B] = computeTransferMatrices(q, model, nbD)
+% J = robot.jacob0(q,'trans');
+ J = jacob0(q, model);
%Discrete linear system x(t+1) = x(t) + J * dq
A = eye(model.nbVarX);
- B = J(1:model.nbVarPos,:) .* model.dt;
+ B = J * model.dt;
[Su, Sx] = transferMatrices(A, B, nbD);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics
+function [x, Tf] = fkine(q, model)
+ Tf = eye(4);
+ T = repmat(Tf, [1,1,size(q,1)]);
+ for n=1:size(q,1)
+ c = cos(q(n));
+ s = sin(q(n));
+ T(:,:,n) = [c, -s, 0, model.l * c; ...
+ s, c, 0, model.l * s; ...
+ 0, 0, 1, 0;
+ 0, 0, 0, 1]; %Homogeneous matrix
+ Tf = Tf * T(:,:,n);
+ end
+ x = Tf(1:2,end);
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with numerical computation
+function J = jacob0(q, model)
+ e = 1E-4;
+ J = zeros(2,size(q,1));
+ for n=1:size(q,1)
+ qtmp = q;
+ qtmp(n) = qtmp(n) + e;
+ J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
+ end
end
\ No newline at end of file
diff --git a/demos/demo_Riemannian_S2_batchLQR01.m b/demos/demo_Riemannian_S2_batchLQR01.m
index 3316c2545752a10c0828db4f7cb2a3ef12b5da6f..95c327f960f3e87f7810b520a09672927d625768 100644
--- a/demos/demo_Riemannian_S2_batchLQR01.m
+++ b/demos/demo_Riemannian_S2_batchLQR01.m
@@ -1,6 +1,7 @@
function demo_Riemannian_S2_batchLQR01
% LQT on a sphere (version with viapoints) by relying on Riemannian manifold (recomputed in an online manner),
% by using position and velocity data (-> acceleration commands)
+% (see also demo_Riemannian_Sd_MPC01.m)
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
@@ -235,8 +236,10 @@ for i=1:model.nbStates
end
%S2 manifold plot
-figure('position',[10,10,2500,700]);
+figure('position',[10,10,1600,500]);
subplot(2,3,[1,4]); hold on; axis off; grid off; rotate3d on;
+set(0,'DefaultAxesLooseInset',[0,0,0,0]);
+set(gca,'LooseInset',[0,0,0,0]);
colormap([.8 .8 .8]);
[X,Y,Z] = sphere(20);
mesh(X,Y,Z);
@@ -250,7 +253,7 @@ for n=1:nbRepros
plot3(r(n).x(1,:), r(n).x(2,:), r(n).x(3,:), '-','linewidth',2,'color',[.6 .6 .6]);
plot3(r(n).x(1,1), r(n).x(2,1), r(n).x(3,1), '.','markersize',24,'color',[.6 .6 .6]);
end
-view(-20,15); axis equal; axis([-1 1 -1 1 -1 1].*1.8); axis vis3d;
+view(-20,15); axis equal; axis([-1 1 -1 1 -1 1].*1.4); axis vis3d;
%Tangent plane plot
subplot(2,3,[2,5]); hold on; axis off;
@@ -282,13 +285,14 @@ end
%% Anim plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-for tt=1:nbData %58:58
+for tt=floor(linspace(1,nbData,nbData)) %58:58
e0 = r(n).x(:,tt);
xTmp = expmap(r(1).s(tt).u(1:model.nbVarPos,:), e0);
%Tangent plane on S2 anim
subplot(2,3,[1,4]); hold on;
- msh = repmat(e0,1,5) + rotM(e0)' * [1 1 -1 -1 1; 1 -1 -1 1 1; 0 0 0 0 0] * 1E0;
- h = patch(msh(1,:),msh(2,:),msh(3,:), [.8 .8 .8],'edgecolor',[.6 .6 .6],'facealpha',.3,'edgealpha',.3);
+ msh = repmat(e0,1,5) + rotM(e0)' * [1 1 -1 -1 1; 1 -1 -1 1 1; 0 0 0 0 0] * 5E-1;
+% h = patch(msh(1,:),msh(2,:),msh(3,:), [.8 .8 .8],'edgecolor',[.6 .6 .6]),'facealpha',.3,'edgealpha',.3);
+ h = plot3(msh(1,:),msh(2,:),msh(3,:), '-','color',[.6 .6 .6]);
h = [h, plot3(e0(1), e0(2), e0(3), '.','markersize',20,'color',[0 0 0])];
%Referential on S2 anim
msh = repmat(e0,1,2) + rotM(e0)' * [1 -1; 0 0; 0 0] * 2E-1;
@@ -315,8 +319,8 @@ for tt=1:nbData %58:58
end
drawnow;
-% pause;
-% print('-dpng','-r600','graphs/S2_batchLQR01.png');
+% print('-dpng',['graphs/anim/S2_batchLQR_anim' num2str(tt,'%.3d') '.png']);
+
delete(h);
end
diff --git a/demos/demo_Riemannian_S2_batchLQR02.m b/demos/demo_Riemannian_S2_batchLQR02.m
index a703b460ff23de58c33b8eb354013fb91552270a..eb1d1c1b0be36e92efd993856f6506a2ad8df149 100644
--- a/demos/demo_Riemannian_S2_batchLQR02.m
+++ b/demos/demo_Riemannian_S2_batchLQR02.m
@@ -220,7 +220,7 @@ for i=1:model.nbStates
end
%Plots
-figure('position',[10,10,1650,1250]); hold on; axis off; grid off; rotate3d on;
+figure('position',[10,10,800,800]); hold on; axis off; grid off; rotate3d on;
colormap([.8 .8 .8]);
[X,Y,Z] = sphere(20);
mesh(X,Y,Z);
diff --git a/demos/demo_Riemannian_S2_batchLQR03.m b/demos/demo_Riemannian_S2_batchLQR03.m
index 9730422296fb20f611fd85194665df73b26b37f1..b5a80812ceef3f3a2216e6d2b3546ae5ccd182bd 100644
--- a/demos/demo_Riemannian_S2_batchLQR03.m
+++ b/demos/demo_Riemannian_S2_batchLQR03.m
@@ -123,9 +123,8 @@ for nb=1:nbIterEM
model.Priors(i) = sum(GAMMA(i,:)) / (nbData*nbSamples);
%Update MuMan
for n=1:nbIter
- xTar = model.MuMan(1:3,i); %Position target
- uTmp = logmap(x0(1:3,:), xTar);
- model.MuMan(:,i) = expmap(uTmp*H(i,:)', xTar);
+ uTmp = logmap(x0(1:3,:), model.MuMan(1:3,i));
+ model.MuMan(:,i) = expmap(uTmp*H(i,:)', model.MuMan(1:3,i));
end
%Update Sigma
model.Sigma(:,:,i) = uTmp * diag(H(i,:)) * uTmp' + eye(size(uTmp,1)) * model.params_diagRegFact;
@@ -186,7 +185,7 @@ for i=1:model.nbStates
end
%Manifold plot
-figure('position',[10,10,1200,1200]); hold on; axis off; grid off; rotate3d on;
+figure('position',[10,10,800,800]); hold on; axis off; grid off; rotate3d on;
colormap([.8 .8 .8]);
[X,Y,Z] = sphere(20);
mesh(X,Y,Z); %,'facealpha',.5
@@ -217,7 +216,7 @@ patch(msh(1,:),msh(2,:),msh(3,:), [.8 .8 .8],'edgecolor',[.6 .6 .6],'facealpha',
%% Tangent space plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[1220,10,650,650]); hold on; axis off; box on;
+figure('position',[820,10,650,650]); hold on; axis off; box on;
plot(0,0,'+','markersize',40,'linewidth',2,'color',[.7 .7 .7]);
plot(0,0,'.','markersize',20,'color',[0 0 0]);
% % plot(u0(1,:), u0(2,:), '.','markersize',12,'color',[.5 .5 .5]);
diff --git a/demos/demo_Riemannian_S2_infHorLQR01.m b/demos/demo_Riemannian_S2_infHorLQR01.m
index c1d6f023b295c99caed3a2356834b621fcd1c9b0..e77a9a26d9d63ed276c1bc0a118a8fcf811ee98e 100644
--- a/demos/demo_Riemannian_S2_infHorLQR01.m
+++ b/demos/demo_Riemannian_S2_infHorLQR01.m
@@ -36,96 +36,96 @@ nbData = 100; %Number of datapoints
nbDrawingSeg = 50; %Number of segments used to draw ellipsoids
nbRepros = 20; %Number of reproductions
-model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
-model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
-model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector in the tangent space
-model.nbVarMan = model.nbVarPos+1; %Dimension of the manifold
-model.dt = 1E-3; %Time step duration
-model.rfactor = 1E-6; %Control cost in LQR
+nbVarPos = 2; %Dimension of position data (here: x1,x2)
+nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
+nbVar = nbVarPos * nbDeriv; %Dimension of state vector in the tangent space
+nbVarMan = nbVarPos+1; %Dimension of the manifold
+dt = 1E-3; %Time step duration
+rfactor = 1E-6; %Control cost in LQR
%Control cost matrix
-R = eye(model.nbVarPos) * model.rfactor;
+R = eye(nbVarPos) * rfactor;
%Target and desired covariance
xTar = [-1; .2; .6];
xTar = xTar / norm(xTar);
-[Ar,~] = qr(randn(model.nbVarPos));
+[Ar,~] = qr(randn(nbVarPos));
uCov = Ar*diag([.1,1.4])*Ar' * 1E-1;
%uCov = diag([.1,.1]) * 1E-1;
-%Eigendecomposition
-[V,D] = eig(uCov);
-U0 = V * D.^.5;
+% %Eigendecomposition
+% [V,D] = eig(uCov);
+% U0 = V * D.^.5;
%% Discrete dynamical System settings (in tangent space)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-A1d = zeros(model.nbDeriv);
-for i=0:model.nbDeriv-1
- A1d = A1d + diag(ones(model.nbDeriv-i,1),i) * model.dt^i * 1/factorial(i); %Discrete 1D
+A1d = zeros(nbDeriv);
+for i=0:nbDeriv-1
+ A1d = A1d + diag(ones(nbDeriv-i,1),i) * dt^i * 1/factorial(i); %Discrete 1D
end
-B1d = zeros(model.nbDeriv,1);
-for i=1:model.nbDeriv
- B1d(model.nbDeriv-i+1) = model.dt^i * 1/factorial(i); %Discrete 1D
+B1d = zeros(nbDeriv,1);
+for i=1:nbDeriv
+ B1d(nbDeriv-i+1) = dt^i * 1/factorial(i); %Discrete 1D
end
-A = kron(A1d, eye(model.nbVarPos)); %Discrete nD
-B = kron(B1d, eye(model.nbVarPos)); %Discrete nD
+A = kron(A1d, eye(nbVarPos)); %Discrete nD
+B = kron(B1d, eye(nbVarPos)); %Discrete nD
%Generate initial conditions
for n=1:nbRepros
- x0(:,n) = [-1; -1; 0] + randn(model.nbVarMan,1)*9E-1;
+ x0(:,n) = [-1; -1; 0] + randn(nbVarMan,1)*9E-1;
x0(:,n) = x0(:,n) / norm(x0(:,n));
end
%% Discrete LQR with infinite horizon (computation centered on xTar)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-duTar = zeros(model.nbVarPos*(model.nbDeriv-1),1);
-Q = blkdiag(inv(uCov), zeros(model.nbVarPos*(model.nbDeriv-1))); %Precision matrix
+duTar = zeros(nbVarPos*(nbDeriv-1),1);
+Q = blkdiag(inv(uCov), zeros(nbVarPos*(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)
for n=1:nbRepros
x = x0(:,n);
- U = [logmap(x,xTar); zeros(model.nbVarPos*(model.nbDeriv-1),1)];
+ U = [logmap(x,xTar); zeros(nbVarPos*(nbDeriv-1),1)];
for t=1:nbData
r(n).x(:,t) = x; %Log data
- %U = [-logmap(x,xTar); U(model.nbVarPos+1:end)];
- ddu = L * [-logmap(x,xTar); duTar-U(model.nbVarPos+1:end)]; %Compute acceleration (with only feedback terms)
+ %U = [-logmap(x,xTar); U(nbVarPos+1:end)];
+ ddu = L * [-logmap(x,xTar); duTar-U(nbVarPos+1:end)]; %Compute acceleration (with only feedback terms)
U = A*U + B*ddu; %Update U
- x = expmap(U(1:model.nbVarPos), xTar);
+ x = expmap(U(1:nbVarPos), xTar);
end
end
%% Discrete LQR with infinite horizon (computation centered on x, for comparison)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-duTar = zeros(model.nbVarPos*(model.nbDeriv-1),1);
+duTar = zeros(nbVarPos*(nbDeriv-1),1);
for n=1:nbRepros
x = x0(:,n);
x_old = x;
- U = zeros(model.nbVar,1);
+ U = zeros(nbVar,1);
for t=1:nbData
r2(n).x(:,t) = x; %Log data
- U(1:model.nbVarPos) = zeros(model.nbVarPos,1); %Set tangent space at x
+ U(1:nbVarPos) = zeros(nbVarPos,1); %Set tangent space at x
%Transportation of velocity vector from x_old to x
Ac = transp(x_old, x);
- U(model.nbVarPos+1:end) = Ac * U(model.nbVarPos+1:end); %Transport of velocity
+ U(nbVarPos+1:end) = Ac * U(nbVarPos+1:end); %Transport of velocity
%Transportation of uCov from xTar to x
Ac = transp(xTar, x);
uCovTmp = Ac * uCov * Ac';
- Q = blkdiag(inv(uCovTmp), zeros(model.nbVarPos*(model.nbDeriv-1))); %Precision matrix
+ Q = blkdiag(inv(uCovTmp), zeros(nbVarPos*(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)
- ddu = L * [logmap(xTar,x); duTar-U(model.nbVarPos+1:end)]; %Compute acceleration
- %ddu = L * [logmap(x,xTar); duTar-U(model.nbVarPos+1:end)]; %Compute acceleration
+ ddu = L * [logmap(xTar,x); duTar-U(nbVarPos+1:end)]; %Compute acceleration
+ %ddu = L * [logmap(x,xTar); duTar-U(nbVarPos+1:end)]; %Compute acceleration
U = A*U + B*ddu; %Update U
x_old = x;
- x = expmap(U(1:model.nbVarPos), x);
+ x = expmap(U(1:nbVarPos), x);
end
end
diff --git a/demos/demo_Riemannian_S3_GMM01.m b/demos/demo_Riemannian_S3_GMM01.m
index 933cac0b8721844c7286db2e5155dc5c23b02434..62b11a9c3a329c7d35cbd17bb41efdab6056863a 100644
--- a/demos/demo_Riemannian_S3_GMM01.m
+++ b/demos/demo_Riemannian_S3_GMM01.m
@@ -137,20 +137,20 @@ function u = logmap(x, mu)
else
Q = QuatMatrix(mu);
end
- u = logfct(Q'*x);
+ u = logfct(Q' * x);
end
function Exp = expfct(u)
normv = sqrt(u(1,:).^2+u(2,:).^2+u(3,:).^2);
- Exp = real([cos(normv) ; u(1,:).*sin(normv)./normv ; u(2,:).*sin(normv)./normv ; u(3,:).*sin(normv)./normv]);
- Exp(:,normv < 1e-16) = repmat([1;0;0;0],1,sum(normv < 1e-16));
+ Exp = real([cos(normv) ; u(1,:) .* sin(normv) ./ normv ; u(2,:) .* sin(normv) ./ normv ; u(3,:) .* sin(normv) ./ normv]);
+ Exp(:,normv < 1e-16) = repmat([1;0;0;0], 1, sum(normv < 1e-16));
end
function Log = logfct(x)
% scale = acos(x(3,:)) ./ sqrt(1-x(3,:).^2);
scale = acoslog(x(1,:)) ./ sqrt(1-x(1,:).^2);
scale(isnan(scale)) = 1;
- Log = [x(2,:).*scale; x(3,:).*scale; x(4,:).*scale];
+ Log = [x(2,:) .* scale; x(3,:) .* scale; x(4,:) .* scale];
end
function Q = QuatMatrix(q)
@@ -160,7 +160,7 @@ function Q = QuatMatrix(q)
q(4) -q(3) q(2) q(1)];
end
-% Arcosine re-defitinion to make sure the distance between antipodal quaternions is zero (2.50 from Dubbelman's Thesis)
+%Arcosine redefinition to make sure the distance between antipodal quaternions is zero (2.50 from Dubbelman's Thesis)
function acosx = acoslog(x)
for n=1:size(x,2)
% sometimes abs(x) is not exactly 1.0
diff --git a/demos/demo_Riemannian_Sd_MPC01.m b/demos/demo_Riemannian_Sd_MPC01.m
index b5f38b84af8f7054eef043bf455d843de78f7b4c..a685dbe3199df55b347cb7b2bb4348ca03fd9d76 100644
--- a/demos/demo_Riemannian_Sd_MPC01.m
+++ b/demos/demo_Riemannian_Sd_MPC01.m
@@ -49,7 +49,7 @@ model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector in the tangent space
model.dt = 1E-2; %Time step duration
model.params_diagRegFact = 1E-4; %Regularization of covariance
-model.rfactor = 1E-10; %Control cost in LQR
+model.rfactor = 1E-8; %Control cost in LQR
e0 = [0; -1; 0]; %Origin on manifold
%Control cost matrix
@@ -256,7 +256,9 @@ for i=1:model.nbStates
end
%S2 manifold plot
-figure('position',[10,10,2500,700]);
+figure('position',[10,10,1600,500]);
+set(0,'DefaultAxesLooseInset',[0,0,0,0]);
+set(gca,'LooseInset',[0,0,0,0]);
subplot(2,3,[1,4]); hold on; axis off; grid off; rotate3d on;
colormap([.8 .8 .8]);
[X,Y,Z] = sphere(20);
@@ -271,7 +273,7 @@ for n=1:nbRepros
plot3(r(n).x(1,:), r(n).x(2,:), r(n).x(3,:), '-','linewidth',2,'color',[.6 .6 .6]);
plot3(r(n).x(1,1), r(n).x(2,1), r(n).x(3,1), '.','markersize',24,'color',[.6 .6 .6]);
end
-view(-20,15); axis equal; axis([-1 1 -1 1 -1 1].*1.8); axis vis3d;
+view(-20,15); axis equal; axis([-1 1 -1 1 -1 1].*1.4); axis vis3d;
%Tangent plane plot
subplot(2,3,[2,5]); hold on; axis off;
@@ -308,8 +310,9 @@ for tt=1:nbData %58:58
xTmp = expmap(r(1).s(tt).u(1:model.nbVarPos,:), e0);
%Tangent plane on S2 anim
subplot(2,3,[1,4]); hold on;
- msh = repmat(e0,1,5) + rotM(e0)' * [1 1 -1 -1 1; 1 -1 -1 1 1; 0 0 0 0 0] * 1E0;
- h = patch(msh(1,:),msh(2,:),msh(3,:), [.8 .8 .8],'edgecolor',[.6 .6 .6],'facealpha',.3,'edgealpha',.3);
+ msh = repmat(e0,1,5) + rotM(e0)' * [1 1 -1 -1 1; 1 -1 -1 1 1; 0 0 0 0 0] * 5E-1;
+ h = patch(msh(1,:), msh(2,:), msh(3,:), [.8 .8 .8],'edgecolor',[.6 .6 .6],'facealpha',.3,'edgealpha',.3);
+% h = plot3(msh(1,:), msh(2,:), msh(3,:), '-','color',[.6 .6 .6]);
h = [h, plot3(e0(1), e0(2), e0(3), '.','markersize',20,'color',[0 0 0])];
%Referential on S2 anim
msh = repmat(e0,1,2) + rotM(e0)' * [1 -1; 0 0; 0 0] * 2E-1;
@@ -336,7 +339,7 @@ for tt=1:nbData %58:58
end
drawnow;
-% pause;
+% print('-dpng',['graphs/anim/S2_MPC_anim' num2str(tt,'%.3d') '.png']);
% print('-dpng','graphs/demo_Riemannian_Sd_MPC01.png');
delete(h);
end
@@ -348,21 +351,21 @@ end
%% Functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function x = expmap(u,x0)
+function x = expmap(u, x0)
theta = sqrt(sum(u.^2,1)); %norm(u,'fro')
x = real(repmat(x0,[1,size(u,2)]) .* repmat(cos(theta),[size(u,1),1]) + u .* repmat(sin(theta)./theta,[size(u,1),1]));
x(:,theta<1e-16) = repmat(x0,[1,sum(theta<1e-16)]);
end
-function u = logmap(x,x0)
+function u = logmap(x, x0)
theta = acos(x0'*x); %acos(trace(x0'*x))
u = (x - repmat(x0,[1,size(x,2)]) .* repmat(cos(theta),[size(x,1),1])) .* repmat(theta./sin(theta),[size(x,1),1]);
u(:,theta<1e-16) = 0;
end
-function Ac = transp(x1,x2,t)
+function Ac = transp(x1, x2, t)
if nargin==2
- t=1;
+ t = 1;
end
u = logmap(x2,x1);
e = norm(u,'fro');
diff --git a/demos/demo_Riemannian_Sd_MPC_infHor01.m b/demos/demo_Riemannian_Sd_MPC_infHor01.m
index 7a1f39c2008d138020b8c16df07c553831a1500c..aabd354626805163972af141f895c8bbcde01aad 100644
--- a/demos/demo_Riemannian_Sd_MPC_infHor01.m
+++ b/demos/demo_Riemannian_Sd_MPC_infHor01.m
@@ -81,13 +81,13 @@ L = (B'*P*B + R) \ B'*P*A; %Feedback gain (discrete version)
for n=1:nbRepros
x = [-1; -1; 1; 0] + randn(model.nbVarPos,1)*9E-1;
x = x / norm(x);
- U = [-logmap(x,xTar); zeros(model.nbVarPos*(model.nbDeriv-1),1)];
+ u = [-logmap(x,xTar); zeros(model.nbVarPos*(model.nbDeriv-1),1)];
for t=1:nbData
r(n).x(:,t) = x; %Log data
%U = [-logmap(x,xTar); U(model.nbVarPos+1:end)];
- ddu = L * [logmap(x,xTar); dxTar-U(model.nbVarPos+1:end)]; %Compute acceleration (with only feedback terms)
- U = A*U + B*ddu; %Update U
- x = expmap(-U(1:model.nbVarPos), xTar);
+ ddu = L * [logmap(x,xTar); dxTar - u(model.nbVarPos+1:end)]; %Compute acceleration (with only feedback terms)
+ u = A * u + B * ddu; %Update U
+ x = expmap(-u(1:model.nbVarPos), xTar);
end
end
@@ -140,31 +140,30 @@ for k=1:4
axis([1 nbData -1 1]);
end
-%3D plot
-figure('PaperPosition',[0 0 6 8],'position',[670,10,650,650]); hold on; axis off; rotate3d on;
-colormap([.9 .9 .9]);
-[X,Y,Z] = sphere(20);
-mesh(X,Y,Z,'facealpha',.3,'edgealpha',.3);
-plot3Dframe(quat2rotm(xTar'), zeros(3,1), eye(3)*.3);
-view(3); axis equal; axis tight; axis vis3d;
-h=[];
-for n=1:min(nbRepros,1)
- for t=1:nbData
- delete(h);
- h = plot3Dframe(quat2rotm(r(n).x(:,t)'), zeros(3,1));
- drawnow;
- if t==1
- %pause;
- end
- end
-end
+% %3D plot
+% figure('PaperPosition',[0 0 6 8],'position',[670,10,650,650]); hold on; axis off; rotate3d on;
+% colormap([.9 .9 .9]);
+% [X,Y,Z] = sphere(20);
+% mesh(X,Y,Z,'facealpha',.3,'edgealpha',.3);
+% plot3Dframe(quat2rotm(xTar'), zeros(3,1), eye(3)*.3);
+% h=[];
+% for n=1:min(nbRepros,1)
+% for t=floor(linspace(1,nbData,10))
+% % delete(h);
+% h = plot3Dframe(quat2rotm(r(n).x(:,t)'), zeros(3,1));
+% % drawnow;
+% if t==1
+% %pause;
+% end
+% end
+% end
+% view(3); axis equal; axis tight; axis vis3d;
%print('-dpng','graphs/demo_Riemannian_Sd_MPC_infHor01.png');
pause;
close all;
end
-
%% Functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function x = expmap(u,x0)
@@ -179,11 +178,11 @@ function u = logmap(x,x0)
u(:,theta<1e-16) = 0;
end
-function Ac = transp(x1,x2,t)
- if nargin==2
- t=1;
- end
- xdir = logmap(x2,x1);
- nrxdir = xdir./norm(xdir,'fro');
- Ac = -x1 * sin(norm(xdir,'fro')*t)*nrxdir' + nrxdir*cos(norm(xdir,'fro')*t)*nrxdir' + eye(size(xdir,1))-nrxdir*nrxdir';
-end
\ No newline at end of file
+% function Ac = transp(x1,x2,t)
+% if nargin==2
+% t=1;
+% end
+% xdir = logmap(x2,x1);
+% nrxdir = xdir./norm(xdir,'fro');
+% Ac = -x1 * sin(norm(xdir,'fro')*t)*nrxdir' + nrxdir*cos(norm(xdir,'fro')*t)*nrxdir' + eye(size(xdir,1))-nrxdir*nrxdir';
+% end
\ No newline at end of file
diff --git a/demos/demo_Riemannian_Sd_interp01.m b/demos/demo_Riemannian_Sd_interp01.m
index 3048a65617d98e1ac4949d5966ac814b69af93d4..75b4220032dd7d23644564b8ab9f905d785af4cc 100644
--- a/demos/demo_Riemannian_Sd_interp01.m
+++ b/demos/demo_Riemannian_Sd_interp01.m
@@ -114,7 +114,7 @@ end
%% Functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function x = expmap(u,x0)
+function x = expmap(u, x0)
th = sqrt(sum(u.^2,1)); %norm(u,'fro')
x = repmat(x0,[1,size(u,2)]) .* repmat(cos(th),[size(u,1),1]) + u .* repmat(sin(th)./th,[size(u,1),1]);
x(:,th<1e-16) = repmat(x0,[1,sum(th<1e-16)]);
@@ -123,7 +123,7 @@ function x = expmap(u,x0)
% end
end
-function u = logmap(x,x0)
+function u = logmap(x, x0)
% %Version 0
% p = (x-x0) - trace(x0'*(x-x0)) * x0;
% if norm(p,'fro')<1e-16
diff --git a/demos/demo_TPGMM_teleoperation01.m b/demos/demo_TPGMM_teleoperation01.m
index 2ca89993bfa68dab87cdc67e0dbb42bb044fb79e..1b50462c09b967a4420a0e6388e2b78a3cd84019 100644
--- a/demos/demo_TPGMM_teleoperation01.m
+++ b/demos/demo_TPGMM_teleoperation01.m
@@ -327,12 +327,13 @@ while (ishandle(H))
% hp = [hp plotEE(xh, [0 .7 0], 0)];
% hp = [hp plotEE(xh2, [.8 0 0], 0)];
drawnow;
-% print('-dpng',['graphs/anim/teleop_drill' num2str(id,'%.3d') '.png']);
- id = id+1;
- if mod(id,10)==0
- print('-dpng',['graphs/anim/teleop_drill' num2str(idf,'%.3d') '.png']);
- idf = idf+1;
- end
+
+% % print('-dpng',['graphs/anim/teleop_drill' num2str(id,'%.3d') '.png']);
+% id = id+1;
+% if mod(id,10)==0
+% print('-dpng',['graphs/anim/teleop_drill' num2str(idf,'%.3d') '.png']);
+% idf = idf+1;
+% end
end
diff --git a/demos/demo_gradientDescent01.m b/demos/demo_gradientDescent01.m
index d971c136de051b428a2037048b9c351934d1c254..47c1defe7c3f738c8a5be56c09c3a51cdbd71e63 100644
--- a/demos/demo_gradientDescent01.m
+++ b/demos/demo_gradientDescent01.m
@@ -1,5 +1,5 @@
function demo_gradientDescent01
-% Optimization with gradient descent for 1D input (Newton's method)
+% Optimization with gradient descent for 1D input (Newton's method for roots finding)
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
@@ -23,7 +23,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbIter = 10; %Number of iterations
+nbIter = 20; %Number of iterations
%% Create manifold
@@ -35,13 +35,14 @@ ym = fct(xm);
%% Gradient descent
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-s(1).x0 = .45;
-[s(1).xg, s(1).yg, s(1).J, s(1).H] = findMin(s(1).x0, nbIter);
+% x0 = -.25;
+x0 = 0.45;
+[xg, yg, J, H] = findMin(x0, nbIter);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,800,1000],'color',[1 1 1]); hold on; axis off;
+figure('position',[10,10,800,800],'color',[1 1 1]); hold on; axis off;
plot(xm, ym, '-','linewidth',2,'color',[.6 .6 .6]);
plot(xm, zeros(1,nbp), '-','linewidth',2,'color',[.6 .6 .6]);
plot(xm, -ones(1,nbp)*.1, '-','linewidth',2,'color',[.6 .6 .6]);
@@ -49,30 +50,30 @@ axis equal; axis([min(xm), max(xm), -.12, max(ym)]);
% print('-dpng',['graphs/demo_1DgradientDescent' num2str(0,'%.2d') '.png']);
% pause(.2);
for t=1:nbIter-1
- plot(s(1).xg(1,t), s(1).yg(1,t), '.','markersize',30,'color',[.6 .6 .6]);
- plot(s(1).xg(1,t), -.1, '.','markersize',30,'color',[.6 .6 .6]);
+ plot(xg(1,t), yg(1,t), '.','markersize',30,'color',[.6 .6 .6]);
+ plot(xg(1,t), -.1, '.','markersize',30,'color',[.6 .6 .6]);
%Plot tangent plane
- dx = s(1).xg(1,t+1) - s(1).xg(1,t);
+ dx = xg(1,t+1) - xg(1,t);
if dx<0
xtmp = [dx, 4E-2];
else
xtmp = [-4E-2, dx];
end
- msh = [xtmp; s(1).J(1,t)'*xtmp] + repmat([s(1).xg(:,t); s(1).yg(:,t)], 1, 2);
- h(1) = plot([s(1).xg(1,t), s(1).xg(1,t)], [-.1, s(1).yg(1,t)], '-','linewidth',1,'color',[.8 .8 .8]);
+ msh = [xtmp; J(1,t)'*xtmp] + repmat([xg(:,t); yg(:,t)], 1, 2);
+ h(1) = plot([xg(1,t), xg(1,t)], [-.1, yg(1,t)], '-','linewidth',1,'color',[.8 .8 .8]);
h(2) = plot(msh(1,:),msh(2,:), '-','linewidth',2,'color',[0 0 .8]);
- h(3) = plot2DArrow([s(1).xg(1,t); -.1], [dx; 0], [0 0 .8], 3, 1E-2);
- h(4) = plot(s(1).xg(1,t), s(1).yg(1,t), '.','markersize',30,'color',[0 0 0]);
- h(5) = plot(s(1).xg(1,t), -.1, '.','markersize',30,'color',[0 0 0]);
+ h(3) = plot2DArrow([xg(1,t); -.1], [dx; 0], [0 0 .8], 3, 1E-2);
+ h(4) = plot(xg(1,t), yg(1,t), '.','markersize',30,'color',[0 0 0]);
+ h(5) = plot(xg(1,t), -.1, '.','markersize',30,'color',[0 0 0]);
% print('-dpng',['graphs/demo_1DgradientDescent' num2str(t,'%.2d') '.png']);
% pause(.2);
% delete(h);
end
-% plot(s(1).xg, s(1).yg, '.','markersize',20,'color',[0 0 0]);
-% plot(s(1).xg, zeros(1,nbIter), '.','markersize',20,'color',[0 0 0]);
-plot(s(1).xg(1,end), s(1).yg(1,end), '.','markersize',40,'color',[.8 0 0]);
-plot(s(1).xg(1,end), -.1, '.','markersize',40,'color',[.8 0 0]);
-% print('-dpng',['graphs/demo_1DgradientDescent' num2str(nbIter+1,'%.2d') '.png']);
+% plot(xg, yg, '.','markersize',20,'color',[0 0 0]);
+% plot(xg, zeros(1,nbIter), '.','markersize',20,'color',[0 0 0]);
+plot(xg(1,end), yg(1,end), '.','markersize',40,'color',[.8 0 0]);
+plot(xg(1,end), -.1, '.','markersize',40,'color',[.8 0 0]);
+% print('-dpng','graphs/NewtonMethod_rootsFinding_final01.png');
% %Plot Jacobian and Hessian
% Jm = grad(xm);
@@ -80,15 +81,15 @@ plot(s(1).xg(1,end), -.1, '.','markersize',40,'color',[.8 0 0]);
% plot(xm, Jm, '-','linewidth',2,'color',[.6 .6 .6]);
% plot(xm, zeros(1,nbp),'-','linewidth',2,'color',[.6 .6 .6]);
% for t=1:nbIter-1
-% plot(s(1).xg(1,t), s(1).J(1,t), '.','markersize',20,'color',[.3 .3 .3]);
+% plot(xg(1,t), J(1,t), '.','markersize',20,'color',[.3 .3 .3]);
% %Plot tangent plane
% xtmp = [-1, 1] * 4E-2;
-% msh = [xtmp; s(1).H(1,t)'*xtmp] + repmat([s(1).xg(:,t); s(1).J(1,t)], 1, 2);
+% msh = [xtmp; H(1,t)'*xtmp] + repmat([xg(:,t); J(1,t)], 1, 2);
% h(1) = plot(msh(1,:),msh(2,:), '-','linewidth',3,'color',[.3 .3 1]);
% pause
% delete(h);
% end
-% plot(s(1).xg(1,end), s(1).J(1,end), '.','markersize',30,'color',[.8 0 0]);
+% plot(xg(1,end), J(1,end), '.','markersize',30,'color',[.8 0 0]);
% %axis equal;
pause;
@@ -109,6 +110,7 @@ end
function H = hess(x)
%H = diff(g,x)
+ %H = hessian(f,x)
H = 10.*exp(-x.^2) - 20.*x.^2.*exp(-x.^2) - 20.*sin(10.*x);
end
@@ -128,7 +130,7 @@ function [xg,yg,J,H] = findMin(x,nbIter)
% %Second order gradient method (Newton's method for optimization used to find minimum of a function (with null Jacobian), https://en.wikipedia.org/wiki/Newton%27s_method_in_optimization)
% H(:,t) = hess(x);
-% x = x - 8E-1 * (H(:,t) \ J(:,t)); %Use of a 8E-1 factor to obtain more stable results
+% x = x - 8E-1 * (H(:,t) \ J(:,t)); %Use of a step size (optional)
%Newton's method for finding the zeros of a function (https://en.wikipedia.org/wiki/Newton%27s_method)
x = x - J(:,t) \ fct(x);
diff --git a/demos/demo_gradientDescent02.m b/demos/demo_gradientDescent02.m
index fe05ec5cde9e2b3e990605a04173089ab4ce4a25..e95ebc56ac5615d880deebefeafbd203e1a43254 100644
--- a/demos/demo_gradientDescent02.m
+++ b/demos/demo_gradientDescent02.m
@@ -1,5 +1,5 @@
function demo_gradientDescent02
-% Optimization with gradient descent for 2D input (Newton's method)
+% Optimization with gradient descent for 2D input (Newton's method for roots finding)
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
@@ -36,13 +36,13 @@ ym = fct(xm);
%% Gradient descent
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-s(1).x0 = [.47; .4]; %(rand(nbVarIn,1)-0.5) .* .8;
-[s(1).xg, s(1).yg, s(1).J] = findMin(s(1).x0,nbIter);
+x0 = [.47; .4]; %(rand(nbVarIn,1)-0.5) .* .8;
+[xg, yg, J] = findMin(x0,nbIter);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,1100,1000],'color',[1 1 1]); hold on; axis off; rotate3d on;
+figure('position',[10,10,800,800],'color',[1 1 1]); hold on; axis off; rotate3d on;
colormap([.7 .7 .7]);
mesh(reshape(xm(1,:),nbp,nbp), reshape(xm(2,:),nbp,nbp), reshape(ym(1,:),nbp,nbp),'facealpha',.8,'edgealpha',.8);
msh = [1 1 -1 -1 1; 1 -1 -1 1 1; -.4*ones(1,5)] .* .5;
@@ -52,24 +52,23 @@ view(53,33); axis equal; axis vis3d; axis([-.55,.55,-.55,.55,-.22,.7]);
% print('-dpng',['graphs/demo_2DgradientDescent' num2str(0,'%.2d') '.png']);
% pause(.4);
for t=1:nbIter-1
- plot3(s(1).xg(1,t), s(1).xg(2,t), s(1).yg(1,t), '.','markersize',20,'color',[.6 .6 .6]);
- plot3(s(1).xg(1,t), s(1).xg(2,t), -.2, '.','markersize',20,'color',[.6 .6 .6]);
+ plot3(xg(1,t), xg(2,t), yg(1,t), '.','markersize',20,'color',[.6 .6 .6]);
+ plot3(xg(1,t), xg(2,t), -.2, '.','markersize',20,'color',[.6 .6 .6]);
%Plot tangent plane
xtmp = [1 1 -1 -1 1; 1 -1 -1 1 1] * 1E-1;
- msh = [xtmp; s(1).J(:,t)'*xtmp] + repmat([s(1).xg(:,t); s(1).yg(1,t)], 1, 5);
+ msh = [xtmp; J(:,t)'*xtmp] + repmat([xg(:,t); yg(1,t)], 1, 5);
h = patch(msh(1,:),msh(2,:),msh(3,:), [.3 .3 1],'edgecolor',[.6 .6 .6],'facealpha',.3,'edgealpha',.3);
- h = [h, mArrow3([s(1).xg(:,t); -.2], [s(1).xg(:,t+1); -.2],'tipWidth',8E-3,'stemWidth',2E-3,'color',[.3 .3 1])]; %*norm(s(1).J(:,t))
- h = [h, plot3(s(1).xg(1,t), s(1).xg(2,t), s(1).yg(1,t), '.','markersize',20,'color',[0 0 0])];
- h = [h, plot3(s(1).xg(1,t), s(1).xg(2,t), -.2, '.','markersize',20,'color',[0 0 0])];
+ h = [h, mArrow3([xg(:,t); -.2], [xg(:,t+1); -.2],'tipWidth',8E-3,'stemWidth',2E-3,'color',[.3 .3 1])]; %*norm(J(:,t))
+ h = [h, plot3(xg(1,t), xg(2,t), yg(1,t), '.','markersize',20,'color',[0 0 0])];
+ h = [h, plot3(xg(1,t), xg(2,t), -.2, '.','markersize',20,'color',[0 0 0])];
% pause(.4);
% print('-dpng',['graphs/demo_2DgradientDescent' num2str(t,'%.2d') '.png']);
% delete(h);
end
-% plot3(s(1).xg(1,:), s(1).xg(2,:), s(1).yg(1,:), '.','markersize',20,'color',[0 0 0]);
-% plot3(s(1).xg(1,:), s(1).xg(2,:), zeros(1,nbIter), '.','markersize',20,'color',[0 0 0]);
-plot3(s(1).xg(1,end), s(1).xg(2,end), s(1).yg(1,end), '.','markersize',40,'color',[.8 0 0]);
-plot3(s(1).xg(1,end), s(1).xg(2,end), -.2, '.','markersize',40,'color',[.8 0 0]);
-
+% plot3(xg(1,:), xg(2,:), yg(1,:), '.','markersize',20,'color',[0 0 0]);
+% plot3(xg(1,:), xg(2,:), zeros(1,nbIter), '.','markersize',20,'color',[0 0 0]);
+plot3(xg(1,end), xg(2,end), yg(1,end), '.','markersize',40,'color',[.8 0 0]);
+plot3(xg(1,end), xg(2,end), -.2, '.','markersize',40,'color',[.8 0 0]);
% print('-dpng',['graphs/demo_2DgradientDescent' num2str(nbIter,'%.2d') '.png']);
pause;
diff --git a/demos/demo_gradientDescent03.m b/demos/demo_gradientDescent03.m
index 8c0fe64ad4935a7328550821f7b1185e69d368d4..024877681452a88defab0c6c6e049f0d1a2b5940 100644
--- a/demos/demo_gradientDescent03.m
+++ b/demos/demo_gradientDescent03.m
@@ -1,5 +1,5 @@
function demo_gradientDescent03
-% Optimization with gradient descent for 1D input and 2D output (Gauss-Newton algorithm)
+% Optimization with gradient descent for 1D input and 2D output (Newton's method for roots finding)
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
@@ -35,13 +35,13 @@ ym = fct(xm);
%% Gradient descent
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-s(1).x0 = .45;
-[s(1).xg, s(1).yg, s(1).J] = findMin(s(1).x0, nbIter);
+x0 = .45;
+[xg, yg, J] = findMin(x0, nbIter);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,800,1300],'color',[1 1 1]); hold on; axis off;
+figure('position',[10,10,800,800],'color',[1 1 1]); hold on; axis off;
ofs = 1;
for k=1:2
plot(xm, ym(k,:)+(k-1)*ofs, '-','linewidth',2,'color',[.6 .6 .6]);
@@ -56,24 +56,24 @@ axis([min(xm), max(xm), -.12, max(ym(k,:))+ofs]);
for t=1:nbIter-1
h=[];
for k=1:2
- plot(s(1).xg(1,t), s(1).yg(k,t)+(k-1)*ofs, '.','markersize',30,'color',[.6 .6 .6]);
+ plot(xg(1,t), yg(k,t)+(k-1)*ofs, '.','markersize',30,'color',[.6 .6 .6]);
%Plot tangent plane
xtmp = [-1, 1] * 4E-2;
- msh = [xtmp; s(1).J(k,t)'*xtmp] + repmat([s(1).xg(:,t); s(1).yg(k,t)+(k-1)*ofs], 1, 2);
+ msh = [xtmp; J(k,t)'*xtmp] + repmat([xg(:,t); yg(k,t)+(k-1)*ofs], 1, 2);
h = [h, plot(msh(1,:), msh(2,:), '-','linewidth',3,'color',[.3 .3 1])];
- h = [h, plot(s(1).xg(1,t), s(1).yg(k,t)+(k-1)*ofs, '.','markersize',30,'color',[0 0 0])];
+ h = [h, plot(xg(1,t), yg(k,t)+(k-1)*ofs, '.','markersize',30,'color',[0 0 0])];
end %k
- plot(s(1).xg(1,t), -.1, '.','markersize',30,'color',[.6 .6 .6]);
- h = [h, plot2DArrow([s(1).xg(1,t); -.1], [s(1).xg(1,t+1)-s(1).xg(1,t); 0], [.3 .3 1], 3, 1E-2)];
- h = [h, plot(s(1).xg(1,t), -.1, '.','markersize',30,'color',[0 0 0])];
+ plot(xg(1,t), -.1, '.','markersize',30,'color',[.6 .6 .6]);
+ h = [h, plot2DArrow([xg(1,t); -.1], [xg(1,t+1)-xg(1,t); 0], [.3 .3 1], 3, 1E-2)];
+ h = [h, plot(xg(1,t), -.1, '.','markersize',30,'color',[0 0 0])];
% print('-dpng',['graphs/demo_multiOutputGradientDescent' num2str(t,'%.2d') '.png']);
% pause(.1);
% delete(h);
end %t
for k=1:2
- plot(s(1).xg(1,end), s(1).yg(k,end)+(k-1)*ofs, '.','markersize',40,'color',[.8 0 0]);
+ plot(xg(1,end), yg(k,end)+(k-1)*ofs, '.','markersize',40,'color',[.8 0 0]);
end
-plot(s(1).xg(1,end), -.1, '.','markersize',40,'color',[.8 0 0]);
+plot(xg(1,end), -.1, '.','markersize',40,'color',[.8 0 0]);
% print('-dpng',['graphs/demo_multiOutputGradientDescent' num2str(nbIter,'%.2d') '.png']);
pause;
diff --git a/demos/demo_gradientDescent04.m b/demos/demo_gradientDescent04.m
index f0298a544a62560996c514f22f744276142d4e21..d2dfc488b1d79c007f8d5da98ac765a611a25a2f 100644
--- a/demos/demo_gradientDescent04.m
+++ b/demos/demo_gradientDescent04.m
@@ -1,5 +1,5 @@
function demo_gradientDescent04
-% Optimization with gradient descent for 2D input and 2D output depicting a planar robot IK problem (Gauss-Newton algorithm).
+% Optimization with gradient descent for 2D input and 2D output depicting a planar robot IK problem (Newton's method for roots finding).
% First run 'startup_rvc' from the robotics toolbox.
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
@@ -46,13 +46,13 @@ ym = fct(xm, xd, robot);
%% Gradient descent
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-s(1).x0 = [.2; 2]; %Initial pose
-[s(1).xg, s(1).yg, s(1).J] = findMin(s(1).x0, xd, robot, nbIter);
+x0 = [.2; 2]; %Initial pose
+[xg, yg, J] = findMin(x0, xd, robot, nbIter);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,2400,1000],'color',[1 1 1]);
+figure('position',[10,10,1400,800],'color',[1 1 1]);
subplot(1,2,1); hold on; axis off; rotate3d on;
colormap([.7 .7 .7]);
@@ -69,7 +69,7 @@ view(53,33); axis equal; axis vis3d; %axis([-.55,.55,-.55,.55,-.22,.7]);
%plot robot
subplot(1,2,2); hold on; axis off;
plot3(xd(1), xd(2), 0, '.','markersize',50,'color',[.8 0 0]);
-% plotArm(s(1).x0, ones(nbDOFs,1)*armLength, [0;0;-20], .02, [.6 .6 .6]);
+% plotArm(x0, ones(nbDOFs,1)*armLength, [0;0;-20], .02, [.6 .6 .6]);
axis equal; axis tight; axis([-.55,.45,-.1,.7]);
% pause(.4);
% print('-dpng',['graphs/demo_robotGradientDescent' num2str(0,'%.2d') '.png']);
@@ -80,22 +80,22 @@ for t=1:nbIter-1
subplot(1,2,1); hold on;
h=[];
for k=1:2
- plot3(s(1).xg(1,t), s(1).xg(2,t), s(1).yg(k,t)+(k-1)*ofs, '.','markersize',20,'color',[.6 .6 .6]);
- plot3(s(1).xg(1,t), s(1).xg(2,t), -3, '.','markersize',20,'color',[.6 .6 .6]);
+ plot3(xg(1,t), xg(2,t), yg(k,t)+(k-1)*ofs, '.','markersize',20,'color',[.6 .6 .6]);
+ plot3(xg(1,t), xg(2,t), -3, '.','markersize',20,'color',[.6 .6 .6]);
% %Plot tangent plane
% xtmp = [1 1 -1 -1 1; 1 -1 -1 1 1] * 5E-1;
-% msh = [xtmp; s(1).J(k,:,t)*xtmp] + repmat([s(1).xg(:,t); s(1).yg(k,t)+(k-1)*ofs], 1, 5);
+% msh = [xtmp; J(k,:,t)*xtmp] + repmat([xg(:,t); yg(k,t)+(k-1)*ofs], 1, 5);
% h = [h, patch(msh(1,:),msh(2,:),msh(3,:), [.3 .3 1],'edgecolor',[.6 .6 .6],'facealpha',.3,'edgealpha',.3)];
-% h = [h, plot3(s(1).xg(1,t), s(1).xg(2,t), s(1).yg(k,t)+(k-1)*ofs, '.','markersize',20,'color',[0 0 0])];
+% h = [h, plot3(xg(1,t), xg(2,t), yg(k,t)+(k-1)*ofs, '.','markersize',20,'color',[0 0 0])];
end %k
- h = [h, mArrow3([s(1).xg(:,t); -3], [s(1).xg(:,t+1); -3],'tipWidth',6E-2,'stemWidth',2E-2,'color',[.3 .3 1])]; %*norm(s(1).J(:,t))
-% h = [h, plot3(s(1).xg(1,t), s(1).xg(2,t), -3, '.','markersize',20,'color',[0 0 0])];
-% h = [h, plot3([s(1).xg(1,t), s(1).xg(1,t)], [s(1).xg(2,t), s(1).xg(2,t)], [-3 s(1).yg(2,t)+ofs], '-','linewidth',1,'color',[.4 .4 .4])];
+ h = [h, mArrow3([xg(:,t); -3], [xg(:,t+1); -3],'tipWidth',6E-2,'stemWidth',2E-2,'color',[.3 .3 1])]; %*norm(J(:,t))
+% h = [h, plot3(xg(1,t), xg(2,t), -3, '.','markersize',20,'color',[0 0 0])];
+% h = [h, plot3([xg(1,t), xg(1,t)], [xg(2,t), xg(2,t)], [-3 yg(2,t)+ofs], '-','linewidth',1,'color',[.4 .4 .4])];
%plot robot
subplot(1,2,2); hold on;
- plotArm(s(1).xg(:,t), ones(nbDOFs,1)*armLength, [0;0;-100+t], .02, [.7 .7 .7]);
- % h = [h, plotArm(s(1).xg(:,t), ones(nbDOFs,1)*armLength, [0;0;-100+t], .02, [.2 .2 .2])];
+ plotArm(xg(:,t), ones(nbDOFs,1)*armLength, [0;0;-100+t], .02, [.7 .7 .7]);
+ % h = [h, plotArm(xg(:,t), ones(nbDOFs,1)*armLength, [0;0;-100+t], .02, [.2 .2 .2])];
% pause(.4);
% print('-dpng',['graphs/demo_robotGradientDescent' num2str(t,'%.2d') '.png']);
@@ -105,12 +105,12 @@ end %t
subplot(1,2,1); hold on;
for k=1:2
- plot3(s(1).xg(1,end), s(1).xg(2,end), s(1).yg(k,end)+(k-1)*ofs, '.','markersize',40,'color',[.8 0 0]);
+ plot3(xg(1,end), xg(2,end), yg(k,end)+(k-1)*ofs, '.','markersize',40,'color',[.8 0 0]);
end
-plot3([s(1).xg(1,end), s(1).xg(1,end)], [s(1).xg(2,end), s(1).xg(2,end)], [-3 ofs], '-','linewidth',1,'color',[1 .2 .2]);
-plot3(s(1).xg(1,end), s(1).xg(2,end), -3, '.','markersize',40,'color',[.8 0 0]);
+plot3([xg(1,end), xg(1,end)], [xg(2,end), xg(2,end)], [-3 ofs], '-','linewidth',1,'color',[1 .2 .2]);
+plot3(xg(1,end), xg(2,end), -3, '.','markersize',40,'color',[.8 0 0]);
subplot(1,2,2); hold on;
-plotArm(s(1).xg(:,t), ones(nbDOFs,1)*armLength, [0;0;-1], .02, [.2 .2 .2]);
+plotArm(xg(:,t), ones(nbDOFs,1)*armLength, [0;0;-1], .02, [.2 .2 .2]);
% pause(.4);
% print('-dpng',['graphs/demo_robotGradientDescent' num2str(nbIter,'%.2d') '.png']);
diff --git a/demos/m_fcts/angDiffFromQuat.m b/demos/m_fcts/angDiffFromQuat.m
index 32c127d0391307e3dfc5584e3e2c147ede284b92..4d5b607cee6805f4b79a359c2217f698631ad22f 100644
--- a/demos/m_fcts/angDiffFromQuat.m
+++ b/demos/m_fcts/angDiffFromQuat.m
@@ -1,4 +1,4 @@
-function omega = angDiffFromQuat(Q1,Q2)
+function omega = angDiffFromQuat(Q1, Q2)
q1 = Quaternion(Q1);
q2 = Quaternion(Q2);
@@ -8,5 +8,4 @@ if(q1.inner(q2)<0)
q2 = Quaternion(-q2.double);
end
-omega = 2 * quaternionLog(q1*q2.inv)';
-
+omega = 2 * quaternionLog(q1 * q2.inv)';
\ No newline at end of file
diff --git a/demos/m_fcts/plot2DArrow.m b/demos/m_fcts/plot2DArrow.m
index bde939bbd2f4dc326ac20cc270027a9d4c6a4f13..7b995079146042e229ec81b0752782f03ee6fac3 100644
--- a/demos/m_fcts/plot2DArrow.m
+++ b/demos/m_fcts/plot2DArrow.m
@@ -1,4 +1,4 @@
-function h = plot2DArrow(pos,dir,col,lnw,sz,alpha)
+function [h, msh] = plot2DArrow(pos, dir, col, lnw, sz, alpha)
% Simple display of a 2D arrow
%
% Writing code takes time. Polishing it and making it available to others takes longer!
@@ -39,7 +39,7 @@ if nargin<6
alpha = 1;
end
if nargin<5
- sz = 1E-1;
+ sz = norm(dir) .* 4E-2;
end
if nargin<4
lnw = 2;
@@ -56,6 +56,6 @@ if norm(dir)>sz
prp = [d(2); -d(1)];
d = d*sz;
prp = prp*sz;
- msh = [msh pos-d pos-d-prp/2 pos pos-d+prp/2 pos-d msh];
- h = patch(msh(1,:),msh(2,:),col,'edgecolor',col,'linewidth',lnw,'edgealpha',alpha,'facealpha',alpha);
-end
+ msh = [msh, pos-d, pos-d-prp/2, pos, pos-d+prp/2, pos-d, msh];
+ h = patch(msh(1,:), msh(2,:), col, 'edgecolor',col,'linewidth',lnw,'edgealpha',alpha,'facealpha',alpha);
+end
\ No newline at end of file
diff --git a/demos/m_fcts/plotGMM.m b/demos/m_fcts/plotGMM.m
index 1f20b1f37847bf1e494f93a9a42f7a4027484214..a2e0c1f4731487f8c79c4f52d0e456a4891484c6 100644
--- a/demos/m_fcts/plotGMM.m
+++ b/demos/m_fcts/plotGMM.m
@@ -63,9 +63,9 @@ for i=1:nbStates
%h = [h patch(MuTmp(1,:), MuTmp(2,:), darkcolor, 'LineStyle', 'none', 'facealpha', valAlpha)];
h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 10, 'color', darkcolor)];
else %Plot without transparency
- h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 10, 'color', darkcolor)];
%Standard plot
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)];
% %Plot only contours
% h = [h plot(X(1,:,i), X(2,:,i), '-', 'color', color, 'lineWidth', 1)];
end
diff --git a/demos/m_fcts/quaternionLog.m b/demos/m_fcts/quaternionLog.m
index bc13743728f0dc6c3e881bb06e2658743e438192..2dc9a64e5e9b2ab0db3f0b8392e4badaee1398d5 100644
--- a/demos/m_fcts/quaternionLog.m
+++ b/demos/m_fcts/quaternionLog.m
@@ -15,5 +15,5 @@ end
if norm(q.v)<realmin
a = [0 0 0];
else
- a = 2 * acos(q.s) * q.v/norm(q.v);
+ a = 2 * acos(q.s) * q.v / norm(q.v);
end
\ No newline at end of file