C++ Trajectory::saveToFile方法代码示例

//.........这里部分代码省略.........
      // 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;
//.........这里部分代码省略.........