diff --git a/demos/demo_OC_DDP_bicopter01.m b/demos/demo_OC_DDP_bicopter01.m
index 760767f7b793f23c9e4990a5aada8300d59241e6..18f0863b0cf461a13d6f6abd408d61594c89c105 100644
--- a/demos/demo_OC_DDP_bicopter01.m
+++ b/demos/demo_OC_DDP_bicopter01.m
@@ -32,169 +32,96 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.sim_time = 3; %Simulation time
-model.dt = 1E-1; %Time step size
-model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.dt = 5E-2; %Time step size
+model.nbData = 100; %Number of datapoints
model.nbIter = 20; %Number of iterations for iLQR
-
+model.nbPoints = 1; %Number of viapoints
model.nbVarPos = 3; %Dimension of position (x1,x2,theta)
model.nbDeriv = 2; %Number of derivatives (nbDeriv=2 for [x; dx] state)
model.nbVarX = model.nbVarPos * model.nbDeriv; %State space dimension
model.nbVarU = 2; %Control space dimension (u1,u2)
-
-model.rfactor = 1E-6; %Control weight teR
-model.Mu = [4; 4; 0; zeros(model.nbVarPos,1)]; %Target
-
model.m = 2.5; %Mass
model.l = 0.5; %Length
model.g = 9.81; %Acceleration due to gravity
model.I = 1.2; %Inertia
+model.rfactor = 1E-6; %Control weight
+model.Mu = [4; 4; 0; zeros(model.nbVarPos,1)]; %Target
-R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
-Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix
+Q = speye(model.nbVarX * model.nbPoints) * 1E3; %Precision matrix
-Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), diag([1E2, 1E2, 1E1, 1E0, 1E0, 1E0])); %Sparse precision matrix (at trajectory level)
-MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+%Time occurrence of viapoints
+tl = linspace(1, model.nbData, model.nbPoints+1);
+tl = round(tl(2:end));
+idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
%% Iterative LQR (iLQR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% delta_x0 = zeros(model.nbVarX, 1);
-% u = zeros(model.nbVarU, model.nbData-1);
-u0 = 0.5 * model.m * model.g * ones(model.nbVarU,1);
-u = repmat(u0, 1, model.nbData-1);
-x0 = zeros(model.nbVarX,1);
+u = zeros(model.nbVarU*(model.nbData-1), 1);
+x0 = zeros(model.nbVarX, 1);
% x0 = [-1; 1; -pi/8; .3; .1; .2];
-% uSigma = zeros(model.nbVarU*(model.nbData-1));
-% xSigma = zeros(model.nbVarX*model.nbData);
-
-for n=1:model.nbIter
+for n=1:model.nbIter
%System evolution
- x = dynSysSimulation(x0, u, model);
-% if n>1
-% x = reshape(Sx * x0 + Su * u(:), model.nbVarX, model.nbData); %Approximated dynamics
-% end
+ x = dynSysSimulation(x0, reshape(u, model.nbVarU, model.nbData-1), model);
%Linearization
- [A, B] = linSys(x, u, model);
- [Su, Sx] = transferMatrices(A, B);
+ [A, B] = linSys(x, reshape(u, model.nbVarU, model.nbData-1), model);
+ Su0 = transferMatrices(A, B);
+ Su = Su0(idx,:);
-% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
- du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
- delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
-% delta_x = reshape(Sx * delta_x0 + Su * delta_u(:), model.nbVarX, model.nbData); %Forward recursion
+ du = (Su' * Q * Su + R) \ (Su' * Q * (model.Mu(:) - x(:,tl)) - R * u); %Gradient
%Estimate step size with line search method
alpha = 1;
- cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ cost0 = (model.Mu(:) - x(:,tl))' * Q * (model.Mu(:) - x(:,tl)) + u' * R * u;
while 1
- utmp = u + delta_u * alpha;
- xtmp = dynSysSimulation(x0, utmp, model);
-% xtmp = reshape(Sx * x0 + Su * utmp(:), model.nbVarX, model.nbData); %Approximated dynamics
-
- cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ utmp = u + du * alpha;
+ xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1), model);
+ cost = (model.Mu(:) - xtmp(:,tl))' * Q * (model.Mu(:) - xtmp(:,tl)) + utmp' * R * utmp; %Compute the cost
if cost < cost0 || alpha < 1E-4
break;
end
alpha = alpha * 0.5;
end
- disp(alpha);
- u = u + delta_u * alpha; %Update control by following gradient
-
-% uSigmaTmp = inv(Su' * Q * Su + R);
-% uSigma = uSigma + uSigmaTmp * alpha^2;
-% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
-
- %Log data
- r(n).x = x;
- r(n).u = u;
+ u = u + du * alpha; %Update control by following gradient
end
-
-%% Stochastic sampling by exploiting the nullspace structure of the problem
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% %Standard stochastic sampling
-% uSigma = inv(Su' * Q * Su + R);
-% % xSigma = Su * uSigma * Su';
-% [V, D] = eig(full(uSigma));
-% U = real(V * D.^.5);
-% nbSamples = 50;
-% sx = zeros(model.nbData*model.nbVarX, nbSamples);
-% for n=1:nbSamples
-% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 1E-3;
-% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% sx(:,n) = xtmp(:);
-% 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(Q));
-% 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*model.nbVarX, nbSamples);
-% for n=1:nbSamples
-% w = randn(model.nbVarU, nbRBF) * 1E0; %Random weights
-% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
-% utmp = u(:) + N * un(:);
-% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% sx(:,n) = xtmp(:);
-% end
+%Log data
+r.x = x;
+r.u = reshape(u, model.nbVarU, model.nbData-1);
%% Plot state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,800,800]); hold on; axis off; grid on; box on;
-% for t=1:model.nbData
-% plotGMM(r(end).x(1:2,t), xSigma(model.nbVarX*(t-1)+[1,2],model.nbVarX*(t-1)+[1,2]).*1E-5, [.8 0 0], .1);
-% end
-
-%iLQR refinement steps
-for k=1:length(r)
- plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',[.7 .7 .7]-.7*k/length(r));
-end
-
-% %Stochastic samples
-% for n=1:nbSamples
-% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
-% end
-
-% plot(r(end).x(1,:),r(end).x(2,:), '-','linewidth',2,'color',[.8 0 0]);
-h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',40,'color',[0 0 0]);
-%Animation
for t=floor(linspace(1,model.nbData,20))
- plot(r(end).x(1,t), r(end).x(2,t), '.','markersize',40,'color',[0 0 0]);
- msh = [r(end).x(1:2,t), r(end).x(1:2,t)] + [cos(r(end).x(3,t)); sin(r(end).x(3,t))] * [-.4, .4];
- plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[0 0 0]);
+ 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]);
end
+plot(r.x(1,:),r.x(2,:), '-','linewidth',2,'color',[.6 .6 .6]);
+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,{'Starting point','Ending point'},'location','southeast','fontsize',40);
+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
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure('position',[1030,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,:))-2, max(r(end).u(kk,:))+2]);
+% for kk=1:size(r.u,1)
+% subplot(size(r.u,1), 1, kk); grid on; hold on; box on; box on;
+% plot(t(1:end-1), r.u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r.u(kk,:))-2, max(r.u(kk,:))+2]);
% xlabel('t'); ylabel(['u_' num2str(kk)]);
% end
@@ -207,8 +134,8 @@ print('-dpng','graphs/DDP_bicopter01.png');
% plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[.8 0 0]);
% axis equal; axis([-.5 4.5 -.2 4.2]);
% for t=1:model.nbData
-% h(1) = plot(r(end).x(1,t), r(end).x(2,t), '.','markersize',40,'color',[0 0 0]);
-% msh = [r(end).x(1:2,t), r(end).x(1:2,t)] + [cos(r(end).x(3,t)); sin(r(end).x(3,t))] * [-.4, .4];
+% h(1) = plot(r.x(1,t), r.x(2,t), '.','markersize',40,'color',[0 0 0]);
+% msh = [r.x(1:2,t), r.x(1:2,t)] + [cos(r.x(3,t)); sin(r.x(3,t))] * [-.4, .4];
% h(2) = plot(msh(1,:), msh(2,:), '-','linewidth',5,'color',[0 0 0]);
% drawnow;
% % print('-dpng',['graphs/anim/DDP_bicopter_anim' num2str(t,'%.3d') '.png']);
diff --git a/demos/demo_OC_DDP_car01.m b/demos/demo_OC_DDP_car01.m
index 62ba642acb930ad21d1f35b385266c8b4ce462a2..4a375c2e8057178831dd43003fe26cf520b93fae 100644
--- a/demos/demo_OC_DDP_car01.m
+++ b/demos/demo_OC_DDP_car01.m
@@ -32,175 +32,99 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.sim_time = 3; %Simulation time
model.dt = 1E-1; %Time step size
-model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.nbData = 100; %Number of datapoints
model.nbIter = 10; %Number of iterations for iLQR
-
+model.nbPoints = 1; %Number of viapoints
model.nbVarX = 4; %Dimension of state (x1,x2,theta,phi)
model.nbVarU = 2; %Control space dimension (v,dphi)
-
+model.l = 0.5; %Length of car
model.rfactor = 1E-6; %Control weight teR
model.Mu = [4; 3; pi/2; 0]; %Target
% model.Mu = [.2; 2; 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
-model.l = 0.5; %Length of car
-
-R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
-% Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
-
-Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), speye(model.nbVarX)*1E2); %Sparse precision matrix (at trajectory level)
-MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+%Time occurrence of viapoints
+tl = linspace(1, model.nbData, model.nbPoints+1);
+tl = round(tl(2:end));
+idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
%% Iterative LQR (iLQR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% delta_x0 = zeros(model.nbVarX, 1);
-u = zeros(model.nbVarU, model.nbData-1);
-% u = repmat([2; 0], 1, model.nbData-1);
-x0 = zeros(model.nbVarX,1);
-
-% uSigma = zeros(model.nbVarU*(model.nbData-1));
-% xSigma = zeros(model.nbVarX*model.nbData);
+u = zeros(model.nbVarU*(model.nbData-1), 1);
+x0 = zeros(model.nbVarX, 1);
for n=1:model.nbIter
%System evolution
- x = dynSysSimulation(x0, u, model);
-% if n>1
-% x = reshape(Sx * x0 + Su * u(:), model.nbVarX, model.nbData); %Approximated dynamics
-% end
+ x = dynSysSimulation(x0, reshape(u, model.nbVarU, model.nbData-1), model);
%Linearization
- [A, B] = linSys(x, u, model);
- Su = transferMatrices(A, B);
+ [A, B] = linSys(x, reshape(u, model.nbVarU, model.nbData-1), model);
+ Su0 = transferMatrices(A, B);
+ Su = Su0(idx,:);
-% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
- du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
- delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
-% delta_x = reshape(Sx * delta_x0 + Su * delta_u(:), model.nbVarX, model.nbData); %Forward recursion
+ du = (Su' * Q * Su + R) \ (Su' * Q * (model.Mu(:) - x(:,tl)) - R * u); %Gradient
%Estimate step size with line search method
alpha = 1;
- cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ cost0 = (model.Mu(:) - x(:,tl))' * Q * (model.Mu(:) - x(:,tl)) + u' * R * u;
while 1
- utmp = u + delta_u * alpha;
- xtmp = dynSysSimulation(x0, utmp, model);
-% xtmp = reshape(Sx * x0 + Su * utmp(:), model.nbVarX, model.nbData); %Approximated dynamics
-
- cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ utmp = u + du * alpha;
+ xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1), model);
+ cost = (model.Mu(:) - xtmp(:,tl))' * Q * (model.Mu(:) - xtmp(:,tl)) + utmp' * R * utmp; %Compute the cost
if cost < cost0 || alpha < 1E-4
break;
end
alpha = alpha * 0.5;
end
-% disp(alpha);
- u = u + delta_u * alpha; %Update control by following gradient
-
-% uSigmaTmp = inv(Su' * Q * Su + R);
-% uSigma = uSigma + uSigmaTmp * alpha^2;
-% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
-
- %Log data
- r(n).x = x;
- r(n).u = u;
+ u = u + du * alpha; %Update control by following gradient
end
-
-%% Stochastic sampling by exploiting the nullspace structure of the problem
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% %Standard stochastic sampling
-% uSigma = inv(Su' * Q * Su + R);
-% % xSigma = Su * uSigma * Su';
-% [V, D] = eig(full(uSigma));
-% U = real(V * D.^.5);
-% sx = zeros(model.nbData*model.nbVarX, nbSamples);
-% nbSamples = 50;
-% for n=1:nbSamples
-% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 1E-3;
-% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% sx(:,n) = xtmp(:);
-% 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(Q));
-% 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*model.nbVarX, nbSamples);
-% for n=1:nbSamples
-% w = randn(model.nbVarU, nbRBF) * 1E0; %Random weights
-% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
-% utmp = u(:) + N * un(:);
-% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% sx(:,n) = xtmp(:);
-% end
+%Log data
+r.x = x;
+r.u = reshape(u, model.nbVarU, model.nbData-1);
%% Plot state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,800,800]); hold on; axis off;
-% for t=1:model.nbData
-% plotGMM(r(end).x(1:2,t), xSigma(model.nbVarX*(t-1)+[1,2],model.nbVarX*(t-1)+[1,2]).*1E-6, [.8 0 0], .1);
-% end
-%Animation
-for t=round(linspace(1,model.nbData,model.nbData))
- R = [cos(r(end).x(3,t)) -sin(r(end).x(3,t)); sin(r(end).x(3,t)) cos(r(end).x(3,t))];
- msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r(end).x(1:2,t),1,5);
- plot(msh(1,:), msh(2,:), '-','linewidth',2,'color',[.8 .8 .8]);
+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]);
end
-
-% %iLQR refinement steps
-% for k=1:length(r)
-% plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
-% end
-
-% %Stochastic samples
-% for n=1:nbSamples
-% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
-% end
-
-plot(r(end).x(1,:),r(end).x(2,:), '-','linewidth',2,'color',[0 0 0]);
+plot(r.x(1,:),r.x(2,:), '-','linewidth',2,'color',[0 0 0]);
%Initial pose
-R = [cos(r(end).x(3,1)) -sin(r(end).x(3,1)); sin(r(end).x(3,1)) cos(r(end).x(3,1))];
-msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r(end).x(1:2,1),1,5);
+R = [cos(r.x(3,1)) -sin(r.x(3,1)); sin(r.x(3,1)) cos(r.x(3,1))];
+msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r.x(1:2,1),1,5);
plot(msh(1,:), msh(2,:), '-','linewidth',2,'color',[0 0 0]);
-h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',20,'color',[0 0 0]);
-%Final pose
-R = [cos(r(end).x(3,end)) -sin(r(end).x(3,end)); sin(r(end).x(3,end)) cos(r(end).x(3,end))];
-msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r(end).x(1:2,end),1,5);
-plot(msh(1,:), msh(2,:), '-','linewidth',4,'color',[0 0 0]);
-h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',20,'color',[0 0 0]);
+h(1) = plot(r.x(1,1),r.x(2,1), '.','markersize',20,'color',[0 0 0]);
+% %Final pose
+% R = [cos(r.x(3,end)) -sin(r.x(3,end)); sin(r.x(3,end)) cos(r.x(3,end))];
+% msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r.x(1:2,end),1,5);
+% plot(msh(1,:), msh(2,:), '-','linewidth',4,'color',[0 0 0]);
+% h(1) = plot(r.x(1,1),r.x(2,1), '.','markersize',20,'color',[0 0 0]);
%Target pose
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',20);
+legend(h,{'Initial pose','Target pose'},'location','northwest','fontsize',30);
axis equal; axis([-1 5 -1 4]);
-% print('-dpng','graphs/DDP_car01_Sigma.png');
+% print('-dpng','graphs/DDP_car01.png');
% %% Plot control space
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure('position',[1030,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,:))-.2, max(r(end).u(kk,:))+.2]);
+% for kk=1:size(r.u,1)
+% subplot(size(r.u,1), 1, kk); grid on; hold on; box on; box on;
+% plot(t(1:end-1), r.u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r.u(kk,:))-.2, max(r.u(kk,:))+.2]);
% xlabel('t'); ylabel(['u_' num2str(kk)]);
% end
@@ -215,10 +139,10 @@ axis equal; axis([-1 5 -1 4]);
% plot(model.Mu(1), model.Mu(2), '.','markersize',40,'color',[.8 0 0]);
% axis equal; axis([-1 5 -1 4]);
% for t=1:model.nbData
-% R = [cos(r(end).x(3,t)) -sin(r(end).x(3,t)); sin(r(end).x(3,t)) cos(r(end).x(3,t))];
-% msh = R * [-.6 -.6 .6 .6 -.6; -.4 .4 .4 -.4 -.4] + repmat(r(end).x(1:2,t),1,5);
+% 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);
% h(1) = plot(msh(1,:), msh(2,:), '-','linewidth',4,'color',[0 0 0]);
-% h(2) = plot(r(end).x(1,t),r(end).x(2,t), '.','markersize',40,'color',[0 0 0]);
+% h(2) = plot(r.x(1,t),r.x(2,t), '.','markersize',40,'color',[0 0 0]);
% % print('-dpng',['graphs/anim/DDP_car_anim' num2str(t,'%.3d') '.png']);
% if t<model.nbData
% delete(h);
diff --git a/demos/demo_OC_DDP_cartpole01.m b/demos/demo_OC_DDP_cartpole01.m
index 7df9337edcbf9056786ebd2ebde59f079dc99bd8..1aa10a2523331c5276762b0cb72d36f1e9e07ab0 100644
--- a/demos/demo_OC_DDP_cartpole01.m
+++ b/demos/demo_OC_DDP_cartpole01.m
@@ -32,154 +32,92 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.sim_time = 3; %Simulation time
-model.dt = 2E-2; %Time step size
-model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.dt = 1E-1; %Time step size
+model.nbData = 100; %Number of datapoints
model.nbIter = 30; %Number of iterations for iLQR
-
+model.nbPoints = 1; %Number of viapoints
model.nbVarPos = 2; %Dimension of position
model.nbDeriv = 2; %Number of derivatives (nbDeriv=2 for [x; dx] state)
model.nbVarX = model.nbVarPos * model.nbDeriv; %State space dimension
model.nbVarU = 1; %Control space dimension
-
-model.rfactor = 1E-5; %Control weight teR
-model.Mu = [2; pi; 0; 0]; %Target
-
model.m = 1; %Pendulum mass
model.M = 5; %Cart mass
model.L = 1; %Pendulum length
model.g = 9.81; %Acceleration due to gravity
model.d = 1; %Cart damping
% model.b = 0.01; %Friction
+model.rfactor = 1E-5; %Control weight teR
+model.Mu = [2; pi; 0; 0]; %Target
-R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
-Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix
+Q = speye(model.nbVarX * model.nbPoints) * 1E3; %Precision matrix
-Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), diag([1E4, 1E4, 1E3, 1E3])); %Sparse precision matrix (at trajectory level)
-MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+%Time occurrence of viapoints
+tl = linspace(1, model.nbData, model.nbPoints+1);
+tl = round(tl(2:end));
+idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
%% Iterative LQR (iLQR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% delta_x0 = zeros(model.nbVarX, 1);
-u = zeros(model.nbVarU, model.nbData-1);
-x0 = zeros(model.nbVarX,1);
+u = zeros(model.nbVarU*(model.nbData-1), 1);
+x0 = zeros(model.nbVarX, 1);
% x0 = [0; pi/4; .4; 0];
-% uSigma = zeros(model.nbVarU*(model.nbData-1));
-% xSigma = zeros(model.nbVarX*model.nbData);
-
-for n=1:model.nbIter
- %sxstem evolution
- x = dynSysSimulation(x0, u, model);
+for n=1:model.nbIter
+ %System evolution
+ x = dynSysSimulation(x0, reshape(u, model.nbVarU, model.nbData-1), model);
%Linearization
- [A, B] = linSys(x, u, model);
- [Su, Sx] = transferMatrices(A, B);
+ [A, B] = linSys(x, reshape(u, model.nbVarU, model.nbData-1), model);
+ Su0 = transferMatrices(A, B);
+ Su = Su0(idx,:);
-% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
- du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
- delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
-% delta_x = reshape(Sx * delta_x0 + Su * delta_u(:), model.nbVarX, model.nbData); %Forward recursion
+ du = (Su' * Q * Su + R) \ (Su' * Q * (model.Mu(:) - x(:,tl)) - R * u); %Gradient
%Estimate step size with line search method
alpha = 1;
- cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ cost0 = (model.Mu(:) - x(:,tl))' * Q * (model.Mu(:) - x(:,tl)) + u' * R * u;
while 1
- utmp = u + delta_u * alpha;
- xtmp = dynSysSimulation(x0, utmp, model);
- cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ utmp = u + du * alpha;
+ xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1), model);
+ cost = (model.Mu(:) - xtmp(:,tl))' * Q * (model.Mu(:) - xtmp(:,tl)) + utmp' * R * utmp; %Compute the cost
if cost < cost0 || alpha < 1E-4
break;
end
alpha = alpha * 0.5;
end
-% disp(alpha);
- u = u + delta_u * alpha; %Update control by following gradient
-
-% uSigmaTmp = inv(Su' * Q * Su + R);
-% uSigma = uSigma + uSigmaTmp * alpha^2;
-% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
-
- %Log data
- r(n).x = x;
- r(n).u = u;
+ u = u + du * alpha; %Update control by following gradient
end
-%% Stochastic sampling by exploiting the nullspace structure of the problem
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% %Standard stochastic sampling
-% uSigma = inv(Su' * Q * Su + R);
-% % xSigma = Su * uSigma * Su';
-% [V, D] = eig(full(uSigma));
-% U = real(V * D.^.5);
-% nbSamples = 50;
-% sx = zeros(model.nbData*model.nbVarX, nbSamples);
-% for n=1:nbSamples
-% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 1E-1;
-% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% sx(:,n) = xtmp(:);
-% 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(Q));
-% 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*model.nbVarX, nbSamples);
-% for n=1:nbSamples
-% w = randn(model.nbVarU, nbRBF) * 1E1; %Random weights
-% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
-% utmp = u(:) + N * un(:);
-% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% sx(:,n) = xtmp(:);
-% end
+%Log data
+r.x = x;
+r.u = reshape(u, model.nbVarU, model.nbData-1);
%% Plot state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,800,800]); hold on; axis on; grid on; box on;
-% for t=1:model.nbData
-% plotGMM(r(end).x(1:2,t), xSigma(model.nbVarX*(t-1)+[1,2],model.nbVarX*(t-1)+[1,2]).*5E-2, [.8 0 0], .05);
-% end
-
-% %iLQR refinement steps
-% for k=1:length(r)
-% plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
-% end
-
-% %Stochastic samples
-% for n=1:nbSamples
-% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
-% end
-
-plot(r(end).x(1,:),r(end).x(2,:), '-','linewidth',2,'color',[.8 0 0]);
-h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',20,'color',[0 0 0]);
-h(2) = plot(model.Mu(1), model.Mu(2), '.','markersize',20,'color',[.8 0 0]);
-legend(h,{'Starting point','Ending point'},'location','southwest');
+plot(r.x(1,:),r.x(2,:), '-','linewidth',2,'color',[.6 .6 .6]);
+h(1) = plot(r.x(1,1),r.x(2,1), '.','markersize',40,'color',[0 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);
axis equal; %axis([-5 5 -4 6]);
xlabel('x'); ylabel('\theta');
+% print('-dpng','graphs/DDP_cartpole01.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 kk=1:size(r.u,1)
+% subplot(size(r.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,:))-2, max(r(end).u(kk,:))+2]);
+% plot(t(1:end-1), r.u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r.u(kk,:))-2, max(r.u(kk,:))+2]);
% xlabel('t'); ylabel(['u_' num2str(kk)]);
% end
@@ -192,7 +130,7 @@ xlabel('x'); ylabel('\theta');
% h=[];
% for t=1:model.nbData
% delete(h);
-% h = plotAR(r(end).x(2,t)-pi/2, model.L, [r(end).x(1,t);0;0], .1, [0 0 0]);
+% h = plotAR(r.x(2,t)-pi/2, model.L, [r.x(1,t);0;0], .1, [0 0 0]);
% drawnow;
% end
diff --git a/demos/demo_OC_DDP_humanoid01.m b/demos/demo_OC_DDP_humanoid01.m
index 9514f4f1b9f43ad0a9fcd6f77b1a2176f203bceb..930e1f65b6d5f1dceab1824212b98edaf80c3269 100644
--- a/demos/demo_OC_DDP_humanoid01.m
+++ b/demos/demo_OC_DDP_humanoid01.m
@@ -32,173 +32,108 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.sim_time = 1; %Simulation time
model.dt = 1E-2; %Time step size
-model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.nbData = 100; %Number of datapoints
model.nbPoints = 2; %Number of viapoints
model.nbIter = 20; %Number of iterations for iLQR
-model.nbVarX = 5; %Dimension of state (x1,x2, q1,q2,q3)
+model.nbVarX = 5; %State space dimension (q1,q2,q3,q4,q5)
model.nbVarU = 3; %Control space dimension (dq1,dq2,dq3)
+% model.nbVarU = 5; %Control space dimension (dq1,dq2,dq3,dq4,dq5)
+model.nbVarF = 3; %Objective function dimension (x1,x2,o)
-model.rfactor = 1E-2; %Control weight term
-model.Mu(:,1) = [3; 1.5; zeros(model.nbVarU,1)]; %Target (x1h,x2h, q1h,q2h,q3h)
-model.Mu(:,2) = [2; 5; zeros(model.nbVarU,1)]; %Target (x1h,x2h, q1h,q2h,q3h)
+model.l = ones(1, model.nbVarX) * 2; %Links lengths
+model.sz = [.2, .2]; %Size of objects
+model.r = 1E-5; %Control weight term
+model.Mu = [[2.5; 1; 0], [3; 4; 0]]; %Viapoints
-model.l = 2; %Link length
+R = speye((model.nbData-1)*model.nbVarU) * model.r; %Control weight matrix (at trajectory level)
+% Q = speye(model.nbVarF * model.nbPoints) * 1E3; %Precision matrix
+Q = kron(eye(model.nbPoints), diag([1E0, 1E0, 0])); %Precision matrix (orientation does not matter)
-R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
-Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
-
-% Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), diag([1E2, 1E2, zeros(1,model.nbVarU)])); %Sparse precision matrix (at trajectory level)
-% MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
-
-Q = blkdiag(sparse((model.nbData/2-1)*model.nbVarX, (model.nbData/2-1)*model.nbVarX), diag([1E2, 1E2, zeros(1,model.nbVarU)]), ...
- sparse((model.nbData/2-1)*model.nbVarX, (model.nbData/2-1)*model.nbVarX), diag([1E2, 1E2, zeros(1,model.nbVarU)])); %Sparse precision matrix (at trajectory level)
-MuQ = [sparse((model.nbData/2-1)*model.nbVarX,1); model.Mu(:,1); ...
- sparse((model.nbData/2-1)*model.nbVarX,1); model.Mu(:,2)]; %Sparse reference (at trajectory level)
-
-% Q = blkdiag(zeros((model.nbData-2)*model.nbVarX), diag([1E2, 1E2, 0, 0, 0, 1E2, 1E2, 0, 0, 0])); %Sparse precision matrix (at trajectory level)
-% MuQ = [zeros((model.nbData-2)*model.nbVarX,1); model.Mu; model.Mu]; %Sparse reference (at trajectory level)
-
-% Q = kron(eye(model.nbData), diag([1E0, 1E0, 0, 0, 0])); %Sparse precision matrix (at trajectory level)
-% MuQ = kron(ones(model.nbData,1), model.Mu); %Sparse reference (at trajectory level)
-
-% 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)
+%Time occurrence of viapoints
+tl = linspace(1, model.nbData, model.nbPoints+1);
+tl = round(tl(2:end));
+idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
%% 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) * 1E-1;
+u = zeros(model.nbVarU*(model.nbData-1), 1);
+a = .7;
+x0 = [pi/2-a; 2*a; -a; pi-.2; pi/2];
+
+A = repmat(eye(model.nbVarX), [1 1 model.nbData-1]);
+% B = repmat(eye(model.nbVarX, model.nbVarU) * model.dt, [1 1 model.nbData-1]);
+phi = [-1,0,0; 2,0,0; -1,0,0; 0,1,0; 0,0,1]; %Joint coordination matrix
+B = repmat(phi * model.dt, [1 1 model.nbData-1]);
-% q0 = zeros(model.nbVarU,1);
-% q0 = ones(model.nbVarU,1) * pi/20;
-q0 = [.7; pi-.2; pi/2];
-x0 = [fkine(q0, model); q0];
+[Su0, Sx0] = transferMatrices(A, B); %Constant Su and Sx for the proposed system
+Su = Su0(idx,:);
for n=1:model.nbIter
- %System evolution
- x = dynSysSimulation(x0, u, model);
-
- %Linearization
- [A, B] = linSys(x, u, model);
- [Su, Sx] = transferMatrices(A, B);
-
-% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
- du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
- 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 - x(:))' * Q * (MuQ - x(:)) + 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);
- cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + 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
-
- %Log data
- r(n).x = [x(1:2,:); pi/2-x(3,:); 2*x(3,:); -x(3,:); x(4:5,:)];
- r(n).u = u;
+ u = u + du * alpha; %Update control by following gradient
end
-
-% %% 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
+%Log data
+r.x = x;
+r.f = fkine0(x, model);
+r.u = reshape(u, model.nbVarU, model.nbData-1);
-%% Plot static robot
+%% Plot state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,1600,800]); hold on; axis off;
-% for t=1:model.nbData
-% plotGMM(r(end).x(1:2,t), xSigma(model.nbVarX*(t-1)+[1,2],model.nbVarX*(t-1)+[1,2]).*1E-2, [.8 0 0], .1);
-% end
-% plotArm(r(end).x(3:end,1), ones(5,1)*model.l, [0; 0; -3], .2, [.8 .8 .8]);
-% plotArm(r(end).x(3:end,end/2), ones(5,1)*model.l, [ofxa; 0; -2], .2, [.6 .6 .6]);
-% plotArm(r(end).x(3:end,end), ones(5,1)*model.l, [2*ofxa; 0; -1], .2, [.4 .4 .4]);
-
-% %iLQR refinement steps
-% for k=1:length(r)
-% plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
-% end
-
-% %Stochastic samples
-% for n=1:nbSamples
-% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
-% end
-
colMat = lines(model.nbPoints);
-tl = floor(linspace(1,model.nbData,3));
-msh = [-1 -1 1 1 -1; -1 1 1 -1 -1] * 6E-1;
-ofxa = 6;
-for i=1:3
- ofx = (i-1)*ofxa;
- plotArm(r(end).x(3:end,tl(i)), ones(5,1)*model.l, [ofx; 0; -2], .2, [.8 .8 .8]-(i-1)*[.2 .2 .2]);
- for j=1:model.nbPoints
- plot2Dframe(eye(2)*4E-1, model.Mu(1:2,j)+[ofx;0], repmat(colMat(j,:),3,1), 6);
- plot(msh(1,:)+model.Mu(1,j)+ofx, msh(2,:)+model.Mu(2,j), '-','linewidth',2,'color',colMat(j,:));
- end
-% for t=floor(linspace(model.nbData/2,model.nbData,2))
-% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(r(end).x(1:2,t), 1, 5);
-% plot3(msh(1,:)+ofx, msh(2,:), ones(5,1)*-2, '-','linewidth',2,'color',[0 0 0]);
-% end
- plot(r(end).x(1,1:tl(i))+ofx, r(end).x(2,1:tl(i)), '-','linewidth',2,'color',[0 0 0]);
- plot(r(end).x(1,1)+ofx, r(end).x(2,1), '.','markersize',40,'color',[0 0 0]);
+msh0 = diag(model.sz) * [-1 -1 1 1 -1; -1 1 1 -1 -1];
+for t=1:model.nbPoints
+ msh(:,:,t) = msh0 + repmat(model.Mu(1:2,t), 1, size(msh0,2));
end
-% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(model.Mu(1:2,1), 1, 5);
-% plot3(msh(1,:), msh(2,:), ones(5,1)*-2, '-','linewidth',2,'color',[0 0 0]);
-axis equal; %axis([-3.5 6.5 -1 5.5]);
+figure('position',[10,10,1000,800],'color',[1,1,1]); hold on; axis off;
+plotArm(r.x(:,1), model.l, [0; 0; -13], .2, [.8 .8 .8]);
+plotArm(r.x(:,model.nbData/2+1), model.l, [0; 0; -12], .2, [.6 .6 .6]);
+plotArm(r.x(:,model.nbData), model.l, [0; 0; -11], .2, [.4 .4 .4]);
+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(eye(2)*4E-1, model.Mu(1:2,t), repmat(colMat(t,:),3,1), 6);
+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_humanoid01.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,1], '.','markersize',40,'color',[.8 0 0]);
-%
-% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(model.Mu(1:2,1), 1, 5);
-% plot3(msh(1,:), msh(2,:), ones(5,1)*-2, ':','linewidth',2,'color',[.8 0 0]);
-% hb = plot3(msh(1,:), msh(2,:), ones(5,1)*-2, '-','linewidth',2,'color',[0 0 0]);
-%
-% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(model.Mu(1:2,2), 1, 5);
-% plot3(msh(1,:), msh(2,:), ones(5,1)*-2, ':','linewidth',2,'color',[.8 0 0]);
-%
-% axis equal; axis([-1 3.7 -.6 5.6]);
-% for t=1:model.nbData
-% h = plotArm(r(end).x(3:end,t), ones(5,1)*model.l, zeros(3,1), .2, [0 0 0]);
-% if t>model.nbData/2
-% msh = [-1 -1 1 1 -1; -1 1 1 -1 -1]*6E-1 + repmat(r(end).x(1:2,t), 1, 5);
-% delete(hb);
-% hb = plot3(msh(1,:), msh(2,:), ones(5,1)*-2, '-','linewidth',2,'color',[0 0 0]);
-% end
-% drawnow;
-% % print('-dpng',['graphs/anim/DDP_humanoid_anim' num2str(t,'%.3d') '.png']);
-% if t<model.nbData
-% delete(h);
+% %% Plot control space
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[1020,10,1000,1000]);
+% t = model.dt:model.dt:model.dt*model.nbData;
+% for kk=1:size(r.u,1)
+% subplot(size(r.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.u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r.u(kk,:))-.1, max(r.u(kk,:))+.1]);
+% xlabel('t'); ylabel(['u_' num2str(kk)]);
% end
-% % for t=model.nbData+1:model.nbData+10
-% % print('-dpng',['graphs/anim/DDP_humanoid_anim' num2str(t,'%.3d') '.png']);
-% % end
pause;
close all;
@@ -221,63 +156,39 @@ 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(3:end,t+1) = x(3:end,t) + u(:,t) * model.dt;
-% Htmp = model.rob.fkine(x(3:end,t+1)); %Forward kinematics
-% x(1:2,t+1) = Htmp.t(1:2);
- x(1:2,t+1) = fkine(x(3:end,t+1), model); %Forward kinematics
- 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 = zeros(model.nbVarX, model.nbVarU);
- for t=1:model.nbData-1
- %Linearize the system
-% J = model.rob.jacob0(x(3:end,t),'trans');
-% Bc(1:2,:) = J(1:2,:);
- Bc(1:2,:) = jacob0(x(3:end,t), model);
- Bc(3:end,:) = eye(model.nbVarU);
- %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.nbVarX)) * 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)
- q = [pi/2-q(1); 2*q(1); -q(1); q(2); q(3)];
- 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 * c; ...
- 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);
+%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
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with analytical computation (for single time step)
+function J = jacob0(x, model)
+ J = 1i * exp(1i * tril(ones(model.nbVarX)) * x).' * tril(ones(model.nbVarX)) * diag(model.l);
+ J = [real(J); imag(J); ones(1, model.nbVarX)]; %x1,x2,o
end
%%%%%%%%%%%%%%%%%%%%%%
-%Jacobian with numerical computation
-function J = jacob0(q, model)
- e = 1E-6;
- J = zeros(2,size(q,1));
- for n=1:size(q,1)
- qtmp = q;
- qtmp(n) = qtmp(n) + e;
- J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
+%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);
+ J = blkdiag(J, Jtmp);
end
end
\ No newline at end of file
diff --git a/demos/demo_OC_DDP_manipulator01.m b/demos/demo_OC_DDP_manipulator01.m
index a80fe6d87520b7478db58877c53a53c22ad4c288..ec726a6129313fde823db20a98dbdbe52e9ef370 100644
--- a/demos/demo_OC_DDP_manipulator01.m
+++ b/demos/demo_OC_DDP_manipulator01.m
@@ -35,7 +35,7 @@ addpath('./m_fcts/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.dt = 1E-2; %Time step size
model.nbData = 100; %Number of datapoints
-model.nbIter = 40; %Number of iterations for iLQR
+model.nbIter = 10; %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)
@@ -47,7 +47,7 @@ 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
+ 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)
@@ -56,6 +56,7 @@ Q = speye(model.nbVarF * model.nbPoints) * 1E3; %Precision matrix
%Time occurrence of viapoints
tl = linspace(1, model.nbData, model.nbPoints+1);
tl = round(tl(2:end));
+idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
%% Iterative LQR (iLQR)
@@ -66,7 +67,6 @@ x0 = [3*pi/4; -pi/2; -pi/4];
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
@@ -101,7 +101,7 @@ r.u = reshape(u, model.nbVarU, model.nbData-1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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));
+ msh(:,:,t) = model.A(:,:,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;
@@ -111,13 +111,13 @@ plotArm(r.x(:,tl(2)), model.l, [0; 0; -1], .2, [.4 .4 .4]);
colMat = lines(model.nbPoints);
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);
+ plot2Dframe(model.A(:,:,t)*4E-1, model.Mu(1:2,t), repmat(colMat(t,:),3,1), 6);
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');
+% print('-dpng','graphs/DDP_manipulator01.png');
%% Timeline plot
diff --git a/demos/demo_OC_DDP_pendulum01.m b/demos/demo_OC_DDP_pendulum01.m
index 4e90b0fc9c4f16239d86e3f29bf61d5b77150825..90f6790ea9b0c2c3d5707d8c249b3dd2edc92123 100644
--- a/demos/demo_OC_DDP_pendulum01.m
+++ b/demos/demo_OC_DDP_pendulum01.m
@@ -32,143 +32,74 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.sim_time = 2; %Simulation time
model.dt = 2E-2; %Time step size
-model.nbData = model.sim_time / model.dt; %Number of datapoints
+model.nbData = 100; %Number of datapoints
model.nbIter = 10; %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)
model.nbVarX = model.nbVarPos * model.nbDeriv; %State space dimension
model.nbVarU = 1; %Control space dimension
-
-model.rfactor = 1E-5; %Control weight teR
-model.Mu = [pi/2; 0]; %Target
-
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.Mu = [pi/2; 0]; %Target
-R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
-Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix
+Q = speye(model.nbVarX * model.nbPoints) * 1E3; %Precision matrix
-Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), speye(model.nbVarX)*1E0); %Sparse precision matrix (at trajectory level)
-MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
+%Time occurrence of viapoints
+tl = linspace(1, model.nbData, model.nbPoints+1);
+tl = round(tl(2:end));
+idx = (tl - 1) * model.nbVarX + [1:model.nbVarX]';
%% Iterative LQR (iLQR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% delta_x0 = zeros(model.nbVarX, 1);
-u = zeros(model.nbVarU, model.nbData-1);
-% x0 = zeros(model.nbVarX,1);
+u = zeros(model.nbVarU*(model.nbData-1), 1);
+% x0 = zeros(model.nbVarX, 1);
x0 = [-pi/2; 0];
-% uSigma = zeros(model.nbVarU*(model.nbData-1));
-% xSigma = zeros(model.nbVarX*model.nbData);
-
-for n=1:model.nbIter
+for n=1:model.nbIter
%System evolution
- x = dynSysSimulation(x0, u, model);
-% if n>1
-% x = reshape(Sx * x0 + Su * u(:), model.nbVarX, model.nbData); %Approximated dynamics
-% end
+ x = dynSysSimulation(x0, reshape(u, model.nbVarU, model.nbData-1), model);
%Linearization
- [A, B] = linSys(x, u, model);
- [Su, Sx] = transferMatrices(A, B);
-
-% du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
- du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
- delta_u = reshape(du_vec, model.nbVarU, model.nbData-1);
-% delta_x = reshape(Sx * delta_x0 + Su * delta_u(:), model.nbVarX, model.nbData);
+ [A, B] = linSys(x, reshape(u, model.nbVarU, model.nbData-1), model);
+ Su0 = transferMatrices(A, B);
+ Su = Su0(idx,:);
+
+ du = (Su' * Q * Su + R) \ (Su' * Q * (model.Mu(:) - x(:,tl)) - R * u); %Gradient
%Estimate step size with line search method
alpha = 1;
- cost0 = (MuQ - x(:))' * Q * (MuQ - x(:)) + u(:)' * R * u(:);
+ cost0 = (model.Mu(:) - x(:,tl))' * Q * (model.Mu(:) - x(:,tl)) + u' * R * u;
while 1
- utmp = u + delta_u * alpha;
- xtmp = dynSysSimulation(x0, utmp, model);
-% xtmp = reshape(Sx * x0 + Su * utmp(:), model.nbVarX, model.nbData); %Approximated dynamics
-
- cost = (MuQ - xtmp(:))' * Q * (MuQ - xtmp(:)) + utmp(:)' * R * utmp(:); %Compute the cost
+ utmp = u + du * alpha;
+ xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1), model);
+ cost = (model.Mu(:) - xtmp(:,tl))' * Q * (model.Mu(:) - xtmp(:,tl)) + utmp' * R * utmp; %Compute the cost
if cost < cost0 || alpha < 1E-4
break;
end
alpha = alpha * 0.5;
end
-% disp(alpha);
- u = u + delta_u * alpha; %Update control by following gradient
-
-% uSigmaTmp = inv(Su' * Q * Su + R);
-% uSigma = uSigma + uSigmaTmp * alpha^2;
-% xSigma = xSigma + Su * uSigmaTmp * Su' * alpha^2;
-
- %Log data
- r(n).x = x;
- r(n).u = u;
+ u = u + du * alpha; %Update control by following gradient
end
-
-%% Stochastic sampling by exploiting the nullspace structure of the problem
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% %Standard stochastic sampling
-% uSigma = inv(Su' * Q * Su + R);
-% % xSigma = Su * uSigma * Su';
-% [V, D] = eig(full(uSigma));
-% U = real(V * D.^.5);
-% nbSamples = 50;
-% sx = zeros(model.nbData*model.nbVarX, nbSamples);
-% for n=1:nbSamples
-% utmp = u(:) + U * randn(model.nbVarU*(model.nbData-1), 1) * 1E-2;
-% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% sx(:,n) = xtmp(:);
-% 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(Q));
-% 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*model.nbVarX, nbSamples);
-% for n=1:nbSamples
-% w = randn(model.nbVarU, nbRBF) * 2E0; %Random weights
-% un = w * H; %Reconstruction of signals by weighted superposition of basis functions
-% utmp = u(:) + N * un(:);
-% xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1) , model);
-% sx(:,n) = xtmp(:);
-% end
+%Log data
+r.x = x;
+r.u = reshape(u, model.nbVarU, model.nbData-1);
%% Plot state space
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,800,800]); hold on; axis on; grid on; box on;
-% for t=1:model.nbData
-% plotGMM(r(end).x(1:2,t), xSigma(2*(t-1)+[1,2],2*(t-1)+[1,2]).*3E-4, [.8 0 0], .05);
-% end
-
-%iLQR refinement steps
-for k=1:length(r)
- plot(r(k).x(1,:), r(k).x(2,:), '-','linewidth',1,'color',(ones(1,3)-k/length(r))*.9);
-end
-
-% %Stochastic samples
-% for n=1:nbSamples
-% plot(sx(1:model.nbVarX:end,n), sx(2:model.nbVarX:end,n), '-','linewidth',1,'color',(ones(1,3)-n/nbSamples)*.9);
-% end
-
-plot(r(end).x(1,:),r(end).x(2,:), '-','linewidth',2,'color',[.8 0 0]);
-h(1) = plot(r(end).x(1,1),r(end).x(2,1), '.','markersize',30,'color',[0 0 0]);
+plot(r.x(1,:),r.x(2,:), '-','linewidth',2,'color',[.6 .6 .6]);
+h(1) = plot(r.x(1,1),r.x(2,1), '.','markersize',30,'color',[0 0 0]);
h(2) = plot(model.Mu(1), model.Mu(2), '.','markersize',30,'color',[.8 0 0]);
-legend(h,{'Initial state','Target'},'location','southeast','fontsize',20);
+legend(h,{'Initial pose','Target pose'},'location','northwest','fontsize',30);
axis equal; axis([-pi pi -2 5]);
set(gca,'xtick',[-pi, -pi/2, 0, pi/2, pi],'xticklabel',{'-\pi','-\pi/2','0','\pi/2','\pi'},'ytick',0,'fontsize',22);
xlabel('$q$','interpreter','latex','fontsize',30);
@@ -180,13 +111,13 @@ ylabel('$\dot{q}$','interpreter','latex','fontsize',30);
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 kk=1:size(r.u,1)
+% subplot(size(r.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,:))-2, max(r(end).u(kk,:))+2]);
+% plot(t(1:end-1), r.u(kk,:), '-','linewidth',2,'color',[0 0 0]);
+% axis([min(t), max(t(1:end-1)), min(r.u(kk,:))-2, max(r.u(kk,:))+2]);
% xlabel('t'); ylabel(['u_' num2str(kk)]);
% end
@@ -196,7 +127,7 @@ ylabel('$\dot{q}$','interpreter','latex','fontsize',30);
% figure('position',[10 10 700 700],'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(end).x(1,t), model.l, [0; 0; t*.1], .05, [.7 .7 .7]-.7*t/model.nbData);
+% 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');
@@ -210,7 +141,7 @@ ylabel('$\dot{q}$','interpreter','latex','fontsize',30);
% h=[];
% for t=1:model.nbData
% % delete(h);
-% h = plotArm(r(end).x(1,t), model.l, zeros(3,1), .05, [0 0 0]);
+% h = plotArm(r.x(1,t), model.l, zeros(3,1), .05, [0 0 0]);
% drawnow;
% % print('-dpng',['graphs/anim/DDP_pendulum_anim' num2str(t,'%.3d') '.png']);
% end