plots upsampled

main.m

@@ -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]')