diff --git a/dmp.cc b/dmp.cc index dcf109ab44a91fabc3e313126a368052d7f7d932..6009a7c192836b69878b2dc9541f9406f049199a 100644 --- a/dmp.cc +++ b/dmp.cc @@ -66,8 +66,9 @@ void Dmp::resetState() { g = zeros<mat>(nDims,1); } - -mat Dmp::getVel(const mat& posIn, const double& dt) { +// Modified version of a part of the DMP software at +//www-clmc.usc.edu/Resources/Software +mat Dmp::getVel(const mat& posIn, const double& dt) { mat pos = posIn.t(); if (firstSample) { g = pos; @@ -97,29 +98,3 @@ mat Dmp::getVel(const mat& posIn, const double& dt) { } -/* -for j = 1:7 - psi = exp(-0.5*((x(j)-c).^2).*D); - f = sum(v(j)*w(:,j).*psi)/sum(psi+1.e-10); - - - - % The dynamical systems: - vd = (alpha_v*(beta_v*(0-x(j))-v(j)))*tau; - xd = v(j)*tau; - - zd = (alpha_z*(beta_z*(g(j)-yact(j))-z(j))+f)*tau; - - yd = z(j)*tau; - - gd = alpha_g*(G(j)-g(j)); - - % State update: - x(j) = xd*dt+x(j); - v(j) = vd*dt+v(j); - z(j) = zd*dt+z(j); - g(j) = gd*dt+g(j); - yrefd(j) = yd; -end -*/ - diff --git a/traj2dmp.cc b/traj2dmp.cc index efc056b61583da861b65e8752b3665036bb64a9b..290b58c102992cf3a58bee6552191bcc13039ed3 100644 --- a/traj2dmp.cc +++ b/traj2dmp.cc @@ -12,6 +12,10 @@ mat smartDiff(const mat& inMat) { //now supports col vecs only } + + +// Modified version of a part of the DMP software at +//www-clmc.usc.edu/Resources/Software Dmp traj2dmp(const mat& trajectory, const double& dt) { int nbrDims = trajectory.n_cols; // Number of dimensions double tau;;