grasp_retriever.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 // Author(s): Peter Brook
00036 
00037 #include <household_objects_database/objects_database.h>
00038 using household_objects_database::ObjectsDatabasePtr;
00039 #include <household_objects_database/database_grasp.h>
00040 using household_objects_database::DatabaseGraspPtr;
00041 #include <household_objects_database/database_perturbation.h>
00042 using household_objects_database::DatabasePerturbationPtr;
00043 
00044 #include <object_manipulator/tools/hand_description.h>
00045 #include <object_manipulation_msgs/GraspPlanning.h>
00046 #include <object_manipulation_msgs/GraspPlanningErrorCode.h>
00047 #include <tf/transform_broadcaster.h>
00048 
00049 //#define PROF_ENABLED
00050 //#include <profiling/profiling.h>
00051 
00052 #include "probabilistic_grasp_planner/grasp_retriever.h"
00053 
00054 //PROF_DECLARE(CLUSTER_REP_CREATE_PROF)
00055 //PROF_DECLARE(CLUSTER_PLANNER_CREATE_PROF)
00056 
00057 namespace probabilistic_grasp_planner {
00058 
00060 
00063 void DatabaseGraspRetriever::pruneGraspList(std::vector<DatabaseGraspPtr> &grasps,
00064                                                double gripper_threshold,
00065                                                double table_clearance_threshold)
00066 {
00067   std::vector<DatabaseGraspPtr>::iterator prune_it = grasps.begin();
00068   int pruned = 0;
00069   while ( prune_it != grasps.end() )
00070   {
00071     //by mistake, table clearance in database is currently in mm
00072     if ((*prune_it)->final_grasp_posture_.get().joint_angles_[0] > gripper_threshold ||
00073         (table_clearance_threshold >= 0.0 && (*prune_it)->table_clearance_.get() < table_clearance_threshold*1.0e3) )
00074     {
00075       prune_it = grasps.erase(prune_it);
00076       pruned++;
00077     }
00078     else
00079     {
00080       prune_it++;
00081     }
00082   }
00083   ROS_DEBUG("Database grasp planner: pruned %d grasps for table collision or gripper angle above threshold", pruned);
00084 }
00085 
00086 void DatabaseGraspRetriever::appendMetadataToGrasps(const std::vector<DatabaseGraspPtr> &db_grasps,
00087                                                       std::vector<GraspWithMetadata> &grasps)
00088 {
00089   grasps.reserve(grasps.size()+db_grasps.size());
00090 
00091   BOOST_FOREACH(DatabaseGraspPtr db_grasp, db_grasps) {
00092     GraspWithMetadata grasp;
00093 
00094     ROS_ASSERT( db_grasp->final_grasp_posture_.get().joint_angles_.size() ==
00095               db_grasp->pre_grasp_posture_.get().joint_angles_.size() );
00096     ROS_ASSERT(db_grasp->scaled_model_id_.get() ==  model_.model_id);
00097 
00098     std::vector<std::string> joint_names = object_manipulator::handDescription().handJointNames(arm_name_);
00099     if (object_manipulator::handDescription().handDatabaseName(arm_name_) != "WILLOW_GRIPPER_2010")
00100     {
00101       //check that the number of joints in the ROS description of this hand
00102       //matches the number of values we have in the database
00103       if (joint_names.size() != db_grasp->final_grasp_posture_.get().joint_angles_.size())
00104       {
00105         ROS_ERROR("Database grasp specification does not match ROS description of hand. "
00106                   "Hand is expected to have %zd joints, but database grasp specifies %zd values",
00107                   joint_names.size(), db_grasp->final_grasp_posture_.get().joint_angles_.size());
00108         continue;
00109       }
00110       //for now we silently assume that the order of the joints in the ROS description of
00111       //the hand is the same as in the database description
00112       grasp.grasp_.pre_grasp_posture.name = joint_names;
00113       grasp.grasp_.grasp_posture.name = joint_names;
00114       grasp.grasp_.pre_grasp_posture.position = db_grasp->pre_grasp_posture_.get().joint_angles_;
00115       grasp.grasp_.grasp_posture.position = db_grasp->final_grasp_posture_.get().joint_angles_;
00116     }
00117     else
00118     {
00119       //unfortunately we have to hack this, as the grasp is really defined by a single
00120       //DOF, but the urdf for the PR2 gripper is not well set up to do that
00121       if ( joint_names.size() != 4 || db_grasp->final_grasp_posture_.get().joint_angles_.size() != 1)
00122       {
00123         ROS_ERROR("PR2 gripper specs and database grasp specs do not match expected values");
00124         continue;
00125       }
00126       grasp.grasp_.pre_grasp_posture.name = joint_names;
00127       grasp.grasp_.grasp_posture.name = joint_names;
00128       //replicate the single value from the database 4 times
00129       grasp.grasp_.pre_grasp_posture.position.resize( joint_names.size(),
00130                                                    db_grasp->pre_grasp_posture_.get().joint_angles_.at(0));
00131       grasp.grasp_.grasp_posture.position.resize( joint_names.size(),
00132                                                db_grasp->final_grasp_posture_.get().joint_angles_.at(0));
00133     }
00134 
00135     //for now the effort and approach distances are not in the database 
00136     //so we hard-code them in here; this will change at some point
00137     grasp.grasp_.pre_grasp_posture.effort.resize(joint_names.size(), 100);
00138     grasp.grasp_.grasp_posture.effort.resize(joint_names.size(), 50);
00139     grasp.grasp_.desired_approach_distance = 0.10;
00140     grasp.grasp_.min_approach_distance = 0.05;
00141 
00142     grasp.grasp_.success_probability = 0.0;
00143 
00144     //the pose of the grasp
00145     //grasp.grasp_.grasp_pose = db_grasp->final_grasp_pose_.get().pose_;
00146 
00147     grasp.energy_function_score_ = db_grasp->quality_.get();
00148     grasp.model_id_ = db_grasp->scaled_model_id_.get();
00149     grasp.grasp_id_ = db_grasp->id_.get();
00150 
00151     tf::Stamped<tf::Pose> base_to_object;
00152 
00153     tf::poseStampedMsgToTF(model_.pose,base_to_object);
00154 
00155     tf::Pose object_to_grasp;
00156     tf::poseMsgToTF(db_grasp->final_grasp_pose_.get().pose_, object_to_grasp);
00157 
00158     //tf::Stamped<tf::Pose> base_to_best_object;
00159     //tf::poseStampedMsgToTF(best_model_pose_.pose, base_to_best_object);
00160     grasp.object_pose_ = base_to_object;
00161 
00162     //tf::Pose best_object_to_grasp(base_to_best_object.inverse()*base_to_object*object_to_grasp);
00163 
00164     tf::Pose base_to_grasp(base_to_object * object_to_grasp);
00165 
00166     tf::poseTFToMsg(base_to_grasp,grasp.grasp_.grasp_pose);
00167 
00168 
00169     double wrist_to_tool_point_offset_ = 0.13;
00170 
00171     tf::Pose offset_tf(tf::Matrix3x3::getIdentity(),tf::Vector3(wrist_to_tool_point_offset_,0,0));
00172 
00174     grasp.tool_point_pose_ = tf::Stamped<tf::Pose>(base_to_grasp * offset_tf,
00175         grasp.object_pose_.stamp_, grasp.object_pose_.frame_id_);
00176 
00177     //insert the new grasp in the list
00178     grasps.push_back(grasp);
00179   }
00180 
00181 }
00182 DatabaseGraspRetriever::DatabaseGraspRetriever(ObjectsDatabasePtr database,
00183                                                const household_objects_database_msgs::DatabaseModelPose &model,
00184                                                const std::string &arm_name,
00185                                                bool prune_compliant_copies,
00186                                                                                            bool use_cluster_rep_grasps) :
00187   GraspRetriever(arm_name),
00188   database_(database), model_(model), prune_compliant_copies_(prune_compliant_copies), use_cluster_rep_grasps_(use_cluster_rep_grasps)
00189 {
00190 
00191 }
00192 
00193 void DatabaseGraspRetriever::fetchFromDB()
00194 {
00195   /*
00196   if (retrieve_from_map(grasps_cache_, model_id, db_grasps))
00197   {
00198     ROS_DEBUG("Robust Database Grasp Planning Node: retrieved %zd grasps from cache", db_grasps.size());
00199   }
00200   else
00201   {
00202 
00203     grasps_cache_.insert(std::make_pair(model_id, db_grasps));
00204   }
00205   */
00206   std::vector<DatabaseGraspPtr> db_grasps;
00207   int model_id=model_.model_id;
00208   //retrieve the raw grasps from the database
00209   if (use_cluster_rep_grasps_)
00210   {
00211         if (!database_->getClusterRepGrasps(model_id, object_manipulator::handDescription().handDatabaseName(arm_name_), db_grasps))
00212         {
00213           ROS_ERROR("Robust Database Grasp Planning Node: grasp retrieval error");
00214         }
00215   }
00216   else
00217   {
00218         if (!database_->getGrasps(model_id, object_manipulator::handDescription().handDatabaseName(arm_name_), db_grasps))
00219         {
00220           ROS_ERROR("Robust Database Grasp Planning Node: grasp retrieval error");
00221         }
00222   }
00223 
00224   ROS_DEBUG("Robust Database Grasp Planning Node: retrieved %zd grasps from database", db_grasps.size());
00225   //pruneGraspList(db_grasps, 0.5,0.0);
00226 
00227   if (prune_compliant_copies_)
00228   {
00229     std::vector<DatabaseGraspPtr>::iterator it = db_grasps.begin();
00230     while (it!=db_grasps.end())
00231     {
00232       if ((*it)->compliant_copy_.get()) it = db_grasps.erase(it);
00233       else it++;
00234     }
00235   }
00236 
00237   appendMetadataToGrasps(db_grasps, grasps_);
00238 }
00239 
00240 void DatabaseGraspRetriever::getGrasps(std::vector<GraspWithMetadata> &grasps)
00241 {
00242   grasps.insert(grasps.end(), grasps_.begin(), grasps_.end());
00243 }
00244 
00245 ClusterRepGraspRetriever::ClusterRepGraspRetriever(ObjectsDatabasePtr database,
00246                                                    const household_objects_database_msgs::DatabaseModelPose &model,
00247                                                    const std::string &arm_name) :
00248   DatabaseGraspRetriever(database, model, arm_name, true, false)
00249 {
00250   //PROF_TIMER_FUNC(CLUSTER_REP_CREATE_PROF);
00251 
00252 }
00253 
00254 void ClusterRepGraspRetriever::fetchFromDB()
00255 {
00256 
00257   /*
00258   if (retrieve_from_map(grasps_cache_, model_id, db_grasps))
00259   {
00260     ROS_DEBUG("Robust Database Grasp Planning Node: retrieved %zd grasps from cache", db_grasps.size());
00261   }
00262   else
00263   {
00264 
00265     grasps_cache_.insert(std::make_pair(model_id, db_grasps));
00266   }
00267   */
00268   std::vector<DatabaseGraspPtr> db_grasps;
00269   int model_id=model_.model_id;
00270   //retrieve the raw grasps from the database
00271   if (!database_->getClusterRepGrasps(model_id,
00272                                       object_manipulator::handDescription().handDatabaseName(arm_name_), db_grasps))
00273   {
00274     ROS_ERROR("Robust Database Grasp Planning Node: grasp retrieval error");
00275   }
00276 
00277   ROS_WARN("Size from DB: %zd",db_grasps.size());
00278 
00279   ROS_DEBUG("Robust Database Grasp Planning Node: retrieved %zd grasps from database", db_grasps.size());
00280   //pruneGraspList(db_grasps, 0.5,0.0);
00281 
00282   ROS_WARN("Size after pruning: %zd",db_grasps.size());
00283   ROS_WARN("Size before appending: %zd",grasps_.size());
00284   appendMetadataToGrasps(db_grasps, grasps_);
00285 
00286   ROS_WARN("Size after appending: %zd",grasps_.size());
00287 }
00288 
00289 void ClusterRepGraspRetriever::getGrasps(std::vector<GraspWithMetadata> &grasps)
00290 {
00291   grasps.insert(grasps.end(), grasps_.begin(), grasps_.end());
00292 }
00293 
00297 void DebugClusterRepGraspRetriever::getGrasps(std::vector<GraspWithMetadata> &grasps)
00298 {
00299   std::vector<GraspWithMetadata>::iterator begin_plus_five(grasps_.begin());
00300   std::advance(begin_plus_five, 5);
00301   grasps.insert(grasps.end(), grasps_.begin(), begin_plus_five);
00302 }
00303 
00304 PerturbationGraspRetriever::PerturbationGraspRetriever(ObjectsDatabasePtr database,
00305                                                        const household_objects_database_msgs::DatabaseModelPose &model,
00306                                                        const std::string &arm_name) :
00307   DatabaseGraspRetriever(database, model, arm_name, false, true), gstar_(NULL)
00308 {
00309   ROS_DEBUG("Created new perturbation grasp retriever for model_id %d",model.model_id);
00310   fetchFromDB();
00311 }
00312 
00313 void PerturbationGraspRetriever::fetchFromDB()
00314 {
00315   std::vector<DatabasePerturbationPtr> perturbations;
00316   database_->getAllPerturbationsForModel(model_.model_id, perturbations);
00317 
00318   ROS_DEBUG("Retrieved %zd perturbations of grasps for model %d from the DB", perturbations.size(), model_.model_id);
00319 
00320   perturbations_.insert(perturbations_.begin(), perturbations.begin(), perturbations.end());
00321 }
00322 
00323 void PerturbationGraspRetriever::getGrasps(std::vector<GraspWithMetadata> &grasps)
00324 {
00325   if (gstar_ == NULL)
00326   {
00327     ROS_ERROR("Requested perturbations for a grasp, but grasp is not set.");
00328     return;
00329   }
00330 
00332   grasps.reserve(grasps.size()+250);
00333   BOOST_FOREACH(DatabasePerturbationPtr &perturbation, perturbations_)
00334   {
00335     int grasp_id = perturbation->grasp_id_.get();
00336     if (grasp_id == gstar_->grasp_id_)
00337     {
00339       GraspWithMetadata grasp_with_metadata;
00340       grasp_with_metadata.model_id_ = model_.model_id;
00341       grasp_with_metadata.grasp_id_ = grasp_id;
00342       grasp_with_metadata.energy_function_score_ = perturbation->score_.get();
00343       grasp_with_metadata.object_pose_ = gstar_->object_pose_;
00344 
00345       object_manipulation_msgs::Grasp grasp;
00346       //grasp.grasp_pose
00347 
00349       //grasp_with_metadata.
00350       std::vector<double> deltas = perturbation->deltas_.get();
00351 
00352       grasps.push_back(grasp_with_metadata);
00353     }
00354   }
00355 }
00356 
00357 ClusterPlannerGraspRetriever::ClusterPlannerGraspRetriever(ros::NodeHandle &nh, const std::string &cluster_planner_name,
00358                                                      const object_manipulation_msgs::GraspableObject &graspable_object,
00359                                                            const std::string &arm_name) :
00360   GraspRetriever(arm_name)
00361 {
00362   //PROF_TIMER_FUNC(CLUSTER_PLANNER_CREATE_PROF);
00363   cluster_planner_srv_ = register_service<object_manipulation_msgs::GraspPlanning>(nh, cluster_planner_name);
00364   cloud_ = graspable_object.cluster;
00365   fetchFromPlanner(graspable_object);
00366 }
00367 
00368 void ClusterPlannerGraspRetriever::fetchFromPlanner(const object_manipulation_msgs::GraspableObject &graspable_object)
00369 {
00370   object_manipulation_msgs::GraspPlanning planner_call;
00373   planner_call.request.target = graspable_object;
00374   planner_call.request.arm_name = arm_name_;
00375   cluster_planner_srv_.call(planner_call);
00376   if (planner_call.response.error_code.value != object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS)
00377   {
00378    ROS_ERROR("Call to cluster planner failed!");
00379   }
00380   grasps_from_cluster_planner_ = planner_call.response.grasps;
00381   ROS_INFO("Got %zd grasps from the cluster planner",grasps_from_cluster_planner_.size());
00382 }
00383 
00384 void ClusterPlannerGraspRetriever::getGrasps(std::vector<GraspWithMetadata> &grasps)
00385 {
00386 
00388   appendGraspsFromClusterPlanner(grasps);
00389 }
00390 
00391 void ClusterPlannerGraspRetriever::appendGraspsFromClusterPlanner(std::vector<GraspWithMetadata> &grasps)
00392 {
00393   grasps.reserve(grasps.size()+grasps_from_cluster_planner_.size());
00394 
00395 
00396   BOOST_FOREACH(object_manipulation_msgs::Grasp &grasp, grasps_from_cluster_planner_)
00397   {
00398     GraspWithMetadata grasp_with_metadata;
00399     grasp_with_metadata.grasp_ = grasp;
00400     //grasp_with_metadata.grasp_.success_probability = 0.0;
00401     grasp_with_metadata.model_id_ = -1;
00402     grasp_with_metadata.grasp_id_ = -1;
00403 
00404     grasp_with_metadata.object_pose_.setIdentity();
00405     grasp_with_metadata.object_pose_.frame_id_ = cloud_.header.frame_id;
00406     grasp_with_metadata.object_pose_.stamp_ = cloud_.header.stamp;
00407 
00408     tf::Pose grasp_in_base_frame;
00409     tf::poseMsgToTF(grasp.grasp_pose, grasp_in_base_frame);
00410 
00411     //tf::Pose best_object_to_grasp(base_to_best_object.inverse() * grasp_in_base_frame);
00412 
00413     tf::poseTFToMsg(grasp_in_base_frame,grasp_with_metadata.grasp_.grasp_pose);
00414 
00415     double wrist_to_tool_point_offset_ = 0.13;
00416 
00417     tf::Pose grasp_to_tool_point(tf::Matrix3x3::getIdentity(),tf::Vector3(wrist_to_tool_point_offset_,0,0));
00418 
00420     grasp_with_metadata.tool_point_pose_ = tf::Stamped<tf::Pose>(
00421         grasp_in_base_frame * grasp_to_tool_point, cloud_.header.stamp,
00422         cloud_.header.frame_id);
00423 
00424 
00425     grasps.push_back(grasp_with_metadata);
00426   }
00427 }
00428 
00429 
00430 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


probabilistic_grasp_planner
Author(s): Peter Brook
autogenerated on Fri Jan 25 2013 15:03:22