tau_adapt=tau*(1+(kc*e^2));% Adaptive time parameter
ya_ddot=get_ya_ddot_lowgain_ff(ya(t-1),ya_dot,y(t-1),ydot,yddot);% Get reference accceleration for actual trajectory
[ydot,yddot]=dmp2vel_acc_ss(y0,y(t-1),g,tau_adapt,w,x,dt,alpha_e,c,D,alpha_z,beta_z,ya(t-1),e,kc);% Get coupled acceleration and velocit,y given DMP and states
[ydot,yddot]=dmp2vel_acc_ss(y0,y(t-1),g,tau_adapt,w,x,dt,alpha_e,c,D,alpha_z,beta_z,ya(t-1),e,kc,tau);% Get coupled acceleration and velocit,y given DMP and states
y(t)=y(t-1)+ydot*dt;
xdot=-alpha_x*x/tau_adapt;
x=x+xdot*dt;
...
...
@@ -103,7 +103,7 @@ y_ddot_log = zeros(size(y));
fort=2:2*P
tau_adapt=tau*(1+(kc*e^2));% Adaptive time parameter
ya_ddot=get_ya_ddot_lowgain_ff(ya(t-1),ya_dot,y(t-1),ydot,yddot);% Get reference accceleration for actual trajectory
[ydot,yddot]=dmp2vel_acc_ss(y0,y(t-1),g,tau_adapt,w,x,dt,alpha_e,c,D,alpha_z,beta_z,ya(t-1),e,kc);% Get coupled acceleration and velocit,y given DMP and states
[ydot,yddot]=dmp2vel_acc_ss(y0,y(t-1),g,tau_adapt,w,x,dt,alpha_e,c,D,alpha_z,beta_z,ya(t-1),e,kc,tau);% Get coupled acceleration and velocit,y given DMP and states