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 (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; }