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

void Trajectory::append(const Trajectory& trajectory)
{
    assert(dim() == trajectory.dim());

    assert(ts_[length() - 1] == trajectory.ts()[0]);

    if (ys_.row(length() - 1).isZero() || trajectory.ys().row(0).isZero())
        assert(ys_.row(length() - 1).isZero() && trajectory.ys().row(0).isZero());
    else
        assert(ys_.row(length() - 1).isApprox(trajectory.ys().row(0)));

    if (yds_.row(length() - 1).isZero() || trajectory.yds().row(0).isZero())
        assert(yds_.row(length() - 1).isZero() && trajectory.yds().row(0).isZero());
    else
        assert(yds_.row(length() - 1).isApprox(trajectory.yds().row(0)));

    if (ydds_.row(length() - 1).isZero() || trajectory.ydds().row(0).isZero())
        assert(ydds_.row(length() - 1).isZero() && trajectory.ydds().row(0).isZero());
    else
        assert(ydds_.row(length() - 1).isApprox(trajectory.ydds().row(0)));

    int new_size = length() + trajectory.length() - 1;

    VectorXd new_ts(new_size);
    new_ts << ts_, trajectory.ts().segment(1, trajectory.length() - 1);
    ts_ = new_ts;

    MatrixXd new_ys(new_size, dim());
    new_ys << ys_, trajectory.ys().block(1, 0, trajectory.length() - 1, dim());
    ys_ = new_ys;

    MatrixXd new_yds(new_size, dim());
    new_yds << yds_, trajectory.yds().block(1, 0, trajectory.length() - 1, dim());
    yds_ = new_yds;

    MatrixXd new_ydds(new_size, dim());
    new_ydds << ydds_, trajectory.ydds().block(1, 0, trajectory.length() - 1, dim());
    ydds_ = new_ydds;

    assert(dim_misc() == trajectory.dim_misc());
    if (dim_misc()==0)
    {
        misc_.resize(new_size,0);
    }
    else
    {
        MatrixXd new_misc(new_size, dim_misc());
        new_misc << misc_, trajectory.misc().block(1, 0, trajectory.length() - 1, dim_misc());
        misc_ = new_misc;
    }
}

void Dmp::computeFunctionApproximatorInputsAndTargets(const Trajectory& trajectory, VectorXd& fa_inputs_phase, MatrixXd& f_target) const
{
  int n_time_steps = trajectory.length();
  double dim_data = trajectory.dim();
  
  if (dim_orig()!=dim_data)
  {
    cout << "WARNING: Cannot train " << dim_orig() << "-D DMP with " << dim_data << "-D data. Doing nothing." << endl;
    return;
  }
  
  // Integrate analytically to get goal, gating and phase states
  MatrixXd xs_ana;
  MatrixXd xds_ana;
  
  // Before, we would make clone of the dmp, and integrate it with the tau, and initial/attractor
  // state of the trajectory. However, Thibaut needed to call this from outside the Dmp as well,
  // with the tau/states of the this object. Therefore, we no longer clone. 
  // Dmp* dmp_clone = static_cast<Dmp*>(this->clone());
  // dmp_clone->set_tau(trajectory.duration());
  // dmp_clone->set_initial_state(trajectory.initial_y());
  // dmp_clone->set_attractor_state(trajectory.final_y());
  // dmp_clone->analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
  analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
  MatrixXd xs_goal   = xs_ana.GOALM(n_time_steps);
  MatrixXd xs_gating = xs_ana.GATINGM(n_time_steps);
  MatrixXd xs_phase  = xs_ana.PHASEM(n_time_steps);
  
  fa_inputs_phase = xs_phase;
  
  // Get parameters from the spring-dampers system to compute inverse
  double damping_coefficient = spring_system_->damping_coefficient();
  double spring_constant     = spring_system_->spring_constant();
  double mass                = spring_system_->mass();
  if (mass!=1.0)
  {
    cout << "WARNING: Usually, spring-damper system of the DMP should have mass==1, but it is " << mass << endl;
  }

  // Compute inverse
  f_target = tau()*tau()*trajectory.ydds() + (spring_constant*(trajectory.ys()-xs_goal) + damping_coefficient*tau()*trajectory.yds())/mass;
  
  //Factor out gating term
  for (unsigned int dd=0; dd<function_approximators_.size(); dd++)
    f_target.col(dd) = f_target.col(dd).array()/xs_gating.array();
 
}

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