C++ (Cpp) trajectory_t Examples

Example #1

File: validity_checker.cpp Project: amazon-picking-challenge/ru_pracsys

        void validity_checker_t::valid_trajectory(const space_t* space, const trajectory_t& input, trajectory_t& result)
        {
            result.clear();

            foreach(state_t* state, input)
            {
                if( !is_valid(state) )
                    return;
                result.copy_onto_back(state);
            }
        }

Example #2

File: validity_checker.cpp Project: amazon-picking-challenge/ru_pracsys

        void validity_checker_t::valid_trajectory_prefix(const space_t* space, const trajectory_t& input, trajectory_t& result, unsigned int size)
        {
            unsigned int traj_size = 0;
            result.clear();

            foreach(state_t* state, input)
            {
                if( !is_valid(state) || traj_size == size )
                    return;
                result.copy_onto_back(state);
                traj_size++;
            }
        }

Example #3

File: default_uniform.cpp Project: rshnn/baxter_planning

        double default_uniform_t::trajectory_cost(const trajectory_t& t)
        {
            PRX_ASSERT(t.size()>0);
            
            double cost = 0;
            trajectory_t::const_iterator i = t.begin();
            trajectory_t::const_iterator j = t.begin();
            j++;

            for ( ; j != t.end(); ++i,++j)
            {
                cost+=dist(*i,*j)*state_cost(*j);
            }
            return cost;
        }

Example #4

File: validity_checker.cpp Project: amazon-picking-challenge/ru_pracsys

        void validity_checker_t::valid_trajectory(trajectory_t& input)
        {

            unsigned int traj_size = 0;

            foreach(state_t* state, input)
            {
                if( !is_valid(state) )
                    break;
                traj_size++;
            }
            input.resize(traj_size);
        }

Example #5

File: rrts.hpp Project: jdmartin86/smp

int 
RRTstar::Planner< typeparams >
::insertTrajectory (vertex_t& vertexStartIn, trajectory_t& trajectoryIn, vertex_t& vertexEndIn) {
  
  // Update the costs
  vertexEndIn.costFromParent = trajectoryIn.evaluateCost();
  vertexEndIn.costFromRoot = vertexStartIn.costFromRoot + vertexEndIn.costFromParent;
  checkUpdateBestVertex (vertexEndIn);
  
  // Update the trajectory between the two vertices
  if (vertexEndIn.trajFromParent)
    delete vertexEndIn.trajFromParent;
  vertexEndIn.trajFromParent = new trajectory_t (trajectoryIn);
  
  // Update the parent to the end vertex
  if (vertexEndIn.parent)
    vertexEndIn.parent->children.erase (&vertexEndIn);
  vertexEndIn.parent = &vertexStartIn;
  
  // Add the end vertex to the set of chilren
  vertexStartIn.children.insert (&vertexEndIn);
  
  return 1;
}

Example #6

File: minimum_time_reachability_thetastar.hpp Project: Megacephalo/srl_global_planner

int smp::minimum_time_reachability<typeparams,NUM_DIMENSIONS>
::get_solution (trajectory_t &trajectory_out) {


  if (!min_cost_vertex)
    return 1;

  trajectory_out.clear();

  for (typename list<state_t*>::iterator it_state = min_cost_trajectory.list_states.begin();
       it_state != min_cost_trajectory.list_states.end(); it_state++) {

    trajectory_out.list_states.push_front (new state_t(**it_state));
  }

  for (typename list<input_t*>::iterator it_input = min_cost_trajectory.list_inputs.begin();
       it_input != min_cost_trajectory.list_inputs.end(); it_input++) {
    trajectory_out.list_inputs.push_front (new input_t(**it_input));
  }



  return 1;
}