void DmpWithGainSchedules::train(const Trajectory& trajectory, std::string save_directory, bool overwrite)
{
// First, train the DMP
Dmp::train(trajectory,save_directory,overwrite);
// Get phase from trajectory
// Integrate analytically to get phase states
MatrixXd xs_ana;
MatrixXd xds_ana;
Dmp::analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
MatrixXd xs_phase = xs_ana.PHASEM(trajectory.length());
// Get targets from trajectory
MatrixXd targets = trajectory.misc();
// The dimensionality of the extra variables in the trajectory must be the same as the number of
// function approximators.
assert(targets.cols()==(int)function_approximators_gains_.size());
// Train each fa_gains, see below
// Some checks before training function approximators
assert(!function_approximators_gains_.empty());
for (unsigned int dd=0; dd<function_approximators_gains_.size(); dd++)
{
// This is just boring stuff to figure out if and where to store the results of training
string save_directory_dim;
if (!save_directory.empty())
{
if (function_approximators_gains_.size()==1)
save_directory_dim = save_directory;
else
save_directory_dim = save_directory + "/gains" + to_string(dd);
}
// Actual training is happening here.
VectorXd cur_target = targets.col(dd);
if (function_approximators_gains_[dd]==NULL)
{
cerr << __FILE__ << ":" << __LINE__ << ":";
cerr << "WARNING: function approximator cannot be trained because it is NULL." << endl;
}
else
{
if (function_approximators_gains_[dd]->isTrained())
function_approximators_gains_[dd]->reTrain(xs_phase,cur_target,save_directory_dim,overwrite);
else
function_approximators_gains_[dd]->train(xs_phase,cur_target,save_directory_dim,overwrite);
}
}
}
void Trajectory::append(const Trajectory& trajectory)
{
assert(dim() == trajectory.dim());
assert(ts_[length() - 1] == trajectory.ts()[0]);
if (ys_.row(length() - 1).isZero() || trajectory.ys().row(0).isZero())
assert(ys_.row(length() - 1).isZero() && trajectory.ys().row(0).isZero());
else
assert(ys_.row(length() - 1).isApprox(trajectory.ys().row(0)));
if (yds_.row(length() - 1).isZero() || trajectory.yds().row(0).isZero())
assert(yds_.row(length() - 1).isZero() && trajectory.yds().row(0).isZero());
else
assert(yds_.row(length() - 1).isApprox(trajectory.yds().row(0)));
if (ydds_.row(length() - 1).isZero() || trajectory.ydds().row(0).isZero())
assert(ydds_.row(length() - 1).isZero() && trajectory.ydds().row(0).isZero());
else
assert(ydds_.row(length() - 1).isApprox(trajectory.ydds().row(0)));
int new_size = length() + trajectory.length() - 1;
VectorXd new_ts(new_size);
new_ts << ts_, trajectory.ts().segment(1, trajectory.length() - 1);
ts_ = new_ts;
MatrixXd new_ys(new_size, dim());
new_ys << ys_, trajectory.ys().block(1, 0, trajectory.length() - 1, dim());
ys_ = new_ys;
MatrixXd new_yds(new_size, dim());
new_yds << yds_, trajectory.yds().block(1, 0, trajectory.length() - 1, dim());
yds_ = new_yds;
MatrixXd new_ydds(new_size, dim());
new_ydds << ydds_, trajectory.ydds().block(1, 0, trajectory.length() - 1, dim());
ydds_ = new_ydds;
assert(dim_misc() == trajectory.dim_misc());
if (dim_misc()==0)
{
misc_.resize(new_size,0);
}
else
{
MatrixXd new_misc(new_size, dim_misc());
new_misc << misc_, trajectory.misc().block(1, 0, trajectory.length() - 1, dim_misc());
misc_ = new_misc;
}
}