From 925541cd098ed21969459e3146dd16d7e7cac24d Mon Sep 17 00:00:00 2001
From: Martin Karlsson <cont-mkr@ulund.org>
Date: Wed, 22 Mar 2017 09:04:01 +0100
Subject: [PATCH] minor bug fixed
---
dmp2vel_acc_ss.m | 8 ++++----
main.m | 4 ++--
2 files changed, 6 insertions(+), 6 deletions(-)
diff --git a/dmp2vel_acc_ss.m b/dmp2vel_acc_ss.m
index 35b8f8b..0fc91f4 100644
--- a/dmp2vel_acc_ss.m
+++ b/dmp2vel_acc_ss.m
@@ -1,4 +1,4 @@
-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
diff --git a/main.m b/main.m
index c368c6c..b0edfa8 100644
--- a/main.m
+++ b/main.m
@@ -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;
--
GitLab