예제 #1
0
int main(int n_args, char* args[])
{
  string directory;
  if (n_args>1)
    directory = string(args[1]);
  
  int n_dims = 1;

  VectorXd viapoint = VectorXd::Constant(n_dims,0.5);
  double viapoint_time = 0.5;
  TaskViapoint task = TaskViapoint(viapoint, viapoint_time);
  
  MatrixXd task_parameters = MatrixXd(1,n_dims);

  int n_time_steps = 51;
  VectorXd ts = VectorXd::LinSpaced(n_time_steps,0,1);
  
    
  int n_demos = 9;
  VectorXd task_parameter_values = VectorXd::LinSpaced(n_demos,-0.4,1.4);
  
  
  Trajectory demonstration;
  for (int i_demo=0; i_demo<n_demos; i_demo++)
  {
    task_parameters.fill(task_parameter_values[i_demo]);
    task.generateDemonstration(task_parameters, ts, demonstration);
  
    cout <<  fixed << setw(1) << setprecision(3);
    if (directory.empty())
    {
      cout << "______________________" << endl;
      cout << task_parameters << endl<< endl;
      cout << demonstration << endl;
    } 
    else
    {
      stringstream stream;
      stream << directory << "/demonstration" << setw(2) << setfill('0') << i_demo << ".txt";
      string filename = stream.str();
 
      cout << "Saving to " << filename << endl;

      std::ofstream outfile;
      outfile.open(filename.c_str()); 
      outfile << demonstration;
      outfile.close();
      
      stringstream stream2;
      stream2 << directory << "/task_parameters" << setw(2) << setfill('0') << i_demo << ".txt";
      filename = stream2.str();
      
      cout << "Saving to " << filename << endl;
      
      outfile.open(filename.c_str()); 
      outfile << task_parameters;
      outfile.close();

      stringstream stream3;
      stream3 << directory << "/viapoint_time" << setw(2) << setfill('0') << i_demo << ".txt";
      filename = stream3.str();
      
      cout << "Saving to " << filename << endl;
      
      outfile.open(filename.c_str()); 
      outfile << viapoint_time;
      outfile.close();
      
    }
  }

  if (!directory.empty())
  {
    cout << endl << "Plot results in Matlab as follows:" << endl << endl;
    cout << "clf; for dd=0:"<<n_demos-1<<", data = load(sprintf('"<<directory<<"demonstration%02d.txt',dd)); tp = load(sprintf('"<<directory<<"task_parameters%02d.txt',dd)); viapoint_time = load(sprintf('"<<directory<<"viapoint_time%02d.txt',dd)); for ss=1:3, subplot(1,3,ss); plot(data(:,1),data(:,ss+1)'); hold on; if (ss==1), plot(viapoint_time,tp,'ro'); end; axis square; axis tight; end; end" << endl;
  }
  
  
}
예제 #2
0
int main(int n_args, char* args[])
{
  string directory;
  if (n_args>1)
    directory = string(args[1]);
  
  int n_dims = 1;

  /*
  // Test writing/reading with txt files
  VectorXd viapoint_io = VectorXd::LinSpaced(n_dims,0.0,0.1);
  VectorXd goal_io = VectorXd::LinSpaced(n_dims,1.0,1.1);
  TaskViapoint task_io = TaskViapoint(viapoint_io, 0.2, 0.3, goal_io, 0.4, 0.5, 0.6, 0.7);
  string filename = "/tmp/task_viapoint.txt";
  cout << task_io.toString() << endl;
  task_io.writeToFile(filename);
  TaskViapoint task_read = TaskViapoint::readFromFile(filename);
  cout << task_read.toString() << endl;
  return 0;
  */

  VectorXd viapoint = VectorXd::Constant(n_dims,0.5);
  double viapoint_time = 0.5;
  TaskViapoint task = TaskViapoint(viapoint, viapoint_time);
  
  MatrixXd task_parameters = MatrixXd(1,n_dims);

  int n_time_steps = 51;
  VectorXd ts = VectorXd::LinSpaced(n_time_steps,0,1);
  
    
  int n_demos = 9;
  VectorXd task_parameter_values = VectorXd::LinSpaced(n_demos,-0.4,1.4);
  
  
  Trajectory demonstration;
  for (int i_demo=0; i_demo<n_demos; i_demo++)
  {
    task_parameters.fill(task_parameter_values[i_demo]);
    task.generateDemonstration(task_parameters, ts, demonstration);
  
    cout <<  fixed << setw(1) << setprecision(3);
    if (directory.empty())
    {
      cout << "______________________" << endl;
      cout << task_parameters << endl<< endl;
      cout << demonstration << endl;
    } 
    else
    {
      stringstream stream;
      stream << directory << "/demonstration" << setw(2) << setfill('0') << i_demo << ".txt";
      string filename = stream.str();
 
      cout << "Saving to " << filename << endl;

      std::ofstream outfile;
      outfile.open(filename.c_str()); 
      outfile << demonstration;
      outfile.close();
      
      stringstream stream2;
      stream2 << directory << "/task_parameters" << setw(2) << setfill('0') << i_demo << ".txt";
      filename = stream2.str();
      
      cout << "Saving to " << filename << endl;
      
      outfile.open(filename.c_str()); 
      outfile << task_parameters;
      outfile.close();

      stringstream stream3;
      stream3 << directory << "/viapoint_time" << setw(2) << setfill('0') << i_demo << ".txt";
      filename = stream3.str();
      
      cout << "Saving to " << filename << endl;
      
      outfile.open(filename.c_str()); 
      outfile << viapoint_time;
      outfile.close();
      
    }
  }

  if (!directory.empty())
  {
    cout << endl << "Plot results in Matlab as follows:" << endl << endl;
    cout << "clf; for dd=0:"<<n_demos-1<<", data = load(sprintf('"<<directory<<"demonstration%02d.txt',dd)); tp = load(sprintf('"<<directory<<"task_parameters%02d.txt',dd)); viapoint_time = load(sprintf('"<<directory<<"viapoint_time%02d.txt',dd)); for ss=1:3, subplot(1,3,ss); plot(data(:,1),data(:,ss+1)'); hold on; if (ss==1), plot(viapoint_time,tp,'ro'); end; axis square; axis tight; end; end" << endl;
  }
  
  /*
  SERIALIZATION DISABLED IN DMP_BBO MODULE
    // create and open a character archive for output
    std::string filename_xml("/tmp/task_viapoint.xml");
  
    std::ofstream ofs(filename_xml);
    boost::archive::xml_oarchive oa(ofs);
    oa << boost::serialization::make_nvp("task",task);
    ofs.close();
  
    std::ifstream ifs(filename_xml);
    boost::archive::xml_iarchive ia(ifs);
    TaskViapoint* task_out;
    ia >> BOOST_SERIALIZATION_NVP(task_out);
    ifs.close();
    
    cout << "___________________________________________" << endl;
    cout << "  filename_xml=" << filename_xml << endl;
    cout << task << endl;
    cout << task_out << endl;
*/

    
}