diff --git a/demos/demo_OC_DDP_bicopter01.m b/demos/demo_OC_DDP_bicopter01.m
index 883d7001e133564b1c3059d143c8f2f49b946f09..248815af9b92352e52547a24140221145f0b3889 100644
--- a/demos/demo_OC_DDP_bicopter01.m
+++ b/demos/demo_OC_DDP_bicopter01.m
@@ -72,7 +72,7 @@ for n=1:model.nbIter
%Gradient
e = model.Mu - x(:,tl);
du = (Su' * Q * Su + R) \ (Su' * Q * e(:) - R * u);
- %Estimate step size with line search method
+ %Estimate step size with backtracking line search method
alpha = 1;
cost0 = e(:)' * Q * e(:) + u' * R * u;
while 1
diff --git a/demos/demo_OC_DDP_humanoid01.m b/demos/demo_OC_DDP_humanoid01.m
index 930e1f65b6d5f1dceab1824212b98edaf80c3269..24ae72f4442c249f315f33229f791f8f39a49a7f 100644
--- a/demos/demo_OC_DDP_humanoid01.m
+++ b/demos/demo_OC_DDP_humanoid01.m
@@ -184,7 +184,7 @@ function J = jacob0(x, model)
end
%%%%%%%%%%%%%%%%%%%%%%
-%Jacobian with analytical computation (for single time step)
+%Jacobian with analytical computation
function J = jacob(x, f, model)
J = [];
for t=1:size(x,2)
diff --git a/demos/demo_OC_DDP_manipulator01.m b/demos/demo_OC_DDP_manipulator01.m
index 601454c7969177518965a6b66eabbe7f52ce26b8..d3d8cccaf4ed953d85d145b49b12ca99db7e2d9b 100644
--- a/demos/demo_OC_DDP_manipulator01.m
+++ b/demos/demo_OC_DDP_manipulator01.m
@@ -44,8 +44,8 @@ model.l = [2, 2, 1]; %Robot links lengths
model.sz = [.2, .3]; %Size of objects
model.r = 1E-6; %Control weight 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 = [[2; 1; -pi/2], [3; 1; -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
@@ -76,7 +76,7 @@ for n=1:model.nbIter
J = jacob(x(:,tl), f, model);
du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u * model.r); %Gradient
- %Estimate step size with line search method
+ %Estimate step size with backtracking line search method
alpha = 1;
cost0 = f(:)' * Q * f(:) + norm(u)^2 * model.r; %u' * R * u
while 1
@@ -186,9 +186,9 @@ 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(1:2,t) = model.R(:,:,t)' * f(1:2,t);
-% end
+ for t=1:size(x,2)
+ f(1:2,t) = model.A(:,:,t)' * f(1:2,t); %Object-centered FK
+ end
% %Bounding boxes
% for t=1:size(f,2)
@@ -213,12 +213,14 @@ function J = jacob0(x, model)
end
%%%%%%%%%%%%%%%%%%%%%%
-%Jacobian with analytical computation (for single time step)
+%Jacobian with analytical computation
function J = jacob(x, f, model)
J = [];
for t=1:size(x,2)
Jtmp = jacob0(x(:,t), model);
-% Jtmp(1:2,:) = model.R(:,:,t)' * Jtmp(1:2,:);
+
+ Jtmp(1:2,:) = model.A(:,:,t)' * Jtmp(1:2,:); %Object-centered Jacobian
+
% %Bounding boxes
% for i=1:2
% if f(i,t)==0
diff --git a/demos/demo_ergodicControl_2D01.m b/demos/demo_ergodicControl_2D01.m
index 3bec9a3108493fc5becda138f7e8b1f1472cdf82..17c4342be2cddd1a28a9bbba99cdfaed3d3c0d29 100644
--- a/demos/demo_ergodicControl_2D01.m
+++ b/demos/demo_ergodicControl_2D01.m
@@ -74,7 +74,6 @@ rg = 0:nbFct-1;
[KX(1,:,:), KX(2,:,:)] = ndgrid(rg, rg);
Lambda = (KX(1,:).^2 + KX(2,:).^2 + 1)'.^-sp; %Weighting vector (Eq.(15))
-% tic
%Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
%Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1]), and removing redundant ones -> keeping ([-1,-1],[-1,1])
op = hadamard(2^(nbVar-1));
@@ -90,12 +89,7 @@ for j=1:nbStates
end
end
w_hat = w_hat / L^nbVar / size(op,2);
-% toc
-% return
-%Suhan: The summation goes from $m \in \{ 1,...,2^{D-1}\}$. But, H is hadamard matrix of size $2^D$,
-%and you also write $A_m$ m-th column's last $D$ elements of $H$. I think something is not clear here.
-
% %Alternative computation of w_hat by discretization (for verification)
% nbRes = 100;
diff --git a/demos/demo_trajGMM02.m b/demos/demo_trajGMM02.m
index 2905e01b7b73e7c92c69f9c18dad98c746b18d56..527e047ff74858fa50e72723dd5a2ba49a7d831e 100644
--- a/demos/demo_trajGMM02.m
+++ b/demos/demo_trajGMM02.m
@@ -49,6 +49,7 @@ nbRepros = 0; %Number of stochastic reproductions
%% Load handwriting movements
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
load('data/2Dletters/S.mat');
Data=[];
for n=1:nbSamples
@@ -144,7 +145,7 @@ end %nbSamples
%% Plot timeline
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 600 600]);
+figure('position',[10 10 500 500]);
for m=1:model.nbVarPos
limAxes = [1, nbData, min(Data(m,:))-1E0, max(Data(m,:))+1E0];
subplot(model.nbVarPos,1,m); hold on;
@@ -189,7 +190,7 @@ end
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if model.nbVarPos>1
- figure('position',[620 10 600 600]); hold on;
+ figure('position',[520 10 500 500]); hold on;
% plotGMM(model.Mu([1,2],:), model.Sigma([1,2],[1,2],:), [.5 .5 .5],.8);
% for n=1:1 %nbSamples
% plotGMM(r(n).Data([1,2],:), r(n).expSigma([1,2],[1,2],:), [1 .2 .2],.1);
@@ -213,7 +214,7 @@ end
%% Plot covariance structure
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[1230 10 600 600]); hold on; axis off;
+figure('position',[1030 10 500 500]); hold on; axis off;
colormap(flipud(gray));
pcolor(abs(S));
axis square; axis([1 nbData*model.nbVarPos 1 nbData*model.nbVarPos]); axis ij; shading flat;