Commit cd97191b authored by Martin Karlsson's avatar Martin Karlsson
Browse files

plots upsampled

parent 6e6213e0
......@@ -9,7 +9,7 @@ traj = max(0,-sin(2*pi*t(1:round(2*end/5))/2.5)); traj = [traj ; traj(end)*ones(
% Determine DMP from demonstration:
T_end = length(traj)*dt;
tau =T_end/3; % Yields 95% convergence at the deomnstration time
tau =T_end/3; % Yields 95% convergence at the demonstration time
P = length(traj); % Number of time steps
w = traj2w(traj,dt, tau, c, D, alpha_z, beta_z, alpha_x, n_kernel); % Basis function weights. Used to determine f.
g = traj(end); % DMP goal point.
......@@ -59,7 +59,8 @@ for t = 2:2*P
% Stopping perturbation
ya_dot = ya_dot + ya_ddot*dt;
if (t > 500 && t < 750)
ya_dot = 0;
ya_ddot_perturbation = -25*ya_dot;
ya_dot = ya_dot + ya_ddot_perturbation*dt;
end
ya(t) = ya(t-1) + ya_dot*dt;
......@@ -73,14 +74,14 @@ t = cumsum(dt*ones(size(y)));
% Plot results
figure
subplot(211)
plot(t(1:10:end),ya(1:10:end),'b-',t(1:10:end),y(1:10:end),'r--', t(1:10:end),y_unpert(1:10:end),'k-.','LineWidth',2)
plot(t,ya,'b-',t,y,'r--', t,y_unpert,'k-.','LineWidth',2)
legend('y_a','y_c','y_u')
axis([0 8 -.2 1])
xlabel('Time [s]')
ylabel('Position [m]')
subplot(212)
plot(t(1:10:end),ya_ddot_log(1:10:end),'b-',t(1:10:end),y_ddot_log(1:10:end),'r--', t(1:10:end),yddot_unpert(1:10:end),'k-.','LineWidth',2)
plot(t,ya_ddot_log,'b-',t,y_ddot_log,'r--', t,yddot_unpert,'k-.','LineWidth',2)
legend('ÿ_a','ÿ_c','ÿ_u')
axis([0 8 -20 20])
xlabel('Time [s]')
......@@ -111,7 +112,8 @@ for t = 2:2*P
% Moving perturbation
ya_dot = ya_dot + ya_ddot*dt;
if (t > 500 && t < 750)
ya_dot = .15;
ya_ddot_perturbation = (.30-ya_dot)*25;
ya_dot = ya_dot + ya_ddot_perturbation*dt;
end
ya(t) = ya(t-1) + ya_dot*dt;
......@@ -125,14 +127,14 @@ t = cumsum(dt*ones(size(y)));
% Plot results
figure
subplot(211)
plot(t(1:10:end),ya(1:10:end),'b-',t(1:10:end),y(1:10:end),'r--', t(1:10:end),y_unpert(1:10:end),'k-.','LineWidth',2)
plot(t,ya,'b-',t,y,'r--', t,y_unpert,'k-.','LineWidth',2)
legend('y_a','y_c','y_u')
axis([0 8 -.2 1.5])
xlabel('Time [s]')
ylabel('Position [m]')
subplot(212)
plot(t(1:10:end),ya_ddot_log(1:10:end),'b-',t(1:10:end),y_ddot_log(1:10:end),'r--', t(1:10:end),yddot_unpert(1:10:end),'k-.','LineWidth',2)
plot(t,ya_ddot_log,'b-',t,y_ddot_log,'r--', t,yddot_unpert,'k-.','LineWidth',2)
legend('ÿ_a','ÿ_c','ÿ_u')
axis([0 8 -20 20])
xlabel('Time [s]')
......
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