graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/DBase/DBPlanner/sql_database

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s):  Corey Goldfeder
00021 //
00022 // $Id: sql_database_manager.cpp,v 1.21 2010/03/04 23:25:29 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00031 #include "sql_database_manager.h"
00032 
00033 #include <sstream>
00034 #include <vector>
00035 using std::make_pair;    // Included from <utility> in "db_manager.h"
00036 using std::stringstream;
00037 using std::vector;
00038 
00039 #include <iostream>
00040 using namespace std;
00041 
00042 //#define PROF_ENABLED
00043 #include "profiling.h"
00044 
00045 PROF_DECLARE(GET_GRASPS_SQL);
00046 PROF_DECLARE(GET_GRASPS_GETCOLUMN);
00047 PROF_DECLARE(GET_GRASPS_GETFIELD);
00048 
00049 namespace db_planner {
00050 
00051 bool SqlDatabaseManager::GetAlignment(const Model& source,
00052                                       const Model& dest, 
00053                                       const string& alignment_method_name, 
00054                                       float alignment[16]) const {
00055   Table results;
00056   stringstream query_text;                  
00057   query_text << "SELECT get_alignment('" << source.ModelName() << "','" 
00058              << dest.ModelName() << "','" << alignment_method_name << "')";
00059   if (!database_.Query(query_text.str(), &results)) return false;
00060   vector<float> alignment_vector;
00061   alignment_vector.reserve(16);
00062   if (results.NumColumns() == 0 || results.NumRows() == 0) return false;
00063   if (!results.GetField(0, 0, &alignment_vector)) return false;
00064   for (int i = 0; i < 16; ++i) alignment[i] = alignment_vector[i];
00065   return true;
00066 }
00067 
00068 bool SqlDatabaseManager::SaveAlignment(const Model& source,
00069                                        const Model& dest, 
00070                                        const string& alignment_method_name, 
00071                                        const float alignment[16]) const {
00072   if (alignment[0] == 0) return false;
00073   if (alignment_method_name == "") return false;
00074   if (source.ModelName() != dest.ModelName()) return false;
00075   return false;
00076 }
00077 
00078 
00080 class LoadModelFunctor {
00081  private:
00082   const Table& table_;
00083   const string& model_root_;
00084   int model_name_column_, thumbnail_path_column_, scale_column_, 
00085       tags_column_, grasping_rescale_column_, geometry_path_column_;
00086   bool initialized_;
00087  public:
00088    LoadModelFunctor(const Table& table, const string& model_root) 
00089        : table_(table), model_root_(model_root) {
00090      initialized_ = 
00091          table_.GetColumnIndex("model_name", &model_name_column_) &&
00092          table_.GetColumnIndex("thumbnail_path", &thumbnail_path_column_) &&
00093          table_.GetColumnIndex("geometry_path", &geometry_path_column_) &&
00094          table_.GetColumnIndex("scale", &scale_column_) &&
00095          table_.GetColumnIndex("tags", &tags_column_) &&
00096          table_.GetColumnIndex("grasping_rescale", &grasping_rescale_column_);         
00097    }
00098   bool operator()(Model* model, const int row) {
00099     string model_name, thumbnail_path, geometry_path;
00100     double scale, grasping_rescale;
00101     vector<string> tags;
00102     if (!initialized_ ||
00103         !table_.GetField(model_name_column_, row, &model_name) || 
00104         !table_.GetField(thumbnail_path_column_, row, &thumbnail_path) ||
00105         !table_.GetField(geometry_path_column_, row, &geometry_path) ||
00106         !table_.GetField(scale_column_, row, &scale) ||
00107         !table_.GetField(tags_column_, row, &tags) ||
00108         !table_.GetField(grasping_rescale_column_, row, &grasping_rescale))
00109       return false;
00110     model->SetModelName(model_name);
00111     model->SetThumbnailPath(model_root_ + thumbnail_path);
00112     model->SetScale(scale);
00113     model->SetTags(tags.begin(), tags.end());
00114     model->SetGeometryPath(model_root_ + geometry_path);
00115     model->SetRescaleFactor(grasping_rescale * scale);
00116         return true;
00117   }
00118 };
00119 
00120 
00121 
00122 bool SqlDatabaseManager::GetNeighbors(const Model& model, 
00123                                       const string& distance_function_name, 
00124                                       const int num_neighbors,
00125                                       vector<pair<Model*, double> >* neighbors) const {
00126   if (neighbors == NULL) return false;
00127   Table results;
00128   stringstream query_text;                  
00129   query_text << "SELECT * FROM get_saved_neighbors('" << model.ModelName()
00130              << "','" << distance_function_name 
00131              << "'," << num_neighbors << ");";
00132   if (!database_.Query(query_text.str(), &results)) return false;
00133   int distance_column;
00134   if (!results.GetColumnIndex("distance", &distance_column)) return false;
00135   // Turn each database row into a Model.
00136   LoadModelFunctor load_model_functor(results, model_root_);
00137   double distance;
00138   const int num_rows = results.NumRows();
00139   neighbors->reserve(num_rows);
00140   for (int row = 0; row < num_rows; ++row) {
00141     Model* neighbor_model = model_allocator_->Get();
00142     if (!load_model_functor(neighbor_model, row) ||
00143         !results.GetField(distance_column, row, &distance)) return false;
00144     neighbors->push_back(make_pair(neighbor_model, distance));
00145   }
00146   return true;
00147 }
00148 
00149 bool SqlDatabaseManager::SaveNeighbors(
00150     const Model& model, 
00151     const string& distance_function_name, 
00152     const vector<pair<Model*, double> >& neighbors) const {
00153   if (model.ModelName() == "") return false;
00154   if (distance_function_name == "") return false;
00155   if (&neighbors == NULL) return false;
00156   return false;
00157 }
00158 
00159 bool SqlDatabaseManager::ModelList(vector<Model*>* model_list, 
00160                                                                    FilterList::FilterType filter) const {
00161   if (model_list == NULL) return false;
00162   Table results;
00163   if (!database_.Query("SELECT * FROM get_models() " + 
00164                                                 FilterList::GetWhereClause(filter) + ";", 
00165                                                 &results)) return false;
00166   // Turn each database row into a Model.
00167   LoadModelFunctor load_model_functor(results, model_root_);
00168   const int num_rows = results.NumRows();
00169   model_list->reserve(num_rows);
00170   for (int row = 0; row < num_rows; ++row) {
00171     Model* model = model_allocator_->Get();
00172         if (!load_model_functor(model, row)){
00173                 return false;
00174         }
00175     model_list->push_back(model);
00176   }
00177   return true;
00178 }
00179 
00181 
00184 static bool NameList(const string& name_type, 
00185                      const DatabaseConnection& database,
00186                      vector<string>* list) {
00187   if (list == NULL) return false;
00188   Table results;
00189   if (!database.Query("SELECT * FROM get_" + name_type + "_names();", 
00190       &results)) return false;
00191   const int num_rows = results.NumRows();
00192   list->reserve(num_rows);
00193   int name_column;
00194   if (!results.GetColumnIndex(name_type + "_name", &name_column)) return false;
00195   string name;
00196   for (int row = 0; row < num_rows; ++row) {
00197     if (!results.GetField(name_column, row, &name)) return false;
00198     list->push_back(name);
00199   }
00200   return true;
00201 }
00202 
00203 bool SqlDatabaseManager::DistanceFunctionList(
00204     vector<string>* distance_function_list) const {
00205   return NameList("distance_function", database_, distance_function_list);
00206 }
00207 
00208 bool SqlDatabaseManager::AlignmentMethodList(
00209     vector<string>* alignment_method_list) const {
00210   return NameList("alignment_method", database_, alignment_method_list);
00211 }
00212 
00213 bool SqlDatabaseManager::GraspTypeList(
00214         vector<string>* type_list) const {
00215                 return NameList("grasp_type", database_, type_list);
00216 }
00217 
00218 bool SqlDatabaseManager::GetGrasps(const Model& model, 
00219                                    const string& hand_name, 
00220                                    vector<Grasp*>* grasp_list) const {
00221   if (grasp_list == NULL) return false;
00222   Table results;
00223   PROF_START_TIMER(GET_GRASPS_SQL);
00224   if (!database_.Query("SELECT * FROM get_grasps('" + model.ModelName() + 
00225                        "','" + hand_name + "');", &results)) 
00226     return false;
00227   PROF_STOP_TIMER(GET_GRASPS_SQL);
00228   // Get the column indices for the columns we care about.
00229   int pregrasp_joints_column, grasp_joints_column, 
00230       pregrasp_position_column, grasp_position_column,
00231       grasp_id_column, epsilon_quality_column, volume_quality_column,
00232           grasp_contacts_column, grasp_source_name_column;
00233   PROF_START_TIMER(GET_GRASPS_GETCOLUMN);
00234   if (!results.GetColumnIndex("grasp_pregrasp_joints", &pregrasp_joints_column) ||
00235       !results.GetColumnIndex("grasp_grasp_joints", &grasp_joints_column) ||
00236       !results.GetColumnIndex("grasp_pregrasp_position", 
00237                               &pregrasp_position_column) ||
00238       !results.GetColumnIndex("grasp_grasp_position", &grasp_position_column) ||
00239       !results.GetColumnIndex("grasp_id", &grasp_id_column) ||
00240       !results.GetColumnIndex("grasp_epsilon_quality", &epsilon_quality_column) ||
00241       !results.GetColumnIndex("grasp_volume_quality", &volume_quality_column) ||
00242           !results.GetColumnIndex("grasp_contacts", &grasp_contacts_column) ||
00243           !results.GetColumnIndex("grasp_source_name", &grasp_source_name_column))
00244     return false;
00245         PROF_STOP_TIMER(GET_GRASPS_GETCOLUMN);
00246   // Turn each database row into a Grasp.
00247   vector<double> pregrasp_joints, grasp_joints, 
00248       pregrasp_position, grasp_position, grasp_contacts;
00249   int grasp_id;
00250   string grasp_source_name;
00251   double epsilon_quality, volume_quality;
00252   const int num_rows = results.NumRows();
00253   grasp_list->reserve(num_rows);
00254   for (int row = 0; row < num_rows; ++row) {
00255     grasp_list->push_back(grasp_allocator_->Get());
00256     Grasp& grasp = *(grasp_list->back());
00257 
00258         pregrasp_joints.clear();
00259         pregrasp_position.clear();
00260         grasp_joints.clear();
00261         grasp_position.clear();
00262 
00263         PROF_START_TIMER(GET_GRASPS_GETFIELD);
00264     if (!results.GetField(pregrasp_joints_column, row, &pregrasp_joints) || 
00265         !results.GetField(grasp_joints_column, row, &grasp_joints) || 
00266         !results.GetField(pregrasp_position_column, row, &pregrasp_position) ||
00267         !results.GetField(grasp_position_column, row, &grasp_position) || 
00268         !results.GetField(grasp_id_column, row, &grasp_id) ||
00269         !results.GetField(epsilon_quality_column, row, &epsilon_quality) ||
00270         !results.GetField(volume_quality_column, row, &volume_quality) ||
00271                 !results.GetField(grasp_contacts_column, row, &grasp_contacts) ||
00272                 !results.GetField(grasp_source_name_column, row, &grasp_source_name))
00273       return false;
00274         PROF_STOP_TIMER(GET_GRASPS_GETFIELD);
00275     grasp.SetSourceModel(model);
00276     grasp.SetHandName(hand_name);
00277     grasp.SetEpsilonQuality(epsilon_quality);
00278     grasp.SetVolumeQuality(volume_quality);
00279     grasp.SetGraspId(grasp_id);
00280 
00281         pregrasp_joints.erase(pregrasp_joints.begin());
00282         pregrasp_position.erase(pregrasp_position.begin());
00283         grasp_joints.erase(grasp_joints.begin());
00284         grasp_position.erase(grasp_position.begin());
00285     
00286         grasp.SetGraspParameters(pregrasp_joints, 
00287                              pregrasp_position, 
00288                              grasp_joints, 
00289                              grasp_position);
00290         grasp.SetContacts(grasp_contacts);
00291         grasp.SetPregraspJoints(pregrasp_joints);
00292         grasp.SetPregraspPosition(pregrasp_position);
00293         grasp.SetFinalgraspJoints(grasp_joints);
00294         grasp.SetFinalgraspPosition(grasp_position);
00295         grasp.SetSource(grasp_source_name);
00296   }
00297   return true;
00298 }
00299 
00300 bool SqlDatabaseManager::SaveGrasp(const Grasp* grasp) const {
00301         Table results;
00302 
00303         stringstream query_text;                  
00304         query_text << "SELECT * FROM save_grasp('" <<
00305           grasp->SourceModel().ModelName() << "','" <<
00306           grasp->HandName() << "', '{5, ";
00307         
00308         vector<double> tempArray;
00309         //pregrasp joints
00310         tempArray = grasp->GetPregraspJoints();
00311         size_t j;
00312         for(j = 0; j < tempArray.size() - 1; ++j){
00313           query_text << tempArray[j] << ", ";
00314         }
00315         query_text << tempArray[j] << "}', ";
00316         
00317         //pregrasp position
00318         query_text << "'{0, ";
00319         tempArray = grasp->GetPregraspPosition();
00320         for(j = 0; j < tempArray.size() - 1; ++j){
00321           query_text << tempArray[j] << ", ";
00322         }
00323         query_text << tempArray[j] << "}', ";
00324         
00325         //finalgrasp joints
00326         query_text << "'{5, ";
00327         tempArray = grasp->GetFinalgraspJoints();
00328         for(j = 0; j < tempArray.size() - 1; ++j){
00329           query_text << tempArray[j] << ", ";
00330         }
00331         query_text << tempArray[j] << "}', ";
00332         
00333         //finalgrasp position
00334         query_text << "'{0, ";
00335         tempArray = grasp->GetFinalgraspPosition();
00336         for(j = 0; j < tempArray.size() - 1; ++j){
00337           query_text << tempArray[j] << ", ";
00338         }
00339         query_text << tempArray[j] << "}', ";
00340         
00341         //contacts
00342         query_text << "'{";
00343         tempArray = grasp->GetContacts();
00344         for(j = 0; j < tempArray.size() - 1; ++j){
00345           query_text << tempArray[j] << ", ";
00346         }
00347         query_text << tempArray[j] << "}', ";
00348         
00349         //energy
00350         query_text << -30.0 * grasp->EpsilonQuality() -100.0 * grasp->VolumeQuality() << ", ";
00351         
00352         //epsilon quality
00353         query_text << grasp->EpsilonQuality() << ", ";
00354         
00355         //volume
00356         query_text << grasp->VolumeQuality() << ", ";
00357         
00358         //source
00359         query_text << "'" << grasp->GetSource().c_str() << "')";
00360         
00361         //std::cout << query_text.str().c_str();
00362         
00363         if (!database_.Query(query_text.str(), &results)) return false;
00364 
00365         return true;
00366 }
00367 
00368 }  // end namespace db_planner
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/DBase/DBPlanner/sql_database_manager.cpp
