diff --git a/demos/demo_OC_DDP_bicopter01.m b/demos/demo_OC_DDP_bicopter01.m
index 18f0863b0cf461a13d6f6abd408d61594c89c105..883d7001e133564b1c3059d143c8f2f49b946f09 100644
--- a/demos/demo_OC_DDP_bicopter01.m
+++ b/demos/demo_OC_DDP_bicopter01.m
@@ -65,21 +65,21 @@ x0 = zeros(model.nbVarX, 1);
for n=1:model.nbIter
%System evolution
x = dynSysSimulation(x0, reshape(u, model.nbVarU, model.nbData-1), model);
-
%Linearization
[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
-
+ %Gradient
+ e = model.Mu - x(:,tl);
+ du = (Su' * Q * Su + R) \ (Su' * Q * e(:) - R * u);
%Estimate step size with line search method
alpha = 1;
- cost0 = (model.Mu(:) - x(:,tl))' * Q * (model.Mu(:) - x(:,tl)) + u' * R * u;
+ cost0 = e(:)' * Q * e(:) + u' * R * u;
while 1
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
+ xtmp = dynSysSimulation(x0, reshape(utmp, model.nbVarU, model.nbData-1), model);
+ etmp = model.Mu - xtmp(:,tl);
+ cost = etmp(:)' * Q * etmp(:) + utmp' * R * utmp; %Compute the cost
if cost < cost0 || alpha < 1E-4
break;
end
@@ -126,9 +126,9 @@ xlabel('x_1'); ylabel('x_2');
% end
-% %% Plot animated robot
+% %% Plot robot (animated)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% figure('position',[10,10,1000,1000]); hold on; axis off;
+% figure('position',[10,10,800,800]); hold on; axis off;
% plot(model.Mu(1), model.Mu(2), '.','markersize',40,'color',[.8 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]);
diff --git a/demos/demo_OC_DDP_car01.m b/demos/demo_OC_DDP_car01.m
index 4a375c2e8057178831dd43003fe26cf520b93fae..a215a14268c2c23f1fd7e0304639455edc0a4b9e 100644
--- a/demos/demo_OC_DDP_car01.m
+++ b/demos/demo_OC_DDP_car01.m
@@ -60,21 +60,21 @@ x0 = zeros(model.nbVarX, 1);
for n=1:model.nbIter
%System evolution
x = dynSysSimulation(x0, reshape(u, model.nbVarU, model.nbData-1), model);
-
%Linearization
[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
-
+ %Gradient
+ e = model.Mu - x(:,tl);
+ du = (Su' * Q * Su + R) \ (Su' * Q * e(:) - R * u);
%Estimate step size with line search method
alpha = 1;
- cost0 = (model.Mu(:) - x(:,tl))' * Q * (model.Mu(:) - x(:,tl)) + u' * R * u;
+ cost0 = e(:)' * Q * e(:) + u' * R * u;
while 1
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
+ etmp = model.Mu - xtmp(:,tl);
+ cost = etmp(:)' * Q * etmp(:) + utmp' * R * utmp; %Compute the cost
if cost < cost0 || alpha < 1E-4
break;
end
@@ -129,7 +129,7 @@ axis equal; axis([-1 5 -1 4]);
% end
-% %% Plot animated robot
+% %% Plot robot (animated)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure('position',[10,10,1000,1000]); hold on; axis off;
% %Target pose
diff --git a/demos/demo_OC_DDP_cartpole01.m b/demos/demo_OC_DDP_cartpole01.m
index 1aa10a2523331c5276762b0cb72d36f1e9e07ab0..4c577d2ac6df7e7f7845786d36842f08f571a05a 100644
--- a/demos/demo_OC_DDP_cartpole01.m
+++ b/demos/demo_OC_DDP_cartpole01.m
@@ -67,21 +67,21 @@ x0 = zeros(model.nbVarX, 1);
for n=1:model.nbIter
%System evolution
x = dynSysSimulation(x0, reshape(u, model.nbVarU, model.nbData-1), model);
-
%Linearization
[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
-
+ %Gradient
+ e = model.Mu - x(:,tl);
+ du = (Su' * Q * Su + R) \ (Su' * Q * e(:) - R * u);
%Estimate step size with line search method
alpha = 1;
- cost0 = (model.Mu(:) - x(:,tl))' * Q * (model.Mu(:) - x(:,tl)) + u' * R * u;
+ cost0 = e(:)' * Q * e(:) + u' * R * u;
while 1
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
+ etmp = model.Mu - xtmp(:,tl);
+ cost = etmp(:)' * Q * etmp(:) + utmp' * R * utmp; %Compute the cost
if cost < cost0 || alpha < 1E-4
break;
end
@@ -121,16 +121,25 @@ xlabel('x'); ylabel('\theta');
% xlabel('t'); ylabel(['u_' num2str(kk)]);
% end
+%% Plot robot (static)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[820 10 800 400],'color',[1 1 1]); hold on; axis off;
+plotArm(model.Mu(2)-pi/2, model.L, [0;0;-1], .1, [.8 0 0]);
+for t=floor(linspace(1, model.nbData, 30))
+ plotArm(r.x(2,t)-pi/2, model.L, [r.x(1,t); 0; t*.1], .1, [.7 .7 .7]-.7*t/model.nbData);
+end
+axis equal; %axis([-4.2 4.2 -1.2 1.2]*2);
+
-% %% Plot animated robot
+% %% Plot robot (animated)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure('position',[10 10 2200 700],'color',[1 1 1]); hold on; axis off;
-% plotAR(model.Mu(2)-pi/2, model.L, [0;0;-1], .1, [.8 0 0]);
+% plotArm(model.Mu(2)-pi/2, model.L, [0;0;-1], .1, [.8 0 0]);
% axis equal; axis([-4.2 4.2 -1.2 1.2]*2);
% h=[];
% for t=1:model.nbData
% delete(h);
-% h = plotAR(r.x(2,t)-pi/2, model.L, [r.x(1,t);0;0], .1, [0 0 0]);
+% h = plotArm(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_manipulator01.m b/demos/demo_OC_DDP_manipulator01.m
index ec726a6129313fde823db20a98dbdbe52e9ef370..601454c7969177518965a6b66eabbe7f52ce26b8 100644
--- a/demos/demo_OC_DDP_manipulator01.m
+++ b/demos/demo_OC_DDP_manipulator01.m
@@ -42,7 +42,7 @@ model.nbVarU = 3; %Control space dimension (dq1,dq2,dq3)
model.nbVarF = 3; %Objective function dimension (x1,x2,o)
model.l = [2, 2, 1]; %Robot links lengths
model.sz = [.2, .3]; %Size of objects
-model.r = 1E-4; %Control weight term
+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
@@ -51,7 +51,8 @@ for t=1:model.nbPoints
end
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 = speye(model.nbVarF * model.nbPoints) * 1E0; %Precision matrix
+% Q = kron(eye(model.nbPoints), diag([1E0, 1E0, 0])); %Precision matrix (by removing orientation constraint)
%Time occurrence of viapoints
tl = linspace(1, model.nbData, model.nbPoints+1);
diff --git a/demos/demo_OC_DDP_pendulum01.m b/demos/demo_OC_DDP_pendulum01.m
index 90f6790ea9b0c2c3d5707d8c249b3dd2edc92123..43e195d104f75b570697da55089e0820c1c9a7a2 100644
--- a/demos/demo_OC_DDP_pendulum01.m
+++ b/demos/demo_OC_DDP_pendulum01.m
@@ -65,21 +65,21 @@ x0 = [-pi/2; 0];
for n=1:model.nbIter
%System evolution
x = dynSysSimulation(x0, reshape(u, model.nbVarU, model.nbData-1), model);
-
%Linearization
[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
-
+ %Gradient
+ e = model.Mu - x(:,tl);
+ du = (Su' * Q * Su + R) \ (Su' * Q * e(:) - R * u);
%Estimate step size with line search method
alpha = 1;
- cost0 = (model.Mu(:) - x(:,tl))' * Q * (model.Mu(:) - x(:,tl)) + u' * R * u;
+ cost0 = e(:)' * Q * e(:) + u' * R * u;
while 1
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
+ etmp = model.Mu - xtmp(:,tl);
+ cost = etmp(:)' * Q * etmp(:) + utmp' * R * utmp; %Compute the cost
if cost < cost0 || alpha < 1E-4
break;
end
@@ -122,18 +122,18 @@ ylabel('$\dot{q}$','interpreter','latex','fontsize',30);
% end
-% %% Plot robot (static)
-% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% 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.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 animated robot
+% %% Plot robot (animated)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
% plotArm(model.Mu(1), model.l, [0;0;-1], .05, [.8 0 0]);