diff --git a/README.md b/README.md
index 07338434002826b14ea16165687ecbcf6be5b96f..b07602610e66de43de7d4b111843d4714cd32311 100644
--- a/README.md
+++ b/README.md
@@ -234,6 +234,7 @@ All the examples are located in the `demos` folder, and the functions are locate
| [demo_search01.m](./demos/demo_search01.m) | [[2]](#ref-2) | EM-based stochastic optimization |
| [demo_spring01.m](./demos/demo_spring01.m) | [[10]](#ref-10) | Influence of the damping ratio in mass-spring-damper systems |
| [demo_stdPGMM01.m](./demos/demo_stdPGMM01.m) | [[1]](#ref-1) | Parametric Gaussian mixture model (PGMM) used as a task-parameterized model, with DS-GMR employed to retrieve continuous movements |
+| [demo_tensor_TTGO01.m](./demos/demo_tensor_TTGO01.m) | [[11]](#ref-11) | Global optimization with tensor trains (TTGO) |
| [demo_TPHDDC01.m](./demos/demo_TPHDDC01.m) | [[1]](#ref-1) | Task-parameterized high dimensional data clustering (TP-HDDC) |
| [demo_TPGMM01.m](./demos/demo_TPGMM01.m) | [[1]](#ref-1) | Task-parameterized Gaussian mixture model (TP-GMM) encoding |
| [demo_TPGMM_bimanualReaching01.m](./demos/demo_TPGMM_bimanualReaching01.m) | [[1]](#ref-1) | Time-invariant task-parameterized GMM applied to a bimanual reaching task |
@@ -347,12 +348,21 @@ If you find PbDlib useful for your research, please cite the related publication
</p>
<p><a name="ref-11">
-[11] Lembono, T.S. and Calinon, S. (2021). <strong>Probabilistic Iterative LQR for Short Time Horizon MPC</strong>. arXiv:2012.06349.
+[11] Lembono, T.S. and Calinon, S. (2021). <strong>Probabilistic Iterative LQR for Short Time Horizon MPC</strong>. In Proc. IEEE/RSJ Intl Conf. on Intelligent Robots and Systems (IROS), pp. 556-562.
</a><br>
-[[pdf]](https://arxiv.org/pdf/2012.06349.pdf)
+[[pdf]](https://calinon.ch/papers/Lembono-IROS2021.pdf)
+[[bib]](https://calinon.ch/papers/Lembono-IROS2021.bib)
<br><strong>(Ref. for iLQR/DDP)</strong>
</p>
+<p><a name="ref-12">
+[12] Shetty, S., Lembono, T., Löw, T. and Calinon, S. (2022). <strong>Tensor Train for Global Optimization Problems in Robotics</strong>. arXiv:2206.05077.
+</a><br>
+[[pdf]](https://calinon.ch/papers/Shetty-arXiv2022.pdf)
+[[bib]](https://calinon.ch/papers/Shetty-arXiv2022.bib)
+<br><strong>(Ref. for low-rank tensor train factorization)</strong>
+</p>
+
### Gallery
| | |
@@ -368,9 +378,9 @@ If you find PbDlib useful for your research, please cite the related publication
### License
-The Matlab/GNU Octave version of PbDlib is maintained by Sylvain Calinon, Idiap Research Institute.
+The Matlab/GNU Octave version of PbDlib is maintained by [Sylvain Calinon](https://calinon.ch), Idiap Research Institute.
-Copyright (c) 2015-2021 Idiap Research Institute, https://idiap.ch/
+Copyright (c) 2015-2022 Idiap Research Institute, https://idiap.ch/
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.
diff --git a/demos/demo_GPR_closedShape02.m b/demos/demo_GPR_closedShape02.m
index ec3f34f0ea3e307aed6636527a19375c5554360e..f060e5205c16fba32acf0b5a92946866b7b4383f 100644
--- a/demos/demo_GPR_closedShape02.m
+++ b/demos/demo_GPR_closedShape02.m
@@ -139,7 +139,7 @@ set(gca,'LooseInset',[0,0,0,0]);
colormap(repmat(linspace(1,.4,64),3,1)');
%Plot center of GP (acting as prior if points on the contours are missing)
-subplot(2,5,1); hold on; axis off; rotate3d on; title('Prior (circular contour)','fontsize',16);
+subplot(2,5,1); hold on; axis off; rotate3d on; title('Prior (circular shape)','fontsize',26);
% 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
@@ -151,7 +151,7 @@ plot3(xc(1)+cos(tl)*rc, xc(2)+sin(tl)*rc, zeros(1,100), '-','linewidth',2,'color
view(3); axis vis3d;
%Plot posterior distribution (3D)
-subplot(2,5,6); hold on; axis off; rotate3d on; title('Posterior','fontsize',16);
+subplot(2,5,6); hold on; axis off; rotate3d on; title('Posterior','fontsize',26);
% 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
@@ -159,24 +159,24 @@ subplot(2,5,6); hold on; axis off; rotate3d on; title('Posterior','fontsize',16)
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(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
+%plot3(x(1,y==1), x(2,y==1), y(y==1)+.04, '.','markersize',38,'color',[.8 0 0]); %Interior points
+plot3(x(1,y==0), x(2,y==0), y(y==0)+.04, '.','markersize',38,'color',[.8 .4 0]); %Border points
+%plot3(x(1,y==-1), x(2,y==-1), y(y==-1)+.04, '.','markersize',38,'color',[0 .6 0]); %Exterior points
view(3); axis vis3d;
%Plot posterior distribution (2D)
-subplot(2,5,[2,3,7,8]); hold on; axis off; title('Distance to contour and gradient','fontsize',16);
+subplot(2,5,[2,3,7,8]); hold on; axis off; title('Distance to contour and gradient','fontsize',26);
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,1:4:end), xs(2,1:4:end), rdy(1,1:4:end), rdy(2,1:4:end),'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]);
+%h(1) = plot(x(1,y==1), x(2,y==1), '.','markersize',48,'color',[.8 0 0]); %Interior points
+h(1) = plot(x(1,y==0), x(2,y==0), '.','markersize',48,'color',[.8 .4 0]); %Border points
+%h(3) = plot(x(1,y==-1), x(2,y==-1), '.','markersize',48,'color',[0 .6 0]); %Exterior points
+h(2) = plot(xc(1)+cos(tl)*rc, xc(2)+sin(tl)*rc, '--','linewidth',2,'color',[0 0 0]);
+[~,h(3)] = contour(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), [0,0], 'linewidth',4,'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]);
@@ -184,22 +184,23 @@ h(4) = plot(xc(1)+cos(tl)*rc, xc(2)+sin(tl)*rc, '--','linewidth',2,'color',[0 0
% 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)','Prior for contour estimation','Estimated contour'},'fontsize',16,'location','southwest');
+%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(h,{'Provided contour points','Prior for contour estimation','Estimated contour'},'fontsize',26,'location','southwest');
% legend('boxoff');
%Plot uncertainty (2D)
-subplot(2,5,[4,5,9,10]); hold on; axis off; title('Uncertainty','fontsize',16);
+subplot(2,5,[4,5,9,10]); hold on; axis off; title('Uncertainty','fontsize',26);
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(x(1,y==1), x(2,y==1), '.','markersize',48,'color',[.8 0 0]); %Interior points
+plot(x(1,y==0), x(2,y==0), '.','markersize',48,'color',[.8 .4 0]); %Border points
+plot(x(1,y==-1), x(2,y==-1), '.','markersize',48,'color',[0 .6 0]); %Exterior points
+contour(Xm, Ym, reshape(ry, nbDataRepro^.5, nbDataRepro^.5), [0,0], 'linewidth',4,'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');
+%print('-dpng','graphs/GPIS_new01.png');
pause;
close all;
end
@@ -249,4 +250,4 @@ function [K, dK] = covFct(x1, x2, p, flag_noiseObs)
if flag_noiseObs==1
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
+end
diff --git a/demos/demo_ID01.m b/demos/demo_ID01.m
index 05cd79651d1e770e751bf33901ff4b17f9363ea2..bb55539225e59852ee80bfb96cd9294dd2efdc89 100644
--- a/demos/demo_ID01.m
+++ b/demos/demo_ID01.m
@@ -110,7 +110,7 @@ for t=1:nbData
% ddq = robot.accel(q', dq', u'); %Compute acceleration
ddq = accel(q, u, model);
- pause
+
% ddq = (u - uG) / M; %Compute acceleration
dq = dq + ddq*dt;
@@ -154,4 +154,4 @@ function J = jacob0(q, model)
qtmp(n) = qtmp(n) + e;
J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
end
-end
\ No newline at end of file
+end
diff --git a/demos/demo_IK01.m b/demos/demo_IK01.m
index 1926d4a94c72e871c5c5320079dbbcf8269236d7..d9eaa231606f402c6e7d3cd275e89a3130847dc4 100644
--- a/demos/demo_IK01.m
+++ b/demos/demo_IK01.m
@@ -84,9 +84,9 @@ function [x, Tf] = fkine(q, model)
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
+ 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);
@@ -102,4 +102,4 @@ function J = jacob0(q, model)
qtmp(n) = qtmp(n) + e;
J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
end
-end
\ No newline at end of file
+end
diff --git a/demos/demo_IK_expForm01.m b/demos/demo_IK_expForm01.m
index b5f53bff6aadd6247250a23b28678753a01622ef..93f6a3faccc5a8155166652f0a8472991dc6e5aa 100644
--- a/demos/demo_IK_expForm01.m
+++ b/demos/demo_IK_expForm01.m
@@ -40,7 +40,7 @@ end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,10,800,650],'color',[1 1 1]); hold on; axis off;
+h = figure('position',[20,10,800,650],'color',[1 1 1]); hold on; axis off;
for t=floor(linspace(1,nbData,20))
colTmp = [.7,.7,.7] - [.7,.7,.7] * t/nbData;
plotArm(q(:,t), model.l, [0;0;-2+t/nbData], .2, colTmp);
@@ -52,8 +52,7 @@ plot(x(1,:), x(2,:), '.','markersize',20,'color',[0 .6 0]);
axis equal; axis tight;
%print('-dpng','graphs/demoIK_expForm01.png');
-pause;
-close all;
+waitfor(h);
end
function u = logmap(x, x0)
@@ -88,6 +87,6 @@ function J = jacob0(q, model)
% J = [real(J); imag(J)]
% J = model.l * 1i * exp(1i * tril(ones(model.nbDOFs)) * q)' * tril(ones(model.nbDOFs)); %Computation in matrix form
- J = 1i * exp(1i * tril(ones(model.nbDOFs)) * q).' * tril(ones(model.nbDOFs)) * diag(model.l); %Computation in matrix form
+ J = 1i * exp(1i * tril(ones(model.nbDOFs)) * q).' * diag(model.l) * tril(ones(model.nbDOFs)); %Computation in matrix form
J = [real(J); imag(J); ones(1, model.nbDOFs)]; %x1,x2,o
-end
\ No newline at end of file
+end
diff --git a/demos/demo_IK_minimal01.m b/demos/demo_IK_minimal01.m
index 2e1ab9e56ee6f7914cb385bc4b44a2d41579b6be..016f7a6c38f0befa5681d20dfcdf6656f7de114e 100644
--- a/demos/demo_IK_minimal01.m
+++ b/demos/demo_IK_minimal01.m
@@ -10,49 +10,52 @@ addpath('./m_fcts/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 1E-2; %Time step
nbData = 100; %Number of datapoints
-model.nbDOFs = 3; %Number of articulations
-model.l = [2, 2, 1]; %Length of each link
-q(:,1) = [pi-3*pi/4; pi/2; pi/4]; %Initial pose
-x(:,1) = fkine(q(:,1), model);
+model.nbVarX = 3; %Number of articulations
+model.l = [2; 2; 1]; %Length of each link
+x(:,1) = [3*pi/4; -pi/2; -pi/4]; %Initial pose
+%x(:,1) = ones(model.nbVarX,1) * pi / model.nbVarX; %Initial pose
%% IK
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-xh = [2; 2];
+fh = [3; 1];
for t=1:nbData-1
- J = jacob(q(:,t), model); %Jacobian
- dq = pinv(J) * 8E0 * (xh - x(:,t));
- q(:,t+1) = q(:,t) + dq * dt;
- x(:,t+1) = fkine(q(:,t+1), model); %FK for a planar robot
+ f(:,t) = fkin(x(:,t), model); %FK for a planar robot
+ J = Jkin(x(:,t), model); %Jacobian
+ dx = pinv(J) * (fh - f(:,t)) * 1E1;
+ x(:,t+1) = x(:,t) + dx * dt;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,10,800,650],'color',[1 1 1]); hold on; axis off;
+h = 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), model.l, [0;0;-2+t/nbData], .2, colTmp);
+ plotArm(x(:,t), model.l, [0;0;-2+t/nbData], .2, colTmp);
end
-plot(xh(1), xh(2), '.','markersize',40,'color',[.8 0 0]);
-plot(x(1,:), x(2,:), '.','markersize',20,'color',[0 .6 0]);
+plot(fh(1), fh(2), '.','markersize',40,'color',[.8 0 0]);
+plot(f(1,:), f(2,:), '.','markersize',20,'color',[0 .6 0]);
axis equal; axis tight;
%print('-dpng','graphs/demoIK_minimal01.png');
-pause;
-close all;
+%pause;
+%close all;
+waitfor(h);
end
%%%%%%%%%%%%%%%%%%%%%%
%Forward kinematics
-function x = fkine(q, model)
- x = [model.l * cos(tril(ones(model.nbDOFs)) * q); ...
- model.l * sin(tril(ones(model.nbDOFs)) * q)];
+function f = fkin(x, model)
+ L = tril(ones(model.nbVarX));
+ f = [model.l' * cos(L * x); ...
+ model.l' * sin(L * x)];
end
%%%%%%%%%%%%%%%%%%%%%%
%Jacobian with analytical computation
-function J = jacob(q, model)
- J = [-sin(tril(ones(model.nbDOFs)) * q)' * diag(model.l) * tril(ones(model.nbDOFs)); ...
- cos(tril(ones(model.nbDOFs)) * q)' * diag(model.l) * tril(ones(model.nbDOFs))];
-end
\ No newline at end of file
+function J = Jkin(x, model)
+ L = tril(ones(model.nbVarX));
+ J = [-sin(L * x)' * diag(model.l) * L; ...
+ cos(L * x)' * diag(model.l) * L];
+end
diff --git a/demos/demo_OC_DDP_bicopter01.m b/demos/demo_OC_DDP_bicopter01.m
index 248815af9b92352e52547a24140221145f0b3889..aff5e1eecdcc5e5ed012e82c6d8a5f3350695384 100644
--- a/demos/demo_OC_DDP_bicopter01.m
+++ b/demos/demo_OC_DDP_bicopter01.m
@@ -99,19 +99,19 @@ figure('position',[10,10,800,800]); hold on; axis off; grid on; box on;
for t=floor(linspace(1,model.nbData,20))
plot(r.x(1,t), r.x(2,t), '.','markersize',40,'color',[.6 .6 .6]);
msh = [r.x(1:2,t), r.x(1:2,t)] + [cos(r.x(3,t)); sin(r.x(3,t))] * [-.4, .4];
- plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[.6 .6 .6]);
+ plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[.7 .7 .7]);
end
-plot(r.x(1,:),r.x(2,:), '-','linewidth',2,'color',[.6 .6 .6]);
+plot(r.x(1,:),r.x(2,:), '-','linewidth',2,'color',[0 0 0]);
msh = [r.x(1:2,1), r.x(1:2,1)] + [cos(r.x(3,1)); sin(r.x(3,1))] * [-.4, .4];
plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[0 0 0]);
h(1) = plot(r.x(1,1),r.x(2,1), '.','markersize',40,'color',[0 0 0]);
msh = [model.Mu(1:2), model.Mu(1:2)] + [cos(model.Mu(3)); sin(model.Mu(3))] * [-.4, .4];
plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[.8 0 0]);
h(2) = plot(model.Mu(1), model.Mu(2), '.','markersize',40,'color',[.8 0 0]);
-legend(h,{'Initial pose','Target pose'},'location','northwest','fontsize',30);
+%legend(h,{'Initial pose','Target pose'},'location','northwest','fontsize',30);
axis equal; axis([-.5 4.5 -.2 4.4]);
xlabel('x_1'); ylabel('x_2');
-% print('-dpng','graphs/DDP_bicopter01.png');
+%print('-dpng','graphs/DDP_bicopter01.png');
% %% Plot control space
@@ -223,4 +223,4 @@ function [A, B] = linSys(x, u, model)
% f(6) = l/I * (u(1) - u(2));
% jacobian(f, x)
% jacobian(f, u)
-end
\ No newline at end of file
+end
diff --git a/demos/demo_OC_DDP_car01.m b/demos/demo_OC_DDP_car01.m
index a215a14268c2c23f1fd7e0304639455edc0a4b9e..97a74ef8369fe9bab12d73d305e448c0d9be8b4f 100644
--- a/demos/demo_OC_DDP_car01.m
+++ b/demos/demo_OC_DDP_car01.m
@@ -94,7 +94,7 @@ figure('position',[10,10,800,800]); hold on; axis off;
for t=round(linspace(1, model.nbData, 20))
R = [cos(r.x(3,t)) -sin(r.x(3,t)); sin(r.x(3,t)) cos(r.x(3,t))];
msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r.x(1:2,t),1,5);
- plot(msh(1,:), msh(2,:), '-','linewidth',2,'color',[.6 .6 .6]);
+ plot(msh(1,:), msh(2,:), '-','linewidth',2,'color',[.7 .7 .7]);
end
plot(r.x(1,:),r.x(2,:), '-','linewidth',2,'color',[0 0 0]);
%Initial pose
@@ -112,9 +112,9 @@ R = [cos(model.Mu(3)) -sin(model.Mu(3)); sin(model.Mu(3)) cos(model.Mu(3))];
msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(model.Mu(1:2),1,5);
plot(msh(1,:), msh(2,:), '-','linewidth',2,'color',[.8 0 0]);
h(2) = plot(model.Mu(1), model.Mu(2), '.','markersize',20,'color',[.8 0 0]);
-legend(h,{'Initial pose','Target pose'},'location','northwest','fontsize',30);
+%legend(h,{'Initial pose','Target pose'},'location','northwest','fontsize',30);
axis equal; axis([-1 5 -1 4]);
-% print('-dpng','graphs/DDP_car01.png');
+%print('-dpng','graphs/DDP_car01.png');
% %% Plot control space
@@ -220,4 +220,4 @@ function [A, B] = linSys(x, u, model)
% f(4) = u(2);
% jacobian(f, x)
% jacobian(f, u)
-end
\ No newline at end of file
+end
diff --git a/demos/demo_OC_DDP_manipulator01.m b/demos/demo_OC_DDP_manipulator01.m
index c2582e7845541873724f3c27d14f90e649ebfe82..c72f266cf57878833beb66b7d2ad680a4307b7c6 100644
--- a/demos/demo_OC_DDP_manipulator01.m
+++ b/demos/demo_OC_DDP_manipulator01.m
@@ -33,66 +33,79 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.dt = 1E-2; %Time step size
-model.nbData = 50; %Number of datapoints
-model.nbIter = 300; %Maximum number of iterations for iLQR
-model.nbPoints = 2; %Number of viapoints
-model.nbVarX = 3; %State space dimension (q1,q2,q3)
-model.nbVarU = 3; %Control space dimension (dq1,dq2,dq3)
-model.nbVarF = 3; %Task space dimension (x1,x2,o)
-model.l = [2, 2, 1]; %Robot links lengths
-model.sz = [.2, .3]; %Size of objects
-model.q = 1E0; %Tracking weighting term
-model.r = 1E-6; %Control weighting term
+param.dt = 1E-2; %Time step size
+param.nbData = 50; %Number of datapoints
+param.nbIter = 300; %Maximum number of iterations for iLQR
+param.nbPoints = 2; %Number of viapoints
+param.nbVarX = 3; %State space dimension (x1,x2,x3)
+param.nbVarU = 3; %Control space dimension (dx1,dx2,dx3)
+param.nbVarF = 3; %Task space dimension (f1,f2,f3, with f3 as orientation)
+param.l = [3; 2; 1]; %Robot links lengths
+param.fmax = [.2, .3, .01]; %Bounding boxes parameters
+param.q = 1E0; %Tracking weighting term
+param.r = 1E-5; %Control weighting term
-% model.Mu = [[2; 1; -pi/2], [3; 1; -pi/2]]; %Viapoints
-model.Mu = [[2; 1; -pi/6], [3; 2; -pi/3]]; %Viapoints
-% model.Mu = [3; 0; -pi/2]; %Viapoints
-for t=1:model.nbPoints
- model.A(:,:,t) = [cos(model.Mu(3,t)), -sin(model.Mu(3,t)); sin(model.Mu(3,t)), cos(model.Mu(3,t))]; %Orientation
+% param.Mu = [[2; 1; -pi/2], [3; 1; -pi/2]]; %Viapoints
+param.Mu = [[2; 1; -pi/6], [3; 2; -pi/3]]; %Viapoints
+% param.Mu = [3; 0; -pi/2]; %Viapoints
+for t=1:param.nbPoints
+ param.A(:,:,t) = [cos(param.Mu(3,t)), -sin(param.Mu(3,t)); sin(param.Mu(3,t)), cos(param.Mu(3,t))]; %Orientation
end
-Q = speye(model.nbVarF * model.nbPoints) * model.q; %Precision matrix
-% Q = kron(eye(model.nbPoints), diag([1E0, 1E0, 0])); %Precision matrix (by removing orientation constraint)
-R = speye(model.nbVarU * (model.nbData-1)) * model.r; %Control weight matrix (at trajectory level)
+Q = speye(param.nbVarF * param.nbPoints) * param.q; %Precision matrix
+% Q = kron(eye(param.nbPoints), diag([1E0, 1E0, 0])); %Precision matrix (by removing orientation constraint)
+R = speye(param.nbVarU * (param.nbData-1)) * param.r; %Control weight matrix (at trajectory level)
%Time occurrence of viapoints
-tl = linspace(1, model.nbData, model.nbPoints+1);
+tl = linspace(1, param.nbData, param.nbPoints+1);
tl = round(tl(2:end));
-idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
+idx = (tl - 1) * param.nbVarX + [1:param.nbVarX]';
%% Iterative LQR (iLQR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-u = zeros(model.nbVarU*(model.nbData-1), 1); %Initial commands
+u = zeros(param.nbVarU*(param.nbData-1), 1); %Initial control commands
x0 = [3*pi/4; -pi/2; -pi/4]; %Initial robot pose
%Transfer matrices (for linear system as single integrator)
-% A = repmat(eye(model.nbVarX), [1 1 model.nbData-1]);
-% B = repmat(eye(model.nbVarX, model.nbVarU) * model.dt, [1 1 model.nbData-1]);
+% A = repmat(eye(param.nbVarX), [1 1 param.nbData-1]);
+% B = repmat(eye(param.nbVarX, param.nbVarU) * param.dt, [1 1 param.nbData-1]);
% [Su0, Sx0] = transferMatrices(A, B); %Constant Su and Sx for the proposed system
-Su0 = [zeros(model.nbVarX, model.nbVarX*(model.nbData-1)); kron(tril(ones(model.nbData-1)), eye(model.nbVarX)*model.dt)];
-Sx0 = kron(ones(model.nbData,1), eye(model.nbVarX));
+Su0 = [zeros(param.nbVarX, param.nbVarX*(param.nbData-1)); kron(tril(ones(param.nbData-1)), eye(param.nbVarX)*param.dt)];
+Sx0 = kron(ones(param.nbData,1), eye(param.nbVarX));
Su = Su0(idx,:);
-for n=1:model.nbIter
- x = reshape(Su0 * u + Sx0 * x0, model.nbVarX, model.nbData); %System evolution
- [f, J] = f_reach(x(:,tl), model);
- du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u * model.r); %Gradient
+for n=1:param.nbIter
+ x = reshape(Su0 * u + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
+ [f, J] = f_reach(x(:,tl), param); %Residuals and Jacobian
+ du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u * param.r); %Gauss-Newton update
%Estimate step size with backtracking line search method
alpha = 1;
- cost0 = f(:)' * Q * f(:) + norm(u)^2 * model.r; %u' * R * u
+ cost0 = f(:)' * Q * f(:) + norm(u)^2 * param.r; %u' * R * u
while 1
utmp = u + du * alpha;
- xtmp = reshape(Su0 * utmp + Sx0 * x0, model.nbVarX, model.nbData);
- ftmp = f_reach(xtmp(:,tl), model);
- cost = ftmp(:)' * Q * ftmp(:) + norm(utmp)^2 * model.r; %utmp' * R * utmp
+ xtmp = reshape(Su0 * utmp + Sx0 * x0, param.nbVarX, param.nbData);
+ ftmp = f_reach(xtmp(:,tl), param);
+ cost = ftmp(:)' * Q * ftmp(:) + norm(utmp)^2 * param.r; %utmp' * R * utmp
if cost < cost0 || alpha < 1E-3
break;
end
alpha = alpha * 0.5;
end
+
+% %Estimate step size with backtracking line search method
+% cost0 = f(:)' * Q * f(:) + norm(u)^2 * param.r; %u' * R * u
+% cost = 1E10;
+% alpha = 2;
+% while (cost > cost0 && alpha > 1E-3)
+% alpha = alpha * 0.5;
+% utmp = u + du * alpha;
+% xtmp = reshape(Su0 * utmp + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
+% ftmp = f_reach(xtmp(:,tl), param); %Residuals
+% cost = ftmp(:)' * Q * ftmp(:) + norm(utmp)^2 * param.r; %Cost
+% end
+
u = u + du * alpha;
if norm(du * alpha) < 1E-2
@@ -101,99 +114,93 @@ for n=1:model.nbIter
end
disp(['iLQR converged in ' num2str(n) ' iterations.']);
-%Log data
-r.x = x;
-r.f = fkine0(x, model);
-r.u = reshape(u, model.nbVarU, model.nbData-1);
-
%% Plot state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-msh0 = diag(model.sz) * [-1 -1 1 1 -1; -1 1 1 -1 -1];
-for t=1:model.nbPoints
- msh(:,:,t) = model.A(:,:,t) * msh0 + repmat(model.Mu(1:2,t), 1, size(msh0,2));
+msh0 = diag(param.fmax(1:2)) * [-1 -1 1 1 -1; -1 1 1 -1 -1];
+for t=1:param.nbPoints
+ msh(:,:,t) = param.A(:,:,t) * msh0 + repmat(param.Mu(1:2,t), 1, size(msh0,2));
end
-figure('position',[10,10,800,800],'color',[1,1,1]); hold on; axis off;
-plotArm(r.x(:,1), model.l, [0; 0; -3], .2, [.8 .8 .8]);
-plotArm(r.x(:,tl(1)), model.l, [0; 0; -2], .2, [.6 .6 .6]);
-plotArm(r.x(:,tl(2)), model.l, [0; 0; -1], .2, [.4 .4 .4]);
-colMat = lines(model.nbPoints);
-for t=1:model.nbPoints
+h = figure('position',[10,10,1600,1200],'color',[1,1,1]); hold on; axis off;
+plotArm(x(:,1), param.l, [0; 0; -3], .2, [.8 .8 .8]);
+plotArm(x(:,tl(1)), param.l, [0; 0; -2], .2, [.6 .6 .6]);
+plotArm(x(:,tl(2)), param.l, [0; 0; -1], .2, [.4 .4 .4]);
+colMat = lines(param.nbPoints);
+for t=1:param.nbPoints
patch(msh(1,:,t), msh(2,:,t), min(colMat(t,:)*1.7,1),'linewidth',2,'edgecolor',colMat(t,:)); %,'facealpha',.2
- plot2Dframe(model.A(:,:,t)*4E-1, model.Mu(1:2,t), repmat(colMat(t,:),3,1), 6);
+ plot2Dframe(param.A(:,:,t)*5E-1, param.Mu(1:2,t), repmat(colMat(t,:),3,1), 2);
end
-plot(r.f(1,:), r.f(2,:), '-','linewidth',2,'color',[0 0 0]);
-plot(r.f(1,1), r.f(2,1), '.','markersize',40,'color',[0 0 0]);
-plot(r.f(1,tl), r.f(2,tl), '.','markersize',30,'color',[0 0 0]);
-axis equal;
-% print('-dpng','graphs/DDP_manipulator01.png');
+%plot(param.Mu(1,:), param.Mu(2,:), '.','markersize',40,'color',[.8 0 0]);
+ftmp = fkin(x, param);
+plot(ftmp(1,:), ftmp(2,:), '-','linewidth',2,'color',[0 0 0]);
+plot(ftmp(1,1), ftmp(2,1), '.','markersize',40,'color',[0 0 0]);
+axis equal; %axis([-2.4 3.5 -0.6 3.8]);
+%print('-dpng','graphs/DDP_manipulator01.png');
-%% Timeline plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[820 10 800 800],'color',[1 1 1]);
-%States plot
-for j=1:model.nbVarF
- subplot(model.nbVarF+model.nbVarU, 1, j); hold on;
- plot(tl, model.Mu(j,:), '.','markersize',35,'color',[.8 0 0]);
- plot(r.f(j,:), '-','linewidth',2,'color',[0 0 0]);
-% set(gca,'xtick',[1,nbData],'xticklabel',{'0','T'},'ytick',[0:2],'fontsize',20);
- ylabel(['$f_' num2str(j) '$'], 'interpreter','latex','fontsize',26);
-% axis([1, nbData, 0, 2.1]);
-end
-%Commands plot
-for j=1:model.nbVarU
- subplot(model.nbVarF+model.nbVarU, 1, model.nbVarF+j); hold on;
- plot(r.u(j,:), '-','linewidth',2,'color',[0 0 0]);
-% set(gca,'xtick',[1,nbData],'xticklabel',{'0','T'},'ytick',[0:2],'fontsize',20);
- ylabel(['$u_' num2str(j) '$'], 'interpreter','latex','fontsize',26);
-% axis([1, nbData, 0, 2.1]);
-end
-xlabel('$t$','interpreter','latex','fontsize',26);
+%%% Timeline plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%figure('position',[820 10 800 800],'color',[1 1 1]);
+%%States plot
+%for j=1:param.nbVarF
+% subplot(param.nbVarF+param.nbVarU, 1, j); hold on;
+% plot(tl, param.Mu(j,:), '.','markersize',35,'color',[.8 0 0]);
+% plot(r.f(j,:), '-','linewidth',2,'color',[0 0 0]);
+%% set(gca,'xtick',[1,nbData],'xticklabel',{'0','T'},'ytick',[0:2],'fontsize',20);
+% ylabel(['$f_' num2str(j) '$'], 'interpreter','latex','fontsize',26);
+%% axis([1, nbData, 0, 2.1]);
+%end
+%%Commands plot
+%r.u = reshape(u, param.nbVarU, param.nbData-1);
+%for j=1:param.nbVarU
+% subplot(param.nbVarF+param.nbVarU, 1, param.nbVarF+j); hold on;
+% plot(r.u(j,:), '-','linewidth',2,'color',[0 0 0]);
+%% set(gca,'xtick',[1,nbData],'xticklabel',{'0','T'},'ytick',[0:2],'fontsize',20);
+% ylabel(['$u_' num2str(j) '$'], 'interpreter','latex','fontsize',26);
+%% axis([1, nbData, 0, 2.1]);
+%end
+%xlabel('$t$','interpreter','latex','fontsize',26);
-% %% Plot value function for reaching costs
-% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% A0 = model.A;
-% Mu0 = model.Mu;
-%
-% nbRes = 20;
-% limAxes = [0 4 0 4];
-% [xx, yy] = ndgrid(linspace(limAxes(1),limAxes(2),nbRes), linspace(limAxes(3),limAxes(4),nbRes));
-% x = [xx(:)'; yy(:)'; zeros(1,nbRes^2)];
-% msh = [min(x(1,:)), min(x(1,:)), max(x(1,:)), max(x(1,:)), min(x(1,:)); ...
-% min(x(2,:)), max(x(2,:)), max(x(2,:)), min(x(2,:)), min(x(2,:))];
-% %Reaching at T/2
-% model.Mu = [repmat(Mu0(1:2,1), [1 nbRes^2]); zeros(1,nbRes^2)];
-% model.A = repmat(A0(:,:,1), [1 1 nbRes^2]);
-% f = f_reach(x, model);
-% z = sum(f.^2, 1);
-% zz = reshape(z, nbRes, nbRes);
-% %Reaching at T
-% model.Mu = [repmat(Mu0(1:2,2), [1 nbRes^2]); zeros(1,nbRes^2)];
-% model.A = repmat(A0(:,:,2), [1 1 nbRes^2]);
-% f2 = f_reach(x, model);
-% z2 = sum(f2.^2, 1);
-% zz2 = reshape(z2, nbRes, nbRes);
-%
-% figure('position',[820,10,860,400],'color',[1,1,1]);
-% colormap(repmat(linspace(1,.4,64),3,1)');
-% %Reaching at T/2
-% subplot(1,2,1); hold on; axis off; title('Reaching cost (for t=T/2)');
-% surface(xx, yy, zz-max(zz(:)), 'EdgeColor','interp','FaceColor','interp');
-% plot(Mu0(1,1), Mu0(2,1),'.','markersize',30,'color',colMat(1,:));
-% plot(msh(1,:), msh(2,:),'-','linewidth',1,'color',[0 0 0]); %border
-% axis equal; axis(limAxes);
-% %Reaching at T
-% subplot(1,2,2); hold on; axis off; title('Reaching cost (for t=T)');
-% surface(xx, yy, zz2-max(zz2(:)), 'EdgeColor','interp','FaceColor','interp');
-% plot(Mu0(1,2), Mu0(2,2),'.','markersize',30,'color',colMat(2,:));
-% plot(msh(1,:), msh(2,:),'-','linewidth',1,'color',[0 0 0]); %border
-% axis equal; axis(limAxes);
+%%% Plot value function for reaching costs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%nbRes = 100;
+%limAxes = [-2.4 3.5 -0.6 3.8];
+%[xx, yy] = ndgrid(linspace(limAxes(1),limAxes(2),nbRes), linspace(limAxes(3),limAxes(4),nbRes));
+%x = [xx(:)'; yy(:)'];
+%msh = [min(x(1,:)), min(x(1,:)), max(x(1,:)), max(x(1,:)), min(x(1,:)); ...
+% min(x(2,:)), max(x(2,:)), max(x(2,:)), min(x(2,:)), min(x(2,:))];
+%%Reaching at T/2
+%f = f_reach2(x, param.Mu(1:2,1), param.A(:,:,1), param.sz);
+%z = sum(f.^2, 1);
+%zz = reshape(z, nbRes, nbRes);
+%%Reaching at T
+%f2 = f_reach2(x, param.Mu(1:2,2), param.A(:,:,2), param.sz);
+%z2 = sum(f2.^2, 1);
+%zz2 = reshape(z2, nbRes, nbRes);
+
+%colMat = lines(param.nbPoints);
+%h = figure('position',[820,10,860,400],'color',[1,1,1]);
+%colormap(repmat(linspace(1,.4,64),3,1)');
+%%Reaching at T/2
+%subplot(1,2,1); hold on; axis off; title('Reaching cost (for t=T/2)');
+%surface(xx, yy, zz-max(zz(:)), 'EdgeColor','interp','FaceColor','interp');
+%contour(xx, yy, zz, [0:.4:max(zz(:))].^2,'color',[.3,.3,.3]);
+%plot(param.Mu(1,1), param.Mu(2,1),'.','markersize',30,'color',colMat(1,:));
+%plot(msh(1,:), msh(2,:),'-','linewidth',1,'color',[0 0 0]); %border
+%%plotArm(x(:,tl(1)), param.l, [0; 0; -2], .2, [.6 .6 .6]);
+%axis equal; axis(limAxes);
+%%Reaching at T
+%subplot(1,2,2); hold on; axis off; title('Reaching cost (for t=T)');
+%surface(xx, yy, zz2-max(zz2(:)), 'EdgeColor','interp','FaceColor','interp');
+%contour(xx, yy, zz2, [0:.4:max(zz2(:))].^2,'color',[.3,.3,.3]);
+%plot(param.Mu(1,2), param.Mu(2,2),'.','markersize',30,'color',colMat(2,:));
+%plot(msh(1,:), msh(2,:),'-','linewidth',1,'color',[0 0 0]); %border
+%axis equal; axis(limAxes);
+%%print('-dpng','graphs/DDP_manipulator01b.png');
-pause;
-close all;
+waitfor(h);
end
% %%%%%%%%%%%%%%%%%%%%%%
@@ -213,13 +220,6 @@ end
% end
% end
-% %%%%%%%%%%%%%%%%%%%%%%
-% %Logarithmic map for R^2 x S^1 manifold
-% function e = logmap(f, f0)
-% e(1:2,:) = f(1:2,:) - f0(1:2,:);
-% e(3,:) = imag(log(exp(f0(3,:)*1i)' .* exp(f(3,:)*1i).'));
-% end
-
%%%%%%%%%%%%%%%%%%%%%%
%Logarithmic map for R^2 x S^1 manifold
function e = logmap(f, f0)
@@ -229,46 +229,79 @@ end
%%%%%%%%%%%%%%%%%%%%%%
%Forward kinematics (in robot coordinate system)
-function f = fkine0(x, model)
- T = tril(ones(size(x,1)));
- f = [model.l * cos(T * x); ...
- model.l * sin(T * x); ...
- mod(sum(x,1)+pi, 2*pi) - pi]; %x1,x2,o (orientation as single Euler angle for planar robot)
+function f = fkin(x, param)
+ L = tril(ones(size(x,1)));
+ f = [param.l' * cos(L * x); ...
+ param.l' * sin(L * x); ...
+ mod(sum(x,1)+pi, 2*pi) - pi]; %f1,f2,f3 (f3 represents orientation as single Euler angle for planar robot)
end
%%%%%%%%%%%%%%%%%%%%%%
-%Jacobian with analytical computation (for single time step)
-function J = jacob0(x, model)
- T = tril(ones(size(x,1)));
- J = [-sin(T * x)' * T * diag(model.l); ...
- cos(T * x)' * T * diag(model.l); ...
- ones(1, size(x,1))]; %x1,x2,o
+%Jacobian of forward kinematics function with analytical computation
+function J = Jkin(x, param)
+ L = tril(ones(size(x,1)));
+ J = [-sin(L * x)' * diag(param.l) * L; ...
+ cos(L * x)' * diag(param.l) * L; ...
+ ones(1, size(x,1))]; %f1,f2,f3 (f3 represents orientation as single Euler angle for planar robot)
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian of forward kinematics function with numerical computation
+function J = Jkin_num(x, param)
+ e = 1E-6;
+
+% %Slow for-loop computation
+% J = zeros(param.nbVarF, param.nbVarX);
+% for n=1:size(x,1)
+% xtmp = x;
+% xtmp(n) = xtmp(n) + e;
+% J(:,n) = (fkine0(xtmp, param) - fkine0(x, param)) / e;
+% end
+
+ %Fast matrix computation
+ X = repmat(x, [1, param.nbVarX]);
+ F1 = fkin(X, param);
+ F2 = fkin(X + eye(param.nbVarX) * e, param);
+ J = (F2 - F1) / e;
end
%%%%%%%%%%%%%%%%%%%%%%
%Cost and gradient for a viapoints reaching task (in object coordinate system)
-function [f, J] = f_reach(x, model)
-% f = fkine0(x, model) - model.Mu; %Error by ignoring manifold
- f = logmap(model.Mu, fkine0(x, model)); %Error by considering manifold
+function [f, J] = f_reach(x, param)
+% f = fkin(x, param) - param.Mu; %Error by ignoring manifold
+ f = logmap(param.Mu, fkin(x, param)); %Error by considering manifold
J = [];
for t=1:size(x,2)
-% f(:,t) = logmap(fkine0(x(:,t), model), model.Mu(:,t));
- f(1:2,t) = model.A(:,:,t)' * f(1:2,t); %Object-centered FK
+ f(1:2,t) = param.A(:,:,t)' * f(1:2,t); %Object-centered FK
+
+ Jtmp = Jkin(x(:,t), param);
+ %Jtmp = Jkin_num(x(:,t), param);
- Jtmp = jacob0(x(:,t), model);
- Jtmp(1:2,:) = model.A(:,:,t)' * Jtmp(1:2,:); %Object-centered Jacobian
+ Jtmp(1:2,:) = param.A(:,:,t)' * Jtmp(1:2,:); %Object-centered Jacobian
%Bounding boxes (optional)
- for i=1:2
- if abs(f(i,t)) < model.sz(i)
+ for i=1:length(param.fmax)
+ if abs(f(i,t)) < param.fmax(i)
f(i,t) = 0;
Jtmp(i,:) = 0;
else
- f(i,t) = f(i,t) - sign(f(i,t)) * model.sz(i);
+ f(i,t) = f(i,t) - sign(f(i,t)) * param.fmax(i);
end
end
J = blkdiag(J, Jtmp);
end
-end
\ No newline at end of file
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Cost in task space for a viapoints reaching task (for visualization)
+function f = f_reach2(f, Mu, A, sz)
+ f = A' * (f - repmat(Mu,1,size(f,2))); %Object-centered error
+ %Bounding boxes (optional)
+ for i=1:2
+ id = abs(f(i,:)) < sz(i);
+ f(i,id) = 0;
+ f(i,~id) = f(i,~id) - sign(f(i,~id)) * sz(i);
+ end
+end
diff --git a/demos/demo_OC_LQT03.m b/demos/demo_OC_LQT03.m
index 6a9af095c637cb43cf8ac88fc9c9f7408bed91d5..196b53b98d812abc8cb1937e7a691f5e59ace7e6 100644
--- a/demos/demo_OC_LQT03.m
+++ b/demos/demo_OC_LQT03.m
@@ -40,12 +40,12 @@ nbRepros = 1; %Number of reproductions in new situations
nbNewRepros = 0; %Number of stochastically generated reproductions
nbData = 200; %Number of datapoints
-model.nbStates = 6; %Number of Gaussians in the GMM
+model.nbStates = 4; %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.nbDeriv = 2; %Number of static & dynamic features (nbDeriv=1 for just x)
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
model.dt = 1E-2; %Time step duration
-model.rfactor = 1E-3; %Control cost in LQR
+model.rfactor = 1E-4; %Control cost in LQR
%Control cost matrix
R = eye(model.nbVarPos) * model.rfactor;
@@ -237,6 +237,7 @@ for n=1:nbSamples
end
for n=1:nbRepros
plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]);
+ plot(r(n).Data(1,:), r(n).Data(2,:), '.','markersize',12,'color',[.8 0 0]);
end
for n=1:nbNewRepros
plot(rnew(n).Data(1,:), rnew(n).Data(2,:), '-','linewidth',1,'color',max(min([0 .7+randn(1)*1E-1 0],1),0));
@@ -353,4 +354,4 @@ xlabel('$t$','fontsize',14,'interpreter','latex');
% print('-dpng','graphs/demo_batchLQR03.png');
pause;
-close all;
\ No newline at end of file
+close all;
diff --git a/demos/demo_OC_LQT_Lagrangian01.m b/demos/demo_OC_LQT_Lagrangian01.m
index b9e4f38b90ef75c3e870b211239a6771f3c37f30..3c70be3ce01607638197108f4af507cfa02c7028 100644
--- a/demos/demo_OC_LQT_Lagrangian01.m
+++ b/demos/demo_OC_LQT_Lagrangian01.m
@@ -152,5 +152,5 @@ end
axis equal;
% print('-dpng','graphs/demo_MPC_Lagrangian01.png');
-pause;
-close all;
\ No newline at end of file
+pause(10);
+close all;
diff --git a/demos/demo_OC_LQT_ballistic01.m b/demos/demo_OC_LQT_ballistic01.m
index c2f0af6a73fdbf37115ebe4451d4b3433ba2ee00..c1cba3904eff2475c64eacade0a46e7746213af1 100644
--- a/demos/demo_OC_LQT_ballistic01.m
+++ b/demos/demo_OC_LQT_ballistic01.m
@@ -109,7 +109,7 @@ legend('boxoff');
% imagesc(Su);
% axis tight; axis equal; axis ij;
-pause;
+pause(10);
close all;
end
@@ -131,4 +131,4 @@ function [Su, Sx] = transferMatrices(A, B)
Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
Su(id2,id3) = B(:,:,t);
end
-end
\ No newline at end of file
+end
diff --git a/demos/demo_OC_LQT_fullQ01.m b/demos/demo_OC_LQT_fullQ01.m
index e33c71963551a7d0e57027c2714c7d72d39a54a1..d41671cfad4362e2452e204b98f54d292a33b248 100644
--- a/demos/demo_OC_LQT_fullQ01.m
+++ b/demos/demo_OC_LQT_fullQ01.m
@@ -39,7 +39,7 @@ nbData = 200; %Number of datapoints
% nbData = 40; %Number of datapoints
nbPoints = 3; %Number of keypoints
nbVarPos = 2; %Dimension of position data (here: x1,x2)
-nbDeriv = 1; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
+nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbVar = nbVarPos * nbDeriv; %Dimension of state vector
dt = 1E-2; %Time step duration
rfactor = 1E-8; %dt^nbDeriv; %Control cost in LQR
@@ -109,23 +109,23 @@ end
% Q(id2(:,2), id2(:,1)) = -eye(nbVarPos)*1E0;
% end
-% %Constraining the velocity before and after the keypoints to be correlated, effectively creating a straight line (version 2)
-% toff = 12;
-% for k=1:nbPoints-1
-% for t=1:toff
-% id2(1:2,1) = id(:,k) - t * nbVar + nbVarPos;
-% id2(1:2,2) = id(:,k) + t * nbVar + nbVarPos;
-% Q(id2(:,1), id2(:,1)) = eye(nbVarPos)*1E0;
-% Q(id2(:,2), id2(:,2)) = eye(nbVarPos)*1E0;
-% Q(id2(:,1), id2(:,2)) = -eye(nbVarPos)*1E0;
-% Q(id2(:,2), id2(:,1)) = -eye(nbVarPos)*1E0;
-% end
-% end
+%Constraining the velocity before and after the keypoints to be correlated, effectively creating a straight line (version 2)
+toff = 12;
+for k=1:nbPoints-1
+ for t=1:toff
+ id2(1:2,1) = id(:,k) - t * nbVar + nbVarPos;
+ id2(1:2,2) = id(:,k) + t * nbVar + nbVarPos;
+ Q(id2(:,1), id2(:,1)) = eye(nbVarPos)*1E0;
+ Q(id2(:,2), id2(:,2)) = eye(nbVarPos)*1E0;
+ Q(id2(:,1), id2(:,2)) = -eye(nbVarPos)*1E0;
+ Q(id2(:,2), id2(:,1)) = -eye(nbVarPos)*1E0;
+ end
+end
% %Constraining the velocity before and after the keypoints to form a desired angle (version 1)
% a = pi/2; %desired angle
% V = [cos(a) -sin(a); sin(a) cos(a)]; %rotation matrix
-% toff = 4;
+% toff = 12;
% for k=1:nbPoints-1
% id2(1:2,1) = id(:,k) - toff * nbVar + nbVarPos;
% id2(1:2,2) = id(:,k) + toff * nbVar + nbVarPos;
@@ -151,7 +151,7 @@ end
% end
% %Constraining the velocities when reaching the keypoints to be correlated (version 1)
-% toff = 0;
+% toff = 12;
% for k=1:nbPoints-1
% id2(1:2,1) = id(:,k) - toff * nbVar + nbVarPos;
% id2(1:2,2) = id(:,k+1) - toff * nbVar + nbVarPos;
@@ -162,7 +162,7 @@ end
% end
% %Constraining the velocities when reaching the keypoints to be correlated (version 2)
-% toff = 8;
+% toff = 12;
% for k=1:nbPoints-1
% for t=1:toff
% id2(1:2,1) = id(:,k) - t * nbVar + nbVarPos;
@@ -192,13 +192,13 @@ end
% Q(id2(:,2), id2(:,1)) = -eye(nbVarPos)*1E0;
% end
-%Constraining R (same control command applied during a time interval)
-toff = 12;
-for k=1:nbPoints-1
- tt = tl(k) + [-toff,toff];
- id2 = tt(1) * nbVarPos +1 : tt(2) * nbVarPos;
- R(id2,id2) = -1E-1 * kron(ones(2*toff), eye(nbVarPos)) + 2 * 1E-1 * eye(length(id2));
-end
+%%Constraining R (same control command applied during a time interval)
+%toff = 12;
+%for k=1:nbPoints-1
+% tt = tl(k) + [-toff,toff];
+% id2 = tt(1) * nbVarPos +1 : tt(2) * nbVarPos;
+% R(id2,id2) = -1E-1 * kron(ones(2*toff), eye(nbVarPos)) + 2 * 1E-1 * eye(length(id2));
+%end
@@ -211,7 +211,7 @@ rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+h = figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',35,'color',[0 0 0]);
for k=1:nbPoints-1
@@ -220,7 +220,7 @@ for k=1:nbPoints-1
end
plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',35,'color',[.8 0 0]);
axis equal;
-% print('-dpng','graphs/MPC_fullQ01a.png');
+% print('-dpng','graphs/LQT_fullQ01a.png');
%Plot 1D
figure; hold on;
@@ -239,5 +239,4 @@ xlabel('t'); ylabel('u_1');
% axis equal; axis ij;
% print('-dpng','graphs/MPC_fullQ_Q01b.png');
-pause;
-close all;
\ No newline at end of file
+waitfor(h);
diff --git a/demos/demo_OC_LQT_fullQ02.m b/demos/demo_OC_LQT_fullQ02.m
index b19be31e5c82df5766d487ab0e8ce3270eb53d9f..49e9d091ce37b10224966d82239776c87311389d 100644
--- a/demos/demo_OC_LQT_fullQ02.m
+++ b/demos/demo_OC_LQT_fullQ02.m
@@ -43,7 +43,7 @@ nbVarPos = 2 * nbAgents; %Dimension of position data (here: x1,x2)
nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbVar = nbVarPos * nbDeriv; %Dimension of state vector
dt = 1E-2; %Time step duration
-rfactor = 1E-8; %dt^nbDeriv; %Control cost in LQR
+rfactor = 1E-8; %dt^nbDeriv; %Control cost in LQR
R = speye((nbData-1)*nbVarPos) * rfactor; %Control cost matrix
Rx = speye(nbData*nbVar) * rfactor;
@@ -86,35 +86,20 @@ for t=1:length(tl)
MuQ(id(:,t)) = rand(nbVarPos,1) - 0.5;
end
-% %Constraining the two agents to meet at the end of the motion
-% tlm = nbData;
-% MuQ(id(3:4,1)) = MuQ(id(1:2,1)); %Proposed meeting point for the two agents
-% Q(id(1:2,1), id(3:4,1)) = -eye(2)*1E0;
-% Q(id(3:4,1), id(1:2,1)) = -eye(2)*1E0;
-
% %Constraining the two agents to meet in the middle of the motion
% tlm = nbData/2;
% idm = [1:nbVarPos] + (tlm-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;
+% Q(idm,idm) = [eye(2), -eye(2); -eye(2), eye(2)];
-%Constraining the two agents to be at the same position at time t1 and t2
+%Constraining the two agents to be at a given position with an offset at time t1 and t2
t1 = nbData/2;
t2 = t1+50;
idm(1:2) = [1:2] + (t1-1) * nbVar;
idm(3:4) = [3:4] + (t2-1) * nbVar;
-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;
+Q(idm,idm) = [eye(2), -eye(2); -eye(2), eye(2)];
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
% %Proposed meeting point for the two agents
% tlm = nbData;
@@ -187,7 +172,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1200 600]);
+h = figure('position',[10 10 2400 600]);
subplot(1,4,1); hold on; axis off;
% set(0,'DefaultAxesLooseInset',[1,1,1,1]*1E2);
% set(gca,'LooseInset',[1,1,1,1]*1E2);
@@ -248,5 +233,4 @@ end
% plot([xlim(1,1),xlim(1,1:2),xlim(1,2:-1:1),xlim(1,1)], [xlim(2,1:2),xlim(2,2:-1:1),xlim(2,1),xlim(2,1)],'-','linewidth',2,'color',[0,0,0]);
% axis equal; axis ij;
-pause;
-close all;
\ No newline at end of file
+waitfor(h);
diff --git a/demos/demo_OC_LQT_fullQ03.m b/demos/demo_OC_LQT_fullQ03.m
index 2c5f41425ec2f35bf54a00e4a16f9349cac6920d..fc8ba3f09d8d653b11d2b249d69ba77216a2561a 100644
--- a/demos/demo_OC_LQT_fullQ03.m
+++ b/demos/demo_OC_LQT_fullQ03.m
@@ -117,13 +117,13 @@ rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+h = figure('position',[10 10 800 800],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
%Agent 1
% for t=1:nbData
% plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2]).*1E-8, [.2 .2 .2], .03); %Plot uncertainty
% end
-h(1) = plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
+hf(1) = plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',25,'color',[0 0 0]);
plot2DArrow(rx(1:2,1), u(1:2).*1E-2, [.8 0 0]);
% plot(rx(1,end-3), rx(2,end-2), '.','markersize',25,'color',[0 .6 0]); %meeting point
@@ -133,12 +133,12 @@ plot2DArrow(rx(1:2,1), u(1:2).*1E-2, [.8 0 0]);
% for t=1:nbData
% plotGMM(rx(3:4,t), xSigma(nbVar*(t-1)+[3,4],nbVar*(t-1)+[3,4]).*1E-8, [.2 .2 .2], .03); %Plot uncertainty
% end
-h(2) = plot(rx(3,:), rx(4,:), '-','linewidth',2,'color',[.6 .6 .6]);
+hf(2) = plot(rx(3,:), rx(4,:), '-','linewidth',2,'color',[.6 .6 .6]);
plot(rx(3,1), rx(4,1), '.','markersize',25,'color',[.6 .6 .6]);
plot2DArrow(rx(3:4,1), u(3:4).*1E-2, [.8 0 0]);
-h(3) = plot(rx(3,tEvent+1), rx(4,tEvent+1), '.','markersize',25,'color',[.4 .8 .4]); %meeting point
+hf(3) = plot(rx(3,tEvent+1), rx(4,tEvent+1), '.','markersize',25,'color',[.4 .8 .4]); %meeting point
-legend(h,{'Agent 1','Agent 2','End point'});
+legend(hf,{'Agent 1','Agent 2','End point'});
% plot(MuQ(id(3)), MuQ(id(4)), '.','markersize',35,'color',[1 .6 .6]);
axis equal;
% print('-dpng','graphs/MPC_fullQ03a.png');
@@ -151,8 +151,7 @@ axis equal;
% set(gca,'xtick',[1,size(Q,1)],'ytick',[1,size(Q,1)]);
% axis square; axis([1 size(Q,1) 1 size(Q,1)]); shading flat;
-pause;
-close all;
+waitfor(h);
end
%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -169,4 +168,4 @@ function [Su, Sx] = transferMatrices(A, B)
Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
Su(id2,id3) = B(:,:,t);
end
-end
\ No newline at end of file
+end
diff --git a/demos/demo_OC_LQT_fullQ04.m b/demos/demo_OC_LQT_fullQ04.m
index 9ab144e73ff0c3f56e764eb6c7c704596d5d5688..b993f543d7410f824060d679106d99ea051aa090 100644
--- a/demos/demo_OC_LQT_fullQ04.m
+++ b/demos/demo_OC_LQT_fullQ04.m
@@ -144,8 +144,8 @@ Q(id(3:4), id(1:2)) = -eye(2);
%% Batch LQT reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0 = [x01; zeros(2,1); m(1)*g; ...
- x02; zeros(2,1); m(2)*g; ...
- x01; zeros(2,1); m(3)*g];
+ x02; zeros(2,1); m(2)*g; ...
+ x01; zeros(2,1); m(3)*g];
u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x0);
rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
@@ -155,7 +155,7 @@ rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
%% 2D plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1200 800],'color',[1 1 1]); hold on; axis off;
+h = figure('position',[10 10 1200 800],'color',[1 1 1]); hold on; axis off;
% %Uncertainty
% for t=1:nbData
% plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2])*3E-7, [0 0 0], .04); %Agent 1 (left hand)
@@ -168,31 +168,31 @@ figure('position',[10 10 1200 800],'color',[1 1 1]); hold on; axis off;
% end
%Agents
-h(1) = plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0 0 0]); %Agent 1 (left hand)
-h(2) = plot(rx(7,:), rx(8,:), '-','linewidth',4,'color',[.6 .6 .6]); %Agent 2 (right hand)
-h(3) = plot(rx(13,:), rx(14,:), ':','linewidth',4,'color',[.8 .4 0]); %Agent 3 (ball)
+hf(1) = plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0 0 0]); %Agent 1 (left hand)
+hf(2) = plot(rx(7,:), rx(8,:), '-','linewidth',4,'color',[.6 .6 .6]); %Agent 2 (right hand)
+hf(3) = plot(rx(13,:), rx(14,:), ':','linewidth',4,'color',[.8 .4 0]); %Agent 3 (ball)
%Events
-h(4) = plot(rx(1,1), rx(2,1), '.','markersize',40,'color',[0 0 0]); %Initial position (left hand)
-h(5) = plot(rx(7,1), rx(8,1), '.','markersize',40,'color',[.6 .6 .6]); %Initial position (right hand)
-h(6) = plot(rx(1,tEvent(1)), rx(2,tEvent(1)), '.','markersize',40,'color',[0 .6 0]); %Release of ball
-h(7) = plot(rx(7,tEvent(2)), rx(8,tEvent(2)), '.','markersize',40,'color',[0 0 .8]); %Hitting of ball
-h(8) = plot(xTar(1), xTar(2), '.','markersize',40,'color',[.8 0 0]); %Ball target
+hf(4) = plot(rx(1,1), rx(2,1), '.','markersize',40,'color',[0 0 0]); %Initial position (left hand)
+hf(5) = plot(rx(7,1), rx(8,1), '.','markersize',40,'color',[.6 .6 .6]); %Initial position (right hand)
+hf(6) = plot(rx(1,tEvent(1)), rx(2,tEvent(1)), '.','markersize',40,'color',[0 .6 0]); %Release of ball
+hf(7) = plot(rx(7,tEvent(2)), rx(8,tEvent(2)), '.','markersize',40,'color',[0 0 .8]); %Hitting of ball
+hf(8) = plot(xTar(1), xTar(2), '.','markersize',40,'color',[.8 0 0]); %Ball target
plot2DArrow(rx(1:2,tEvent(1)), diff(rx(1:2,tEvent(1):tEvent(1)+1),1,2)*12E0, [0 .6 0], 4, .01);
plot2DArrow(rx(7:8,tEvent(2)), diff(rx(7:8,tEvent(2):tEvent(2)+1),1,2)*12E0, [0 0 .8], 4, .01);
axis equal; axis([1, 2.2, -0.3, 0.2]);
-% text(rx(1,1)+.01, rx(2,1)-.01,'$t=0$','interpreter','latex','fontsize',20);
-% text(rx(1,1)+.01, rx(2,1)-.03,'$t=\frac{3T}{4}$','interpreter','latex','fontsize',20);
-% text(rx(7,1)+.01, rx(8,1)+.02,'$t=0$','interpreter','latex','fontsize',20);
-% text(rx(7,1)+.01, rx(8,1),'$t=\frac{3T}{4}$','interpreter','latex','fontsize',20);
-% text(rx(1,tEvent(1))-.01, rx(2,tEvent(1))-.02,'$t=\frac{T}{4}$','interpreter','latex','fontsize',20);
-% text(rx(13,tEvent(2))+.01, rx(14,tEvent(2))+.01,'$t=\frac{T}{2}$','interpreter','latex','fontsize',20);
-% text(xTar(1)+.02, xTar(2),'$t=T$','interpreter','latex','fontsize',20);
-% legend(h, {'Left hand motion (Agent 1)','Right hand motion (Agent 2)','Ball motion (Agent 3)', ...
-% 'Left hand initial point','Right hand initial point','Ball releasing point','Ball hitting point','Ball target'}, 'fontsize',20,'location','northwest');
-% legend('boxoff');
-% print('-dpng','graphs/MPC_tennisServe01.png');
+text(rx(1,1)+.01, rx(2,1)-.01,'$t=0$','interpreter','latex','fontsize',20);
+text(rx(1,1)+.01, rx(2,1)-.03,'$t=\frac{3T}{4}$','interpreter','latex','fontsize',20);
+text(rx(7,1)+.01, rx(8,1)+.02,'$t=0$','interpreter','latex','fontsize',20);
+text(rx(7,1)+.01, rx(8,1),'$t=\frac{3T}{4}$','interpreter','latex','fontsize',20);
+text(rx(1,tEvent(1))-.01, rx(2,tEvent(1))-.02,'$t=\frac{T}{4}$','interpreter','latex','fontsize',20);
+text(rx(13,tEvent(2))+.01, rx(14,tEvent(2))+.01,'$t=\frac{T}{2}$','interpreter','latex','fontsize',20);
+text(xTar(1)+.02, xTar(2),'$t=T$','interpreter','latex','fontsize',20);
+legend(hf, {'Left hand motion (Agent 1)','Right hand motion (Agent 2)','Ball motion (Agent 3)', ...
+'Left hand initial point','Right hand initial point','Ball releasing point','Ball hitting point','Ball target'}, 'fontsize',20,'location','northwest');
+legend('boxoff');
+%print('-dpng','graphs/iLQR_tennisServe01.png');
% %% 2D animated plot
@@ -326,8 +326,7 @@ axis equal; axis([1, 2.2, -0.3, 0.2]);
% set(gca,'xtick',[1,size(Q,1)],'ytick',[1,size(Q,1)]);
% axis square; axis([1 size(Q,1) 1 size(Q,1)]); shading flat;
-pause;
-close all;
+waitfor(h);
end
%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -344,4 +343,4 @@ function [Su, Sx] = transferMatrices(A, B)
Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
Su(id2,id3) = B(:,:,t);
end
-end
\ No newline at end of file
+end
diff --git a/demos/demo_OC_LQT_recursive_augmSigma01.m b/demos/demo_OC_LQT_recursive_augmSigma01.m
index 93fed8540554036d1f596114c5a9a4cc0460554d..f84b78e6f1a2cb4084c586eb681cf45e9e253eda 100644
--- a/demos/demo_OC_LQT_recursive_augmSigma01.m
+++ b/demos/demo_OC_LQT_recursive_augmSigma01.m
@@ -43,7 +43,7 @@ model.nbStates = 5; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
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 = 0.1 * model.dt^model.nbDeriv; %Control cost in LQR
model.rfactor = 1E-6; %Control cost in LQR
@@ -141,7 +141,7 @@ 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) on augmented state
+%Reproduction
for n=1:nbRepros
if n==1
x = [Data(:,1); 1];
@@ -151,8 +151,13 @@ for n=1:nbRepros
r(n).x0 = x;
for t=1:nbData
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 on augmented state (resulting in FB and FF terms)
+ Ka = (B' * P(:,:,t) * B + R) \ B' * P(:,:,t) * A; %FB term
+
+ u = -Ka * x; %Feedback control on augmented state (resulting in feedback and feedforward terms on state)
+% K = Ka(:,1:model0.nbVar);
+% uff = -Ka(:,end) - K * model0.Mu(:,qList(t));
+% u = K * (model0.Mu(:,qList(t)) - x(1:end-1)) + uff; %Acceleration command with FB and FF terms computed explicitly from Ka
+
x = A * x + B * u; %Update of state vector
end
end
@@ -173,8 +178,8 @@ for n=1:nbRepros
for t=1:nbData
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
+ uff = -(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) + uff; %Acceleration command with FB and FF terms
x = A0 * x + B0 * u; %Update of state vector
end
end
@@ -183,7 +188,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot position
-figure('position',[10 10 800 800],'color',[1 1 1]); axis off; hold on;
+h = 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]);
@@ -193,7 +198,7 @@ for n=1:nbRepros
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');
+legend(h,'LQR with augmented state space',' LQT with FB and FF terms','fontsize',26);
% %Plot velocity
% figure('position',[1020 10 1000 1000],'color',[1 1 1]); hold on;
@@ -211,5 +216,6 @@ legend(h,'LQR with augmented state space',' LQT with FB and FF terms');
% xlabel('dx_1'); ylabel('dx_2');
% print('-dpng','graphs/demo_iterativeLQR_augmSigma01.png');
-pause;
-close all;
\ No newline at end of file
+%pause;
+%close all;
+waitfor(h);
diff --git a/demos/demo_Riemannian_S3_GMM01.m b/demos/demo_Riemannian_S3_GMM01.m
index c7d36f0763f9b0a7a4211136a2dee3be55a24a43..05bc9db925c3af0ba545887c15dda5752ffd81ea 100644
--- a/demos/demo_Riemannian_S3_GMM01.m
+++ b/demos/demo_Riemannian_S3_GMM01.m
@@ -154,14 +154,14 @@ 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)
Q = [q(1) -q(2) -q(3) -q(4);
- q(2) q(1) -q(4) q(3);
- q(3) q(4) q(1) -q(2);
- q(4) -q(3) q(2) q(1)];
+ 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
%Arcosine redefinition to make sure the distance between antipodal quaternions is zero (2.50 from Dubbelman's Thesis)
@@ -194,4 +194,4 @@ function Ac = transp(g,h)
uv = vm / mn;
Rpar = eye(4) - sin(mn)*(g*uv') - (1-cos(mn))*(uv*uv');
Ac = E' * QuatMatrix(h)' * Rpar * QuatMatrix(g) * E; %Transportation operator from g to h
-end
\ No newline at end of file
+end
diff --git a/demos/demo_Riemannian_S3_interp01.m b/demos/demo_Riemannian_S3_interp01.m
index 1420ff2872324fbe04ee608e291360275eb55f78..5bd78ab465991fa2a197efa15f2ac3f5e6bf8c5c 100644
--- a/demos/demo_Riemannian_S3_interp01.m
+++ b/demos/demo_Riemannian_S3_interp01.m
@@ -195,12 +195,12 @@ end
function Q = QuatMatrix(q)
Q = [q(1) -q(2) -q(3) -q(4);
- 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
+ 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
-% 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
@@ -230,4 +230,4 @@ function Ac = transp(g,h)
uv = vm / mn;
Rpar = eye(4) - sin(mn)*(g*uv') - (1-cos(mn))*(uv*uv');
Ac = E' * QuatMatrix(h)' * Rpar * QuatMatrix(g) * E; %Transportation operator from g to h
-end
\ No newline at end of file
+end
diff --git a/demos/demo_Riemannian_Sd_interp01.m b/demos/demo_Riemannian_Sd_interp01.m
index 96d736eeef5867e60e7fd9067fb46cad8e5246ad..2c90453548ef70c869c44578608f60d5fa766dd3 100644
--- a/demos/demo_Riemannian_Sd_interp01.m
+++ b/demos/demo_Riemannian_Sd_interp01.m
@@ -167,12 +167,13 @@ function Ac = transp(x1, x2, t)
end
%As in https://ronnybergmann.net/mvirt/manifolds/Sn.html (gives the same result as transp(x1,x2,t)*v)
+%https://towardsdatascience.com/geodesic-regression-d0334de2d9d8
function v = transp2(x, y, v)
d = acos(x'*y);
v = v - (logmap(y,x) + logmap(x,y)) .* (logmap(y,x)' * v) ./ d.^2;
end
-% function x = geodesic(u, x0, t)
+function x = geodesic(u, x0, t)
normu = norm(u,'fro');
x = x0 * cos(normu*t) + u./normu * sin(normu*t);
-end
\ No newline at end of file
+end
diff --git a/demos/demo_TPGMM_teleoperation01.m b/demos/demo_TPGMM_teleoperation01.m
index 1b50462c09b967a4420a0e6388e2b78a3cd84019..8ac4170a3bf6c4189f0295cabae81ed3b0a73c35 100644
--- a/demos/demo_TPGMM_teleoperation01.m
+++ b/demos/demo_TPGMM_teleoperation01.m
@@ -1,6 +1,8 @@
function demo_TPGMM_teleoperation01
% Time-invariant task-parameterized GMM applied to a teleoperation task (position and orientation).
%
+% Octave users should use: pkg load image, pkg load statistics
+%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon16JIST,
% author="Calinon, S.",
@@ -49,7 +51,7 @@ model.rfactor = 5E-4; %Control cost in LQR
model.tfactor = 1E-2; %Teleoperator cost
R = eye(model.nbVarOut) * model.rfactor; %Control cost matrix
-imgVis = 1; %Visualization option
+imgVis = 0; %Visualization option
if imgVis==1
global img alpha
[img, ~, alpha] = imread('data/drill06.png');
@@ -396,4 +398,4 @@ if strcmp(muoseside,'normal')==1
end
if strcmp(muoseside,'alt')==1
setappdata(gcf,'mb',1);
-end
\ No newline at end of file
+end
diff --git a/demos/demo_ergodicControl_1D01.m b/demos/demo_ergodicControl_1D01.m
index a2e1ca4f820c6b9e7a4a323bc1353ad65a91e153..a488020a3d1de39f805c0eca96548458262d684c 100644
--- a/demos/demo_ergodicControl_1D01.m
+++ b/demos/demo_ergodicControl_1D01.m
@@ -294,5 +294,5 @@ xlabel('$x$','interpreter','latex','fontsize',28);
% % id = id + 1;
% end
-pause;
-close all;
\ No newline at end of file
+pause(10);
+close all;
diff --git a/demos/demo_grabData01.m b/demos/demo_grabData01.m
index 5587bbf4d1d6131237ecd26e88e753083d2657e2..c9c97a913c006469d1920de7f33babb3761071e2 100644
--- a/demos/demo_grabData01.m
+++ b/demos/demo_grabData01.m
@@ -43,7 +43,7 @@ nbSamples = 5;
%% Collect data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Ude left mouse button to draw trajectories, use right mouse button to quit');
+disp('Use left mouse button to draw trajectories, use right mouse button to quit');
Data = grabDataFromCursor(nbData);
nbSamples = size(Data,2) / nbData;
for n=1:nbSamples
@@ -68,4 +68,4 @@ axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
%print('-dpng','graphs/demo_grabData01.png');
pause;
-close all;
\ No newline at end of file
+close all;
diff --git a/demos/demo_manipulabilityTracking_mainTask01.m b/demos/demo_manipulabilityTracking_mainTask01.m
index 2fbebb4b681a2706afcad4f67d2d61d6f9ae5503..3a8c2279103692dd2483fd5de580f2f745d129e2 100644
--- a/demos/demo_manipulabilityTracking_mainTask01.m
+++ b/demos/demo_manipulabilityTracking_mainTask01.m
@@ -1,6 +1,6 @@
function demo_manipulabilityTracking_mainTask01
% Matching of a desired manipulability ellipsoid as the main task (no desired position)
-% using the formulation with the manipulability Jacobien (Mandel notation).
+% using the formulation with the manipulability Jacobian (Mandel notation).
%
% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox.
@@ -57,10 +57,9 @@ J = robot.jacob0(q'); % Symbolic Jacobian
% Define the desired manipulability
q_Me_d = [pi/16 ; pi/4 ; pi/8 ; -pi/8]; % task 1
% q_Me_d = [pi/2 ; -pi/6; -pi/2 ; -pi/2]; % task 2
-
J_Me_d = robot.jacob0(q_Me_d); % Current Jacobian
J_Me_d = J_Me_d(1:2,:);
-Me_d = (J_Me_d*J_Me_d');
+Me_d = J_Me_d * J_Me_d';
%% Testing Manipulability Transfer
@@ -74,8 +73,7 @@ it = 1; % Iterations counter
h1 = [];
gmm_c = [];
-% Initial end-effector position (compatibility with 9.X and 10.X versions
-% of robotics toolbox)
+% Initial end-effector position (compatibility with 9.X and 10.X versions of robotics toolbox)
Htmp = robot.fkine(q0); % Forward Kinematics
if isobject(Htmp) % SE3 object verification
x0 = Htmp.t(1:2);
@@ -89,12 +87,12 @@ figure('position',[10 10 1000 450],'color',[1 1 1]);
while( it < nbIter )
delete(h1);
- Jt = robot.jacob0(qt); % Current Jacobian
- Jt_full = Jt;
- Jt = Jt(1:2,:);
Htmp = robot.fkine(qt); % Forward Kinematics (needed for plots)
- Me_ct = (Jt*Jt'); % Current manipulability
+ Jt_full = robot.jacob0(qt); % Current Jacobian
+ Jt = Jt_full(1:2,:);
+ Me_ct = Jt * Jt'; % Current manipulability
+ % Log data
Me_track(:,:,it) = Me_ct;
qt_track(:,it) = qt;
@@ -109,24 +107,21 @@ while( it < nbIter )
Jm_t = compute_red_manipulability_Jacobian(Jt_full, 1:2);
% Compute desired joint velocities
- M_diff = logmap(Me_d,Me_ct);
- dq_T1 = pinv(Jm_t)*Km*symmat2vec(M_diff);
+ M_diff = logmap(Me_d, Me_ct);
+ dq_T1 = pinv(Jm_t) * Km * symmat2vec(M_diff);
% Plotting robot and manipulability ellipsoids
- subplot(1,2,1);
- hold on;
+ subplot(1,2,1); hold on;
if(it == 1)
plotGMM(xt, 1E-2*Me_d, [0.2 0.8 0.2], .4); % Scaled matrix!
end
h1 = plotGMM(xt, 1E-2*Me_ct, [0.8 0.2 0.2], .4); % Scaled matrix!
colTmp = [1,1,1] - [.8,.8,.8] * (it+10)/nbIter;
plotArm(qt, ones(nbDOFs,1)*armLength, [0; 0; it*0.1], .2, colTmp);
- axis square;
- axis equal;
+ axis square; axis equal;
xlabel('x_1'); ylabel('x_2');
- subplot(1,2,2);
- hold on; axis equal;
+ subplot(1,2,2); hold on; axis equal;
delete(gmm_c);
gmm_c = plotGMM([0;0], 1E-2*Me_ct, [0.8 0.2 0.2], .1); % Scaled matrix!
if(it == 1)
@@ -135,7 +130,7 @@ while( it < nbIter )
drawnow;
% Updating joint position
- qt = qt + (dq_T1)*dt;
+ qt = qt + (dq_T1) * dt;
it = it + 1; % Iterations++
end
@@ -201,23 +196,21 @@ xlabel('$t$','fontsize',22,'Interpreter','latex'); ylabel('$d$','fontsize',22,'I
end
%%%%%%%%%%%%%%%%%%
+% Compute the force manipulability Jacobian (symbolic) in the form of a matrix using Mandel notation
function Jm_red = compute_red_manipulability_Jacobian(J, taskVar)
- % Compute the force manipulability Jacobian (symbolic) in the form of a
- % matrix using Mandel notation.
if nargin < 2
- taskVar = 1:6;
+ taskVar = 1:6;
end
-
Jm = compute_manipulability_Jacobian(J);
Jm_red = [];
- for i = 1:size(Jm,3)
+ for i=1:size(Jm,3)
Jm_red = [Jm_red, symmat2vec(Jm(taskVar,taskVar,i))];
end
end
%%%%%%%%%%%%%%%%%%
+% Compute the force manipulability Jacobian (symbolic)
function Jm = compute_manipulability_Jacobian(J)
- % Compute the force manipulability Jacobian (symbolic).
J_grad = compute_joint_derivative_Jacobian(J);
Jm = tmprod(J_grad,J,2) + tmprod(permute(J_grad,[2,1,3]),J,1);
% mat_mult = kdlJacToArma(J)*reshape( arma::mat(permDerivJ.memptr(), permDerivJ.n_elem, 1, false), columns, columns*rows);
@@ -225,57 +218,56 @@ function Jm = compute_manipulability_Jacobian(J)
end
%%%%%%%%%%%%%%%%%%
+% Compute the Jacobian derivative w.r.t joint angles (hybrid Jacobian representation). Ref: H. Bruyninck and J. de Schutter, 1996
function J_grad = compute_joint_derivative_Jacobian(J)
- % Compute the Jacobian derivative w.r.t joint angles (hybrid Jacobian
- % representation). Ref: H. Bruyninck and J. de Schutter, 1996
nb_rows = size(J,1); % task space dim.
nb_cols = size(J,2); % joint space dim.
J_grad = zeros(nb_rows, nb_cols, nb_cols);
- for i = 1:nb_cols
- for j = 1:nb_cols
+ for i=1:nb_cols
+ for j=1:nb_cols
J_i = J(:,i);
J_j = J(:,j);
if j < i
- J_grad(1:3,i,j) = cross(J_j(4:6,:),J_i(1:3,:));
- J_grad(4:6,i,j) = cross(J_j(4:6,:),J_i(4:6,:));
+ J_grad(1:3,i,j) = cross(J_j(4:6,:), J_i(1:3,:));
+ J_grad(4:6,i,j) = cross(J_j(4:6,:), J_i(4:6,:));
elseif j > i
- J_grad(1:3,i,j) = -cross(J_j(1:3,:),J_i(4:6,:));
+ J_grad(1:3,i,j) = -cross(J_j(1:3,:), J_i(4:6,:));
else
- J_grad(1:3,i,j) = cross(J_i(4:6,:),J_i(1:3,:));
+ J_grad(1:3,i,j) = cross(J_i(4:6,:), J_i(1:3,:));
end
end
end
end
%%%%%%%%%%%%%%%%%%
-function U = logmap(X,S)
- % Logarithm map (SPD manifold)
+% Logarithm map (SPD manifold)
+function U = logmap(X, S)
N = size(X,3);
for n = 1:N
% U(:,:,n) = S^.5 * logm(S^-.5 * X(:,:,n) * S^-.5) * S^.5;
% tic
- % U(:,:,n) = S * logm(S\X(:,:,n));
+ % U(:,:,n) = S * logm(S \ X(:,:,n));
% toc
% tic
- [v,d] = eig(S\X(:,:,n));
- U(:,:,n) = S * v*diag(log(diag(d)))*v^-1;
+ [V, D] = eig(S \ X(:,:,n));
+ U(:,:,n) = S * V * diag(log(diag(D))) / V;
% toc
end
end
%%%%%%%%%%%%%%%%%%
+% Vectorization of a symmetric matrix
function v = symmat2vec(M)
- % Vectorization of a symmetric matrix
N = size(M,1);
v = diag(M);
for n = 1:N-1
- v = [v; sqrt(2).*diag(M,n)]; % Mandel notation
+ v = [v; sqrt(2) .* diag(M,n)]; % Mandel notation
end
end
%%%%%%%%%%%%%%%%%%
-function [S,iperm] = tmprod(T,U,mode)
- % Mode-n tensor-matrix product
+% Mode-n tensor-matrix product
+function [S,iperm] = tmprod(T, U, mode)
size_tens = ones(1,mode);
size_tens(1:ndims(T)) = size(T);
N = length(size_tens);
@@ -291,7 +283,7 @@ function [S,iperm] = tmprod(T,U,mode)
size_tens = size_tens(perm);
S = T;
if mode ~= 1
- S = permute(S,perm);
+ S = permute(S,perm);
end
% n-mode product
@@ -301,4 +293,4 @@ function [S,iperm] = tmprod(T,U,mode)
% Inverse permutation
iperm(perm(1:N)) = 1:N;
S = permute(S,iperm);
-end
\ No newline at end of file
+end
diff --git a/demos/demo_manipulabilityTracking_mainTask02.m b/demos/demo_manipulabilityTracking_mainTask02.m
index f8e5f4c3a9449ea053a4980be3812a94c49e3877..6c4479f8a620271273db45306f1230c0247e208c 100644
--- a/demos/demo_manipulabilityTracking_mainTask02.m
+++ b/demos/demo_manipulabilityTracking_mainTask02.m
@@ -1,6 +1,6 @@
function demo_manipulabilityTracking_mainTask02
% Matching of a desired manipulability ellipsoid as the main task (no desired position)
-% using the formulation with the manipulability Jacobien (Mandel notation).
+% using the formulation with the manipulability Jacobian (Mandel notation).
% The matrix gain used for the manipulability tracking controller is now defined as the inverse of
% a 2nd-order covariance matrix representing the variability information obtained from a learning algorithm.
%
@@ -114,8 +114,7 @@ for n = 1:4
%% Final plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Robot and manipulability ellipsoids
- figure('position',[10 10 900 900],'color',[1 1 1]);
- hold on;
+ figure('position',[10 10 900 900],'color',[1 1 1]); hold on;
p = [];
for it = 1:2:nbIter-1
colTmp = [.95,.95,.95] - [.8,.8,.8] * (it)/nbIter;
@@ -132,8 +131,7 @@ for n = 1:4
plotGMM(xt(:,end), 1E-2*Me_d, [0.2 0.8 0.2], .4);
plotGMM(xt_track(:,1,n), 1E-2*Me_track(:,:,1,n), [0.2 0.2 0.8], .4);
plotGMM(xt(:,end), 1E-2*Me_ct, clrmap(n,:), .4);
- axis equal
- xlim([-3,9]); ylim([-4,8])
+ axis equal; xlim([-3,9]); ylim([-4,8]);
set(gca,'fontsize',22,'xtick',[],'ytick',[]);
xlabel('$x_1$','fontsize',38,'Interpreter','latex'); ylabel('$x_2$','fontsize',38,'Interpreter','latex');
title(names{n});
@@ -145,8 +143,7 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Manipulability ellipsoids
for n = 1:4
- figure('position',[10 10 1200 300],'color',[1 1 1]);
- hold on;
+ figure('position',[10 10 1200 300],'color',[1 1 1]); hold on;
for it = 1:10:nbIter-1
plotGMM([it*dt;0], 2E-4*Me_d, [0.2 0.8 0.2], .1);
plotGMM([it*dt;0], 2E-4*Me_track(:,:,it,n), clrmap(n,:), .3);
@@ -158,8 +155,7 @@ for n = 1:4
title(names{n});
end
-figure('position',[10 10 900 900],'color',[1 1 1]);
-hold on;
+figure('position',[10 10 900 900],'color',[1 1 1]); hold on;
for it = 1:2:nbIter-1
for n = 1:4
plotGMM(xt_track(:,it,n), 1E-2*Me_track(:,:,it,n), clrmap(n,:), .15);
@@ -169,7 +165,8 @@ plotGMM(xt_track(:,1,1), 1E-2*Me_track(:,:,1,1), [0.1 0.1 0.4], .6);
axis equal;
set(gca,'fontsize',18);
xlabel('$x_1$','fontsize',30,'Interpreter','latex'); ylabel('$x_2$','fontsize',30,'Interpreter','latex');
-title('End-effector and manipulability ellipsoids trajectories')
+title('End-effector and manipulability ellipsoids trajectories');
+
end
%%%%%%%%%%%%%%%%%%
@@ -208,12 +205,12 @@ function J_grad = compute_joint_derivative_Jacobian(J)
J_i = J(:,i);
J_j = J(:,j);
if j < i
- J_grad(1:3,i,j) = cross(J_j(4:6,:),J_i(1:3,:));
- J_grad(4:6,i,j) = cross(J_j(4:6,:),J_i(4:6,:));
+ J_grad(1:3,i,j) = cross(J_j(4:6,:), J_i(1:3,:));
+ J_grad(4:6,i,j) = cross(J_j(4:6,:), J_i(4:6,:));
elseif j > i
- J_grad(1:3,i,j) = -cross(J_j(1:3,:),J_i(4:6,:));
+ J_grad(1:3,i,j) = -cross(J_j(1:3,:), J_i(4:6,:));
else
- J_grad(1:3,i,j) = cross(J_i(4:6,:),J_i(1:3,:));
+ J_grad(1:3,i,j) = cross(J_i(4:6,:), J_i(1:3,:));
end
end
end
@@ -226,11 +223,11 @@ function U = logmap(X,S)
for n = 1:N
% U(:,:,n) = S^.5 * logm(S^-.5 * X(:,:,n) * S^-.5) * S^.5;
% tic
- % U(:,:,n) = S * logm(S\X(:,:,n));
+ % U(:,:,n) = S * logm(S \ X(:,:,n));
% toc
% tic
- [v,d] = eig(S\X(:,:,n));
- U(:,:,n) = S * v*diag(log(diag(d)))*v^-1;
+ [V,D] = eig(S \ X(:,:,n));
+ U(:,:,n) = S * V * diag(log(diag(d))) / V;
% toc
end
end
@@ -241,7 +238,7 @@ function v = symmat2vec(M)
N = size(M,1);
v = diag(M);
for n = 1:N-1
- v = [v; sqrt(2).*diag(M,n)]; % Mandel notation
+ v = [v; sqrt(2) .* diag(M,n)]; % Mandel notation
end
end
@@ -273,4 +270,4 @@ function [S,iperm] = tmprod(T,U,mode)
% Inverse permutation
iperm(perm(1:N)) = 1:N;
S = permute(S,iperm);
-end
\ No newline at end of file
+end
diff --git a/demos/demo_proMP01.m b/demos/demo_proMP01.m
index 43592e3e9fa5d9b7a94243db8bf9780d8766395d..6b923891523fb56077bd5a47e0518b1513b44311 100644
--- a/demos/demo_proMP01.m
+++ b/demos/demo_proMP01.m
@@ -1,23 +1,9 @@
-function demo_proMP01
-% ProMP with several forms of basis functions, inspired by the original form with RBFs described in
-% Paraschos, A., Daniel, C., Peters, J. and Neumann, G., "Probabilistic Movement Primitives", NIPS'2013
-%
-% If this code is useful for your research, please cite the related publication:
-% @incollection{Calinon19MM,
-% author="Calinon, S.",
-% title="Mixture Models for the Analysis, Edition, and Synthesis of Continuous Time Series",
-% booktitle="Mixture Models and Applications",
-% publisher="Springer",
-% editor="Bouguila, N. and Fan, W.",
-% year="2019",
-% pages="39--57",
-% doi="10.1007/978-3-030-23876-6_3"
-% }
+% Movement primitives applied to 2D trajectory encoding and decoding
%
-% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
-% Written by Sylvain Calinon, http://calinon.ch/
+% Copyright (c) 2019 Idiap Research Institute, https://idiap.ch/
+% Written by Sylvain Calinon, https://calinon.ch/
%
-% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
+% This file is part of PbDlib, https://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
@@ -29,142 +15,163 @@ function demo_proMP01
% 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/');
+% along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
+function demo_proMP01
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbStates = 8; %Number of basis functions
-nbVar = 2; %Dimension of position data (here: x1,x2)
-nbSamples = 5; %Number of demonstrations
-nbData = 200; %Number of datapoints in a trajectory
-nbRepros = 5; %Number of reproductions
+param.nbFct = 7; %Number of basis functions (odd number for Fourier basis functions)
+param.nbVar = 2; %Dimension of position data (here: x1,x2)
+param.nbData = 100; %Number of datapoints in a trajectory
+param.basisName = 'RBF'; %PIECEWISE, RBF, BERNSTEIN, FOURIER
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
-load('data/2Dletters/C.mat');
-x=[];
-for n=1:nbSamples
- s(n).x = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
- x = [x, s(n).x(:)]; %Reorganize as trajectory datapoints
+load('./data/2Dletters/S.mat'); %Planar trajectories for writing alphabet letters
+x = spline(1:size(demos{1}.pos,2), demos{1}.pos, linspace(1,size(demos{1}.pos,2),param.nbData))(:); %Resampling
+
+if isequal(param.basisName,'FOURIER')
+ %Fourier basis functions require additional symmetrization (mirroring) if the signal is a discrete motion
+ X = reshape(x, param.nbVar, param.nbData);
+ X = [X, fliplr(X)]; %Build real and even signal, characterized by f(-x) = f(x)
+ x = X(:);
+ param.nbData = param.nbData * 2;
end
-t = linspace(0,1,nbData); %Time range
-%% ProMP with radial basis functions
+%% Movement primitives encoding and decoding
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Compute basis functions Psi and activation weights w
-tMu = linspace(t(1), t(end), nbStates);
-m(1).phi = zeros(nbData,nbStates);
-for i=1:nbStates
- m(1).phi(:,i) = gaussPDF(t, tMu(i), 1E-2);
-% m(1).phi(:,i) = mvnpdf(t', tMu(i), 1E-2);
+if isequal(param.basisName,'PIECEWISE')
+ phi = buildPhiPiecewise(param.nbData, param.nbFct);
+elseif isequal(param.basisName,'RBF')
+ phi = buildPhiRBF(param.nbData, param.nbFct);
+elseif isequal(param.basisName,'BERNSTEIN')
+ phi = buildPhiBernstein(param.nbData, param.nbFct);
+elseif isequal(param.basisName,'FOURIER')
+ phi = buildPhiFourier(param.nbData, param.nbFct);
end
-% m(1).phi = m(1).phi ./ repmat(sum(m(1).phi,2),1,nbStates); %Optional rescaling
-m(1).Psi = kron(m(1).phi, eye(nbVar)); %Eq.(27)
-m(1).w = (m(1).Psi' * m(1).Psi + eye(nbVar*nbStates).*1E-8) \ m(1).Psi' * x; %m(1).w = pinv(m(1).Psi) * x'; %Eq.(28)
-%Distribution in parameter space
-m(1).Mu_w = mean(m(1).w,2);
-m(1).Sigma_w = cov(m(1).w') + eye(nbVar*nbStates) * 1E0;
-%Trajectory distribution
-m(1).Mu = m(1).Psi * m(1).Mu_w; %Eq.(29)
-m(1).Sigma = m(1).Psi * m(1).Sigma_w * m(1).Psi'; %Eq.(29)
-
-
-%% ProMP with Bernstein basis functions
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Compute basis functions Psi and activation weights w
-m(2).phi = zeros(nbData,nbStates);
-for i=0:nbStates-1
- m(2).phi(:,i+1) = factorial(nbStates-1) ./ (factorial(i) .* factorial(nbStates-1-i)) .* (1-t).^(nbStates-1-i) .* t.^i; %Bernstein basis functions
+
+Psi = kron(phi, eye(param.nbVar)); %Transform to multidimensional basis functions
+w = (Psi' * Psi + eye(param.nbVar*param.nbFct).*realmin) \ Psi' * x; %Estimation of superposition weights from data
+x_hat = Psi * w; %Reconstruction of data
+
+if isequal(param.basisName,'FOURIER')
+ %Fourier basis functions require de-symmetrization of the signal after processing (for visualization)
+ param.nbData = param.nbData/2;
+ x = x(1:param.nbData*param.nbVar);
+ x_hat = x_hat(1:param.nbData*param.nbVar);
+ phi = real(phi); %for plotting
+ %Psi = Psi(1:param.nbData*param.nbVar,:);
end
-m(2).Psi = kron(m(2).phi, eye(nbVar)); %Eq.(27)
-m(2).w = (m(2).Psi' * m(2).Psi + eye(nbVar*nbStates).*1E-8) \ m(2).Psi' * x; %m(2).w = pinv(m(2).Psi) * x; %Eq.(28)
-%Distribution in parameter space
-m(2).Mu_w = mean(m(2).w,2);
-m(2).Sigma_w = cov(m(2).w') + eye(nbVar*nbStates) * 1E0;
-%Trajectory distribution
-m(2).Mu = m(2).Psi * m(2).Mu_w; %Eq.(29)
-m(2).Sigma = m(2).Psi * m(2).Sigma_w * m(2).Psi'; %Eq.(29)
-%% ProMP with Fourier basis functions (here, only DCT)
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Compute basis functions Psi and activation weights w
-m(3).phi = zeros(nbData,nbStates);
-for i=1:nbStates
- xTmp = zeros(1,nbData);
- xTmp(i) = 1;
- m(3).phi(:,i) = idct(xTmp);
-end
-m(3).Psi = kron(m(3).phi, eye(nbVar)); %Eq.(27)
-m(3).w = (m(3).Psi' * m(3).Psi + eye(nbVar*nbStates).*1E-8) \ m(3).Psi' * x; %m(3).w = pinv(m(3).Psi) * x; %Eq.(28)
-%Distribution in parameter space
-m(3).Mu_w = mean(m(3).w,2);
-m(3).Sigma_w = cov(m(3).w') + eye(nbVar*nbStates) * 1E0;
-%Trajectory distribution
-m(3).Mu = m(3).Psi * m(3).Mu_w; %Eq.(29)
-m(3).Sigma = m(3).Psi * m(3).Sigma_w * m(3).Psi'; %Eq.(29)
-
-
-%% Conditioning with trajectory distribution (reconstruction from partial data)
+%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-in0 = [1,nbData]; %Time steps input indices
-out0 = 2:nbData-1; %Time steps output indices
-in = [];
-for i=1:length(in0)
- in = [in, (in0(i)-1)*nbVar+[1:nbVar]]; %Trajectory distribution input indices
+h = figure('position',[10 10 1800 500]);
+clrmap = lines(param.nbFct);
+
+%Plot signals
+subplot(1,3,1); hold on; axis off;
+l(1) = plot(x(1:2:end,:), x(2:2:end,:), 'linewidth',2,'color',[.2 .2 .2]);
+l(2) = plot(x_hat(1:2:end,:), x_hat(2:2:end,:), '-','linewidth',2,'color',[.9 .0 .0]);
+% legend(l,{'Demonstration','Reproduction'},'fontsize',20); % Throw an error with octave
+axis tight; axis equal;
+
+%Plot basis functions (display only the real part for Fourier basis functions)
+subplot(1,3,2); hold on;
+for i=1:param.nbFct
+ plot(phi(:,i), 'linewidth',2,'color',clrmap(i,:));
end
-out = [];
-for i=1:length(out0)
- out = [out, (out0(i)-1)*nbVar+[1:nbVar]]; %Trajectory distribution output indices
+if isequal(param.basisName,'FOURIER')
+ plot([param.nbData,param.nbData], [-1,1], 'k:','linewidth',4);
end
-%Reproduction by Gaussian conditioning
-for k=1:3
- for n=1:nbRepros
- m(k).Mu2(in,n) = x(in,1) + repmat((rand(nbVar,1)-0.5)*2, length(in0), 1) ;
+axis tight;
+xlabel('t','fontsize',26); ylabel('\phi_k','fontsize',26);
+
+%Plot Psi*Psi' matrix (covariance matrix at trajectory level)
+subplot(1,3,3); hold on; axis off; title('\Psi\Psi^T','fontsize',26);
+msh = [0 1 1 0 0; 0 0 1 1 0];
+colormap(flipud(gray));
+imagesc(abs(Psi * Psi'));
+plot(msh(1,:)*size(Psi,1), msh(2,:)*size(Psi,1), 'k-');
+if isequal(param.basisName,'FOURIER')
+ plot(msh(1,:)*size(Psi,1)/2, msh(2,:)*size(Psi,1)/2, 'k:','linewidth',4);
+end
+axis tight; axis square; axis ij;
-% %Conditional distribution with trajectory distribution
-% m(k).Mu2(out,n) = m(k).Mu(out) + m(k).Sigma(out,in) / m(k).Sigma(in,in) * (m(k).Mu2(in,n) - m(k).Mu(in));
+waitfor(h);
+end
- %Efficient computation of conditional distribution by exploiting ProMP structure
- m(k).Mu2(out,n) = m(k).Psi(out,:) * ...
- (m(k).Mu_w + m(k).Sigma_w * m(k).Psi(in,:)' / (m(k).Psi(in,:) * m(k).Sigma_w * m(k).Psi(in,:)') * (m(k).Mu2(in,n) - m(k).Psi(in,:) * m(k).Mu_w));
- end
+
+%%Likelihood of datapoint(s) for a Gaussian parameterized with center and full covariance
+%function prob = gaussPDF(Data, Mu, Sigma)
+% [nbVar,nbData] = size(Data);
+% Data = Data - repmat(Mu,1,nbData);
+% prob = sum((Sigma\Data).*Data,1);
+% prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * abs(det(Sigma)) + realmin);
+%end
+
+%Building piecewise constant basis functions
+function phi = buildPhiPiecewise(nbData, nbFct)
+% iList = round(linspace(0, nbData, nbFct+1));
+% phi = zeros(nbData, nbFct);
+% for i=1:nbFct
+% phi(iList(i)+1:iList(i+1),i) = 1;
+% end
+ phi = kron(eye(nbFct), ones(ceil(nbData/nbFct),1));
+ phi = phi(1:nbData,:);
end
+%Building radial basis functions (RBFs)
+function phi = buildPhiRBF(nbData, nbFct)
+ t = linspace(0, 1, nbData);
+ tMu = linspace(t(1), t(end), nbFct);
+
+% %Version 1
+% phi = zeros(nbData, nbFct);
+% for i=1:nbFct
+% phi(:,i) = gaussPDF(t, tMu(i), 1E-2);
+% %phi(:,i) = mvnpdf(t', tMu(i), 1E-2); %Requires statistics package/toolbox
+% end
+
+% %Version 2
+% %D = repmat(t', [1,nbFct]) - repmat(tMu, [nbData,1]);
+% D = t' * ones(1,nbFct) - ones(nbData,1) * tMu;
+% phi = exp(-1E2 * D.^2);
+
+ %Version 3
+ phi = exp(-1E1 * (t' - tMu).^2);
+
+ %Optional rescaling
+ %phi = phi ./ repmat(sum(phi,2), 1, nbFct);
+end
-%% Plot
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1800 1300]);
-clrmap = lines(nbStates);
-methods = {'ProMP with Radial basis functions','ProMP with Bernstein basis functions','ProMP with Fourier basis functions'};
-for k=1:3
- %Plot signal
- subplot(3,3,k); hold on; axis off; title(methods{k},'fontsize',16);
- plot(x(1:2:end,:), x(2:2:end,:), '.','markersize',10,'color',[.7 .7 .7]);
- for n=1:nbRepros
- plot(m(k).Mu2(1:2:end,n), m(k).Mu2(2:2:end,n), '-','lineWidth',2,'color',[1 .6 .6]);
+%Building Bernstein basis functions
+function phi = buildPhiBernstein(nbData, nbFct)
+ t = linspace(0, 1, nbData);
+ phi = zeros(nbData, nbFct);
+ for i=1:nbFct
+ phi(:,i) = factorial(nbFct-1) ./ (factorial(i-1) .* factorial(nbFct-i)) .* (1-t).^(nbFct-i) .* t.^(i-1);
end
- plot(m(k).Mu(1:2:end), m(k).Mu(2:2:end), '-','lineWidth',2,'color',[0 0 0]);
- axis tight; axis equal;
- %Plot activation functions
- subplot(3,3,3+k); hold on; axis off; title('\phi_k','fontsize',16);
- for i=1:nbStates
- plot(1:nbData, m(k).phi(:,i),'linewidth',2,'color',clrmap(i,:));
- end
- axis([1, nbData, min(m(k).phi(:)), max(m(k).phi(:))]);
- %Plot Psi*Psi' matrix
- subplot(3,3,6+k); hold on; axis off; title('\Psi\Psi^T','fontsize',16);
- colormap(flipud(gray));
- imagesc(abs(m(k).Psi * m(k).Psi'));
- axis tight; axis square; axis ij;
-end %k
-
-% print('-dpng','graphs/demo_proMP01.png');
-pause;
-close all;
\ No newline at end of file
+end
+
+%Building Fourier basis functions
+function phi = buildPhiFourier(nbData, nbFct)
+ t = linspace(0, 1, nbData);
+
+% %Computation for general signals (incl. complex numbers)
+% d = ceil((nbFct-1)/2);
+% k = -d:d;
+% phi = exp(t' * k * 2 * pi * 1i);
+% %phi = cos(t' * k * 2 * pi); %Alternative computation for real signal
+
+ %Alternative computation for real and even signal
+ k = 0:nbFct-1;
+ phi = cos(t' * k * 2 * pi);
+ %phi(:,2:end) = phi(:,2:end) * 2;
+ %invPhi = cos(k' * t * 2 * pi) / nbData;
+ %invPsi = kron(invPhi, eye(param.nbVar));
+end
diff --git a/demos/demo_proMP_Fourier01.m b/demos/demo_proMP_Fourier01.m
index 380966ba20d2ec3621ce57706a0396689f50fcf6..b9eb2be15c014d8440475a2b61328faecbce742f 100644
--- a/demos/demo_proMP_Fourier01.m
+++ b/demos/demo_proMP_Fourier01.m
@@ -53,7 +53,7 @@ end
k = -5:5;
nbFct = length(k);
t = linspace(0,1,nbData);
-Psi = exp(t' * k * 2 * pi * 1i) / nbData;
+Psi = exp(t' * k * 2 * pi * 1i); % / nbData;
% w = (Psi' * Psi + eye(nbStates).*1E-18) \ Psi' * x;
w = pinv(Psi) * x;
@@ -159,4 +159,4 @@ function Sigma = cov_angle(phi)
Mu = mean_angle(phi);
e = phi - repmat(Mu, size(phi,1), 1);
Sigma = cov(angle(exp(1i*e)));
-end
\ No newline at end of file
+end
diff --git a/demos/demo_proMP_Fourier_sampling02.m b/demos/demo_proMP_Fourier_sampling02.m
index f6981aa7cfe9618adb625131be15b740ec0a477c..3f4ef65decf63149de438a3b7773be953a6fae6d 100644
--- a/demos/demo_proMP_Fourier_sampling02.m
+++ b/demos/demo_proMP_Fourier_sampling02.m
@@ -49,7 +49,7 @@ end
%Simulate phase variations
for n=2:nbSamples
- x(:,n) = circshift(x(:,1),(n-nbSamples/2-1)*4);
+ x(:,n) = circshift(x(:,1), (n-nbSamples/2-1)*4);
end
@@ -178,4 +178,4 @@ function Sigma = cov_angle(phi)
Mu = mean_angle(phi);
e = phi - repmat(Mu, size(phi,1), 1);
Sigma = cov(angle(exp(1i*e)));
-end
\ No newline at end of file
+end