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

void DmpWithGainSchedules::analyticalSolution(const Eigen::VectorXd& ts, Trajectory& trajectory) const
{
  Eigen::MatrixXd xs,  xds;
  Dmp::analyticalSolution(ts, xs, xds);
  statesAsTrajectory(ts, xs, xds, trajectory);

  // add output fa_gains as misc variables
  MatrixXd fa_gains;
  computeFunctionApproximatorOutputExtendedDimensions(xs.PHASEM(xs.rows()), fa_gains); 
  trajectory.set_misc(fa_gains);
}

Trajectory getDemoTrajectory(const VectorXd& ts)
{
  bool use_viapoint_traj= false;
  Trajectory trajectory;
  int n_dims=0;
  if (use_viapoint_traj)
  {
    n_dims = 1;
    VectorXd y_first = VectorXd::Zero(n_dims);
    VectorXd y_last  = VectorXd::Ones(n_dims);
    double viapoint_time = 0.25;
    double viapoint_location = 0.5;
  
    VectorXd y_yd_ydd_viapoint = VectorXd::Zero(3*n_dims);
    y_yd_ydd_viapoint.segment(0*n_dims,n_dims).fill(viapoint_location); // y         
    trajectory = Trajectory::generatePolynomialTrajectoryThroughViapoint(ts,y_first,y_yd_ydd_viapoint,viapoint_time,y_last); 
  }
  else
  {
    n_dims = 2;
    VectorXd y_first = VectorXd::LinSpaced(n_dims,0.0,0.7); // Initial state
    VectorXd y_last  = VectorXd::LinSpaced(n_dims,0.4,0.5); // Final state
    trajectory =  Trajectory::generateMinJerkTrajectory(ts, y_first, y_last);
  }
  
  // Generated trajectory, now generate extra dimensions in width
  MatrixXd misc(ts.rows(),n_dims);
  for (int ii=0; ii<misc.rows(); ii++)
  {
    misc(ii,0) = (1.0*ii)/misc.rows();
    if (n_dims>1)
      misc(ii,1) = sin((8.0*ii)/misc.rows());
  }
  
  trajectory.set_misc(misc);
  
  return trajectory;
}

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