コード例 #1
0
void DmpWithGainSchedules::train(const Trajectory& trajectory, std::string save_directory, bool overwrite)
{
  // First, train the DMP
  Dmp::train(trajectory,save_directory,overwrite);
  
  // Get phase from trajectory
  // Integrate analytically to get phase states
  MatrixXd xs_ana;
  MatrixXd xds_ana;
  Dmp::analyticalSolution(trajectory.ts(),xs_ana,xds_ana);
  MatrixXd xs_phase  = xs_ana.PHASEM(trajectory.length());


  // Get targets from trajectory
  MatrixXd targets = trajectory.misc();
  
  // The dimensionality of the extra variables in the trajectory must be the same as the number of
  // function approximators.
  assert(targets.cols()==(int)function_approximators_gains_.size());
  
  // Train each fa_gains, see below
  // Some checks before training function approximators
  assert(!function_approximators_gains_.empty());
  
  for (unsigned int dd=0; dd<function_approximators_gains_.size(); dd++)
  {
    // This is just boring stuff to figure out if and where to store the results of training
    string save_directory_dim;
    if (!save_directory.empty())
    {
      if (function_approximators_gains_.size()==1)
        save_directory_dim = save_directory;
      else
        save_directory_dim = save_directory + "/gains" + to_string(dd);
    }
    
    // Actual training is happening here.
    VectorXd cur_target = targets.col(dd);
    if (function_approximators_gains_[dd]==NULL)
    {
      cerr << __FILE__ << ":" << __LINE__ << ":";
      cerr << "WARNING: function approximator cannot be trained because it is NULL." << endl;
    }
    else
    {
      if (function_approximators_gains_[dd]->isTrained())
        function_approximators_gains_[dd]->reTrain(xs_phase,cur_target,save_directory_dim,overwrite);
      else
        function_approximators_gains_[dd]->train(xs_phase,cur_target,save_directory_dim,overwrite);
    }

  }
}
コード例 #2
0
ファイル: Trajectory.cpp プロジェクト: humm/dovecot
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;
    }
}