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;
}
}
void Dmp::computeFunctionApproximatorInputsAndTargets(const Trajectory& trajectory, VectorXd& fa_inputs_phase, MatrixXd& f_target) const
{
int n_time_steps = trajectory.length();
double dim_data = trajectory.dim();
if (dim_orig()!=dim_data)
{
cout << "WARNING: Cannot train " << dim_orig() << "-D DMP with " << dim_data << "-D data. Doing nothing." << endl;
return;
}
// Integrate analytically to get goal, gating and phase states
MatrixXd xs_ana;
MatrixXd xds_ana;
// Before, we would make clone of the dmp, and integrate it with the tau, and initial/attractor
// state of the trajectory. However, Thibaut needed to call this from outside the Dmp as well,
// with the tau/states of the this object. Therefore, we no longer clone.
// Dmp* dmp_clone = static_cast<Dmp*>(this->clone());
// dmp_clone->set_tau(trajectory.duration());
// dmp_clone->set_initial_state(trajectory.initial_y());
// dmp_clone->set_attractor_state(trajectory.final_y());
// dmp_clone->analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
MatrixXd xs_goal = xs_ana.GOALM(n_time_steps);
MatrixXd xs_gating = xs_ana.GATINGM(n_time_steps);
MatrixXd xs_phase = xs_ana.PHASEM(n_time_steps);
fa_inputs_phase = xs_phase;
// Get parameters from the spring-dampers system to compute inverse
double damping_coefficient = spring_system_->damping_coefficient();
double spring_constant = spring_system_->spring_constant();
double mass = spring_system_->mass();
if (mass!=1.0)
{
cout << "WARNING: Usually, spring-damper system of the DMP should have mass==1, but it is " << mass << endl;
}
// Compute inverse
f_target = tau()*tau()*trajectory.ydds() + (spring_constant*(trajectory.ys()-xs_goal) + damping_coefficient*tau()*trajectory.yds())/mass;
//Factor out gating term
for (unsigned int dd=0; dd<function_approximators_.size(); dd++)
f_target.col(dd) = f_target.col(dd).array()/xs_gating.array();
}
/** Main function
* \param[in] n_args Number of arguments
* \param[in] args Arguments themselves
* \return Success of exection. 0 if successful.
*/
int main(int n_args, char** args)
{
string save_directory;
if (n_args!=2)
{
cerr << "Usage: " << args[0] << " <directory>" << endl;
return -1;
}
save_directory = string(args[1]);
// GENERATE A TRAJECTORY
double tau = 0.5;
int n_time_steps = 51;
VectorXd ts = VectorXd::LinSpaced(n_time_steps,0,tau); // Time steps
Trajectory trajectory = getDemoTrajectory(ts); // getDemoTrajectory() is implemented below main()
int n_dims = trajectory.dim();
// MAKE THE FUNCTION APPROXIMATORS
// Initialize some meta parameters for training LWR function approximator
int n_basis_functions = 25;
int input_dim = 1;
double intersection = 0.5;
MetaParametersLWR* meta_parameters = new MetaParametersLWR(input_dim,n_basis_functions,intersection);
FunctionApproximatorLWR* fa_lwr = new FunctionApproximatorLWR(meta_parameters);
// Clone the function approximator for each dimension of the DMP
vector<FunctionApproximator*> function_approximators(n_dims);
for (int dd=0; dd<n_dims; dd++)
function_approximators[dd] = fa_lwr->clone();
// CONSTRUCT AND TRAIN THE DMP
cout << "** Initialize DMP." << endl;
// Initialize the DMP
Dmp::DmpType dmp_type = Dmp::KULVICIUS_2012_JOINING;
//dmp_type = Dmp::IJSPEERT_2002_MOVEMENT;
Dmp* dmp_tmp = new Dmp(n_dims, function_approximators, dmp_type);
cout << "** Initialize DmpWithGainSchedules." << endl;
int n_gains = trajectory.dim_misc();
// Clone the function approximator for each extra dimension of the DMP
vector<FunctionApproximator*> function_approximators_gains(n_gains);
for (int dd=0; dd<n_gains; dd++)
function_approximators_gains[dd] = fa_lwr->clone();
DmpWithGainSchedules* dmp_gains = new DmpWithGainSchedules(dmp_tmp,function_approximators_gains);
cout << "** Train DmpWithGainSchedules." << endl;
// And train it. Passing the save_directory will make sure the results are saved to file.
bool overwrite = true;
dmp_gains->train(trajectory,save_directory,overwrite);
// INTEGRATE DMP TO GET REPRODUCED TRAJECTORY
cout << "** Integrate DMP analytically." << endl;
Trajectory traj_reproduced;
tau = 0.9;
n_time_steps = 91;
ts = VectorXd::LinSpaced(n_time_steps,0,tau); // Time steps
dmp_gains->analyticalSolution(ts,traj_reproduced);
// Integrate again, but this time get more information
MatrixXd xs_ana, xds_ana, forcing_terms_ana, fa_output_ana, fa_gains;
dmp_gains->analyticalSolution(ts,xs_ana,xds_ana,forcing_terms_ana,fa_output_ana,fa_gains);
// WRITE THINGS TO FILE
trajectory.saveToFile(save_directory,"demonstration_traj.txt",overwrite);
traj_reproduced.saveToFile(save_directory,"reproduced_traj.txt",overwrite);
MatrixXd output_ana(ts.size(),1+xs_ana.cols()+xds_ana.cols());
output_ana << ts, xs_ana, xds_ana;
saveMatrix(save_directory,"reproduced_ts_xs_xds.txt",output_ana,overwrite);
saveMatrix(save_directory,"reproduced_forcing_terms.txt",forcing_terms_ana,overwrite);
saveMatrix(save_directory,"reproduced_fa_output.txt",fa_output_ana,overwrite);
saveMatrix(save_directory,"reproduced_fa_gains.txt",fa_gains,overwrite);
// INTEGRATE STEP BY STEP
cout << "** Integrate DMP step-by-step." << endl;
VectorXd x(dmp_gains->dim(),1);
VectorXd xd(dmp_gains->dim(),1);
VectorXd x_updated(dmp_gains->dim(),1);
VectorXd gains(dmp_gains->dim_gains(),1);
MatrixXd xs_step(n_time_steps,x.size());
MatrixXd xds_step(n_time_steps,xd.size());
MatrixXd gains_all(n_time_steps,gains.size());
cout << std::setprecision(3) << std::fixed << std::showpos;
double dt = ts[1];
dmp_gains->integrateStart(x,xd,gains);
xs_step.row(0) = x;
xds_step.row(0) = xd;
gains_all.row(0) = gains;
for (int t=1; t<n_time_steps; t++)
{
dmp_gains->integrateStep(dt,x,x_updated,xd,gains);
x = x_updated;
xs_step.row(t) = x;
xds_step.row(t) = xd;
gains_all.row(t) = gains;
if (save_directory.empty())
{
// Not writing to file, output on cout instead.
//cout << x.transpose() << " | " << xd.transpose() << endl;
}
}
MatrixXd output_step(ts.size(),1+xs_ana.cols()+xds_ana.cols()+gains_all.cols());
output_step << ts, xs_step, xds_step, gains_all;
saveMatrix(save_directory,"reproduced_step_ts_xs_xds_gains.txt",output_step,overwrite);
delete meta_parameters;
delete fa_lwr;
delete dmp_gains;
return 0;
}