Commit b88bb44e authored by Sylvain Calinon's avatar Sylvain Calinon

Addition of Kp and Kv from L in demos (for analysis)

parent 4d61d33e
......@@ -158,13 +158,15 @@ for n=1:nbRepros
end
xlabel('t'); ylabel('|Kp|');
%Plot accelerations due to feedback and feedforward terms
figure; hold on;
n=1; k=1;
plot(r(n).FB(k,:),'r-','linewidth',2);
plot(r(n).FF(k,:),'b-','linewidth',2);
legend('ddx feedback','ddx feedforward');
xlabel('t'); ylabel(['ddx_' num2str(k)]);
% %Plot accelerations due to feedback and feedforward terms
% figure; hold on;
% n=1; k=1;
% plot(r(n).FB(k,:),'r-','linewidth',2);
% plot(r(n).FF(k,:),'b-','linewidth',2);
% legend('ddx feedback','ddx feedforward');
% xlabel('t'); ylabel(['ddx_' num2str(k)]);
%pause;
%close all;
......@@ -158,13 +158,15 @@ for n=1:nbRepros
end
xlabel('t'); ylabel('|Kp|');
%Plot accelerations due to feedback and feedforward terms
figure; hold on;
n=1; k=1;
plot(r(n).FB(k,:),'r-','linewidth',2);
plot(r(n).FF(k,:),'b-','linewidth',2);
legend('ddx feedback','ddx feedforward');
xlabel('t'); ylabel(['ddx_' num2str(k)]);
% %Plot accelerations due to feedback and feedforward terms
% figure; hold on;
% n=1; k=1;
% plot(r(n).FB(k,:),'r-','linewidth',2);
% plot(r(n).FF(k,:),'b-','linewidth',2);
% legend('ddx feedback','ddx feedforward');
% xlabel('t'); ylabel(['ddx_' num2str(k)]);
%pause;
%close all;
......@@ -80,17 +80,21 @@ for t=1:nbData
%Compute acceleration (with both feedback and feedforward terms)
ddx = -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf
r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
r.FF(:,t) = M(:,t);
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
%Log data
%Log data (with additional variables collected for analysis purpose)
r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx);
%r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
%r.FF(:,t) = M(:,t);
%r.Kp(:,:,t) = L(:,1:nbVarOut,t);
%r.Kv(:,:,t) = L(:,nbVarOut+1:end,t);
r.kpDet(t) = det(L(:,1:nbVarOut,t));
r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
%Note that if [V,D] = eigs(L(:,1:nbVarOut)), we have L(:,nbVarOut+1:end) = V * (2*D).^.5 * V'
end
......
......@@ -67,15 +67,18 @@ for t=1:nbData
%Compute acceleration (with feedback and feedforward terms)
ddx = -L * [x-r.currTar(:,t); dx] + M; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
r.FF(:,t) = M;
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
%Log data
%Log data (with additional variables collected for analysis purpose)
r.Data(:,t) = [DataIn(:,t); x];
r.ddxNorm(t) = norm(ddx);
%r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
%r.FF(:,t) = M;
%r.Kp(:,:,t) = L(:,1:nbVarOut);
%r.Kv(:,:,t) = L(:,nbVarOut+1:end);
r.kpDet(t) = det(L(:,1:nbVarOut));
r.kvDet(t) = det(L(:,nbVarOut+1:end));
%Note that if [V,D] = eigs(L(:,1:nbVarOut)), we have L(:,nbVarOut+1:end) = V * (2*D).^.5 * V'
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment