Commit 25347db8 authored by Martin Karlsson's avatar Martin Karlsson
Browse files

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.
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
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
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
%% 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]')
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
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