Commit 3247ca2d authored by Martin Karlsson's avatar Martin Karlsson
Browse files

minor changes, added README

parent 6876d7fc
......@@ -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
*/
......@@ -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;;
......
Supports Markdown
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