grasp_adjust.cpp
Go to the documentation of this file.
00001 /*
00002  * <name of package>
00003  * Copyright (c) 2010, Willow Garage, Inc.
00004  * All rights reserved.
00005  *
00006  * Software License Agreement (BSD License)
00007  *
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of Willow Garage nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00036 
00037 
00038 #include <ros/ros.h>
00039 
00040 #include <tf/tf.h>
00041 #include <tf/transform_listener.h>
00042 #include <tf/transform_broadcaster.h>
00043 
00044 #include <boost/bind.hpp>
00045 
00046 #include "pcl/io/pcd_io.h"
00047 #include "pcl/point_types.h"
00048 #include <pcl_ros/transforms.h>
00049 
00050 #include <visualization_msgs/Marker.h>
00051 #include <visualization_msgs/MarkerArray.h>
00052 
00053 #include <dynamic_reconfigure/server.h>
00054 #include "pr2_grasp_adjust/EstimateConfig.h"
00055 #include "pr2_grasp_adjust/DebugConfig.h"
00056 #include "pr2_grasp_adjust/GraspAdjust.h"
00057 
00058 #include <iostream>
00059 #include <fstream>
00060 #include <queue>
00061 
00062 //#include <object_manipulation_msgs/ManipulationResult.h>
00063 #include <object_manipulation_msgs/GraspPlanningErrorCode.h>
00064 #include <object_manipulator/tools/shape_tools.h>
00065 #include <object_manipulator/tools/msg_helpers.h>
00066 
00067 #include "gripper_model.h"
00068 #include "helpers.h"
00069 #include "grasp_adjust.h"
00070 
00071 /*
00072 #define PROF_ENABLED
00073 #include <profiling/profiling.h>
00074 PROF_DECLARE(TOTAL_TIMER)
00075 PROF_DECLARE(TRANSFORM_INPUT_CLOUD)
00076 PROF_DECLARE(DO_DESCENT)
00077 PROF_DECLARE(PROF_1)
00078 */
00079 
00080 typedef pcl::PointXYZRGBNormal PointT;
00081 
00082 const double WORST_SCORE = 0.0;
00083 const int GLOBAL_SEARCH =  pr2_grasp_adjust::Estimate_global_search;
00084 const int LOCAL_SEARCH =  pr2_grasp_adjust::Estimate_local_search;
00085 const int SINGLE_POSE =  pr2_grasp_adjust::Estimate_single_pose;
00086 
00087     GraspAdjust::GraspAdjust():
00088             nh_("/"),
00089             nh_pvt_("~"),
00090             training_(0)
00091     {
00092 //        // Clouds in and out
00093 //        pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(output_topic_, 1);
00094 //        pub_cloud_roi_ = nh_.advertise<sensor_msgs::PointCloud2>("/roi_snapshot", 1);
00095 //
00096 //        // Marker topics
00097 //        pub_marker_ = nh_.advertise<visualization_msgs::Marker>("/markers", 1);
00098 //        pub_marker_array_ = nh_.advertise<visualization_msgs::MarkerArray>("/markers_array", 1);
00099 
00100         // if training, open the output file
00101 //        if(training_){
00102 //            ROS_INFO("in training mode, opening training_output.txt");
00103 //            train_file_.open("/u/hsiao/gripper_stereo_data/training_data/training_output.txt");
00104 //        }
00105 
00106         ROS_INFO("Grasp adjust: Finished constructor!");
00107     }
00108 
00109 
00110     void GraspAdjust::setInputCloud(const pcl::PointCloud<PointT>  &cloud){
00111       this->last_cloud_ = cloud;
00112     }
00113 
00114 
00115 
00124     bool GraspAdjust::doGradientDescent(GripperModel &seed, int steps)
00125     {
00126         //PROF_TIMER_FUNC(DO_DESCENT)
00127 
00128         int steps_remaining = steps;
00129         char fields[6] = {'R', 'Y', 'Z', 'X', 'P', 'Y'};
00130 
00131         double previousBest = seed.score_;
00132         double scoreChange = 1;
00133 
00134         ROS_DEBUG(" - - - - - - - - - - Doing a Gradient Descent with (up to) % 3d Steps - - - - - - -", steps_remaining);
00135 
00136         while( scoreChange > config_.epsilon && steps_remaining > 0 && ros::ok())
00137         {
00138             steps_remaining--;
00139             double linear_step = config_.linear_step_size;
00140             double angular_step = config_.angular_step_size;
00141             int mod = 0;
00142 
00143             if(config_.debug_stepping)
00144                 ROS_INFO(" - - - - - - - - - - Steps remaining: % 3d - - - - - - -", steps_remaining);
00145 
00146             pcl::PointCloud<PointT> temp;
00147 
00148             box_filter(cloud_in_, temp, seed.bounding_box_, false, false);
00149 
00150 
00151             for(unsigned int field = 0; field < 6; field++)
00152             {
00153                 if( !config_.search_6_dof && field >= 4 )
00154                     continue;
00155 
00156                 if(config_.debug_stepping)
00157                 {
00158                     ROS_INFO(" - - ");
00159                     ROS_INFO(" - - %c step - - ", fields[field]);
00160                 }
00161                 for( mod = -config_.gradient_search_window; mod <=config_.gradient_search_window; mod += 1)
00162                 {
00163                     if(mod == 0) continue;
00164 
00165                     tf::Transform initial = seed.wrist_frame_;
00166 
00167                     tf::Transform T_rot_only = initial;
00168                     T_rot_only.setOrigin(tf::Vector3(0, 0, 0));
00169 
00170                     float x, y, z, roll, pitch, yaw;
00171                     x = linear_step*mod*(field == 3);
00172                     y = linear_step*mod*(field == 1);
00173                     z = linear_step*mod*(field == 2);
00174                     roll = angular_step*mod*(field == 0);
00175                     pitch = angular_step*mod*(field == 4);
00176                     yaw = angular_step*mod*(field == 5);
00177 
00178 
00179                     tf::Transform T_trans = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(x, y, z));
00180                     tf::Transform T_rot;
00181                     tf::Transform T_offset;
00182 
00183                     if( config_.search_6_dof )
00184                     {
00185                         T_rot = tf::Transform( tf::createQuaternionFromRPY(roll, pitch, yaw), tf::Vector3( 0, 0, 0));
00186                         T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot * DEFAULT_GRIPPER.tool_frame_.inverse();
00187                     }
00188                     else
00189                     {
00190                         T_rot = tf::Transform( tf::createQuaternionFromRPY(0, 0, roll), tf::Vector3( 0, 0, 0));
00191                         T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot_only.inverse() * T_rot * T_rot_only * DEFAULT_GRIPPER.tool_frame_.inverse();
00192                     }
00193 
00194                     GripperModel gripper = seed;
00195                     gripper.transform( initial * T_offset * initial.inverse() );
00196 
00197                     {
00198                         //PROF_TIMER_FUNC(PROF_1)
00199                         gripper.evaluateGrasp(temp, weights_, centroid_ );
00200                     }
00201                     if(gripper.score_ > seed.score_ + config_.epsilon)
00202                     {
00203                         seed = gripper;
00204                     }
00205                 }
00206                 if(config_.debug_stepping){
00207                     seed.evaluateGrasp(temp, weights_, centroid_, true );
00208                     seed.publishModel(*pub_marker_array_, std::string("local"), 0, ros::Duration(10.0), 0.5f);
00209                     ros::Duration(config_.pause_time).sleep();
00210                 }
00211             } // for field
00212              scoreChange = seed.score_ - previousBest;
00213              previousBest = seed.score_;
00214          } // while score
00215 
00216         ROS_DEBUG("                                                - Took %d steps", steps - steps_remaining);
00217         if(scoreChange < config_.epsilon) return false;
00218         return true;
00219     }
00220 
00221     int GraspAdjust::findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
00222         std::vector<geometry_msgs::PoseStamped> *adjusted_poses,
00223         std::vector<double> *pose_scores,
00224         int search_mode,
00225         std::string common_frame)
00226     {
00227       std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> ranked_poses;
00228 
00229       int value = findGrasps(grasp_pose_in, &ranked_poses, search_mode, common_frame);
00230 
00231       // Convert to a list of messages format...
00232       while(!ranked_poses.empty())
00233       {
00234         GripperModel model = ranked_poses.top();
00235         ranked_poses.pop();
00236 
00237         geometry_msgs::PoseStamped ps;
00238         model.toPoseStamped(ps);
00239         adjusted_poses->push_back(ps);
00240         pose_scores->push_back(model.score_);
00241       }
00242       return value;
00243     }
00244 
00245 
00256     int GraspAdjust::findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
00257         std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> *ranked_poses,
00258         int search_mode,
00259         std::string common_frame)
00260     {
00261         ROS_INFO("\nFind grasps!");
00262         //PROF_RESET_ALL;
00263         //PROF_START_TIMER(TOTAL_TIMER);
00264 
00265         now_ = ros::Time(0);
00266 
00267         if(strcmp(last_cloud_.header.frame_id.c_str(), common_frame.c_str()))
00268         {
00269             ROS_DEBUG("Transforming cloud with %d points from %s to %s.", (int)last_cloud_.points.size(), last_cloud_.header.frame_id.c_str(), common_frame.c_str());
00270             if(!listener_->waitForTransform(common_frame.c_str(), last_cloud_.header.frame_id, now_, ros::Duration(2.0)) )
00271             {
00272                 ROS_ERROR("Couldn't get transform for cloud, returning FAILURE!");
00273                 return object_manipulation_msgs::GraspPlanningErrorCode::TF_ERROR;
00274             }
00275             pcl_ros::transformPointCloudWithNormals(common_frame.c_str(), last_cloud_, cloud_in_, *listener_);
00276         }
00277         else
00278         {
00279             cloud_in_ = last_cloud_;
00280         }
00281 
00282         if(pub_cloud_->getNumSubscribers()){
00283             sensor_msgs::PointCloud2 debug1;
00284             pcl::toROSMsg(cloud_in_, debug1);
00285             pub_cloud_->publish(debug1);
00286         }
00287 
00288         geometry_msgs::PoseStamped grasp_pose = grasp_pose_in;
00289         if(strcmp(grasp_pose.header.frame_id.c_str(), common_frame.c_str()))
00290         {
00291             ROS_DEBUG("Transforming input grasp pose from %s to %s.", grasp_pose.header.frame_id.c_str(), common_frame.c_str());
00292 
00293             if(!listener_->waitForTransform(common_frame.c_str(), grasp_pose.header.frame_id, now_, ros::Duration(2.0)) )
00294             {
00295                 ROS_ERROR("Couldn't get transform for pose, returning FAILURE!");
00296                 return object_manipulation_msgs::GraspPlanningErrorCode::TF_ERROR;
00297             }
00298             listener_->transformPose(common_frame.c_str(), grasp_pose, grasp_pose);
00299         }
00300 
00301         tf::Transform common_T_grasp;
00302         tf::poseMsgToTF(grasp_pose.pose, common_T_grasp);
00303 
00304         starting_gripper_ = DEFAULT_GRIPPER;
00305         starting_gripper_.setStamp(now_);
00306         starting_gripper_.transform(common_T_grasp, common_frame.c_str());
00307 
00308         GripperModel globalGripper = starting_gripper_;
00309         globalGripper.state_.set(GraspState::NO_OBJECT);
00310         globalGripper.score_ = WORST_SCORE;
00311 
00312         weights_.setWeights(    config_.centroid_weight,
00313                                 config_.symmetry_weight,
00314                                 config_.point_weight,
00315                                 config_.Iyz_weight,
00316                                 config_.collision_weight,
00317                                 config_.normal_weight
00318                                 );
00319 
00320 
00321 
00322         ROS_DEBUG("Publishing initial grasp pose...");
00323         starting_gripper_.publishModel(*pub_marker_array_, std::string("starting gripper"), 13, ros::Duration());
00324 
00325 
00326         if(config_.show_model)
00327         {
00328             ROS_DEBUG("Publishing default model...");
00329             GripperModel default_gripper = DEFAULT_GRIPPER;
00330             default_gripper.header.stamp = now_;
00331             default_gripper.publishModel(*pub_marker_array_, std::string("model"), 0, ros::Duration(10.0), 1.0f);
00332         }
00333 
00334         ROS_DEBUG("Removing points far from pose...");
00335         // ---------------------------------------
00336 
00337         pcl::PointCloud<PointT> cloud_roi;
00338         //Box roi_box = Box(tf::Pose(tf::Quaternion(0,0,0,1), tf::Vector3(0.3, 0, 0)), tf::Vector3(0.4, 0.2, 0.2));
00339         //roi_box.frame = starting_gripper_.wrist_frame_ * roi_box.frame;
00340 
00341         box_filter(cloud_in_, cloud_roi, starting_gripper_.roi_box_, true, false);
00342         ROS_INFO("Cloud_in had %d points, cloud_roi has %d points", (int)cloud_in_.points.size(), (int)cloud_roi.points.size());
00343         object_manipulator::drawBox(*pub_marker_, starting_gripper_.roi_box_, "roi_box", 11);
00344 
00345         if(cloud_roi.points.size() == 0)
00346         {
00347             ROS_INFO("Couldn't find any points near starting gripper pose!");
00348             return object_manipulation_msgs::GraspPlanningErrorCode::OTHER_ERROR;
00349         }
00350 
00351         double min_distance = 100000.0;
00352         {
00353             PointT point;
00354             for(unsigned int i = 0; i < cloud_roi.points.size(); i++ )
00355             {
00356                 point = cloud_roi.points[i];
00357                 if(point.x < min_distance)
00358                     min_distance = point.x;
00359             }
00360         }
00361         min_distance += DEFAULT_GRIPPER.roi_box_.frame.getOrigin().getX();
00362 
00363         // restore to common frame
00364         pcl_ros::transformPointCloudWithNormals( cloud_roi, cloud_roi, starting_gripper_.roi_box_.frame);
00365 
00366         if(pub_cloud_roi_->getNumSubscribers()){
00367             sensor_msgs::PointCloud2 debug1;
00368             pcl::toROSMsg(cloud_roi, debug1);
00369             pub_cloud_roi_->publish(debug1);
00370         }
00371 
00372 
00373 
00374         ROS_DEBUG("Doing table filter...");
00375         // ---------------------------------------
00376 
00377         listener_->waitForTransform(common_frame.c_str(), "base_footprint", now_, ros::Duration(2.0));
00378 
00379         Box table_box = Box(tf::Pose(tf::Quaternion(0,0,0,1), tf::Vector3(0.5,0, config_.table_height + 0.2)), tf::Vector3(1, 1.5, 2*0.2));
00380         table_box.header.stamp = now_;
00381         table_box.header.frame_id = "base_footprint";
00382         object_manipulator::drawBox(*pub_marker_, table_box, "table_box", 1, ros::Duration(10.0), object_manipulator::msg::createColorMsg(0.6, 0.3, 0.3, 0.5));
00383 
00384         pcl::PointCloud<PointT> cloud_base_link;
00385         pcl_ros::transformPointCloudWithNormals("base_footprint", cloud_in_, cloud_base_link, *listener_);
00386         box_filter(cloud_base_link, cloud_base_link, table_box, false);
00387         pcl_ros::transformPointCloudWithNormals(common_frame.c_str(), cloud_base_link, cloud_base_link, *listener_);
00388 
00389         ROS_DEBUG("Table filter left %d points...", (int)cloud_base_link.points.size());
00390 
00391         if(cloud_base_link.points.size() == 0)
00392         {
00393             ROS_INFO("Couldn't find any points in the table box!");
00394             return object_manipulation_msgs::GraspPlanningErrorCode::OTHER_ERROR;
00395         }
00396 
00397         centroid_ = tf::Vector3(0,0,0);
00398         for(unsigned int i = 0; i < cloud_base_link.points.size(); i++ )
00399         {
00400             PointT point = cloud_base_link.points[i];
00401             centroid_ += tf::Vector3(point.x, point.y, point.z);
00402         }
00403         centroid_ /= cloud_base_link.points.size() + 0.001;
00404 
00405 
00406         //pcl::PointCloud<PointT> cloud_pose_frame;
00407         //pcl_ros::transformPointCloudWithNormals(cloud_in_, cloud_pose_frame, common_T_grasp.inverse());
00408 
00409 
00410 
00411         ROS_INFO("Input cloud: min_distance %f; centroid [%s]  % 4.2f, % 4.2f, % 4.2f",
00412                  min_distance,
00413                  cloud_base_link.header.frame_id.c_str(),
00414                  centroid_.getX(),
00415                  centroid_.getY(),
00416                  centroid_.getZ());
00417 
00418         cloud_in_ = cloud_roi;
00419 
00420         // --------------------------------------------
00421 
00422         double x = 0, y = 0, z = 0, roll = 0, pitch = 0, yaw = 0;
00423         double start_x = 0, start_y = 0, start_z = 0;
00424         double start_roll = 0.0, start_pitch = 0.0, start_yaw = 0.0;
00425 
00426 
00428         // Make list of points to check
00429 
00430 
00431 
00432         std::vector< params > param_list;
00433         std::vector<tf::Transform> seed_list;
00434         ROS_INFO("Populating seeds...");
00435         int g_num = config_.grid_step_num;
00436         double g_size = config_.grid_step_size;
00437         if(search_mode == GLOBAL_SEARCH)
00438         {
00439 
00440             for( start_roll = -M_PI/4; start_roll < M_PI/4 + 0.1; start_roll += M_PI/4)
00441             {
00442                 for( start_z = -g_size * g_num; start_z < g_size*g_num + 0.01; start_z+= g_size)
00443                 {
00444                     for( start_y = -g_size * g_num; start_y < g_size * g_num + 0.01; start_y+=g_size)
00445                     {
00446                         for( start_x = 0; start_x < g_size * g_num + 0.01; start_x+=g_size)
00447                         {
00448                             x = start_x + min_distance - 0.15; // TODO magic number (front-ish of palm)!
00449                             if(config_.search_6_dof)
00450                             {
00451                                 //for( start_pitch = -M_PI/4; start_pitch < M_PI/4 + 0.1; start_pitch += M_PI/4)
00452                                 {
00453                                 //    for( start_yaw = -M_PI/4; start_yaw < M_PI/4 + 0.1; start_yaw += M_PI/4)
00454                                     {
00455                                         params vars(x, start_y, start_z, start_roll, start_pitch, start_yaw);
00456                                         param_list.push_back(vars);
00457                                     }
00458                                 }
00459                             }
00460                             else
00461                             {
00462                                 params vars(x, start_y, start_z, start_roll, 0.0, 0.0);
00463                                 param_list.push_back(vars);
00464                             }
00465                         }
00466                     }
00467                 }
00468             }
00469         }
00470         else if(search_mode == LOCAL_SEARCH)
00471         {
00472             x = start_x + min_distance - 0.15; // TODO magic number (front-ish of palm)!
00473             y = start_y;
00474             z = start_z;
00475 
00476             for( start_roll = -M_PI/4; start_roll < M_PI/4 + 0.1; start_roll += M_PI/4)
00477             {
00478                 //for( start_pitch = -M_PI/4; start_pitch < M_PI/4 + 0.1; start_pitch += M_PI/4)
00479                 //{
00480                 //    for( start_yaw = -M_PI/4; start_yaw < M_PI/4 + 0.1; start_yaw += M_PI/4)
00481                 //    {
00482                         params vars(x, start_y, start_z, start_roll, 0.0, 0.0);
00483                         param_list.push_back(vars);
00484                 //    }
00485                 //}
00486             }
00487         }
00488         else if( search_mode == SINGLE_POSE) // for debugging, when we want to see changes to exactly one pose
00489         {
00490             ROS_INFO("Populating with single pose...");
00491             x = debug_.seed_x + min_distance - 0.15;
00492             y = debug_.seed_y;
00493             z = debug_.seed_z;
00494             roll = debug_.seed_roll * M_PI/180.0;
00495             pitch = debug_.seed_pitch * M_PI/180.0;
00496             yaw = debug_.seed_yaw * M_PI/180.0;
00497 
00498             params vars(x, y, z, roll, pitch, yaw);
00499             param_list.push_back(vars);
00500         }
00501         ROS_DEBUG("Param list populated with %d starting positions!", (int) param_list.size());
00502 
00503 
00504         // Create the transform list from the parameters
00505         for( unsigned int i = 0; i < param_list.size(); i++)
00506         {
00507             x =     param_list[i].X;
00508             y =     param_list[i].Y;
00509             z =     param_list[i].Z;
00510             roll =  param_list[i].r;
00511             pitch = param_list[i].p;
00512             yaw =   param_list[i].y;
00513 
00514             tf::Transform T_rot_only = common_T_grasp;
00515             T_rot_only.setOrigin(tf::Vector3(0, 0, 0));
00516 
00517             tf::Transform T_rot;
00518             tf::Transform T_offset;
00519             tf::Transform T_trans = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(x, y, z));
00520 
00521             if( config_.search_6_dof )
00522             {
00523                 T_rot = tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw), tf::Vector3( 0, 0, 0));
00524                 T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot * DEFAULT_GRIPPER.tool_frame_.inverse();
00525             }
00526             else
00527             {
00528                 T_rot = tf::Transform(tf::createQuaternionFromRPY(0, 0, roll), tf::Vector3( 0, 0, 0));
00529                 T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot_only.inverse() * T_rot * T_rot_only * DEFAULT_GRIPPER.tool_frame_.inverse();
00530             }
00531 
00532             tf::Transform common_T_candidate = common_T_grasp * T_offset * common_T_grasp.inverse();
00533             seed_list.push_back(common_T_candidate);
00534         }
00535         ROS_INFO("Seed list populated with %d starting positions!", (int) seed_list.size());
00536 
00537         std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> seed_queue;
00538         std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> progress_queue;
00539         std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> maxima_queue;
00540 
00541         for( unsigned int i = 0; i < seed_list.size(); i++ )
00542         {
00543             tf::Transform T = seed_list[i];
00544             GripperModel gripper = starting_gripper_;
00545             gripper.transform(T);
00546             pcl::PointCloud<PointT> temp;
00547             box_filter(cloud_in_, temp, gripper.bounding_box_, false, false);
00548             gripper.evaluateGrasp(temp, weights_, centroid_ );
00549 
00550             if(search_mode == SINGLE_POSE)
00551                 seed_queue.push(gripper);
00552             else
00553             {
00554                 if(gripper.state_.no_object && !gripper.state_.inCollision()) {
00555                     //do nothing
00556                 }
00557                 else
00558                 {
00559                     seed_queue.push(gripper);
00560 
00561                     if(config_.debug_stepping)
00562                     {
00563                         ROS_INFO("seed has score %f", gripper.score_ );
00564                         gripper.publishModel(*pub_marker_array_, std::string("seeds"), i);
00565                         ros::Duration(config_.pause_time).sleep();
00566                     }
00567                 }
00568             }
00569 
00570 
00571         }
00572         ROS_INFO("Seed queue has %d members", (int)seed_queue.size());
00573 
00574         if(search_mode == SINGLE_POSE)
00575         {
00576             GripperModel seed = seed_queue.top();
00577             tf::Transform initial = seed.wrist_frame_;
00578 
00579             tf::Transform T_rot_only = initial;
00580             T_rot_only.setOrigin(tf::Vector3(0, 0, 0));
00581 
00582             x = debug_.offset_x; //min_distance - (0.15 + grasp_pose.pose.position.x);
00583             y = debug_.offset_y;
00584             z = debug_.offset_z;
00585             roll = debug_.offset_roll * M_PI/180.0;
00586             pitch = debug_.offset_pitch * M_PI/180.0;
00587             yaw = debug_.offset_yaw * M_PI/180.0;
00588 
00589             tf::Transform T_trans = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(x, y, z));
00590             tf::Transform T_rot;
00591             tf::Transform T_offset;
00592 
00593 
00594             if( config_.search_6_dof )
00595             {
00596                 T_rot = tf::Transform( tf::createQuaternionFromRPY(roll, pitch, yaw), tf::Vector3( 0, 0, 0));
00597                 T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot * DEFAULT_GRIPPER.tool_frame_.inverse();
00598             }
00599             else
00600             {
00601                 T_rot = tf::Transform( tf::createQuaternionFromRPY(0, 0, roll), tf::Vector3( 0, 0, 0));
00602                 T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot_only.inverse() * T_rot * T_rot_only * DEFAULT_GRIPPER.tool_frame_.inverse();
00603             }
00604 
00605             GripperModel gripper = seed;
00606             gripper.transform( initial * T_offset * initial.inverse() );
00607 
00608             // TODO this is wrong
00609             Box bbox = gripper.bounding_box_;
00610             bbox.header.frame_id = common_frame.c_str();
00611             bbox.frame.setRotation(tf::Quaternion(0,0,0,1));
00612             object_manipulator::drawBox(*pub_marker_, bbox, "bounding_box", 10);
00613 
00614 
00615 
00616 //            if(pub_cloud_roi_.getNumSubscribers()){
00617 //                sensor_msgs::PointCloud2 debug1;
00618 //                pcl::toROSMsg(temp, debug1);
00619 //                pub_cloud_roi_.publish(debug1);
00620 //            }
00621             pcl::PointCloud<PointT> temp;
00622             box_filter(cloud_in_, temp, gripper.bounding_box_, false, false);
00623             gripper.evaluateGrasp(temp, weights_, centroid_, config_.debug_stepping);
00624 
00625             maxima_queue.push(gripper);
00626         }
00627         else
00628         {
00629             while( !seed_queue.empty() )
00630             {
00631                 GripperModel seed = seed_queue.top();
00632                 seed_queue.pop();
00633 
00634                 int n = config_.init_gradient_steps;
00635                 if( doGradientDescent(seed, n) )
00636                 {
00637                     maxima_queue.push(seed);
00638                 }
00639                 else
00640                 {
00641                     progress_queue.push(seed);
00642                     if(config_.debug_stepping)
00643                     {
00644                         ROS_DEBUG("seed has score %f", seed.score_ );
00645                     }
00646                 }
00647             } // while queue
00648 
00649             ROS_INFO("After two steps, progress queue has %d members and maxima_queue has %d memebers",
00650                      (int)progress_queue.size(),
00651                      (int)maxima_queue.size()
00652                     );
00653 
00654             int descents = std::min((int)progress_queue.size(), (int)config_.full_gradient_descents - (int)maxima_queue.size()) ;
00655             for( int i = 0; i < descents; i++ )
00656             {
00657                 GripperModel seed = progress_queue.top();
00658                 progress_queue.pop();
00659 
00660                 int n = 25;
00661                 if( doGradientDescent(seed, n) )
00662                     maxima_queue.push(seed);
00663                 //else
00664                     // Discard
00665                     // progress_queue.push(seed);
00666 
00667             } // for queue
00668         }
00669 
00670         ROS_INFO("After all steps, maxima_queue has %d memebers", (int)maxima_queue.size() );
00671 
00672         std::vector<GripperModel> maxima_list;
00673         int maxima_count = 0;
00674         while(!maxima_queue.empty() && maxima_count < config_.full_gradient_descents){
00675           GripperModel model = maxima_queue.top();
00676           maxima_queue.pop();
00677           if(! std::isnan(model.score_) )
00678           {
00679             maxima_list.push_back(maxima_queue.top());
00680             maxima_count++;
00681           }
00682         }
00683         for( unsigned int i = 0; i < maxima_list.size(); i++)
00684             ROS_INFO("maxima % 2d has score %f", i, maxima_list[i].score_ );
00685 
00686 
00688 
00689         if(maxima_list.size() > 0)
00690         {
00691           globalGripper = maxima_list[0];
00692           maxima_list[0].publishModel(*pub_marker_array_, std::string("previous_best_pick"), 0, ros::Duration(), 1.0f);
00693           for(unsigned int i = 1; i < maxima_list.size(); i++)
00694           {
00695             maxima_list[i].publishModel(*pub_marker_array_, std::string("alternatives"), i, ros::Duration(), 0.5f);
00696           }
00697         }
00698 
00699         int pose_count = 0;
00700         for(unsigned int i = 0; i < maxima_list.size(); i++)
00701         {
00702           if(!maxima_list[i].state_.inCollision() && maxima_list[i].score_ > config_.minimum_score )
00703           {
00704             ranked_poses->push(maxima_list[i]);
00705             pose_count++;
00706           }
00707         }
00708 
00709         //PROF_STOP_TIMER(TOTAL_TIMER);
00710         //PROF_PRINT_ALL;
00711 
00712         int return_value = 0;
00713         if(pose_count == 0) return_value = object_manipulation_msgs::GraspPlanningErrorCode::OTHER_ERROR;
00714         else return_value = object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS;
00715 
00716         ROS_INFO("GraspAdjust::findGrasps finished, returning %d with %d (total) grasps",
00717                  return_value, (int)ranked_poses->size());
00718 
00719         return return_value;
00720 
00721 
00722     }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends


pr2_grasp_adjust
Author(s): Adam Leeper
autogenerated on Wed Jan 23 2013 15:27:22