grasp_generator.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 #include "bayesian_grasp_planner/grasp_generator.h"
00036 #include <object_manipulation_msgs/GraspPlanning.h>
00037 #include <object_manipulator/tools/hand_description.h>
00038 
00039 namespace bayesian_grasp_planner {
00040 
00041 GraspGeneratorServiceCaller::GraspGeneratorServiceCaller(ros::NodeHandle &nh, const std::string service_name,
00042                                                  const object_manipulation_msgs::GraspableObject &graspable_object, 
00043                                                          const std::string arm_name) : 
00044   object_(graspable_object),arm_name_(arm_name),service_name_(service_name)
00045 {
00046   service_ = register_service<object_manipulation_msgs::GraspPlanning>(nh, service_name);
00047 }
00048 
00049 void GraspGeneratorServiceCaller::generateGrasps()
00050 {
00051   object_manipulation_msgs::GraspPlanning planner_call;
00052 
00053   ROS_INFO("calling grasp service %s", service_name_.c_str());
00054 
00055   //leaves several grasp planning arguments empty (OK for cluster planner, maybe not for others)
00056   planner_call.request.target = object_;
00057   planner_call.request.arm_name = arm_name_;
00058   service_.call(planner_call);
00059   if (planner_call.response.error_code.value != object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS)
00060   {
00061    ROS_ERROR("Call to service-based planner failed!");
00062   }
00063   ROS_INFO("Got %zd grasps from the service-based planner", planner_call.response.grasps.size());
00064   appendMetadataToGrasps(planner_call.response.grasps, grasps_);
00065 }
00066 
00067 
00068 void GraspGeneratorServiceCaller::appendMetadataToGrasps(std::vector<object_manipulation_msgs::Grasp> &grasp_msgs,
00069                                                          std::vector<GraspWM> &grasps)
00070 {
00071   grasps.reserve(grasps.size()+grasp_msgs.size());
00072 
00073 
00074   BOOST_FOREACH(object_manipulation_msgs::Grasp &grasp, grasp_msgs)
00075   {
00076     GraspWM grasp_with_metadata;
00077     grasp_with_metadata.grasp_ = grasp;
00078     grasp_with_metadata.model_id_ = -1;
00079     grasp_with_metadata.grasp_id_ = -1;
00080 
00081     grasp_with_metadata.object_pose_.setIdentity();
00082     grasp_with_metadata.object_pose_.frame_id_ = object_.cluster.header.frame_id;
00083     grasp_with_metadata.object_pose_.stamp_ = object_.cluster.header.stamp;
00084     grasp_with_metadata.energy_function_score = grasp.success_probability;
00085 
00086     tf::Pose grasp_in_base_frame;
00087     tf::poseMsgToTF(grasp.grasp_pose, grasp_in_base_frame);
00088     tf::poseTFToMsg(grasp_in_base_frame,grasp_with_metadata.grasp_.grasp_pose);
00089     double wrist_to_tool_point_offset_ = 0.13;
00090     tf::Pose grasp_to_tool_point(tf::Matrix3x3::getIdentity(),tf::Vector3(wrist_to_tool_point_offset_,0,0));
00091 
00093     grasp_with_metadata.tool_point_pose_ = tf::Stamped<tf::Pose>(
00094         grasp_in_base_frame * grasp_to_tool_point, object_.cluster.header.stamp,
00095         object_.cluster.header.frame_id);
00096 
00097     grasps.push_back(grasp_with_metadata);
00098   }
00099 }
00100 
00101 GraspGeneratorDatabaseRetriever::GraspGeneratorDatabaseRetriever
00102   (boost::shared_ptr<household_objects_database::ObjectsDatabase> database,
00103    household_objects_database_msgs::DatabaseModelPose model, const std::string arm_name,
00104    bool cluster_reps) : 
00105     database_(database),
00106     model_(model),
00107     arm_name_(arm_name),
00108     cluster_reps_(cluster_reps)
00109 {
00110   if (cluster_reps_) ROS_INFO("creating cluster reps GraspGeneratorDatabaseRetriever for model_id %d", model.model_id);
00111   else ROS_INFO("creating GraspGeneratorDatabaseRetriever for model_id %d", model.model_id);
00112 }
00113 
00114 void GraspGeneratorDatabaseRetriever::generateGrasps()
00115 {
00116   std::vector<household_objects_database::DatabaseGraspPtr> db_grasps;
00117   ROS_INFO("generating grasps by pulling them from database for model %d", model_.model_id);
00118 
00119   //retrieve the raw grasps from the database
00120   bool result;
00121   if (cluster_reps_)
00122   {
00123     result = database_->getClusterRepGrasps(model_.model_id, 
00124                                         object_manipulator::handDescription().handDatabaseName(arm_name_), db_grasps);
00125   }
00126   else
00127   {
00128     result = database_->getGrasps(model_.model_id, 
00129                                   object_manipulator::handDescription().handDatabaseName(arm_name_), db_grasps);
00130   }
00131   if (!result) ROS_ERROR("Robust Database Grasp Planning Node: grasp retrieval error");
00132   ROS_WARN("Size from DB: %zd",db_grasps.size());
00133   appendMetadataToGrasps(db_grasps, grasps_);
00134   ROS_WARN("Size after appending: %zd",grasps_.size());
00135 }
00136 
00137 
00138 void GraspGeneratorDatabaseRetriever::appendMetadataToGrasps(
00139                                             const std::vector<household_objects_database::DatabaseGraspPtr> &db_grasps,
00140                                             std::vector<GraspWM> &grasps)
00141 {
00142   grasps.reserve(grasps.size()+db_grasps.size());
00143 
00144   BOOST_FOREACH(household_objects_database::DatabaseGraspPtr db_grasp, db_grasps) {
00145     GraspWM grasp;
00146 
00147     ROS_ASSERT( db_grasp->final_grasp_posture_.get().joint_angles_.size() ==
00148               db_grasp->pre_grasp_posture_.get().joint_angles_.size() );
00149     ROS_ASSERT(db_grasp->scaled_model_id_.get() ==  model_.model_id);
00150 
00151     std::vector<std::string> joint_names = object_manipulator::handDescription().handJointNames(arm_name_);
00152     if (object_manipulator::handDescription().handDatabaseName(arm_name_) != "WILLOW_GRIPPER_2010")
00153     {
00154       //check that the number of joints in the ROS description of this hand
00155       //matches the number of values we have in the database
00156       if (joint_names.size() != db_grasp->final_grasp_posture_.get().joint_angles_.size())
00157       {
00158         ROS_ERROR("Database grasp specification does not match ROS description of hand. "
00159                   "Hand is expected to have %zd joints, but database grasp specifies %zd values",
00160                   joint_names.size(), db_grasp->final_grasp_posture_.get().joint_angles_.size());
00161         continue;
00162       }
00163       //for now we silently assume that the order of the joints in the ROS description of
00164       //the hand is the same as in the database description
00165       grasp.grasp_.pre_grasp_posture.name = joint_names;
00166       grasp.grasp_.grasp_posture.name = joint_names;
00167       grasp.grasp_.pre_grasp_posture.position = db_grasp->pre_grasp_posture_.get().joint_angles_;
00168       grasp.grasp_.grasp_posture.position = db_grasp->final_grasp_posture_.get().joint_angles_;
00169     }
00170     else
00171     {
00172       //unfortunately we have to hack this, as the grasp is really defined by a single
00173       //DOF, but the urdf for the PR2 gripper is not well set up to do that
00174       if ( joint_names.size() != 4 || db_grasp->final_grasp_posture_.get().joint_angles_.size() != 1)
00175       {
00176         ROS_ERROR("PR2 gripper specs and database grasp specs do not match expected values");
00177         continue;
00178       }
00179       grasp.grasp_.pre_grasp_posture.name = joint_names;
00180       grasp.grasp_.grasp_posture.name = joint_names;
00181       //replicate the single value from the database 4 times
00182       grasp.grasp_.pre_grasp_posture.position.resize( joint_names.size(),
00183                                                    db_grasp->pre_grasp_posture_.get().joint_angles_.at(0));
00184       grasp.grasp_.grasp_posture.position.resize( joint_names.size(),
00185                                                db_grasp->final_grasp_posture_.get().joint_angles_.at(0));
00186     }
00187 
00188     //for now the effort and approach distances are not in the database 
00189     //so we hard-code them in here; this will change at some point
00190     grasp.grasp_.pre_grasp_posture.effort.resize(joint_names.size(), 100);
00191     grasp.grasp_.grasp_posture.effort.resize(joint_names.size(), 50);
00192     grasp.grasp_.desired_approach_distance = 0.10;
00193     grasp.grasp_.min_approach_distance = 0.05;
00194 
00195     grasp.grasp_.success_probability = 0.0;
00196 
00197     //the pose of the grasp
00198     grasp.energy_function_score = db_grasp->scaled_quality_.get();
00199     grasp.model_id_ = db_grasp->scaled_model_id_.get();
00200     grasp.grasp_id_ = db_grasp->id_.get();
00201     tf::Stamped<tf::Pose> base_to_object;
00202     tf::poseStampedMsgToTF(model_.pose,base_to_object);
00203     tf::Pose object_to_grasp;
00204     tf::poseMsgToTF(db_grasp->final_grasp_pose_.get().pose_, object_to_grasp);
00205     grasp.object_pose_ = base_to_object;
00206     tf::Pose base_to_grasp(base_to_object * object_to_grasp);
00207     tf::poseTFToMsg(base_to_grasp,grasp.grasp_.grasp_pose);
00208 
00210     double wrist_to_tool_point_offset_ = 0.13;
00211     tf::Pose offset_tf(tf::Matrix3x3::getIdentity(),tf::Vector3(wrist_to_tool_point_offset_,0,0));
00212     grasp.tool_point_pose_ = tf::Stamped<tf::Pose>(base_to_grasp * offset_tf,
00213         grasp.object_pose_.stamp_, grasp.object_pose_.frame_id_);
00214 
00215     //insert the new grasp in the list
00216     grasps.push_back(grasp);
00217   }
00218 }
00219 
00220 
00221 } //namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


bayesian_grasp_planner
Author(s): Kaijen Hsiao and Matei Ciocarlie
autogenerated on Fri Jan 25 2013 15:02:36