grasp_adjust.cpp

Go to the documentation of this file.

/*
 * <name of package>
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Software License Agreement (BSD License)
 *
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Willow Garage nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */



#include <ros/ros.h>

#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include <boost/bind.hpp>

#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include <pcl_ros/transforms.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <dynamic_reconfigure/server.h>
#include "pr2_grasp_adjust/EstimateConfig.h"
#include "pr2_grasp_adjust/DebugConfig.h"
#include "pr2_grasp_adjust/GraspAdjust.h"

#include <iostream>
#include <fstream>
#include <queue>

//#include <object_manipulation_msgs/ManipulationResult.h>
#include <object_manipulation_msgs/GraspPlanningErrorCode.h>
#include <object_manipulator/tools/shape_tools.h>
#include <object_manipulator/tools/msg_helpers.h>

#include "gripper_model.h"
#include "helpers.h"
#include "grasp_adjust.h"

/*
#define PROF_ENABLED
#include <profiling/profiling.h>
PROF_DECLARE(TOTAL_TIMER)
PROF_DECLARE(TRANSFORM_INPUT_CLOUD)
PROF_DECLARE(DO_DESCENT)
PROF_DECLARE(PROF_1)
*/

typedef pcl::PointXYZRGBNormal PointT;

const double WORST_SCORE = 0.0;
const int GLOBAL_SEARCH =  pr2_grasp_adjust::Estimate_global_search;
const int LOCAL_SEARCH =  pr2_grasp_adjust::Estimate_local_search;
const int SINGLE_POSE =  pr2_grasp_adjust::Estimate_single_pose;

    GraspAdjust::GraspAdjust():
            nh_("/"),
            nh_pvt_("~"),
            training_(0)
    {
//        // Clouds in and out
//        pub_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>(output_topic_, 1);
//        pub_cloud_roi_ = nh_.advertise<sensor_msgs::PointCloud2>("/roi_snapshot", 1);
//
//        // Marker topics
//        pub_marker_ = nh_.advertise<visualization_msgs::Marker>("/markers", 1);
//        pub_marker_array_ = nh_.advertise<visualization_msgs::MarkerArray>("/markers_array", 1);

        // if training, open the output file
//        if(training_){
//            ROS_INFO("in training mode, opening training_output.txt");
//            train_file_.open("/u/hsiao/gripper_stereo_data/training_data/training_output.txt");
//        }

        ROS_INFO("Grasp adjust: Finished constructor!");
    }


    void GraspAdjust::setInputCloud(const pcl::PointCloud<PointT>  &cloud){
      this->last_cloud_ = cloud;
    }



    bool GraspAdjust::doGradientDescent(GripperModel &seed, int steps)
    {
        //PROF_TIMER_FUNC(DO_DESCENT)

        int steps_remaining = steps;
        char fields[6] = {'R', 'Y', 'Z', 'X', 'P', 'Y'};

        double previousBest = seed.score_;
        double scoreChange = 1;

        ROS_DEBUG(" - - - - - - - - - - Doing a Gradient Descent with (up to) % 3d Steps - - - - - - -", steps_remaining);

        while( scoreChange > config_.epsilon && steps_remaining > 0 && ros::ok())
        {
            steps_remaining--;
            double linear_step = config_.linear_step_size;
            double angular_step = config_.angular_step_size;
            int mod = 0;

            if(config_.debug_stepping)
                ROS_INFO(" - - - - - - - - - - Steps remaining: % 3d - - - - - - -", steps_remaining);

            pcl::PointCloud<PointT> temp;

            box_filter(cloud_in_, temp, seed.bounding_box_, false, false);


            for(unsigned int field = 0; field < 6; field++)
            {
                if( !config_.search_6_dof && field >= 4 )
                    continue;

                if(config_.debug_stepping)
                {
                    ROS_INFO(" - - ");
                    ROS_INFO(" - - %c step - - ", fields[field]);
                }
                for( mod = -config_.gradient_search_window; mod <=config_.gradient_search_window; mod += 1)
                {
                    if(mod == 0) continue;

                    tf::Transform initial = seed.wrist_frame_;

                    tf::Transform T_rot_only = initial;
                    T_rot_only.setOrigin(tf::Vector3(0, 0, 0));

                    float x, y, z, roll, pitch, yaw;
                    x = linear_step*mod*(field == 3);
                    y = linear_step*mod*(field == 1);
                    z = linear_step*mod*(field == 2);
                    roll = angular_step*mod*(field == 0);
                    pitch = angular_step*mod*(field == 4);
                    yaw = angular_step*mod*(field == 5);


                    tf::Transform T_trans = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(x, y, z));
                    tf::Transform T_rot;
                    tf::Transform T_offset;

                    if( config_.search_6_dof )
                    {
                        T_rot = tf::Transform( tf::createQuaternionFromRPY(roll, pitch, yaw), tf::Vector3( 0, 0, 0));
                        T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot * DEFAULT_GRIPPER.tool_frame_.inverse();
                    }
                    else
                    {
                        T_rot = tf::Transform( tf::createQuaternionFromRPY(0, 0, roll), tf::Vector3( 0, 0, 0));
                        T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot_only.inverse() * T_rot * T_rot_only * DEFAULT_GRIPPER.tool_frame_.inverse();
                    }

                    GripperModel gripper = seed;
                    gripper.transform( initial * T_offset * initial.inverse() );

                    {
                        //PROF_TIMER_FUNC(PROF_1)
                        gripper.evaluateGrasp(temp, weights_, centroid_ );
                    }
                    if(gripper.score_ > seed.score_ + config_.epsilon)
                    {
                        seed = gripper;
                    }
                }
                if(config_.debug_stepping){
                    seed.evaluateGrasp(temp, weights_, centroid_, true );
                    seed.publishModel(*pub_marker_array_, std::string("local"), 0, ros::Duration(10.0), 0.5f);
                    ros::Duration(config_.pause_time).sleep();
                }
            } // for field
             scoreChange = seed.score_ - previousBest;
             previousBest = seed.score_;
         } // while score

        ROS_DEBUG("                                                - Took %d steps", steps - steps_remaining);
        if(scoreChange < config_.epsilon) return false;
        return true;
    }

    int GraspAdjust::findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
        std::vector<geometry_msgs::PoseStamped> *adjusted_poses,
        std::vector<double> *pose_scores,
        int search_mode,
        std::string common_frame)
    {
      std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> ranked_poses;

      int value = findGrasps(grasp_pose_in, &ranked_poses, search_mode, common_frame);

      // Convert to a list of messages format...
      while(!ranked_poses.empty())
      {
        GripperModel model = ranked_poses.top();
        ranked_poses.pop();

        geometry_msgs::PoseStamped ps;
        model.toPoseStamped(ps);
        adjusted_poses->push_back(ps);
        pose_scores->push_back(model.score_);
      }
      return value;
    }


    int GraspAdjust::findGrasps(const geometry_msgs::PoseStamped &grasp_pose_in,
        std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> *ranked_poses,
        int search_mode,
        std::string common_frame)
    {
        ROS_INFO("\nFind grasps!");
        //PROF_RESET_ALL;
        //PROF_START_TIMER(TOTAL_TIMER);

        now_ = ros::Time(0);

        if(strcmp(last_cloud_.header.frame_id.c_str(), common_frame.c_str()))
        {
            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());
            if(!listener_->waitForTransform(common_frame.c_str(), last_cloud_.header.frame_id, now_, ros::Duration(2.0)) )
            {
                ROS_ERROR("Couldn't get transform for cloud, returning FAILURE!");
                return object_manipulation_msgs::GraspPlanningErrorCode::TF_ERROR;
            }
            pcl_ros::transformPointCloudWithNormals(common_frame.c_str(), last_cloud_, cloud_in_, *listener_);
        }
        else
        {
            cloud_in_ = last_cloud_;
        }

        if(pub_cloud_->getNumSubscribers()){
            sensor_msgs::PointCloud2 debug1;
            pcl::toROSMsg(cloud_in_, debug1);
            pub_cloud_->publish(debug1);
        }

        geometry_msgs::PoseStamped grasp_pose = grasp_pose_in;
        if(strcmp(grasp_pose.header.frame_id.c_str(), common_frame.c_str()))
        {
            ROS_DEBUG("Transforming input grasp pose from %s to %s.", grasp_pose.header.frame_id.c_str(), common_frame.c_str());

            if(!listener_->waitForTransform(common_frame.c_str(), grasp_pose.header.frame_id, now_, ros::Duration(2.0)) )
            {
                ROS_ERROR("Couldn't get transform for pose, returning FAILURE!");
                return object_manipulation_msgs::GraspPlanningErrorCode::TF_ERROR;
            }
            listener_->transformPose(common_frame.c_str(), grasp_pose, grasp_pose);
        }

        tf::Transform common_T_grasp;
        tf::poseMsgToTF(grasp_pose.pose, common_T_grasp);

        starting_gripper_ = DEFAULT_GRIPPER;
        starting_gripper_.setStamp(now_);
        starting_gripper_.transform(common_T_grasp, common_frame.c_str());

        GripperModel globalGripper = starting_gripper_;
        globalGripper.state_.set(GraspState::NO_OBJECT);
        globalGripper.score_ = WORST_SCORE;

        weights_.setWeights(    config_.centroid_weight,
                                config_.symmetry_weight,
                                config_.point_weight,
                                config_.Iyz_weight,
                                config_.collision_weight,
                                config_.normal_weight
                                );



        ROS_DEBUG("Publishing initial grasp pose...");
        starting_gripper_.publishModel(*pub_marker_array_, std::string("starting gripper"), 13, ros::Duration());


        if(config_.show_model)
        {
            ROS_DEBUG("Publishing default model...");
            GripperModel default_gripper = DEFAULT_GRIPPER;
            default_gripper.header.stamp = now_;
            default_gripper.publishModel(*pub_marker_array_, std::string("model"), 0, ros::Duration(10.0), 1.0f);
        }

        ROS_DEBUG("Removing points far from pose...");
        // ---------------------------------------

        pcl::PointCloud<PointT> cloud_roi;
        //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));
        //roi_box.frame = starting_gripper_.wrist_frame_ * roi_box.frame;

        box_filter(cloud_in_, cloud_roi, starting_gripper_.roi_box_, true, false);
        ROS_INFO("Cloud_in had %d points, cloud_roi has %d points", (int)cloud_in_.points.size(), (int)cloud_roi.points.size());
        object_manipulator::drawBox(*pub_marker_, starting_gripper_.roi_box_, "roi_box", 11);

        if(cloud_roi.points.size() == 0)
        {
            ROS_INFO("Couldn't find any points near starting gripper pose!");
            return object_manipulation_msgs::GraspPlanningErrorCode::OTHER_ERROR;
        }

        double min_distance = 100000.0;
        {
            PointT point;
            for(unsigned int i = 0; i < cloud_roi.points.size(); i++ )
            {
                point = cloud_roi.points[i];
                if(point.x < min_distance)
                    min_distance = point.x;
            }
        }
        min_distance += DEFAULT_GRIPPER.roi_box_.frame.getOrigin().getX();

        // restore to common frame
        pcl_ros::transformPointCloudWithNormals( cloud_roi, cloud_roi, starting_gripper_.roi_box_.frame);

        if(pub_cloud_roi_->getNumSubscribers()){
            sensor_msgs::PointCloud2 debug1;
            pcl::toROSMsg(cloud_roi, debug1);
            pub_cloud_roi_->publish(debug1);
        }



        ROS_DEBUG("Doing table filter...");
        // ---------------------------------------

        listener_->waitForTransform(common_frame.c_str(), "base_footprint", now_, ros::Duration(2.0));

        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));
        table_box.header.stamp = now_;
        table_box.header.frame_id = "base_footprint";
        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));

        pcl::PointCloud<PointT> cloud_base_link;
        pcl_ros::transformPointCloudWithNormals("base_footprint", cloud_in_, cloud_base_link, *listener_);
        box_filter(cloud_base_link, cloud_base_link, table_box, false);
        pcl_ros::transformPointCloudWithNormals(common_frame.c_str(), cloud_base_link, cloud_base_link, *listener_);

        ROS_DEBUG("Table filter left %d points...", (int)cloud_base_link.points.size());

        if(cloud_base_link.points.size() == 0)
        {
            ROS_INFO("Couldn't find any points in the table box!");
            return object_manipulation_msgs::GraspPlanningErrorCode::OTHER_ERROR;
        }

        centroid_ = tf::Vector3(0,0,0);
        for(unsigned int i = 0; i < cloud_base_link.points.size(); i++ )
        {
            PointT point = cloud_base_link.points[i];
            centroid_ += tf::Vector3(point.x, point.y, point.z);
        }
        centroid_ /= cloud_base_link.points.size() + 0.001;


        //pcl::PointCloud<PointT> cloud_pose_frame;
        //pcl_ros::transformPointCloudWithNormals(cloud_in_, cloud_pose_frame, common_T_grasp.inverse());



        ROS_INFO("Input cloud: min_distance %f; centroid [%s]  % 4.2f, % 4.2f, % 4.2f",
                 min_distance,
                 cloud_base_link.header.frame_id.c_str(),
                 centroid_.getX(),
                 centroid_.getY(),
                 centroid_.getZ());

        cloud_in_ = cloud_roi;

        // --------------------------------------------

        double x = 0, y = 0, z = 0, roll = 0, pitch = 0, yaw = 0;
        double start_x = 0, start_y = 0, start_z = 0;
        double start_roll = 0.0, start_pitch = 0.0, start_yaw = 0.0;


        // Make list of points to check



        std::vector< params > param_list;
        std::vector<tf::Transform> seed_list;
        ROS_INFO("Populating seeds...");
        int g_num = config_.grid_step_num;
        double g_size = config_.grid_step_size;
        if(search_mode == GLOBAL_SEARCH)
        {

            for( start_roll = -M_PI/4; start_roll < M_PI/4 + 0.1; start_roll += M_PI/4)
            {
                for( start_z = -g_size * g_num; start_z < g_size*g_num + 0.01; start_z+= g_size)
                {
                    for( start_y = -g_size * g_num; start_y < g_size * g_num + 0.01; start_y+=g_size)
                    {
                        for( start_x = 0; start_x < g_size * g_num + 0.01; start_x+=g_size)
                        {
                            x = start_x + min_distance - 0.15; // TODO magic number (front-ish of palm)!
                            if(config_.search_6_dof)
                            {
                                //for( start_pitch = -M_PI/4; start_pitch < M_PI/4 + 0.1; start_pitch += M_PI/4)
                                {
                                //    for( start_yaw = -M_PI/4; start_yaw < M_PI/4 + 0.1; start_yaw += M_PI/4)
                                    {
                                        params vars(x, start_y, start_z, start_roll, start_pitch, start_yaw);
                                        param_list.push_back(vars);
                                    }
                                }
                            }
                            else
                            {
                                params vars(x, start_y, start_z, start_roll, 0.0, 0.0);
                                param_list.push_back(vars);
                            }
                        }
                    }
                }
            }
        }
        else if(search_mode == LOCAL_SEARCH)
        {
            x = start_x + min_distance - 0.15; // TODO magic number (front-ish of palm)!
            y = start_y;
            z = start_z;

            for( start_roll = -M_PI/4; start_roll < M_PI/4 + 0.1; start_roll += M_PI/4)
            {
                //for( start_pitch = -M_PI/4; start_pitch < M_PI/4 + 0.1; start_pitch += M_PI/4)
                //{
                //    for( start_yaw = -M_PI/4; start_yaw < M_PI/4 + 0.1; start_yaw += M_PI/4)
                //    {
                        params vars(x, start_y, start_z, start_roll, 0.0, 0.0);
                        param_list.push_back(vars);
                //    }
                //}
            }
        }
        else if( search_mode == SINGLE_POSE) // for debugging, when we want to see changes to exactly one pose
        {
            ROS_INFO("Populating with single pose...");
            x = debug_.seed_x + min_distance - 0.15;
            y = debug_.seed_y;
            z = debug_.seed_z;
            roll = debug_.seed_roll * M_PI/180.0;
            pitch = debug_.seed_pitch * M_PI/180.0;
            yaw = debug_.seed_yaw * M_PI/180.0;

            params vars(x, y, z, roll, pitch, yaw);
            param_list.push_back(vars);
        }
        ROS_DEBUG("Param list populated with %d starting positions!", (int) param_list.size());


        // Create the transform list from the parameters
        for( unsigned int i = 0; i < param_list.size(); i++)
        {
            x =     param_list[i].X;
            y =     param_list[i].Y;
            z =     param_list[i].Z;
            roll =  param_list[i].r;
            pitch = param_list[i].p;
            yaw =   param_list[i].y;

            tf::Transform T_rot_only = common_T_grasp;
            T_rot_only.setOrigin(tf::Vector3(0, 0, 0));

            tf::Transform T_rot;
            tf::Transform T_offset;
            tf::Transform T_trans = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(x, y, z));

            if( config_.search_6_dof )
            {
                T_rot = tf::Transform(tf::createQuaternionFromRPY(roll, pitch, yaw), tf::Vector3( 0, 0, 0));
                T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot * DEFAULT_GRIPPER.tool_frame_.inverse();
            }
            else
            {
                T_rot = tf::Transform(tf::createQuaternionFromRPY(0, 0, roll), tf::Vector3( 0, 0, 0));
                T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot_only.inverse() * T_rot * T_rot_only * DEFAULT_GRIPPER.tool_frame_.inverse();
            }

            tf::Transform common_T_candidate = common_T_grasp * T_offset * common_T_grasp.inverse();
            seed_list.push_back(common_T_candidate);
        }
        ROS_INFO("Seed list populated with %d starting positions!", (int) seed_list.size());

        std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> seed_queue;
        std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> progress_queue;
        std::priority_queue<GripperModel, std::vector<GripperModel>, GripperModel::compareModels> maxima_queue;

        for( unsigned int i = 0; i < seed_list.size(); i++ )
        {
            tf::Transform T = seed_list[i];
            GripperModel gripper = starting_gripper_;
            gripper.transform(T);
            pcl::PointCloud<PointT> temp;
            box_filter(cloud_in_, temp, gripper.bounding_box_, false, false);
            gripper.evaluateGrasp(temp, weights_, centroid_ );

            if(search_mode == SINGLE_POSE)
                seed_queue.push(gripper);
            else
            {
                if(gripper.state_.no_object && !gripper.state_.inCollision()) {
                    //do nothing
                }
                else
                {
                    seed_queue.push(gripper);

                    if(config_.debug_stepping)
                    {
                        ROS_INFO("seed has score %f", gripper.score_ );
                        gripper.publishModel(*pub_marker_array_, std::string("seeds"), i);
                        ros::Duration(config_.pause_time).sleep();
                    }
                }
            }


        }
        ROS_INFO("Seed queue has %d members", (int)seed_queue.size());

        if(search_mode == SINGLE_POSE)
        {
            GripperModel seed = seed_queue.top();
            tf::Transform initial = seed.wrist_frame_;

            tf::Transform T_rot_only = initial;
            T_rot_only.setOrigin(tf::Vector3(0, 0, 0));

            x = debug_.offset_x; //min_distance - (0.15 + grasp_pose.pose.position.x);
            y = debug_.offset_y;
            z = debug_.offset_z;
            roll = debug_.offset_roll * M_PI/180.0;
            pitch = debug_.offset_pitch * M_PI/180.0;
            yaw = debug_.offset_yaw * M_PI/180.0;

            tf::Transform T_trans = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(x, y, z));
            tf::Transform T_rot;
            tf::Transform T_offset;


            if( config_.search_6_dof )
            {
                T_rot = tf::Transform( tf::createQuaternionFromRPY(roll, pitch, yaw), tf::Vector3( 0, 0, 0));
                T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot * DEFAULT_GRIPPER.tool_frame_.inverse();
            }
            else
            {
                T_rot = tf::Transform( tf::createQuaternionFromRPY(0, 0, roll), tf::Vector3( 0, 0, 0));
                T_offset = T_trans * DEFAULT_GRIPPER.tool_frame_ * T_rot_only.inverse() * T_rot * T_rot_only * DEFAULT_GRIPPER.tool_frame_.inverse();
            }

            GripperModel gripper = seed;
            gripper.transform( initial * T_offset * initial.inverse() );

            // TODO this is wrong
            Box bbox = gripper.bounding_box_;
            bbox.header.frame_id = common_frame.c_str();
            bbox.frame.setRotation(tf::Quaternion(0,0,0,1));
            object_manipulator::drawBox(*pub_marker_, bbox, "bounding_box", 10);



//            if(pub_cloud_roi_.getNumSubscribers()){
//                sensor_msgs::PointCloud2 debug1;
//                pcl::toROSMsg(temp, debug1);
//                pub_cloud_roi_.publish(debug1);
//            }
            pcl::PointCloud<PointT> temp;
            box_filter(cloud_in_, temp, gripper.bounding_box_, false, false);
            gripper.evaluateGrasp(temp, weights_, centroid_, config_.debug_stepping);

            maxima_queue.push(gripper);
        }
        else
        {
            while( !seed_queue.empty() )
            {
                GripperModel seed = seed_queue.top();
                seed_queue.pop();

                int n = config_.init_gradient_steps;
                if( doGradientDescent(seed, n) )
                {
                    maxima_queue.push(seed);
                }
                else
                {
                    progress_queue.push(seed);
                    if(config_.debug_stepping)
                    {
                        ROS_DEBUG("seed has score %f", seed.score_ );
                    }
                }
            } // while queue

            ROS_INFO("After two steps, progress queue has %d members and maxima_queue has %d memebers",
                     (int)progress_queue.size(),
                     (int)maxima_queue.size()
                    );

            int descents = std::min((int)progress_queue.size(), (int)config_.full_gradient_descents - (int)maxima_queue.size()) ;
            for( int i = 0; i < descents; i++ )
            {
                GripperModel seed = progress_queue.top();
                progress_queue.pop();

                int n = 25;
                if( doGradientDescent(seed, n) )
                    maxima_queue.push(seed);
                //else
                    // Discard
                    // progress_queue.push(seed);

            } // for queue
        }

        ROS_INFO("After all steps, maxima_queue has %d memebers", (int)maxima_queue.size() );

        std::vector<GripperModel> maxima_list;
        int maxima_count = 0;
        while(!maxima_queue.empty() && maxima_count < config_.full_gradient_descents){
          GripperModel model = maxima_queue.top();
          maxima_queue.pop();
          if(! std::isnan(model.score_) )
          {
            maxima_list.push_back(maxima_queue.top());
            maxima_count++;
          }
        }
        for( unsigned int i = 0; i < maxima_list.size(); i++)
            ROS_INFO("maxima % 2d has score %f", i, maxima_list[i].score_ );



        if(maxima_list.size() > 0)
        {
          globalGripper = maxima_list[0];
          maxima_list[0].publishModel(*pub_marker_array_, std::string("previous_best_pick"), 0, ros::Duration(), 1.0f);
          for(unsigned int i = 1; i < maxima_list.size(); i++)
          {
            maxima_list[i].publishModel(*pub_marker_array_, std::string("alternatives"), i, ros::Duration(), 0.5f);
          }
        }

        int pose_count = 0;
        for(unsigned int i = 0; i < maxima_list.size(); i++)
        {
          if(!maxima_list[i].state_.inCollision() && maxima_list[i].score_ > config_.minimum_score )
          {
            ranked_poses->push(maxima_list[i]);
            pose_count++;
          }
        }

        //PROF_STOP_TIMER(TOTAL_TIMER);
        //PROF_PRINT_ALL;

        int return_value = 0;
        if(pose_count == 0) return_value = object_manipulation_msgs::GraspPlanningErrorCode::OTHER_ERROR;
        else return_value = object_manipulation_msgs::GraspPlanningErrorCode::SUCCESS;

        ROS_INFO("GraspAdjust::findGrasps finished, returning %d with %d (total) grasps",
                 return_value, (int)ranked_poses->size());

        return return_value;


    }