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

void PlanningProblem::computeCost(Trajectory &plan_)
{
    if(!this->checkPlanValidity(plan_)) {
        plan_.cost.safety = INFINITY;
        plan_.cost.smoothness = INFINITY;
        plan_.cost.length = INFINITY;
    }
    else {
        plan_.computeCost();
        plan_.cost.safety = 0;
        ObstacleSet desired_ob_set = stat_obstacles;
        for(int i=0; i< plan_.length(); i++) {
            Station st = plan_.getStation(i);
            float min_dist = INFINITY;
            b2Vec2 tmpv1, tmpv2;
            for(int j=0; j<desired_ob_set.size(); j++) {
                Obstacle* ob = desired_ob_set[j];
                float dist_ = distToObstacle(st, *ob, tmpv1, tmpv2);
                min_dist = min(min_dist, dist_);
            }
            st.cost.min_dist_to_obs = min_dist;
            plan_.EditStation(i, st);
            plan_.cost.safety += plan_.getStation(i).cost.safety_penalty() / plan_.length();
        }
    }
}

Station PlanningProblem::getNextStation(const Station &st, Trajectory &path)
{
    if(path.isEmpty())
        return this->goal.goal_point;
    if(path.length() <= 2)
        return path.getLastStation();

    float max_memberance = 0;
    int nearest_segment_index = -1;
    for(uint i=1; i<path.length(); i++) { // it doesnt use the last segment (go towards goal on last segment)
        Vector2D pnt_1 = path.getStation(i-1).getPosition().to2D();
        Vector2D pnt_2 = path.getStation(i).getPosition().to2D();

        float dist_st_segment = (st.getPosition().to2D() - pnt_1).lenght() +
                                                (st.getPosition().to2D() - pnt_2).lenght();
        float segment_len = (pnt_1 - pnt_2).lenght();

        float segment_mem = segment_len /dist_st_segment;
        if(segment_mem > max_memberance) {
            max_memberance = segment_mem;
            nearest_segment_index = i;
        }
    }

    return path.getStation(nearest_segment_index);
}

bool PlanningProblem::replan(const ObstacleSet &ob_set, Trajectory &trajec)
{
    if(trajec.length() < 2)
        return false;

    Vector2D goals_diff = (goal.goal_point.getPosition() - trajec.getLastStation().getPosition()).to2D();
    if(goals_diff.lenght() > agent->radius())
        return false;

    Vector2D inits_dist = (initialState.getPosition() - trajec.getFirstStation().getPosition()).to2D();
    if(inits_dist.lenght() > agent->radius())
        return false;

    trajec.EditStation(0, initialState);
    trajec.EditStation(trajec.length() -1, goal.goal_point);
    return true;
}

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;
}

Trajectory PlanningProblem::PruneTrajectory(Trajectory &input_plan, const ObstacleSet& ob_set)
{
    if(input_plan.length() < 3)
        return input_plan;

    Trajectory prunned_plan;
    int st_index = 1;
    prunned_plan.appendState(input_plan.getStation(0));
    while((input_plan.length() - st_index) > 1) {
        Station st_A = prunned_plan.getLastStation();
        Station st_B = input_plan.getStation(st_index +1);
        if(pathHasCollision(st_A, st_B, ob_set)) {
            Station new_inserted_st = input_plan.getStation(st_index);
            float new_teta = (new_inserted_st.getPosition().to2D() -
                              prunned_plan.getLastStation().getPosition().to2D()).arctan();
            new_inserted_st.setPosition(Vector3D(new_inserted_st.getPosition().to2D(), new_teta));
            prunned_plan.appendState(new_inserted_st);
        }
        st_index ++;
    }
    prunned_plan.appendState(input_plan.getLastStation());
    return prunned_plan;
//    for(int i=0; i< p.length(); i++) {
//        if(i == 0) {
//            opt_plan.appendState(p.getStation(0));
//            continue;
//        }
//        Station _st = p.getStation(i);
//        float min_dist_to_ob;
//        b2Vec2 st_colid_point;
//        b2Vec2 ob_colid_point;
//        Obstacle* ob_ = nearestObstacle(_st, stat_obstacles, min_dist_to_ob, st_colid_point, ob_colid_point);
//        if(ob_ != NULL && min_dist_to_ob < agent->radius() * 1.5) {
//            Vector2D bad_direc = (Vector2D(ob_colid_point) - Vector2D(st_colid_point)).normalized();
//            _st.setPosition(_st.getPosition() - bad_direc.to3D() * 0.5);
//        }
//        if(CheckValidity(_st))
//            opt_plan.appendState(_st);
//        else
//            opt_plan.appendState(p.getStation(i));
//    }
}

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);
    }

  }
}

bool PlanningProblem::checkPlanValidity(Trajectory &plan, float tolerance_coeff)
{
    if(plan.length() == 0)
        return false;
    if(Station::euclideanDistance(plan.getStation(0), initialState) > tolerance_coeff* agent->radius())
        return false;    
    if(goal.minDistTo(plan.getLastStation()) > tolerance_coeff * agent->radius())
        return false;
    if(Station::euclideanDistance(plan.getLastStation(), goal.goal_point) > tolerance_coeff * agent->radius() )
        return false;
//    for(int i=0; i<plan.length(); i++) {
//        if(!CheckValidity(plan.getStation(i)))
//            return false;
//    }
    return true;
}

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();
 
}

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;
    }
}

vector<Segment> generateSegments(Trajectory &t, double epsilon, int index){
	//generate graph.
	vector<vector<int>> g(t.length());
	for(int i = 0; i<t.length()-1; i++){
		const Point &p = t.points[i];
		const Point &q = t.points[i+1];
		const Point e1(1,0);
		double thetamin;
		double thetamax;
		double theta = acos((q-p).dotProd(e1)/(rootDistance(p,q)));
		double dtheta = atan(epsilon/rootDistance(p,q));
		thetamin = theta-dtheta;
		thetamax = theta+dtheta;
		g[i].push_back(i+1);
		int j = i+2;
		if (j<t.length()){
			theta = acos((t.points[j]-p).dotProd(e1)/(rootDistance(p,t.points[j])));
		}
		while(j<t.length() && theta>=thetamin && theta<= thetamax){
			g[i].push_back(j);
			double dtheta = atan(epsilon/rootDistance(p,t.points[j]));
			if (thetamin<theta-dtheta) thetamin = theta-dtheta;
			if (thetamax>theta+dtheta) thetamax = theta+dtheta;
			j++;
			if (j<t.length()){
				theta = acos((t.points[j]-p).dotProd(e1)/(rootDistance(p,t.points[j])));
			}

		}
	}

	//do DFS.
	queue<int> Q;
	Q.push(0);

	bool added[t.length()];
	for (int i; i< t.length(); i++) added[i] = false;
	added[0]=true;

	int dist[t.length()];
	for (int i; i< t.length(); i++) dist[i] = t.length();
	dist[0] = 0;


	int parent[t.length()];
	for (int i; i < t.length(); i++) parent[i] = -1;


	while(!Q.empty()){
		int ver = Q.front();
		Q.pop();
		for(auto n : g[ver]){
			if (!added[n]){
				parent[n] = ver;
				dist[n] = dist[ver]+1;
				added[n] = true;
				Q.push(n);
			}
		}
	}
	int n = t.length()-1;
	vector<Segment> ret;
	while(n!=0){
		ret.push_back(Segment(t.points[parent[n]],t.points[n], index, n-parent[n]));
		n = parent[n];
	}

	return ret;

}