/** 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)
{
if (n_args<3 || n_args>5)
{
help(args[0]);
return -1;
}
if (string(args[1]).compare("--help")==0)
{
help(args[0]);
return 0;
}
string dmp_filename = string(args[1]);
string traj_filename = string(args[2]);
string sample_filename = "";
if (n_args>3)
sample_filename = string(args[3]);
string dmp_output_filename = "";
if (n_args>4)
dmp_output_filename = string(args[4]);
cout << "C++ | Executing ";
for (int ii=0; ii<n_args; ii++) cout << " " << args[ii];
cout << endl;
// Read dmp from xml file
cout << "C++ | Reading dmp from file '" << dmp_filename << "'" << endl;
std::ifstream ifs(dmp_filename);
boost::archive::xml_iarchive ia(ifs);
Dmp* dmp;
ia >> BOOST_SERIALIZATION_NVP(dmp);
ifs.close();
cout << "C++ | " << *dmp << endl;
// Read sample file, if necessary
if (!sample_filename.empty())
{
cout << "C++ | Reading sample from file '" << sample_filename << "'" << endl;
VectorXd sample;
if (!loadMatrix(sample_filename, sample))
{
cerr << "C++ | WARNING: Could not read sample file. Executing default DMP instead." << endl;
}
else
{
// Set DMP parameters to sample
dmp->setParameterVectorSelected(sample);
// Save dmp whose parameters have been perturbed, if necessary
if (!dmp_output_filename.empty())
{
cout << "C++ | Saving dmp to file '" << dmp_output_filename << "'" << endl;
std::ofstream ofs(dmp_output_filename);
boost::archive::xml_oarchive oa(ofs);
oa << boost::serialization::make_nvp("dmp",dmp);
ofs.close();
}
}
}
// Integrate DMP longer than the tau with which it was trained
double integration_time = 1.5*dmp->tau();
double frequency_Hz = 100.0;
cout << "C++ | Integrating dmp for " << integration_time << "s at " << (int)frequency_Hz << "Hz" << endl;
int n_time_steps = floor(frequency_Hz*integration_time);
VectorXd ts = VectorXd::LinSpaced(n_time_steps,0,integration_time); // Time steps
// Save trajectory
cout << "C++ | Saving trajectory to file '" << traj_filename << "'" << endl;
Trajectory trajectory;
dmp->analyticalSolution(ts,trajectory);
// Now we have the end-effector trajectory. Compute the ball trajectory.
MatrixXd y_endeff = trajectory.ys();
MatrixXd yd_endeff = trajectory.yds();
MatrixXd ydd_endeff = trajectory.ydds();
MatrixXd y_ball(n_time_steps,2);
MatrixXd yd_ball(n_time_steps,2);
MatrixXd ydd_ball(n_time_steps,2);
double dt = 1.0/frequency_Hz;
bool ball_in_hand = true;
for (int ii=0; ii<n_time_steps; ii++)
{
if (ball_in_hand)
{
// If the ball is in your hand, it moves along with your hand
y_ball.row(ii) = y_endeff.row(ii);
yd_ball.row(ii) = yd_endeff.row(ii);
ydd_ball.row(ii) = ydd_endeff.row(ii);
if (ts(ii)>0.6)
{
// Release the ball to throw it!
ball_in_hand = false;
}
}
else // ball_in_hand is false => ball is flying through the air
{
ydd_ball(ii,0) = 0.0;
ydd_ball(ii,1) = -9.81; // Gravity
// Euler integration
yd_ball.row(ii) = yd_ball.row(ii-1) + dt*ydd_ball.row(ii);
y_ball.row(ii) = y_ball.row(ii-1) + dt*yd_ball.row(ii);
if (y_ball(ii,1)<-0.3)
{
// Ball hits the floor (floor is at -0.3)
y_ball(ii,1) = -0.3;
yd_ball.row(ii) = VectorXd::Zero(2);
ydd_ball.row(ii) = VectorXd::Zero(2);
}
}
//if x(t_i-1,BALL_IN_CUP)
// % If the ball is in the cup, it does not move
// x(t_i,BALL_X:BALL_Y) = x(t_i-1,BALL_X:BALL_Y);
// x(t_i,BALL_IN_CUP) = 1; % Once in the cup, always in the cup
//
//else
//
// if x(t_i,HOLD_BALL)
// % If the ball is in your hand, it moves along with your hand
// x(t_i,BALL_X:BALL_Y) = x(t_i,REF_X:REF_Y);
// x(t_i,BALL_XD) = diff(x([t_i-1 t_i],BALL_X))/dt;
// x(t_i,BALL_YD) = diff(x([t_i-1 t_i],BALL_Y))/dt;
//
// else
// % If the ball is not in your hand, it simply falls
// x(t_i,BALL_XDD) = 0;
// x(t_i,BALL_YDD) = -g;
//
// % Euler integration
// x(t_i,BALL_XD:BALL_YD) = x(t_i-1,BALL_XD:BALL_YD) + dt*x(t_i,BALL_XDD:BALL_YDD);
// x(t_i,BALL_X:BALL_Y) = x(t_i-1,BALL_X:BALL_Y) + dt*x(t_i,BALL_XD:BALL_YD);
//
// end
}
trajectory.set_misc(y_ball);
bool overwrite = true;
trajectory.saveToFile(traj_filename, overwrite);
delete dmp;
return 0;
}
/** 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;
}
