Commit e77e0cee authored by Sylvain CALINON's avatar Sylvain CALINON

Jacobian for all manipulator examples corrected

parent 9636aae1
......@@ -113,5 +113,4 @@ legend(h,{'Observed data','Regenerated data'});
% print('-dpng','graphs/demo_AR01.png');
pause;
close all;
\ No newline at end of file
close all;
\ No newline at end of file
......@@ -99,7 +99,7 @@ x = pinvJw * (Q1 * Mu1) + Nw * Mu2;
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,100,2200,1200]); hold on; box on; %axis off;
figure('position',[10,10,1200,800]); hold on; box on; %axis off;
plotGMM(Mu1, S1, [.8 0 0],.3);
plotGMM(Mu2, S2, [0 .8 0],.3);
plot(x(1), x(2), 'ko','markersize',8,'linewidth',2);
......
......@@ -93,7 +93,7 @@ Mu123 = S123 * (Q12 * Mu12 + Q3 * Mu3);
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,100,1800,1200]); hold on; box on; rotate3d on; axis off;
figure('position',[10,10,1200,800]); hold on; box on; rotate3d on; axis off;
plot3(Mu1(1), Mu1(2), Mu1(3), '.','markersize',20,'color',[.8 0 0]);
plot3(Mu2(1), Mu2(2), Mu2(3), '.','markersize',20,'color',[0 .7 0]);
......
......@@ -84,7 +84,7 @@ end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,400,1200]);
figure('position',[10,10,400,800]);
set(0,'DefaultAxesLooseInset',[0,0,0,0]);
set(gca,'LooseInset',[0,0,0,0]);
colormap(repmat(linspace(1,.2,64),3,1)');
......
......@@ -121,4 +121,4 @@ end
%print('-dpng','graphs/demo_Gaussian01.png');
pause;
close all;
close all;
\ No newline at end of file
......@@ -65,7 +65,7 @@ slope = model.Sigma(out,in)/model.Sigma(in,in);
mrg = [10 10];
limAxes = [min(Data(1,:))-mrg(1) max(Data(1,:))+mrg(1) min(Data(2,:))-mrg(2) max(Data(2,:))+mrg(2)];
figure('PaperPosition',[0 0 4 3],'position',[10,50,1600,1200]); hold on; %axis off;
figure('PaperPosition',[0 0 4 3],'position',[10,10,1200,800]); hold on; %axis off;
plot(DataIn+[-50,50], MuOut+slope*[-50,50], ':','linewidth',1,'color',[.7 .3 .3]);
plot([model.Mu(1) model.Mu(1)], [limAxes(3) model.Mu(2)], ':','linewidth',1,'color',[.7 .3 .3]);
plot([limAxes(1) model.Mu(1)], [model.Mu(2) model.Mu(2)], ':','linewidth',1,'color',[.7 .3 .3]);
......@@ -94,4 +94,4 @@ ylabel('$x_2$','fontsize',16,'interpreter','latex');
%print('-dpng','-r600','graphs/demo_Gaussian02.png');
pause;
close all;
close all;
\ No newline at end of file
......@@ -121,7 +121,7 @@ SigmaOut3 = xSigmaOut + A * SigmaIn * A';
mrg = [10 10];
limAxes = [min(Data(1,:))-mrg(1) max(Data(1,:))+mrg(1) min(Data(2,:))-mrg(2) max(Data(2,:))+mrg(2)];
figure('PaperPosition',[0 0 4 3],'position',[10,50,1600,1200]); hold on; %axis off;
figure('PaperPosition',[0 0 4 3],'position',[10,10,1200,800]); hold on; %axis off;
plot(DataIn+[-50,50], MuOut+slope*[-50,50], ':','linewidth',1,'color',[.7 .3 .3]);
plot([model.Mu(1) model.Mu(1)], [limAxes(3) model.Mu(2)], ':','linewidth',1,'color',[.7 .3 .3]);
plot([limAxes(1) model.Mu(1)], [model.Mu(2) model.Mu(2)], ':','linewidth',1,'color',[.7 .3 .3]);
......@@ -156,4 +156,4 @@ legend(h(:,1), {'Proposed formulation','Product with marginal input distribution
%print('-dpng','-r600','graphs/demo_Gaussian03.png');
pause;
close all;
close all;
\ No newline at end of file
......@@ -70,7 +70,7 @@ Sigma = Sigma - Mu*Mu';
%% Plot 1D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
limAxes = [-3 7 0 1];
figure('PaperPosition',[0 0 6 4],'position',[10,50,1600,800]); hold on; %axis off; 5.5 2.475
figure('PaperPosition',[0 0 6 4],'position',[10,10,1600,800]); hold on; %axis off; 5.5 2.475
set(gca,'linewidth',2);
Pt = zeros(1,400);
for i=1:model.nbStates
......@@ -117,4 +117,4 @@ set(gca,'Xtick',[]); set(gca,'Ytick',[]);
% % print('-dpng','graphs/demo_lawTotalCov2D03.png');
pause;
close all;
close all;
\ No newline at end of file
......@@ -76,4 +76,4 @@ axis equal;
%print('-dpng','graphs/demo_Gaussian05.png');
pause;
close all;
close all;
\ No newline at end of file
......@@ -61,4 +61,4 @@ plot(h2,'ro','markersize',10,'linewidth',2);
xlabel('x'); ylabel('p(x)');
pause;
close all;
close all;
\ No newline at end of file
......@@ -118,7 +118,7 @@ slope0 = model.Sigma0(out,in) / model.Sigma0(in,in);
mrg = [10 10];
limAxes = [min(Data(1,:))-mrg(1) max(Data(1,:))+mrg(1) min(Data(2,:))-mrg(2) max(Data(2,:))+mrg(2)];
figure('PaperPosition',[0 0 9 5],'position',[10,50,1600,1200]); hold on; %axis off;
figure('PaperPosition',[0 0 9 5],'position',[10,10,1200,800]); hold on; %axis off;
plot(model.Mu(in)+[-50,50], model.Mu(out)+slope0*[-50,50], '-','linewidth',1,'color',[1 .5 .5]);
plot(model.Mu(in)+[-50,50], model.Mu(out)+slope*[-50,50], '-','linewidth',1,'color',[.3 .8 .3]);
......
......@@ -47,12 +47,12 @@ 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)];
model.l * sin(tril(ones(model.nbDOFs)) * q)];
end
%%%%%%%%%%%%%%%%%%%%%%
%Jacobian with analytical computation
function J = jacob(q, model)
J = [-sin(tril(ones(model.nbDOFs)) * q)' * tril(ones(model.nbDOFs)) * diag(model.l); ...
cos(tril(ones(model.nbDOFs)) * q)' * tril(ones(model.nbDOFs)) * diag(model.l)];
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 demo_OC_DDP_humanoid01
% iLQR applied to a planar 5-link humanoid problem with constraints between joints.
% (see also demo_OC_DDP_CoM01.m)
% (see also demo_OC_DDP_CoM01.m and demo_OC_DDP_bimanual01.m)
%
% If this code is useful for your research, please cite the related publication:
% @article{Lembono21,
......
function demo_OC_DDP_manipulator01
% iLQR applied to a 2D manipulator problem
% iLQR applied to a planar manipulator
% (viapoints task with position+orientation, including bounding boxes on f(x))
%
% If this code is useful for your research, please cite the related publication:
......@@ -34,25 +34,27 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.dt = 1E-2; %Time step size
model.nbData = 100; %Number of datapoints
model.nbIter = 20; %Number of iterations for iLQR
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; %Objective function dimension (x1,x2,o)
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.r = 1E-6; %Control weight term
model.q = 1E0; %Tracking weighting term
model.r = 1E-6; %Control weighting term
model.Mu = [[2; 1; -pi/6], [3; 2; -pi/3]]; %Viapoints
% 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
end
R = speye((model.nbData-1)*model.nbVarU) * model.r; %Control weight matrix (at trajectory level)
Q = speye(model.nbVarF * model.nbPoints) * 1E0; %Precision matrix
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)
%Time occurrence of viapoints
tl = linspace(1, model.nbData, model.nbPoints+1);
......@@ -62,22 +64,20 @@ idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
%% Iterative LQR (iLQR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u = zeros(model.nbVarU*(model.nbData-1), 1);
x0 = [3*pi/4; -pi/2; -pi/4];
u = zeros(model.nbVarU*(model.nbData-1), 1); %Initial 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]);
% [Su0, Sx0] = transferMatrices(A, B); %Constant Su and Sx for the proposed system
Su0 = [zeros(model.nbVarX, model.nbVarX*(model.nbData-1)); tril(kron(ones(model.nbData-1), eye(model.nbVarX)*model.dt))];
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));
idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
Su = Su0(idx,:);
for n=1:model.nbIter
x = reshape(Su0 * u + Sx0 * x0, model.nbVarX, model.nbData); %System evolution
f = fkine(x(:,tl), model);
J = jacob(x(:,tl), f, model);
[f, J] = f_reach(x(:,tl), model);
du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u * model.r); %Gradient
%Estimate step size with backtracking line search method
......@@ -86,7 +86,7 @@ for n=1:model.nbIter
while 1
utmp = u + du * alpha;
xtmp = reshape(Su0 * utmp + Sx0 * x0, model.nbVarX, model.nbData);
ftmp = fkine(xtmp(:,tl), model);
ftmp = f_reach(xtmp(:,tl), model);
cost = ftmp(:)' * Q * ftmp(:) + norm(utmp)^2 * model.r; %utmp' * R * utmp
if cost < cost0 || alpha < 1E-3
break;
......@@ -94,7 +94,12 @@ for n=1:model.nbIter
alpha = alpha * 0.5;
end
u = u + du * alpha;
if norm(du * alpha) < 1E-2
break; %Stop iLQR when solution is reached
end
end
disp(['iLQR converged in ' num2str(n) ' iterations.']);
%Log data
r.x = x;
......@@ -147,6 +152,46 @@ for j=1:model.nbVarU
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);
pause;
close all;
end
......@@ -168,70 +213,61 @@ 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)
e(1:2,:) = f(1:2,:) - f0(1:2,:);
e(3,:) = imag(log(exp(f0(3,:)*1i)' .* exp(f(3,:)*1i).'));
e(1:2,:) = f0(1:2,:) - f(1:2,:);
e(3,:) = imag(log(exp(f0(3,:)*1i).' .* exp(f(3,:)*1i)'));
end
%%%%%%%%%%%%%%%%%%%%%%
%Forward kinematics (in robot coordinate system)
function f = fkine0(x, model)
% f = [model.l * cos(tril(ones(model.nbVarU)) * x); ...
% model.l * sin(tril(ones(model.nbVarU)) * x)];
f = model.l * exp(1i * tril(ones(model.nbVarU)) * x);
f = [real(f); imag(f); mod(sum(x)+pi, 2*pi) - pi]; %x1,x2,o (orientation as single Euler angle for planar robot)
end
%%%%%%%%%%%%%%%%%%%%%%
%Forward kinematics (in object coordinate system)
function f = fkine(x, model)
% f = fkine0(x, model) - model.Mu; %Error by ignoring manifold
f = logmap(fkine0(x, model), model.Mu); %Error by considering manifold
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
end
% %Bounding boxes
% for t=1:size(f,2)
% for i=1:2
% if abs(f(i,t)) < model.sz(i)
% f(i,t) = 0;
% else
% f(i,t) = f(i,t) - sign(f(i,t)) * model.sz(i);
% end
% end
% end
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)
end
%%%%%%%%%%%%%%%%%%%%%%
%Jacobian with analytical computation (for single time step)
function J = jacob0(x, model)
% J = [-sin(tril(ones(model.nbVarU)) * x)' * tril(ones(model.nbVarU)) * diag(model.l); ...
% cos(tril(ones(model.nbVarU)) * x)' * tril(ones(model.nbVarU)) * diag(model.l)];
J = 1i * exp(1i * tril(ones(model.nbVarU)) * x).' * tril(ones(model.nbVarU)) * diag(model.l);
J = [real(J); imag(J); ones(1, model.nbVarX)]; %x1,x2,o
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
end
%%%%%%%%%%%%%%%%%%%%%%
%Jacobian with analytical computation
function J = jacob(x, f, model)
%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
J = [];
for t=1:size(x,2)
Jtmp = jacob0(x(:,t), model);
% f(:,t) = logmap(fkine0(x(:,t), model), model.Mu(:,t));
f(1:2,t) = model.A(:,:,t)' * f(1:2,t); %Object-centered FK
Jtmp = jacob0(x(:,t), model);
Jtmp(1:2,:) = model.A(:,:,t)' * Jtmp(1:2,:); %Object-centered Jacobian
% %Bounding boxes
% for i=1:2
% if f(i,t)==0
% Jtmp(i,:) = 0;
% end
% end
%Bounding boxes (optional)
for i=1:2
if abs(f(i,t)) < model.sz(i)
f(i,t) = 0;
Jtmp(i,:) = 0;
else
f(i,t) = f(i,t) - sign(f(i,t)) * model.sz(i);
end
end
J = blkdiag(J, Jtmp);
end
......
......@@ -34,7 +34,7 @@ addpath('./m_fcts/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.dt = 2E-2; %Time step size
model.nbData = 100; %Number of datapoints
model.nbIter = 10; %Number of iterations for iLQR
model.nbIter = 300; %Maximum number of iterations for iLQR
model.nbPoints = 1; %Number of viapoints
model.nbVarPos = 1; %Dimension of position
model.nbDeriv = 2; %Number of derivatives (nbDeriv=2 for [x; dx] state)
......@@ -44,11 +44,12 @@ model.l = 1; %Link length
model.m = 1; %Mass
model.g = 9.81; %Acceleration due to gravity
model.b = 0.01; %Friction (optional)
model.rfactor = 1E-2; %Control weight teR
model.wq = 1E3; %Tracking weight
model.wr = 1E-2; %Control weight
model.Mu = [pi/2; 0]; %Target
R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix
Q = speye(model.nbVarX * model.nbPoints) * 1E3; %Precision matrix
Q = speye(model.nbVarX * model.nbPoints) * model.wq; %Precision matrix
R = speye(model.nbVarU * (model.nbData-1)) * model.wr; %Control weight matrix
%Time occurrence of viapoints
tl = linspace(1, model.nbData, model.nbPoints+1);
......@@ -86,7 +87,12 @@ for n=1:model.nbIter
alpha = alpha * 0.5;
end
u = u + du * alpha; %Update control by following gradient
if norm(du * alpha) < 1E-2
break; %Stop iLQR when solution is reached
end
end
disp(['iLQR converged in ' num2str(n) ' iterations.']);
%Log data
r.x = x;
......@@ -122,15 +128,15 @@ ylabel('$\dot{q}$','interpreter','latex','fontsize',30);
% end
%% Plot robot (static)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[820 10 800 800],'color',[1 1 1]); hold on; axis off;
plotArm(model.Mu(1), model.l, [0;0;20], .05, [.8 0 0]);
for t=floor(linspace(1, model.nbData, 30))
plotArm(r.x(1,t), model.l, [0; 0; t*.1], .05, [.7 .7 .7]-.7*t/model.nbData);
end
axis equal; axis([-1.2 1.2 -1.2 1.2]);
% print('-dpng','graphs/DDP_pendulum01.png');
% %% Plot robot (static)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure('position',[820 10 800 800],'color',[1 1 1]); hold on; axis off;
% plotArm(model.Mu(1), model.l, [0;0;20], .05, [.8 0 0]);
% for t=floor(linspace(1, model.nbData, 30))
% plotArm(r.x(1,t), model.l, [0; 0; t*.1], .05, [.7 .7 .7]-.7*t/model.nbData);
% end
% axis equal; axis([-1.2 1.2 -1.2 1.2]);
% % print('-dpng','graphs/DDP_pendulum01.png');
% %% Plot robot (animated)
......@@ -196,7 +202,7 @@ function [A, B] = linSys(x, u, model)
m = model.m;
A = zeros(model.nbVarX, model.nbVarX, model.nbData-1);
B = zeros(model.nbVarX, model.nbVarU, model.nbData-1);
Bc = [0; (m * l^2)^2];
Bc = [0; 1/(m * l^2)];
for t=1:model.nbData-1
%Linearize the system
% Ac = [0, 1; sin(x(1,t)) * g / l, -b / (m * l^2)]; %With friction
......
function demo_OC_LQT01
% Batch LQR with viapoints and simple/double/triple integrator system.
% Batch LQT with viapoints and simple/double/triple integrator system.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
......@@ -41,72 +41,64 @@ 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)
nbVar = nbVarPos * nbDeriv; %Dimension of state vector
dt = 1E-1; %Time step duration
rfactor = 1E-3; %dt^nbDeriv; %Control cost in LQR
R = speye((nbData-1)*nbVarPos) * rfactor; %Control cost matrix
q = 1E0; %Tracking cost in LQR
r = 1E-3; %dt^nbDeriv; %Control cost in LQR
%Time occurrence of viapoints
tl = linspace(1, nbData, nbPoints+1);
tl = round(tl(2:end));
idx = (tl - 1) * nbVar + [1:nbVarPos]';
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A1d = zeros(nbDeriv);
for i=0:nbDeriv-1
A1d = A1d + diag(ones(nbDeriv-i,1),i) * dt^i * 1/factorial(i); %Discrete 1D
end
B1d = zeros(nbDeriv,1);
for i=1:nbDeriv
B1d(nbDeriv-i+1) = dt^i * 1/factorial(i); %Discrete 1D
end
A = kron(A1d, speye(nbVarPos)); %Discrete nD
B = kron(B1d, speye(nbVarPos)); %Discrete nD
%Build Sx and Su transfer matrices
Su = sparse(nbVar*nbData, nbVarPos*(nbData-1));
Sx = kron(ones(nbData,1), speye(nbVar));
M = B;
for n=2:nbData
id1 = (n-1)*nbVar+1:nbData*nbVar;
Sx(id1,:) = Sx(id1,:) * A;
id1 = (n-1)*nbVar+1:n*nbVar;
id2 = 1:(n-1)*nbVarPos;
Su(id1,id2) = M;
M = [A*M(:,1:nbVarPos), M];
%% Dynamical System settings
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nbDeriv==1
%Build Su0 and Sx0 transfer matrices (for nbDeriv=1)
Su0 = [zeros(nbVar, nbVar*(nbData-1)); kron(tril(ones(nbData-1)), eye(nbVar)*dt)];
Sx0 = kron(ones(nbData,1), eye(nbVar));
else
A1d = zeros(nbDeriv);
for i=0:nbDeriv-1
A1d = A1d + diag(ones(nbDeriv-i,1),i) * dt^i * 1/factorial(i); %Discrete 1D
end
B1d = zeros(nbDeriv,1);
for i=1:nbDeriv
B1d(nbDeriv-i+1) = dt^i * 1/factorial(i); %Discrete 1D
end
A = kron(A1d, speye(nbVarPos)); %Discrete nD
B = kron(B1d, speye(nbVarPos)); %Discrete nD
%Build Su0 and Sx0 transfer matrices
Su0 = sparse(nbVar*nbData, nbVarPos*(nbData-1));
Sx0 = kron(ones(nbData,1), speye(nbVar));
M = B;
for n=2:nbData
id1 = (n-1)*nbVar+1:nbData*nbVar;
Sx0(id1,:) = Sx0(id1,:) * A;
id1 = (n-1)*nbVar+1:n*nbVar;
id2 = 1:(n-1)*nbVarPos;
Su0(id1,id2) = M;
M = [A*M(:,1:nbVarPos), M];
end
end
% %Build Sx and Su transfer matrices (for nbDeriv=1)
% Su = [zeros(nbVar, nbVar*(nbData-1)); tril(kron(ones(nbData-1), eye(nbVar)*dt))];
% Sx = kron(ones(nbData,1), eye(nbVar));
Su = Su0(idx,:);
Sx = Sx0(idx,:);
%% Task setting (viapoints passing)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tl = linspace(1, nbData, nbPoints+1);
tl = round(tl(2:end));
MuQ = zeros(nbVar*nbData,1);
Q = zeros(nbVar*nbData);
Mu = rand(nbVarPos, nbPoints); %Viapoints
Q = speye(nbVarPos * nbPoints) * q; %Tracking weight matrix
R = speye(nbVarPos * (nbData-1)) * r; %Control weight matrix (at trajectory level)
%Position tracking
for t=1:length(tl)
id(:,t) = [1:nbVarPos] + (tl(t)-1) * nbVar;
Q(id(:,t), id(:,t)) = eye(nbVarPos);
MuQ(id(:,t)) = rand(nbVarPos,1) - 0.5;
end
% %Position+velocity tracking
% for t=1:length(tl)
% id(:,t) = [1:nbVar] + (tl(t)-1) * nbVar;
% Q(id(:,t), id(:,t)) = eye(nbVar);
% MuQ(id(:,t)) = [rand(nbVarPos,1) - 0.5; zeros(nbVar-nbVarPos,1)];
% end
%% Batch LQR reproduction
%% Batch LQT reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0 = zeros(nbVar,1);
u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x0);
rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
u = (Su' * Q * Su + R) \ Su' * Q * (Mu(:) - Sx * x0);
rx = reshape(Sx0*x0+Su0*u, nbVar, nbData); %Reshape data for plotting
u = reshape(u, 2, nbData-1); %Reshape data for plotting
uSigma = inv(Su' * Q * Su + R); % + eye((nbData-1)*nbVarU) * 1E-10;
xSigma = Su * uSigma * Su';
% uSigma = inv(Su' * Q * Su + R); % + eye((nbData-1)*nbVarU) * 1E-10;
% xSigma = Su0 * uSigma * Su0';
%% Plot 2D
......@@ -117,9 +109,9 @@ figure('position',[10 10 600 600]); hold on; axis off;
% end
plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
plot(rx(1,1), rx(2,1), '.','markersize',35,'color',[0 0 0]);
plot(MuQ(id(1,:)), MuQ(id(2,:)), '.','markersize',35,'color',[.8 0 0]);
plot(Mu(1,:), Mu(2,:), '.','markersize',35,'color',[.8 0 0]);
axis equal;
% print('-dpng','graphs/demo_LQT01.png');
% print('-dpng','graphs/demo_OC_LQT01.png');
%% Timeline plot
......@@ -129,17 +121,17 @@ labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$'};
figure('position',[620 10 600 850],'color',[1 1 1]);
%State profile
for j=1:nbVar
subplot(nbVar+nbVarPos+1,1,j); hold on;
subplot(nbVar+nbVarPos,1,j); hold on;
plot(rx(j,:), '-','linewidth',2,'color',[0 0 0]);
if j<=nbVarPos
plot(tl, MuQ(id(j,:)), '.','markersize',40,'color',[.8 0 0]);
plot(tl, Mu(j,:), '.','markersize',40,'color',[.8 0 0]);
end
axis([1, nbData, min(rx(j,:))-1E-3, max(rx(j,:))+1E-3]);
axis([1, nbData, min(rx(j,:))-1E-2, max(rx(j,:))+1E-2]);
ylabel(labList{j},'fontsize',18,'interpreter','latex');
end
%Control profile
for j=1:nbVarPos
subplot(nbVar+nbVarPos+1,1,nbVar+1+j); hold on;
subplot(nbVar+nbVarPos,1,nbVar+j); hold on;
plot(u(j,:), '-','linewidth',2,'color',[0 0 0]);
axis([1, nbData-1, min(u(j,:))-1E-6, max(u(j,:))+1E-6]);
ylabel(['$u_' num2str(j) '$'],'fontsize',18,'interpreter','latex');
......
......@@ -74,6 +74,7 @@ bx = 8;
%% Load handwriting data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
demos=[];
load('data/2Dletters/G.mat');
Data=[];
for n=1:nbSamples
......@@ -167,7 +168,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
figure('position',[10 10 800 800],'color',[1 1 1]); hold on; axis off;
% Plot GMM and Data;
plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5],.3);
......
......@@ -33,13 +33,13 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 400; %Number of datapoints
nbRepros = 10; %Number of stochastic reproductions
nbPoints = 3; %Number of keypoints
nbData = 50; %Number of datapoints
nbRepros = 60; %Number of stochastic reproductions
nbPoints = 1; %Number of keypoints
nbVarPos = 2; %Dimension of position data (here: x1,x2)
nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbDeriv = 1; %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
dt = 1E-1; %Time step duration
rfactor = 1E-4; %dt^nbDeriv; %Control cost in LQR
R = speye((nbData-1)*nbVarPos) * rfactor;
Rx = speye(nbData*nbVar) * rfactor;
......@@ -48,36 +48,42 @@ x0 = zeros(nbVar,1); %Initial point
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A1d = zeros(nbDeriv);
for i=0:nbDeriv-1
A1d = A1d + diag(ones(nbDeriv-i,1),i) * dt^i * 1/factorial(i); %Discrete 1D
end
B1d = zeros(nbDeriv,1);
for i=1:nbDeriv
B1d(nbDeriv-i+1) = dt^i * 1/factorial(i); %Discrete 1D
end
A = kron(A1d, speye(nbVarPos)); %Discrete nD
B = kron(B1d, speye(nbVarPos)); %Discrete nD
%Build Sx and Su transfer matrices
Su = sparse(nbVar*nbData, nbVarPos*(nbData-1));
Sx = kron(ones(nbData,1), speye(nbVar));
M = B;
for n=2:<