diff --git a/README.md b/README.md
index fba8b14b746e2f6de6637af31ab1804da82b25f0..46d7aecc1aaf25422e67c43300dbcd2a830dd371 100644
--- a/README.md
+++ b/README.md
@@ -136,9 +136,9 @@ All the examples are located in the `demos` folder, and the functions are locate
| [demo_OC_DDP_humanoid01.m](./demos/demo_OC_DDP_humanoid01.m) | [[2]](#ref-2) | iLQR applied to a planar 5-link humanoid problem with constraints between joints |
| [demo_OC_DDP_manipulator01.m](./demos/demo_OC_DDP_manipulator01.m) | [[2]](#ref-2) | iLQR applied to a 2D manipulator problem |
| [demo_OC_DDP_pendulum01.m](./demos/demo_OC_DDP_pendulum01.m) | [[2]](#ref-2) | iLQR applied to an actuated inverted pendulum problem |
-| [demo_OC_LQT01.m](./demos/demo_OC_LQT01.m) | [[2]](#ref-2) | Batch solution of linear quadratic optimal control (unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data (see also demo_OC_LQT_iterativeLQR01.m) |
-| [demo_OC_LQT02.m](./demos/demo_OC_LQT02.m) | [[2]](#ref-2) | Same as demo_OC_LQT01.m but with a GMM encoding of only position data |
-| [demo_OC_LQT03.m](./demos/demo_OC_LQT03.m) | [[2]](#ref-2) | Same as demo_OC_LQT01.m but by tracking only position references |
+| [demo_OC_LQT01.m](./demos/demo_OC_LQT01.m) | [[2]](#ref-2) | Batch solution of linear quadratic tracking (LQT) optimal control problem (example with viapoints and simple/double/triple integrator system) |
+| [demo_OC_LQT02.m](./demos/demo_OC_LQT02.m) | [[2]](#ref-2) | Same as demo_OC_LQT01.m but with a GMM encoding of the reference (example by tracking position and velocity reference) |
+| [demo_OC_LQT03.m](./demos/demo_OC_LQT03.m) | [[2]](#ref-2) | Same as demo_OC_LQT01.m but with a GMM encoding of the reference (example by tracking only position reference) |
| [demo_OC_LQT04.m](./demos/demo_OC_LQT04.m) | [[2]](#ref-2) | Control of a spring attached to a point with batch LQR (with augmented state space) |
| [demo_OC_LQT_augmSigma01.m](./demos/demo_OC_LQT_augmSigma01.m) | [[2]](#ref-2) | Batch LQR with augmented covariance to transform a tracking problem to a regulation problem |
| [demo_OC_LQT_ballistic01.m](./demos/demo_OC_LQT_ballistic01.m) | [[2]](#ref-2) | Batch LQT with augmented state to solve simple ballistic problem |
@@ -322,7 +322,7 @@ If you find PbDlib useful for your research, please cite the related publication
</a><br>
[[pdf]](https://calinon.ch/papers/Berio-GI2017.pdf)
[[bib]](https://calinon.ch/papers/Berio-GI2017.bib)
-<br><strong>(Ref. for Bezier curves as batch LQT with viapoints)</strong>
+<br><strong>(Ref. for Bezier curves as LQT with viapoints)</strong>
</p>
<p><a name="ref-9">
@@ -336,7 +336,7 @@ If you find PbDlib useful for your research, please cite the related publication
<p><a name="ref-10">
[10] EPFL EE613 course "Machine Learning for Engineers"
</a><br>
-[[url]](http://calinon.ch/teaching.htm)
+[[url]](https://calinon.ch/teaching_EPFL.htm)
<br><strong>(Ref. for machine learning teaching material)</strong>
</p>
@@ -346,7 +346,7 @@ If you find PbDlib useful for your research, please cite the related publication
|-------------------------|-------------------------|
|  <br> [demo\_GMM\_semiTied01.m](./demos/demo_GMM_semiTied01.m) |  <br> [demo\_GMR\_3Dviz01.m](./demos/demo_GMR_3Dviz01.m) |
|  <br> [demo\_GMR\_polyFit01.m](./demos/demo_GMR_polyFit01.m) |  <br> [demo\_HMM01.m](./demos/demo_HMM01.m) |
-|  <br> [demo\_MPC\_iterativeLQR01.m](./demos/demo_MPC_iterativeLQR01.m) |  <br> [demo\_Riemannian\_SPD\_GMR01.m](./demos/demo_Riemannian_SPD_GMR01.m) |
+|  <br> [demo\_OC\_LQT\_recursive02.m](./demos/demo_OC_LQT_recursive02.m) |  <br> [demo\_Riemannian\_SPD\_GMR01.m](./demos/demo_Riemannian_SPD_GMR01.m) |
|  <br> [demo\_Riemannian\_SPD\_interp01.m](./demos/demo_Riemannian_SPD_interp01.m) |  <br> [demo\_Riemannian\_SPD\_interp02.m](./demos/demo_Riemannian_SPD_interp02.m) |
|  <br> [demo\_Riemannian\_SPD\_interp03.m](./demos/demo_Riemannian_SPD_interp03.m) |  <br> [demo\_Riemannian\_S2\_GMM01.m](./demos/demo_Riemannian_S2_GMM01.m) |
|  <br> [demo\_TPMPC01.m](./demos/demo_TPMPC01.m) |  <br> [demo\_TPGMR01.m](./demos/demo_TPGMR01.m) |
@@ -363,4 +363,4 @@ PbDlib is free software: you can redistribute it and/or modify it under the term
PbDlib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
-You should have received a copy of the GNU General Public License along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
+You should have received a copy of the GNU General Public License along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
diff --git a/demos/demo_OC_DDP_manipulator01.m b/demos/demo_OC_DDP_manipulator01.m
index 3331f67add369c42371919cce974566d2403c3cd..3420b633ad9995f58b63edc7ac1bdb9735b17858 100644
--- a/demos/demo_OC_DDP_manipulator01.m
+++ b/demos/demo_OC_DDP_manipulator01.m
@@ -1,7 +1,8 @@
function demo_OC_DDP_manipulator01
-% iLQR applied to a 2D manipulator problem
+% iLQR applied to a 2D manipulator problem with fast computation
+% (viapoints task with position+orientation, including bounding boxes on f(x))
%
-% Sylvain Calinon, 2020
+% Sylvain Calinon, 2021
addpath('./m_fcts/');
@@ -10,241 +11,112 @@ addpath('./m_fcts/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.dt = 1E-2; %Time step size
model.nbData = 100; %Number of datapoints
-model.nbIter = 20; %Number of iterations for iLQR
+model.nbIter = 40; %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 = 2; %Objective function dimension (x1,x2)
-
-model.rfactor = 1E-2; %Control weight term
-% model.Mu = [2; 4]; %Target (x1h,x2h)
-model.Mu = rand(model.nbVarF, model.nbPoints) * 5; %Viapoints
-
-model.l = [2,2,2]; %Link length
-
-R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
-% Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
-
-% Q = kron(eye(model.nbData), inv(model.Sigma)); %Sparse precision matrix (at trajectory level)
-% MuQ = kron(ones(model.nbData,1), model.Mu); %Sparse reference (at trajectory level)
+model.nbVarF = 3; %Objective function dimension (x1,x2,o)
+model.l = [2, 2, 1]; %Links length
+model.sz = [.2, .3]; %Size of objects
+model.r = 1E-4; %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
+for t=1:model.nbPoints
+ model.R(:,:,t) = [cos(model.Mu(3,t)), -sin(model.Mu(3,t)); sin(model.Mu(3,t)), cos(model.Mu(3,t))]; %Orientation
+end
-% Q = blkdiag(sparse((model.nbData-1)*2,(model.nbData-1)*2), diag([1E1, 1E1])); %Sparse precision matrix (at trajectory level)
-% MuQ = [sparse((model.nbData-1)*2,1); model.Mu]; %Sparse reference (at trajectory level)
+R = speye((model.nbData-1)*model.nbVarU) * model.r; %Control weight matrix (at trajectory level)
+Q = speye(model.nbVarF * model.nbPoints) * 1E3; %Precision matrix
-%Sparse reference with a set of via-points
+%Time occurrence of viapoints
tl = linspace(1, model.nbData, model.nbPoints+1);
tl = round(tl(2:end));
-MuQ = sparse(model.nbVarF * model.nbData, 1);
-Q = sparse(model.nbVarF * model.nbData, model.nbVarF * model.nbData);
-for t=1:length(tl)
- id(:,t) = (tl(t) - 1) * model.nbVarF + [1:model.nbVarF];
- Q(id(:,t), id(:,t)) = eye(model.nbVarF) * 1E3;
- MuQ(id(:,t)) = model.Mu(:,t);
-end
%% Iterative LQR (iLQR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% u = zeros(model.nbVarU, model.nbData-1);
-% u = repmat(linspace(0,1E-3,model.nbData-1), model.nbVarU, 1);
-u = ones(model.nbVarU, model.nbData-1) * pi/8;
-x0 = zeros(model.nbVarX,1);
+u = zeros(model.nbVarU*(model.nbData-1), 1);
+x0 = [3*pi/4; -pi/2; -pi/4];
-% uSigma = zeros(model.nbVarU*(model.nbData-1));
-% xSigma = zeros(model.nbVarX*model.nbData);
+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
+idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
+Su = Su0(idx,:);
for n=1:model.nbIter
- %System evolution
- x = dynSysSimulation(x0, u, model);
-% x = reshape(Su * u(:) + Sx * x0, model.nbVarX, model.nbData);
-
- %Linearization
- if n==1 %Constant Su and Sx for the proposed system
- [A, B] = linSys(x, u, model);
- [Su, Sx] = transferMatrices(A, B);
- end
-
-% f=[];
-% % J=[];
-% % JQJ0=[]; JQ0=[];
-% for t=1:model.nbData
-% ftmp = fkine(x(:,t), model);
-% f = [f; ftmp]; %Endeffector positions in lifted form
-%
-% % Jtmp = jacob0(x(:,t), model);
-% % J = blkdiag(J, Jtmp); %Jacobians in lifted form
-%
-% % %Alternative computation (equivalent if no constraint between time steps)
-% % Jtmp = jacob0(x(:,t), model);
-% % id = (t-1) * model.nbVarF + [1:model.nbVarF];
-% % JQJ0 = blkdiag(JQJ0, Jtmp' * Q(id,id) * Jtmp);
-% % JQ0 = blkdiag(JQ0, Jtmp' * Q(id,id));
-% end
-% f = fkine(x, model);
-
- %Alternative computation (fast version exploiting sparsity of Q)
- f = sparse(model.nbData*model.nbVarF, 1);
- JQJ = sparse(model.nbData*model.nbVarX, model.nbData*model.nbVarX);
- JQ = sparse(model.nbData*model.nbVarX, model.nbData*model.nbVarF);
- for t=1:length(tl)
- Jtmp = jacob0(x(:,tl(t)), model);
- idu = (tl(t) - 1) * model.nbVarU + [1:model.nbVarU];
- idf = (tl(t) - 1) * model.nbVarF + [1:model.nbVarF];
- JQJ(idu,idu) = Jtmp' * Q(idf,idf) * Jtmp;
- JQ(idu,idf) = Jtmp' * Q(idf,idf);
- f(idf) = fkine(x(:,tl(t)), model);
- end
-
-% du_vec = (Su' * J' * Q * J * Su + R) \ (Su' * J' * Q * (MuQ - f(:)) - R * u(:)); %Control trajectory
- du_vec = (Su' * JQJ * Su + R) \ (Su' * JQ * (MuQ - f(:)) - R * u(:)); %Control trajectory (alternative computation)
-
- delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
+ x = reshape(Su0 * u + Sx0 * x0, model.nbVarX, model.nbData); %System evolution
+ f = fkine(x(:,tl), model);
+ 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
alpha = 1;
- cost0 = (MuQ - f(:))' * Q * (MuQ - f(:)) + u(:)' * R * u(:);
+ cost0 = f(:)' * Q * f(:) + norm(u)^2 * model.r; %u' * R * u
while 1
- utmp = u + delta_u * alpha;
-% xtmp = dynSysSimulation(x0, utmp, model);
- xtmp = reshape(Su * utmp(:) + Sx * x0, model.nbVarX, model.nbData);
-% ftmp = [];
-% for t=1:model.nbData
-% ftmp = [ftmp; fkine(xtmp(:,t), model)]; %Endeffector positions in lifted form
-% end
- ftmp = fkine(xtmp, model);
- cost = (MuQ - ftmp(:))' * Q * (MuQ - ftmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ utmp = u + du * alpha;
+ xtmp = reshape(Su0 * utmp + Sx0 * x0, model.nbVarX, model.nbData);
+ ftmp = fkine(xtmp(:,tl), model);
+ cost = ftmp(:)' * Q * ftmp(:) + norm(utmp)^2 * model.r; %utmp' * R * utmp
if cost < cost0 || alpha < 1E-3
break;
end
alpha = alpha * 0.5;
end
- u = u + delta_u * alpha; %Update control by following gradient
-
-% uSigmaTmp = inv(Su' * J' * Q * J * Su + R);
-% uSigma = uSigma + uSigmaTmp * alpha^2;
-% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
-
-% %Log data
-% r(n).x = x;
-% r(n).f = f; %reshape(f, model.nbVarF, model.nbData);
-% r(n).u = u;
+ u = u + du * alpha; %Update control by following gradient
end
%Log data
-r(1).x = x;
-r(1).f = fkine(x, model);
-r(1).u = u;
-
-
-%% Stochastic sampling by exploiting the nullspace structure of the problem
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% %Standard stochastic sampling
-% uSigma = inv(Su' * J' * Q * J * Su + R);
-% % xSigma = Su * uSigma * Su';
-% [V, D] = eig(full(uSigma));
-% U = real(V * D.^.5);
-% nbSamples = 50;
-% for n=1:nbSamples
-% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 5E-2;
-% qtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% ftmp = [];
-% for t=1:model.nbData
-% % Htmp = model.rob.fkine(qtmp(:,t)); %Forward kinematics
-% % ftmp = [ftmp; Htmp.t(1:2)]; %Endeffector positions in lifted form
-% ftmp = [ftmp; fkine(qtmp(:,t), model)]; %Endeffector positions in lifted form
-% end
-% sx(:,n) = ftmp;
-% end
-
-% %Generate structured stochastic u through Bezier curves
-% nbSamples = 50;
-% nbRBF = 18;
-% H = zeros(nbRBF, model.nbData-1);
-% tl = linspace(0, 1, model.nbData-1);
-% nbDeg = nbRBF - 1;
-% for i=0:nbDeg
-% H(i+1,:) = factorial(nbDeg) / (factorial(i) * factorial(nbDeg-i)) * (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions
-% end
-% %Nullspace planning
-% [V,D] = eig(full(J' * Q * J));
-% U = V * D.^.5;
-% N = eye((model.nbData-1)*model.nbVarU) - Su' * U / (U' * (Su * Su') * U + Rx) * U' * Su; %Nullspace projection matrix
-% sx = zeros(model.nbData*2, nbSamples);
-% for n=1:nbSamples
-% w = randn(model.nbVarU, nbRBF) * 5E-1; %Random weights
-% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
-% utmp = u(:) + N * un(:);
-% qtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% ftmp = [];
-% for t=1:model.nbData
-% % Htmp = model.rob.fkine(qtmp(:,t)); %Forward kinematics
-% % f = [f; Htmp.t(1:2)]; %Endeffector positions in lifted form
-% ftmp = [ftmp; fkine(qtmp(:,t), model)]; %Endeffector positions in lifted form
-% end
-% sx(:,n) = ftmp;
-% end
+r.x = x;
+r.f = fkine0(x, model);
+r.u = reshape(u, model.nbVarU, model.nbData-1);
%% Plot state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,1000,800],'color',[1,1,1]); hold on; axis off;
-plotArm(r(end).x(:,1), ones(model.nbVarU,1)*model.l, [0; 0; -3], .2, [.8 .8 .8]);
-plotArm(r(end).x(:,model.nbData/2+1), ones(model.nbVarU,1)*model.l, [0; 0; -2], .2, [.6 .6 .6]);
-plotArm(r(end).x(:,model.nbData), ones(model.nbVarU,1)*model.l, [0; 0; -1], .2, [.4 .4 .4]);
-
-% for k=1:length(r)
-% plot(r(k).f(1,:), r(k).f(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
-% end
-
-% %Stochastic samples
-% for n=1:nbSamples
-% plot(sx(1:2:end,n), sx(2:2:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
-% end
+msh0 = diag(model.sz) * [-1 -1 1 1 -1; -1 1 1 -1 -1];
+for t=1:model.nbPoints
+ msh(:,:,t) = model.R(:,:,t) * msh0 + repmat(model.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 i=1:model.nbPoints
- plot2Dframe(eye(2)*4E-1, model.Mu(:,i), repmat(colMat(i,:),3,1), 6);
+for t=1:model.nbPoints
+ patch(msh(1,:,t), msh(2,:,t), min(colMat(t,:)*1.7,1),'linewidth',2,'edgecolor',colMat(t,:)); %,'facealpha',.2
+ plot2Dframe(model.R(:,:,t)*4E-1, model.Mu(1:2,t), repmat(colMat(t,:),3,1), 6);
end
-
-plot(r(end).f(1,:), r(end).f(2,:), '-','linewidth',2,'color',[0 0 0]);
-h(1) = plot(r(end).f(1,1), r(end).f(2,1), '.','markersize',40,'color',[0 0 0]);
-% h(2) = plot3(model.Mu(1,:), model.Mu(2,:), ones(1,size(model.Mu,2)), '.','markersize',40,'color',[.8 0 0]);
-% legend(h,{'Initial endeffector position','Target endeffector viapoint position'},'location','northwest','fontsize',20);
-axis equal; %axis([-3.5 6.5 -2 5.5]);
-% print('-dpng','graphs/DDP_manipulator01.png');
-
-
-% %% Plot control space
-% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% figure('position',[1020,10,1000,1000]);
-% t = model.dt:model.dt:model.dt*model.nbData;
-% for kk=1:size(r(1).u,1)
-% subplot(size(r(1).u,1), 1, kk); grid on; hold on; box on; box on;
-% for k=1:length(r)
-% plot(t(1:end-1), r(k).u(kk,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
-% end
-% plot(t(1:end-1), r(end).u(kk,:), '-','linewidth',2,'color',[0 0 0]);
-% axis([min(t), max(t(1:end-1)), min(r(end).u(kk,:))-.1, max(r(end).u(kk,:))+.1]);
-% xlabel('t'); ylabel(['u_' num2str(kk)]);
-% 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_manipulator_fast01.png');
-% %% Plot animated robot
-% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
-% plot3(model.Mu(1), model.Mu(2), 1, '.','markersize',40,'color',[.8 0 0]);
-% axis equal; axis([-3.5 6.5 -1 5.5]);
-% for t=1:model.nbData
-% h = plotArm(r(end).x(:,t), ones(model.nbVarU,1)*model.l, zeros(3,1), .2, [0 0 0]);
-% drawnow;
-% % print('-dpng',['graphs/anim/DDP_manipulator_anim' num2str(t,'%.3d') '.png']);
-% if t<model.nbData
-% delete(h);
-% end
-% end
-% % for t=model.nbData+1:model.nbData+10
-% % print('-dpng',['graphs/anim/DDP_manipulator_anim' num2str(t,'%.3d') '.png']);
-% % end
+%% 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);
pause;
close all;
@@ -267,63 +139,62 @@ function [Su, Sx] = transferMatrices(A, B)
end
%%%%%%%%%%%%%%%%%%%%%%
-%Given the control trajectory u and initial state x0, compute the whole state trajectory
-function x = dynSysSimulation(x0, u, model)
- x = zeros(model.nbVarX, model.nbData);
- x(:,1) = x0;
- for t=1:model.nbData-1
- x(:,t+1) = x(:,t) + u(:,t) * model.dt;
- 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
%%%%%%%%%%%%%%%%%%%%%%
-%Linearize the system along the trajectory computing the matrices A and B
-function [A, B] = linSys(x, u, model)
- A = zeros(model.nbVarX, model.nbVarX, model.nbData-1);
- B = zeros(model.nbVarX, model.nbVarU, model.nbData-1);
- Ac = zeros(model.nbVarX);
- Bc = eye(model.nbVarX, model.nbVarU);
- for t=1:model.nbData-1
- %Discretize the linear system
- A(:,:,t) = eye(model.nbVarX) + Ac * model.dt;
- B(:,:,t) = Bc * model.dt;
- end
+%Forward kinematics (in robot coordinate system)
+function f = fkine0(x, model)
+ 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
-function [x, Tf] = fkine(q, model)
-% Tf = eye(4);
-% T = repmat(Tf, [1,1,size(q,1)]);
-% for n=1:size(q,1)
-% c = cos(q(n));
-% s = sin(q(n));
-% T(:,:,n) = [c, -s, 0, model.l(n) * c; ...
-% s, c, 0, model.l(n) * s; ...
-% 0, 0, 1, 0; ...
-% 0, 0, 0, 1]; %Homogeneous matrix
-% Tf = Tf * T(:,:,n);
+%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(1:2,t) = model.R(:,:,t)' * f(1:2,t);
+% 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
-% x = Tf(1:2,end);
-
-% x = [model.l * cos(tril(ones(model.nbVarU)) * q); model.l * sin(tril(ones(model.nbVarU)) * q)];
- x = model.l * exp(1i * tril(ones(model.nbVarU)) * q); %Computation in matrix form
- x = [real(x); imag(x)];
end
%%%%%%%%%%%%%%%%%%%%%%
-%Jacobian
-function J = jacob0(q, model)
-% e = 1E-6;
-% J = zeros(model.nbVarF, model.nbVarX);
-% for n=1:size(q,1)
-% qtmp = q;
-% qtmp(n) = qtmp(n) + e;
-% J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e; %Numeric computation
-% end
-% J
-
- J = 1i * exp(1i * tril(ones(model.nbVarU)) * q)' * tril(ones(model.nbVarU)) * diag(model.l); %Computation in matrix form
- J = [-real(J); imag(J)];
+%Jacobian with analytical computation (for single time step)
+function J = jacob0(x, model)
+ 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
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with analytical computation (for single time step)
+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,:);
+% %Bounding boxes
+% for i=1:2
+% if f(i,t)==0
+% Jtmp(i,:) = 0;
+% end
+% end
+ J = blkdiag(J, Jtmp);
+ end
end
\ No newline at end of file