/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/DBase/DBPlanner/planner.h

Go to the documentation of this file.

#ifndef DB_PLANNER_PLANNER_H
#define DB_PLANNER_PLANNER_H

#include <string>
#include <vector>
#include "aligner.h"
#include "grasp.h"
#include "grasp_ranker.h"
#include "model.h"
#include "neighbor_finder.h"
#include "training_planner.h"
using std::string;
using std::vector;

namespace db_planner {

template <class Input>
class Planner {
 private:
  const NeighborFinder<Input>& neighbor_finder_;
  const TrainingPlanner& training_planner_;
  const GraspRanker& grasp_selector_;     
  const Aligner<Input>& aligner_;
  const int num_neighbors_;
  vector<Grasp> grasps_;
  vector<Grasp>::iterator current_grasp_;
  const Input& input_;
 public:
  Planner(const NeighborFinder<Input>& neighbor_finder,
          const TrainingPlanner& training_planner,
          const GraspRanker& grasp_ranker,
          const Aligner<Input>& aligner, 
          const int num_neighbors) 
            : neighbor_finder_(neighbor_finder),
              training_planner_(training_planner),
              grasp_ranker_(grasp_ranker),
              aligner_(aligner),
              num_neighbors_(num_neighbors) { }
  // CreatePlan populates the grasps_ vector with un-aligned grasps.
  bool CreatePlan(const Input& input) {
    // We need to store the input_ so we can use it later in alignments.
    input_ = input;
    // Find the k-nearest neighbors.
    vector<string> neighbors(num_neighbors_);
    if (!neighbor_finder_.Find(input_, num_neighbors_, &neighbors)) 
      return false;
    // Get the grasps for those model names
    grasps_.clear();
    for (vector<string>::const_iterator neighbor_name = neighbors.begin();
         neighbor_name != neighbors.end(); ++neighbor_name) {
      vector<Grasp> trained_grasps;
      training_planner_.TrainedGrasps(*neighbor_name, &trained_grasps);
      grasps_.insert(grasps_.end(), 
                     trained_grasps.begin(), 
                     trained_grasps.end());
    }
    // Rank the grasps from those neighbors.
    if (!grasp_ranker_.Rank(&grasps_)) 
      return false;    
    current_grasp_ = grasps_.begin();
    return true;
  }

  // GetAlignedGrasp returns the next grasp from the grasps_ vector aligned
  // with the input. This continues until there are no more grasps. Grasps
  // which fail to be aligned are silently consumed.
  bool NextGrasp(Grasp* grasp) {
    for ( ; current_grasp_ != grasps_.end(); current_grasp_++) {
      float alignemnt[16];
      if (aligner_.Align(*(current_grasp_->SourceModel()), input_, alignemnt)) {
        *grasp = current_grasp_->Transform(alignemnt);
        return true;
      }
    }
    return false;
  }
};


}  // end namespace db_planner


#endif  // DB_PLANNER_PLANNER_H

---

graspit
Author(s):
autogenerated on Wed Jan 25 10:58:52 2012