KUKADU_SHARED_PTR<Dmp> GeneralDmpLearner::fitTrajectories() {
int dataPointsNum = joints.n_rows;
vec g(degFreedom);
vec y0(degFreedom);
vec dy0(degFreedom);
vec ddy0(degFreedom);
vector<vec> dmpCoeffs;
vector<vec> sampleYs;
vector<vec> fitYs;
vector<mat> designMatrices;
vec timeVec = joints.col(0);
mat all_y;
mat all_dy;
mat all_ddy;
// retrieve all columns for different degrees of freedom
vector<vec> trajectories;
for(int i = 1; i <= degFreedom; ++i) {
vec trajectory = joints.col(i);
trajectories.push_back(trajectory);
vec vec_dy = computeDiscreteDerivatives(timeVec, trajectory);
vec vec_ddy = computeDiscreteDerivatives(timeVec, vec_dy);
all_y = join_rows(all_y, trajectory);
all_dy = join_rows(all_dy, vec_dy);
all_ddy = join_rows(all_ddy, vec_ddy);
}
vector<trajectory_learner_internal> dmpResAll = fitTrajectory(timeVec, all_y, all_dy, all_ddy);
for(int i = 0; i < dmpResAll.size(); ++i) {
trajectory_learner_internal dmpRes = dmpResAll.at(i);
vec dmpCoeff = dmpRes.coeff;
vec fity = dmpRes.fity;
g(i) = (all_y.col(i))(dataPointsNum - 1);
y0(i) = all_y.col(i)(0);
dy0(i) = all_dy.col(i)(0);
ddy0(i) = all_ddy.col(i)(0);
dmpCoeffs.push_back(dmpCoeff);
fitYs.push_back(fity);
designMatrices.push_back(dmpRes.desMat);
}
for (int i = 0; i < all_y.n_cols; ++i) sampleYs.push_back(all_y.col(i));
return createDmpInstance(timeVec, sampleYs, fitYs, dmpCoeffs, dmpBase, designMatrices, tau, az, bz, ax);
}
void Skel_OPB_Comp::calc_buf(INT ** output,U_INT1 *** input)
{
if (first_line_in_pack())
{
for (INT d =0; d < dim_in() ; d++)
{
U_INT1 ** l = input[d];
for (int y = y0Buf() ; y < y1Buf() ; y++)
convert
(
_im_init[y-y0Buf()],
l[y]+x0Buf(),
x1Buf()-x0Buf()
);
Skeleton
(
_skel[d],
_im_init,
_sz.x,
_sz.y,
_larg
);
}
}
for (INT d =0; d < dim_in() ; d++)
convert
(
output[d]+x0(),
_skel[d][y_in_pack()-dy0()]-dx0(),
tx()
);
if (_AvecDist)
convert
(
output[1]+x0(),
_im_init[y_in_pack()-dy0()]-dx0(),
tx()
);
}
scalar liform_surf(int n, double *wt, Func<scalar> *u_ext[], Func<double> *v, Geom<double> *e, ExtData<scalar> *ext)
{
cplx ii = cplx(0.0, 1.0);
scalar result = 0;
for (int i = 0; i < n; i++) {
scalar3 dx0(0, 0, 0), dy0(0, 0, 0), dz0(0, 0, 0);
scalar3 ev0(0.0, 0.0, 0.0);
ev0 = exact(e->x[i], e->y[i], e->z[i], dx0, dy0, dz0);
scalar curl_e[3];
scalar dx[3], dy[3], dz[3];
dx[0] = dx0[0]; dx[1] = dx0[1]; dx[2] = dx0[2];
dy[0] = dy0[0]; dy[1] = dy0[1]; dy[2] = dy0[2];
dz[0] = dz0[0]; dz[1] = dz0[1]; dz[2] = dz0[2];
calc_curl(dx, dy, dz, curl_e);
scalar tpe[3];
scalar ev[3];
ev[0] = ev0[0]; ev[1] = ev0[1]; ev[2] = ev0[2];
calc_tan_proj(e->nx[i], e->ny[i], e->nz[i], ev, tpe);
scalar g[3] = {
(e->nz[i] * curl_e[1] - e->ny[i] * curl_e[2]) - ii * kappa * tpe[0],
(e->nx[i] * curl_e[2] - e->nz[i] * curl_e[0]) - ii * kappa * tpe[1],
(e->ny[i] * curl_e[0] - e->nx[i] * curl_e[1]) - ii * kappa * tpe[2],
};
// tpv is tangencial projection of v (test function)
scalar vv[3] = { v->val0[i], v->val1[i], v->val2[i] };
scalar tpv[3];
calc_tan_proj(e->nx[i], e->ny[i], e->nz[i], vv, tpv);
result += wt[i] * (g[0] * tpv[0] + g[1] * tpv[1] + g[2] * tpv[2]);
}
return result;
}
