reproduction_DS.m 1.39 KB
Newer Older
Sylvain Calinon's avatar
Sylvain Calinon committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37
function r = reproduction_DS(DataIn, model, r, currPos)
% Reproduction with a virtual spring-damper system with constant impedance parameters 
%
% Author: Sylvain Calinon, 2014
%         http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite  
% the following reference in any academic publication that uses this code or part of it: 
%
% @inproceedings{Calinon14ICRA,
%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
%   title="A task-parameterized probabilistic model with minimal intervention control",
%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
%   year="2014",
%   month="May-June",
%   address="Hong Kong, China",
%   pages="3339--3344"
% }

nbData = size(DataIn,2);
nbVarOut = model.nbVar - size(DataIn,1);

%% Reproduction with constant impedance parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
  L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
  %Compute acceleration
  ddx =  -L * [x-r.currTar(:,t); dx];
  %Update velocity and position
  dx = dx + ddx * model.dt;
  x = x + dx * model.dt;
  %Log data
  r.Data(:,t) = [DataIn(:,t); x]; 
  r.ddxNorm(t) = norm(ddx);
  r.kpDet(t) = det(L(:,1:nbVarOut));
38
  r.kvDet(t) = det(L(:,nbVarOut+1:end));
Sylvain Calinon's avatar
Sylvain Calinon committed
39 40 41 42
end