int main(int n_args, char** args)
{
string flag;
string directory;
if (n_args>1) {
flag = string(args[1]);
if (string("--help")==flag)
{
usage(args[0]);
return 0;
}
}
Dmp* dmp = NULL;
// Some default values for integration
double dt = 0.004;
int T = 250;
// Some default values for dynamical system
double tau = 0.6;
int dim = 3;
VectorXd y_init(dim);
y_init << 0.5, 0.4, 1.0;
VectorXd y_attr(dim);
y_attr << 0.8, 0.1, 0.1;
if (string("--file")==flag)
{
if (n_args>2) {
// 0 1 2 3 4
// exec --file <input_filename>
// exec --file <input_filename> [output_filename_analytical]
// exec --file <input_filename> [output_filename_analytical] [output_filename_step]
/*
string filename(args[2]);
cout << "Reading DMP from file '" << filename << "'" << endl;
ifstream file;
file.open(filename.c_str());
if (!file.is_open())
{
cerr << __FILE__ << ":" << __LINE__ << ": Can't find file '" << filename << "'. Abort." << endl;
return -1;
}
dmp = new Dmp();
file >> *dmp;
file.ignore(100,'=');
file >> dt;
file.ignore(100,'=');
file >> T;
file.close();
*/
if (n_args>3)
// 0 1 2 3
// exec --file <input_filename> [directory]
directory = string(args[3]);
}
else
{
usage(args[1]);
return -1;
}
}
else
{
if (n_args>1)
// 0 1
// exec [save_directory]
directory = string(args[1]);
}
if (dmp==NULL)
{
Dmp::DmpType dmp_type = Dmp::KULVICIUS_2012_JOINING;
dmp_type = Dmp::IJSPEERT_2002_MOVEMENT;
int input_dim = 1;
int n_basis_functions = 9;
// Dmp was not initialized above. Do so here now.
vector<FunctionApproximator*> function_approximators(dim);
// Initialize one LWR for each dimension
VectorXd offsets = VectorXd::Zero(n_basis_functions);
MatrixXd slopes(dim,n_basis_functions);
slopes << 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 10, 20, 100, -30, -120, 10, 10, -20,
0, 0, 100, 0, 100, 0, 0, 0, 0;
VectorXd centers = VectorXd::LinSpaced(n_basis_functions,0,1);
VectorXd widths = VectorXd::Constant(n_basis_functions,centers[1]-centers[0]);
if (dmp_type==Dmp::IJSPEERT_2002_MOVEMENT)
{
centers = VectorXd::LinSpaced(n_basis_functions,0,tau);
double alpha = 4.0;
centers = (-alpha*(centers/tau)).array().exp();
for (int ii=0; ii<(widths.size()-1); ii++)
widths[ii] = fabs(0.5*(centers[ii+1]-centers[ii]));
widths[widths.size()-1] = widths[widths.size()-2];
slopes = 3*slopes;
}
//double intersection_ratio = 0.5;
for (int dd=0; dd<dim; dd++)
{
VectorXd cur_slopes = slopes.row(dd);
MetaParametersLWR* meta_parameters = new MetaParametersLWR(input_dim,n_basis_functions);
ModelParametersLWR* model_parameters = new ModelParametersLWR(centers,widths,cur_slopes,offsets);
function_approximators[dd] = new FunctionApproximatorLWR(meta_parameters,model_parameters);
}
dmp = new Dmp(tau, y_init, y_attr, function_approximators, dmp_type);
dmp->set_name("testDmp");
}
VectorXd x(dmp->dim(),1);
VectorXd xd(dmp->dim(),1);
VectorXd x_updated(dmp->dim(),1);
dmp->integrateStart(x,xd);
MatrixXd xs_step(T,x.size());
MatrixXd xds_step(T,xd.size());
xs_step.row(0) = x;
xds_step.row(0) = xd;
cout << "** Integrate step-by-step." << endl;
VectorXd ts = VectorXd::Zero(T);
for (int t=1; t<T; t++)
{
dmp->integrateStep(dt,x,x_updated,xd);
x = x_updated;
xs_step.row(t) = x;
xds_step.row(t) = xd;
if (directory.empty())
{
// Not writing to file, output on cout instead.
cout << x.transpose() << " | " << xd.transpose() << endl;
}
ts(t) = t*dt;
}
cout << "** Integrate analytically." << endl;
MatrixXd xs_ana;
MatrixXd xds_ana;
MatrixXd forcing_terms_ana, fa_output_ana;
dmp->analyticalSolution(ts,xs_ana,xds_ana,forcing_terms_ana,fa_output_ana);
if (!directory.empty())
{
cout << "** Write data." << endl;
bool overwrite=true;
MatrixXd output_ana(T,1+xs_ana.cols()+xds_ana.cols());
output_ana << xs_ana, xds_ana, ts;
saveMatrix(directory,"analytical.txt",output_ana,overwrite);
saveMatrix(directory,"forcing_terms_analytical.txt",forcing_terms_ana,overwrite);
saveMatrix(directory,"fa_output_analytical.txt",fa_output_ana,overwrite);
MatrixXd output_step(T,1+xs_ana.cols()+xds_ana.cols());
output_step << xs_step, xds_step, ts;
saveMatrix(directory,"step.txt",output_step,overwrite);
MatrixXd tau_mat(1,1);
tau_mat(0,0) = dmp->tau();
saveMatrix(directory,"tau.txt",tau_mat,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[])
{
// If program has an argument, it is a directory to which to save files too (or --help)
string directory;
if (n_args>1)
{
if (string(args[1]).compare("--help")==0)
{
cout << "Usage: " << args[0] << " [directory] (directory: optional directory to save data to)" << endl;
return 0;
}
else
{
directory = string(args[1]);
}
}
// Make the task
int n_dim=2;
VectorXd viapoint = VectorXd::LinSpaced(n_dim,1.5,2);
double viapoint_time = 0.2;
TaskViapoint* task = new TaskViapoint(viapoint,viapoint_time);
// Some DMP parameters
double tau = 0.6;
VectorXd y_init = VectorXd::Constant(n_dim,1.0);
VectorXd y_attr = VectorXd::Constant(n_dim,3.0);
int n_basis_functions = 4;
VectorXd centers = VectorXd::LinSpaced(n_basis_functions,0,1);
VectorXd widths = VectorXd::Constant(n_basis_functions,0.2);
VectorXd slopes = VectorXd::Zero(n_basis_functions);
VectorXd offsets = VectorXd::Zero(n_basis_functions);
ModelParametersLWR* model_parameters = new ModelParametersLWR(centers,widths,slopes,offsets);
vector<FunctionApproximator*> function_approximators(n_dim);
for (int i_dim=0; i_dim<n_dim; i_dim++)
function_approximators[i_dim] = new FunctionApproximatorLWR(model_parameters);
Dmp* dmp = new Dmp(tau, y_init, y_attr, function_approximators, Dmp::KULVICIUS_2012_JOINING);
// Make the task solver
double integrate_dmp_beyond_tau_factor = 1.25;
double dt=0.01;
set<string> parameters_to_optimize;
parameters_to_optimize.insert("offsets");
TaskSolverParallel* task_solver = new TaskSolverDmp(dmp, parameters_to_optimize, dt, integrate_dmp_beyond_tau_factor);
// Make the initial distribution
vector<VectorXd> mean_init_vec;
dmp->getParameterVectorSelected(mean_init_vec);
vector<DistributionGaussian*> distributions(n_dim);
for (int i_dim=0; i_dim<n_dim; i_dim++)
{
//cout << mean_init_vec[i_dim].transpose() << endl;
VectorXd mean_init = mean_init_vec[i_dim];
MatrixXd covar_init = 1000.0*MatrixXd::Identity(mean_init.size(),mean_init.size());
distributions[i_dim] = new DistributionGaussian(mean_init,covar_init);
}
// Make the parameter updater
double eliteness = 10;
double covar_decay_factor = 0.9;
string weighting_method("PI-BB");
Updater* updater = new UpdaterCovarDecay(eliteness, covar_decay_factor, weighting_method);
// Run the optimization
int n_updates = 40;
int n_samples_per_update = 15;
bool overwrite = true;
runEvolutionaryOptimizationParallel(task, task_solver, distributions, updater, n_updates, n_samples_per_update,directory,overwrite);
}
/** 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;
}
int main(int n_args, char** args)
{
// Generate a trajectory
double tau = 0.5;
int n_time_steps = 51;
VectorXd ts = VectorXd::LinSpaced(n_time_steps,0,tau); // Time steps
int n_dims;
Trajectory trajectory;
bool use_viapoint_traj= false;
if (use_viapoint_traj)
{
n_dims = 1;
VectorXd y_first = VectorXd::Zero(n_dims);
VectorXd y_last = VectorXd::Ones(n_dims);
double viapoint_time = 0.25;
double viapoint_location = 0.5;
VectorXd y_yd_ydd_viapoint = VectorXd::Zero(3*n_dims);
y_yd_ydd_viapoint.segment(0*n_dims,n_dims).fill(viapoint_location); // y
trajectory = Trajectory::generatePolynomialTrajectoryThroughViapoint(ts,y_first,y_yd_ydd_viapoint,viapoint_time,y_last);
}
else
{
n_dims = 2;
VectorXd y_first = VectorXd::LinSpaced(n_dims,0.0,0.7); // Initial state
VectorXd y_last = VectorXd::LinSpaced(n_dims,0.4,0.5); // Final state
trajectory = Trajectory::generateMinJerkTrajectory(ts, y_first, y_last);
}
// Initialize some meta parameters for training LWR function approximator
int n_basis_functions = 3;
int input_dim = 1;
double intersection = 0.56;
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();
// Initialize the DMP
Dmp* dmp = new Dmp(n_dims, function_approximators, Dmp::KULVICIUS_2012_JOINING);
for (int trained=0; trained<=1; trained++)
{
cout << "______________________________________________" << endl;
if (trained==1)
{
cout << "Training Dmp..." << endl;
dmp->train(trajectory);
}
// create and open a character archive for output
std::string filename("/tmp/dmp_");
filename += to_string(trained)+".xml";
std::ofstream ofs(filename);
boost::archive::xml_oarchive oa(ofs);
oa << boost::serialization::make_nvp("dmp",dmp);
ofs.close();
std::ifstream ifs(filename);
boost::archive::xml_iarchive ia(ifs);
Dmp* dmp_out;
ia >> BOOST_SERIALIZATION_NVP(dmp_out);
ifs.close();
cout << "___________________________________________" << endl;
cout << " filename=" << filename << endl;
cout << *dmp << endl;
cout << *dmp_out << endl;
delete dmp_out;
}
delete meta_parameters;
delete fa_lwr;
delete dmp;
return 0;
}
