C++ Trajectory类代码示例

Trajectory Trajectory::generatePolynomialTrajectoryThroughViapoint(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_yd_ydd_viapoint, double viapoint_time, const VectorXd& y_to)
{

    int n_dims = y_from.size();
    assert(n_dims==y_to.size());
    assert(3*n_dims==y_yd_ydd_viapoint.size()); // Contains y, yd and ydd, so *3

    int n_time_steps = ts.size();

    int viapoint_time_step = 0;
    while (viapoint_time_step<n_time_steps && ts[viapoint_time_step]<viapoint_time)
        viapoint_time_step++;

    if (viapoint_time_step>=n_time_steps)
    {
        cerr << __FILE__ << ":" << __LINE__ << ":";
        cerr << "ERROR: the time vector does not contain any time smaller than " << viapoint_time << ". Returning min-jerk trajectory WITHOUT viapoint." <<  endl;
        return Trajectory();
    }

    VectorXd yd_from        = VectorXd::Zero(n_dims);
    VectorXd ydd_from       = VectorXd::Zero(n_dims);

    VectorXd y_viapoint     = y_yd_ydd_viapoint.segment(0*n_dims,n_dims);
    VectorXd yd_viapoint    = y_yd_ydd_viapoint.segment(1*n_dims,n_dims);
    VectorXd ydd_viapoint   = y_yd_ydd_viapoint.segment(2*n_dims,n_dims);

    VectorXd yd_to          = VectorXd::Zero(n_dims);
    VectorXd ydd_to         = VectorXd::Zero(n_dims);

    Trajectory traj = Trajectory::generatePolynomialTrajectory(ts.segment(0, viapoint_time_step + 1), y_from, yd_from, ydd_from, y_viapoint, yd_viapoint, ydd_viapoint);
    traj.append(Trajectory::generatePolynomialTrajectory(ts.segment(viapoint_time_step, n_time_steps - viapoint_time_step), y_viapoint, yd_viapoint, ydd_viapoint, y_to, yd_to, ydd_to));

    return traj;
}

void XTCReader::read(Trajectory &trajectory) {
  while (true) {
    SmartPointer<Positions> positions = new Positions;
    if (!read(*positions, trajectory.getTopology().get())) break;
    trajectory.add(positions);
  }
}

void CalibrateAction::visualize_trajectory(Trajectory &traj)
{
    // publishing to rviz
    geometry_msgs::PoseArray poses;
    for(unsigned int i=0; i<traj.getPointsSize();i++)
    {
        geometry_msgs::Pose temp_pose;
        double x_, y_,th_;
        traj.getPoint(i, x_, y_, th_);
        tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
        temp_pose.position.x = x_;
        temp_pose.position.y = y_;
        poses.poses.push_back(temp_pose);
        //ROS_INFO("Pose: x: %f y: %f th: %f", x_, y_, th_);
    }
    geometry_msgs::Pose temp_pose, temp_pose2;
    double x_, y_,th_;
    traj.getPoint(0, x_, y_, th_);
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose.orientation);
    temp_pose.position.x = x_;
    temp_pose.position.y = y_;
    traj.getEndpoint(x_, y_, th_);
    tf::quaternionTFToMsg(tf::createQuaternionFromYaw(th_),temp_pose2.orientation);
    temp_pose2.position.x = x_;
    temp_pose2.position.y = y_;
    //ROS_INFO("Visualize trajectory with %i points. First Point (%2.1f,%2.1f), last Point (%2.1f,%2.1f)", traj.getPointsSize(),temp_pose.position.x,temp_pose.position.y,temp_pose2.position.x,temp_pose2.position.y);
    poses.header.frame_id = cost_map->getGlobalFrameID();;
    estTraj_pub.publish(poses);
}

void Dmp::train(const Trajectory& trajectory, string save_directory, bool overwrite)
{
  
  // Set tau, initial_state and attractor_state from the trajectory 
  set_tau(trajectory.duration());
  set_initial_state(trajectory.initial_y());
  set_attractor_state(trajectory.final_y());
  
  VectorXd fa_input_phase;
  MatrixXd f_target;
  computeFunctionApproximatorInputsAndTargets(trajectory, fa_input_phase, f_target);
  
  // Some checks before training function approximators
  assert(!function_approximators_.empty());
  
  for (unsigned int dd=0; dd<function_approximators_.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_.size()==1)
        save_directory_dim = save_directory;
      else
        save_directory_dim = save_directory + "/dim" + boost::lexical_cast<string>(dd);
    }
    
    // Actual training is happening here.
    VectorXd fa_target = f_target.col(dd);
    if (function_approximators_[dd]->isTrained())
      function_approximators_[dd]->reTrain(fa_input_phase,fa_target,save_directory_dim,overwrite);
    else
      function_approximators_[dd]->train(fa_input_phase,fa_target,save_directory_dim,overwrite);
  }
}

void TrajectoryAnalyzer::analyze(Trajectory & trajectory,
                                 std::vector<std::shared_ptr<ProteinSegment>> & protein_segments,
                                 const double & temperature,
                                 const int ensemble_size) {

    std::vector<ProteinSegmentEnsemble> protein_segment_ensembles;

    LOGD << "Fitting protein segments with trajectory frames and computing force constants.";
    while (trajectory.has_next()) {
        for (std::shared_ptr<ProteinSegment> const & protein_segment : protein_segments) {
            protein_segment_ensembles.push_back(ProteinSegmentEnsemble(protein_segment));
        }

        LOGD << "adding frames to protein segment ensembles";
        int frame_nr = 0;
        while (trajectory.has_next() && ++frame_nr <= ensemble_size) {
            Frame frame = trajectory.get_next_frame();

            #pragma omp parallel for
            for (size_t i = 0; i < protein_segment_ensembles.size(); i++) {
                protein_segment_ensembles[i].add_frame(frame);
            }

            LOGV_IF(frame_nr % 100 == 0) << "read " << frame_nr << " frames";
        }
        LOGD << "read total number of " << frame_nr << " frames";

        LOGD << "computing force constants for protein segment ensembles.";
        #pragma omp parallel for
        for (size_t i = 0; i < protein_segment_ensembles.size(); i++) {
            protein_segment_ensembles[i].compute_force_constant(temperature);
        }
    }
    LOGD << "Finished analyzing trajectory";
}

Trajectory::Trajectory(const Trajectory& source_traj, const std::string& planning_group):
  planning_group_name_(planning_group),
  discretization_(source_traj.discretization_)
{
	ROS_INFO("Pezzotto safe at ChomTrajectory constructor");
  num_joints_ = source_traj.getNumJoints();

  num_points_ = source_traj.num_points_;
  start_index_ = source_traj.getStartIndex();
  end_index_ = (num_points_ - 1);
  duration_ = (num_points_ - 1)*discretization_;

  // allocate the memory:
  init();

  full_trajectory_index_.resize(num_points_);

  // now copy the trajectories over:
  for (int i=0; i<num_points_; i++)
  {
    full_trajectory_index_[i] = i;
    for (int j=0; j<num_joints_; j++)
    {
      (*this)(i,j) = source_traj(i, j);
    }
  }
}

// --------------------------------------------------------------------------
// commence le movement qui va vers l'entree d'un pont juste un peu decalle en y (~15cm)
// --------------------------------------------------------------------------
bool StrategyAttackCL::gotoBridgeEntryNear(Millimeter y)
{
    Trajectory t;
    disableBridgeCaptors();
    LOG_COMMAND("gotoBridgeEntry Hard Near:%d\n", (int)y);
    // on ne va pas loin en y donc on fait un joli mouvement en S
    // met les servos en position
    Millimeter y2 = RobotPos->y();
    if (y>1800) { // pont du bord
      Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
    } else if (y<1500) { //  pont du bord ou pont contre le milieu
      Move->go2Target(Point(BRIDGE_ENTRY_NAVIGATE_X, y2), 3, 30);
    } else { // ponts au milieu
      t.push_back(Point(BRIDGE_DETECT_BUMP_X-100, y2));
      t.push_back(Point(BRIDGE_ENTRY_NAVIGATE_X, (2*y+y2)/3)); 
      Move->followTrajectory(t, TRAJECTORY_RECTILINEAR, 3, 30);
    }
     
    Events->wait(evtEndMove);
    Move->enableAccelerationController(false);
    if (checkEndEvents()) return false; 
    if(!Events->isInWaitResult(EVENTS_MOVE_END)) {
      // collision
      Move->backward(100);
      Events->wait(evtEndMoveNoCollision);
      return false;
    }
    
    // va en face du pont
    enableBridgeCaptors();
    MvtMgr->setRobotDirection(MOVE_DIRECTION_FORWARD);
    Move->go2Target(BRIDGE_DETECT_BUMP_X, y, 2, 30);
    // dont need to wait, it is done by the upper function
    return true;
}

double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    // Bug, should never happen
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }

    if(sum_scores_)
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}

  /**
   * return a score for trajectory traj
   */
  virtual double scoreTrajectory(Trajectory &traj)
  { 
      if(traj.getPointsSize()==0)
        return 0.0;

      double px, py, pth;
      traj.getPoint(0, px, py, pth);
      
      double dist_to_goal = hypot(px-goal_x_, py-goal_y_);
      if(dist_to_goal < .2){
        return 0.0;
      }

      
      double start_diff;
      if(!getAngleDiff(px,py,pth,&start_diff))
        return -4.0;

      if(fabs(start_diff) < max_trans_angle_){
	return 0.0;
      }

      if(fabs(traj.xv_) > 0.0 || fabs(traj.yv_) > 0.0)
        return -3.0;
      else if( sign(start_diff) != sign(traj.thetav_) )
        return -2.0;
        
      traj.getPoint(traj.getPointsSize()-1, px, py, pth);
      double end_diff;
      if(!getAngleDiff(px,py,pth,&end_diff))
        return -1.0;
        
      return fabs(end_diff);
  }

void Scene::AddTrajectoryAsSpaceObject(const SpaceObject& obj, Frame frame, Date startingDate, const SpaceObject& relativeTo)
{
	Trajectory tj = Trajectory(obj, frame, Units::Metric::kilometers, distanceScale);
	tj.SetRelativeTo(relativeTo);
	standaloneTrajectories.push_back(/*Trajectory(obj, frame, Units::Metric::kilometers)*/tj);
	Time time(10000, Units::Common::days);
	standaloneTrajectories.at(standaloneTrajectories.size() - 1).SetIncrementalParams(startingDate, time, 10000);
}

QPainterPath getTrajectoryPath(const Trajectory& traj, double timespanMs, double timeLCMs) {
    QPainterPath p;
    p.moveTo(traj.x(0)*fieldXConvert, traj.y(0)*fieldXConvert);
    for (int i = 1; i*timeLCMs < timespanMs; i++) {
        p.lineTo(traj.x(i*timeLCMs*0.001)*fieldXConvert, traj.y(i*timeLCMs*0.001)*fieldXConvert);
    }
    return p;
}

bool HasMajorAirports(Trajectory &trajectory, 
 std::vector<std::string> &airports)
{
  return std::binary_search(airports.begin(),airports.end(),
   trajectory.front().string_property("arr")) &&
   std::binary_search(airports.begin(),airports.end(),
   trajectory.front().string_property("dep"));
}

 void traj_cb(const occgrid_planner::TrajectoryConstPtr & msg) {
     frame_id_ = msg->header.frame_id;
     delay_ = 0.0;
     traj_.clear();
     for (unsigned int i=0;i<msg->Ts.size();i++) {
         traj_.insert(Trajectory::value_type(msg->Ts[i].header.stamp.toSec(), msg->Ts[i]));
     }
     ROS_INFO("Trajectory received");
 }

void Trajectory::copyFrom(Trajectory &other)
{
    m_states_vec.clear();
    for(int i=0; i<other.length(); i++) {
//        other.getStation(i).getPosition().print(std::cout);
        Station st = other.getStation(i);
        this->appendState(st);
    }
    this->cost = other.cost;
}

bool TrajectoryTest::equal(const Trajectory<CvPoint>& t1, const Trajectory<CvPoint>& t2) const
{
	if (t1.getId() != t2.getId())
	{
		CPPUNIT_ASSERT (t1.getId() == t2.getId());
		return false;
	}

	if (t1.size() != t2.size())
	{
		return false;
	}

	for (unsigned i = 0; i < t1.size(); ++i)
	{
		const TrajectoryElement<CvPoint> e1 = t1.getTrajectoryElement(i);
		const TrajectoryElement<CvPoint> e2 = t2.getTrajectoryElement(i);
		if (e1 != e2)
		{
			return false;
		}
	}

	return true;
}