Commit 886f5e21 authored by Sylvain CALINON's avatar Sylvain CALINON
Browse files

m_fcts updated

parent e9adb51d
......@@ -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
......@@ -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
......@@ -78,4 +78,4 @@ end %nb
% print('-dpng','graphs/demo_Bezier_illustr01.png');
pause;
close all;
close all;
\ No newline at end of file
......@@ -163,4 +163,4 @@ axis equal;
% % print('-dpng','graphs/demo_Bezier_highOrder02.png');
pause;
close all;
close all;
\ No newline at end of file
......@@ -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
......@@ -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
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;