Commit 25347db8 by Martin Karlsson

### first commit

parents
 Matlab simulation code written by Martin Karlsson, Dept. Automatic Control, Lund University, Lund, Sweden. martin.karlsson@control.lth.se Code example for the scientific paper "Two-Degree-of-Freedom Control for Trajectory Tracking and Perturbation Recovery during Execution of Dynamical Movement Primitives" (IFAC 2017) by Martin Karlsson, Fredrik Bagge Carlson, Anders Robertsson and Rolf Johansson. The main.m file produces two plots: one with a stopping perturbation, and one with a moving perturbation.
dmp2acc.m 0 → 100644
 function [ yddot] = dmp2acc(y0, y, ydot, g, tau, w, x, c, D, alpha_z, beta_z) psi = exp(-0.5*((x-c).^2).*D); f = sum(w'*psi)/sum(psi+1.e-10) * x * (g-y0); yddot = 1/tau^2 * (alpha_z*(beta_z*(g-y) - tau*ydot) + f); end
dmp2vel_acc_ss.m 0 → 100644
 function [ydot, yddot] = dmp2vel_acc_ss(y0, y, g, tau, w, x, dt, alpha_e, c, D, alpha_z, beta_z,ya,e,kc) persistent z if isempty(z) z = 0; 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 = z + dt*z_dot; ydot = z/tau; yddot = (z_dot*tau-z*2*kc*e*(alpha_e*(ya-y-e)))/tau^2; end
dmpParams.m 0 → 100644
 alpha_e = 5; alpha_z = 25; alpha_x = 1; beta_z = 6.25; n_kernel = 15; arr = (0:1/(n_kernel-1):1)'*0.5; ct = (0:1/(n_kernel-1):1)'; cx = exp(-alpha_x*3*ct); c = cx; kc = 10000; D = (diff(c)*0.55).^2; D = 1./[D;D(end)]; \ No newline at end of file
 function [ ya_ddot ] = get_ya_ddot_lowgain_ff(ya, ya_dot, y, ydot, yddot) kp = 25; kv = 10; ya_ddot = kp*(y-ya) + kv*(ydot-ya_dot) + yddot; end
main.m 0 → 100644
 %% Preliminaries clc; close all; clear all dmpParams; % Simulate demonstrated trajectory: dt = 1/250; t = linspace(0,10,10/dt)'; traj = max(0,-sin(2*pi*t(1:round(2*end/5))/2.5)); traj = [traj ; traj(end)*ones(50,1)]; % Determine DMP from demonstration: T_end = length(traj)*dt; tau =T_end/3; % Yields 95% convergence at the deomnstration 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. %% Simulate unperturbed DMP %Initialize variables x = 1; % Phase parameter ydot = 0; y = zeros(size(traj)); y(1) = traj(1); y0 = y(1); yddot_unpert = zeros(size(traj)); % Simulate over time t for t = 2:2*P yddot = dmp2acc(y0,y(t-1),ydot,g,tau, w, x, c, D, alpha_z, beta_z); % Get acceleration given DMP and states ydot = ydot + yddot*dt; y(t) = y(t-1) + ydot*dt; xdot = -alpha_x*x/tau; x = x + xdot*dt; yddot_unpert(t) = yddot; end y_unpert = y; %% Simulate DMP execution with stopping perturbation %Initialize variables x = 1; % Phase variable y = zeros(size(traj)); y(1) = traj(1); % Coupled trajectory y0 = y(1); % Start position ya = y; % Actual trajectory ya_dot = 0; e = 0; % Low-pass filtered error state ya_ddot_log = zeros(size(y)); y_ddot_log = zeros(size(y)); % Simulate over time t 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 y(t) = y(t-1) + ydot*dt; xdot = -alpha_x*x/tau_adapt; x = x + xdot*dt; % Stopping perturbation ya_dot = ya_dot + ya_ddot*dt; if (t > 500 && t < 750) ya_dot = 0; end ya(t) = ya(t-1) + ya_dot*dt; e_dot = alpha_e*(ya(t)-y(t)-e); e = e + e_dot*dt; ya_ddot_log(t) = ya_ddot; y_ddot_log(t) = yddot; end 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) 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) legend('ÿ_a','ÿ_c','ÿ_u') axis([0 8 -20 20]) xlabel('Time [s]') ylabel('Acceleration [m/s^2]') %% Simulate DMP execution with moving perturbation % Initialize variables x = 1; % Phase variable ydot = 0; yddot = 0; y = zeros(size(traj)); y(1) = traj(1); y0 = y(1); ya = y; ya_dot = 0; e = 0; % Low-pass filtered error state ya_ddot_log = zeros(size(y)); 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 y(t) = y(t-1) + ydot*dt; xdot = -alpha_x*x/tau_adapt; x = x + xdot*dt; % Moving perturbation ya_dot = ya_dot + ya_ddot*dt; if (t > 500 && t < 750) ya_dot = .15; end ya(t) = ya(t-1) + ya_dot*dt; e_dot = alpha_e*(ya(t)-y(t)-e); e = e + e_dot*dt; ya_ddot_log(t) = ya_ddot; y_ddot_log(t) = yddot; end 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) 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) legend('ÿ_a','ÿ_c','ÿ_u') axis([0 8 -20 20]) xlabel('Time [s]') ylabel('Acceleration [m/s^2]')
traj2w.m 0 → 100644
 function w = traj2w(y_demo, dt, tau, c, D, alpha_z, beta_z, alpha_x, n_kernel) P = length(y_demo); % Nbr time steps g = y_demo(end); y0 = y_demo(1); ydot_demo = [diff(y_demo); 0]/dt; yddot_demo = [diff(ydot_demo); 0]/dt; f_target = tau^2*yddot_demo -alpha_z*(beta_z*(g-y_demo) -tau*ydot_demo); x = zeros(size(y_demo)); x(1) = 1; for t=2:P x_dot = -alpha_x*x(t-1)/tau; x(t) = x(t-1) + x_dot*dt; end psi = zeros(P, n_kernel); for t = 1:P exp(-0.5*((x(t)-c).^2).*D) psi(t,:) = exp(-0.5*((x(t)-c).^2).*D); psi(t,:) end s = x*(g-y0); w = zeros(n_kernel,1); for i = 1:n_kernel Gamma_i = diag(psi(:,i)); w(i) = s'*Gamma_i * f_target / (s'*Gamma_i*s); end
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!