$search
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(btMatrix3x3::getIdentity(),btVector3(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(btMatrix3x3::getIdentity(),btVector3(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