Commit 925541cd authored by Martin Karlsson's avatar Martin Karlsson
Browse files

minor bug fixed

parent 03f1d63d
function [ydot, yddot] = dmp2vel_acc_ss(y0, y, g, tau, w, x, dt, alpha_e, c, D, alpha_z, beta_z,ya,e,kc)
function [ydot, yddot] = dmp2vel_acc_ss(y0, y, g, tau_adapt, w, x, dt, alpha_e, c, D, alpha_z, beta_z,ya,e,kc,tau)
persistent z
if isempty(z)
......@@ -7,9 +7,9 @@ end
psi = exp(-0.5*((x-c).^2).*D);
f = sum(w'*psi)/sum(psi+1.e-10) * x * (g-y0);
z_dot = 1/tau * (alpha_z*(beta_z*(g-y) - z) + f);
z_dot = 1/tau_adapt * (alpha_z*(beta_z*(g-y) - z) + f);
z = z + dt*z_dot;
ydot = z/tau;
yddot = (z_dot*tau-z*2*kc*e*(alpha_e*(ya-y-e)))/tau^2;
ydot = z/tau_adapt;
yddot = (z_dot*tau_adapt-tau*z*2*kc*e*(alpha_e*(ya-y-e)))/tau_adapt^2;
end
......@@ -51,7 +51,7 @@ y_ddot_log = zeros(size(y));
for t = 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
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));
for t = 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
y(t) = y(t-1) + ydot*dt;
xdot = -alpha_x*x/tau_adapt;
x = x + xdot*dt;
......
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