void TaskSolverDmp::performRollouts(const vector<MatrixXd>& samples, const MatrixXd& task_parameters, MatrixXd& cost_vars) const
{
// n_dofs-D Dmp, n_parallel=n_dofs
// vector<Matrix> Matrix
// samples = n_dofs x n_samples x sum(nmodel_parameters_)
// task_parameters = n_samples x n_task_pars
// cost_vars = n_samples x (n_time_steps*n_cost_vars)
int n_dofs = samples.size();
assert(n_dofs>0);
assert(n_dofs==dmp_->dim_orig());
int n_samples = samples[0].rows();
for (int dd=1; dd<n_dofs; dd++)
{
assert(samples[dd].rows()==n_samples);
}
VectorXd ts = VectorXd::LinSpaced(n_time_steps_,0.0,integrate_time_);
int n_cost_vars = 4*n_dofs+1;
cost_vars.resize(n_samples,n_time_steps_*n_cost_vars);
vector<VectorXd> model_parameters_vec(n_dofs);
for (int k=0; k<n_samples; k++)
{
for (int dd=0; dd<n_dofs; dd++)
model_parameters_vec[dd] = samples[dd].row(k);
dmp_->setParameterVectorSelected(model_parameters_vec, use_normalized_parameter_);
int n_dims = dmp_->dim(); // Dimensionality of the system
MatrixXd xs_ana(n_time_steps_,n_dims);
MatrixXd xds_ana(n_time_steps_,n_dims);
MatrixXd forcing_terms(n_time_steps_,n_dofs);
forcing_terms.fill(0.0);
dmp_->analyticalSolution(ts,xs_ana,xds_ana,forcing_terms);
/*
// Here's the non-analytical version, which is slower.
MatrixXd xs(n_time_steps_,n_dims);
MatrixXd xds(n_time_steps_,n_dims);
// Use integrateStart to get the initial x and xd
VectorXd x(n_dims);
VectorXd xd(n_dims);
dmp_->integrateStart(x,xd);
xs.row(0) = x.transpose();
xds.row(0) = xd.transpose();
// Use DynamicalSystemSystem::integrateStep to integrate numerically step-by-step
double dt = ts[1]-ts[0];
for (int ii=1; ii<n_time_steps_; ii++)
{
dmp_->integrateStep(dt,x, x, xd);
// previous x updated x updated xd
xs.row(ii) = x.transpose();
xds.row(ii) = xd.transpose();
}
MatrixXd ys;
MatrixXd yds;
MatrixXd ydds;
dmp_->statesAsTrajectory(xs,xds,ys,yds,ydds);
*/
MatrixXd ys_ana;
MatrixXd yds_ana;
MatrixXd ydds_ana;
dmp_->statesAsTrajectory(xs_ana,xds_ana,ys_ana,yds_ana,ydds_ana);
int offset = 0;
for (int tt=0; tt<n_time_steps_; tt++)
{
cost_vars.block(k,offset,1,n_dofs) = ys_ana.row(tt); offset += n_dofs;
cost_vars.block(k,offset,1,n_dofs) = yds_ana.row(tt); offset += n_dofs;
cost_vars.block(k,offset,1,n_dofs) = ydds_ana.row(tt); offset += n_dofs;
cost_vars.block(k,offset,1,1) = ts.row(tt); offset += 1;
cost_vars.block(k,offset,1,n_dofs) = forcing_terms.row(tt); offset += n_dofs;
}
}
}
