gripper_model.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <pcl_ros/transforms.h>
00004 
00005 #include <tf/tf.h>
00006 #include <tf/transform_listener.h>
00007 #include <tf/transform_broadcaster.h>
00008 
00009 #include <visualization_msgs/Marker.h>
00010 #include <visualization_msgs/MarkerArray.h>
00011 
00012 #include <object_manipulator/tools/shape_tools.h>
00013 
00014 
00015 #include "gripper_model.h"
00016 \
00017 /*
00018 #define PROF_ENABLED
00019 #include <profiling/profiling.h>
00020 */
00021 
00022 //PROF_DECLARE(TRANSFORM)
00023 //PROF_DECLARE(BOX_FILTER)
00024 //PROF_DECLARE(BOX_FILTER_GRIPPER)
00025 //PROF_DECLARE(NORMALS)
00026 
00027 GripperModel DEFAULT_GRIPPER;
00028 
00029 void GripperModel::toPoseStamped(geometry_msgs::PoseStamped &ps)
00030 {
00031   tf::pointTFToMsg(wrist_frame_.getOrigin(), ps.pose.position);
00032   tf::quaternionTFToMsg(wrist_frame_.getRotation(), ps.pose.orientation);
00033   ps.header.frame_id = this->header.frame_id;
00034   ps.header.stamp = this->header.stamp;
00035 }
00036 
00037 
00038 void GripperModel::transform(const geometry_msgs::Transform &T, const std::string &frame_id){
00039   tf::Transform tfT;
00040   tf::transformMsgToTF(T, tfT);
00041   transform(tfT, frame_id);
00042 }
00043 
00044 void GripperModel::transform(const tf::Transform &T, const std::string &frame_id){
00045     palm_.transform(T);
00046     l_finger_.transform(T);
00047     r_finger_.transform(T);
00048     space_.transform(T);
00049     tool_frame_= T*tool_frame_;
00050     extended_.transform(T);
00051     wrist_frame_ = T*wrist_frame_;
00052     bounding_box_.transform(T);
00053     roi_box_.transform(T);
00054 
00055     setFrameId(frame_id.c_str());
00056 }
00057 
00058 void GripperModel::setFrameId(const std::string &frame_id)
00059 {
00060     if(!frame_id.empty())
00061     {
00062         header.frame_id = frame_id;
00063         palm_.header.frame_id = frame_id;
00064         l_finger_.header.frame_id = frame_id;
00065         r_finger_.header.frame_id = frame_id;
00066         space_.header.frame_id = frame_id;
00067         extended_.header.frame_id = frame_id;
00068         bounding_box_.header.frame_id = frame_id;
00069         roi_box_.header.frame_id = frame_id;
00070     }
00071 }
00072 
00073 void GripperModel::setStamp(const ros::Time &stamp)
00074 {
00075     header.stamp = stamp;
00076     palm_.header.stamp = stamp;
00077     l_finger_.header.stamp = stamp;
00078     r_finger_.header.stamp = stamp;
00079     space_.header.stamp = stamp;
00080     extended_.header.stamp = stamp;
00081     bounding_box_.header.stamp = stamp;
00082     roi_box_.header.stamp = stamp;
00083 }
00084 
00085 void GripperModel::setHeader(const std_msgs::Header &header)
00086 {
00087     this->setFrameId(header.frame_id.c_str());
00088     this->setStamp(header.stamp);
00089 }
00090 
00091 void GripperModel::reset( bool resetStateAndScore)
00092 {
00093     //##return the xyz bounding box ranges for the parts of the gripper model placed at 3-list pos and 3x3 scipy matrix rot
00094     //#uses a really simple model of the fully-open gripper as three boxes (2 fingers and a palm), with a box for the empty space in-between
00095     //#gripper_boxes is a 4-list (palm, left finger, right finger, space) of 2-lists (min, max) of 3-lists (x,y,z) of corner coords
00096     //#origin of gripper is at the ?_wrist_roll_link
00097     //def gripper_model(self):
00098 
00099     wrist_frame_.setOrigin(tf::Vector3(0,0,0));
00100     wrist_frame_.setRotation(tf::Quaternion(0,0,0,1));
00101     header.frame_id = "r_wrist_roll_link";
00102 
00103       // Safer model
00104     palm_ = Box(     tf::Pose(     tf::Quaternion(0,0,0,1),
00105                                 tf::Vector3( (.04 + .138)/2, 0, 0 ) ),
00106                     tf::Vector3(0.138-0.04, 0.0755 + 0.0755, 0.026 + 0.026)  );
00107     l_finger_ = Box( tf::Pose(     tf::Quaternion(0,0,0,1),
00108                                     tf::Vector3( (0.185 + 0.138)/2, (0.0425 + 0.0755)/2, 0 ) ),
00109                     tf::Vector3( 0.185 - 0.138, 0.0755 - 0.0425, 0.0115 + 0.0115) );
00110 
00111     r_finger_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
00112                                 tf::Vector3( (0.185 + 0.138)/2, -(0.0425 + 0.0755)/2, 0 )   ),
00113                     tf::Vector3( 0.185 - 0.138, 0.0755 - 0.0425, 0.0115 + 0.0115)  );
00114 
00115     space_ = Box(    tf::Pose(   tf::Quaternion(0,0,0,1),
00116                                 tf::Vector3( (0.175 + .138)/2, 0, 0 ) ),
00117                     tf::Vector3(0.175-0.138, 0.015 + 0.015, 0.0075 + 0.0075) );
00118 
00119     extended_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
00120                                 tf::Vector3( (0.175 + .138)/2, 0, 0 )  ),
00121                     tf::Vector3(0.175-0.138, 0.0405 + 0.0405, 0.02 + 0.02)   );
00122 
00123     bounding_box_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
00124                                     tf::Vector3( 0.1, 0, 0 )  ),
00125                         tf::Vector3(0.2, 0.2, 0.2)  );
00126 
00127       // Aggressive model
00128 //    palm_ = Box(     tf::Pose(     tf::Quaternion(0,0,0,1),
00129 //                                tf::Vector3( (.04 + .128)/2, 0, 0 ) ),
00130 //                    tf::Vector3(0.128-0.04, 0.0755 + 0.0755, 0.026 + 0.026)  );
00131 
00132 //    l_finger_ = Box( tf::Pose(     tf::Quaternion(0,0,0,1),
00133 //                                    tf::Vector3( (0.185 + 0.138)/2, (0.0475 + 0.0755)/2, 0 ) ),
00134 //                    tf::Vector3( 0.185 - 0.138, 0.0755 - 0.0475, 0.0115 + 0.0115) );
00135 
00136 //    r_finger_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
00137 //                                tf::Vector3( (0.185 + 0.138)/2, -(0.0475 + 0.0755)/2, 0 )   ),
00138 //                    tf::Vector3( 0.185 - 0.138, 0.0755 - 0.0475, 0.0115 + 0.0115)  );
00139 
00140 //    space_ = Box(    tf::Pose(   tf::Quaternion(0,0,0,1),
00141 //                                tf::Vector3( (0.180 + .128)/2, 0, 0 ) ),
00142 //                    tf::Vector3(0.180-0.128, 0.015 + 0.015, 0.0075 + 0.0075) );
00143 
00144 //    extended_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
00145 //                                tf::Vector3( (0.180 + .128)/2, 0, 0 )  ),
00146 //                    tf::Vector3(0.180-0.128, 0.0405 + 0.0405, 0.02 + 0.02)   );
00147 
00148 //    bounding_box_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
00149 //                                    tf::Vector3( 0.1, 0, 0 )  ),
00150 //                        tf::Vector3(0.2, 0.2, 0.2)  );
00151 
00152     roi_box_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
00153                                     tf::Vector3( 0.25, 0, 0 )  ),
00154                     tf::Vector3(0.3, 0.18, 0.18)  );
00155 
00156     tool_frame_ = tf::Pose(   tf::Quaternion(0,0,0,1),
00157                              tf::Vector3(0.15, 0, 0)  );
00158 
00159     if( resetStateAndScore )
00160     {
00161         score_ = 0;
00162         state_.reset();
00163     }
00164 }
00165 
00166 
00167 void GripperModel::publishModel(ros::Publisher &pub, std::string ns, int id, ros::Duration lifetime, double opacity, int action)
00168 {
00169 
00170     if(pub.getNumSubscribers() < 1)
00171         return;
00172 
00173     std_msgs::ColorRGBA red =         object_manipulator::msg::createColorMsg(0.8, 0.2, 0.2, opacity);
00174     std_msgs::ColorRGBA green =       object_manipulator::msg::createColorMsg(0.2, 1.0, 0.2, opacity);
00175     std_msgs::ColorRGBA light_blue =  object_manipulator::msg::createColorMsg(0.2, 0.7, 0.8, opacity);
00176     std_msgs::ColorRGBA gray =        object_manipulator:: msg::createColorMsg(0.5, 0.5, 0.5, opacity);
00177 
00178     visualization_msgs::MarkerArray array;
00179     visualization_msgs::Marker marker;
00180 
00181     marker.lifetime = lifetime;
00182     marker.header = this->header;
00183     marker.ns = "gripper_model_" + ns;
00184     marker.type = visualization_msgs::Marker::CUBE;
00185     marker.action = action;
00186 
00187     int num_markers = 6;
00188 
00189     marker.id = num_markers*id + 0;
00190     tf::poseTFToMsg(this->palm_.frame, marker.pose);
00191     tf::vector3TFToMsg(this->palm_.dims, marker.scale);
00192     marker.color = this->state_.collide_palm?(red):(gray);
00193     array.markers.push_back(marker);
00194 
00195     marker.id = num_markers*id + 1;
00196     tf::poseTFToMsg(this->l_finger_.frame, marker.pose);
00197     tf::vector3TFToMsg(this->l_finger_.dims, marker.scale);
00198     marker.color = this->state_.collide_l?(red):(gray);
00199     array.markers.push_back(marker);
00200 
00201     marker.id = num_markers*id + 2;
00202     tf::poseTFToMsg(this->r_finger_.frame, marker.pose);
00203     tf::vector3TFToMsg(this->r_finger_.dims, marker.scale);
00204     marker.color = this->state_.collide_r?(red):(gray);
00205     array.markers.push_back(marker);
00206 
00207     marker.id =num_markers*id + 3;
00208     tf::poseTFToMsg(this->space_.frame, marker.pose);
00209     tf::vector3TFToMsg(this->space_.dims, marker.scale);
00210     marker.color = this->state_.valid?(green):(gray);
00211     array.markers.push_back(marker);
00212 
00213     marker.id = num_markers*id + 4;
00214     tf::poseTFToMsg(this->extended_.frame, marker.pose);
00215     tf::vector3TFToMsg(this->extended_.dims, marker.scale);
00216     marker.color = light_blue;
00217     array.markers.push_back(marker);
00218 
00219     marker.id = num_markers*id + 5;
00220     marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00221     tf::Pose text_frame = palm_.frame;
00222     text_frame.setOrigin(text_frame.getOrigin()
00223                          - text_frame.getBasis().getColumn(0)*palm_.dims.getX()/1.9
00224                          - text_frame.getBasis().getColumn(1)*palm_.dims.getY()/1.8);
00225     tf::poseTFToMsg(text_frame, marker.pose);
00226     marker.scale.z = 0.07;
00227     char text[10];
00228     sprintf(text, "% 2d", id);
00229     marker.text = text;
00230     marker.color = object_manipulator::msg::createColorMsg(0.8, 0.2, 0.2, 0.8);
00231     array.markers.push_back(marker);
00232 
00233     pub.publish(array);
00234 }
00235 // *****************************************************************************
00245 double GripperModel::evaluateGrasp(const pcl::PointCloud<PointT> &cloud,
00246                      FeatureWeights &weights,
00247                      tf::Vector3 default_centroid,
00248                      bool debug_print,
00249                      bool weights_as_output_features)
00250 {
00251     GripperModel* model = this;
00252 
00253     pcl::PointCloud<PointT> transformed;
00254     pcl::PointCloud<PointT> filtered;
00255 
00256     FeatureWeights features(0, 0, 0, 0, 0, 0);
00257 
00258     {
00259         //PROF_TIMER_FUNC(TRANSFORM)
00260         pcl_ros::transformPointCloudWithNormals(cloud, transformed, model->wrist_frame_.inverse());
00261         default_centroid = model->wrist_frame_.inverse()*default_centroid;
00262     }
00263 
00264     // ----------------------------------------------------------------- //
00265     // * Check for collisions wit the gripper boxes
00266     // ----------------------------------------------------------------- //
00267 
00268     int collisions = 0;
00269     {
00270         //PROF_TIMER_FUNC(BOX_FILTER_GRIPPER)
00271         model->state_.reset();
00272 
00273         box_filter(transformed, filtered, DEFAULT_GRIPPER.palm_, false, false );
00274         if(filtered.points.size() != 0) model->state_.set(GraspState::COLLIDE_PALM);
00275         collisions += filtered.points.size();
00276 
00277         box_filter(transformed, filtered, DEFAULT_GRIPPER.l_finger_, false, false );
00278         if(filtered.points.size() != 0) model->state_.set(GraspState::COLLIDE_L);
00279         collisions += filtered.points.size();
00280 
00281         box_filter(transformed, filtered, DEFAULT_GRIPPER.r_finger_, false, false );
00282         if(filtered.points.size() != 0) model->state_.set(GraspState::COLLIDE_R);
00283         collisions += filtered.points.size();
00284 
00285         box_filter(transformed, filtered, DEFAULT_GRIPPER.space_, false, false);
00286     }
00287     features.collision = collisions;
00288 
00289     // get points!
00290     int points_enclosed = filtered.points.size();
00291     features.point = pow(points_enclosed, 0.25);
00292     //int Iyz_min = 2;
00293     //features.Iyz = (points_enclosed > Iyz_min);
00294 
00295     if(!model->state_.inCollision()) {
00296       if(filtered.points.size() < 20)  // TODO: magic number!
00297         model->state_.set(GraspState::NO_OBJECT);
00298       else
00299         model->state_.set(GraspState::VALID);
00300     }
00301 
00302     // ----------------------------------------------------------------- //
00303     // * Use extended box for normals, symmetry features
00304     // ----------------------------------------------------------------- //
00305     box_filter(transformed, filtered, DEFAULT_GRIPPER.extended_, false, true );
00306 
00307     double normal_cost = 0;
00308     int count[4] = {0, 0, 0, 0};
00309     double PIby4 = M_PI/4;
00310 
00311     tf::Vector3 centroid(0, 0, 0);
00312     double fraction = 0;
00313 
00314     double Iyz = 0;
00315 
00316     if(filtered.points.size() > 0)
00317     {
00318       //PROF_TIMER_FUNC(NORMALS)
00319       for(unsigned int i = 0; i < filtered.points.size(); i++ )
00320       {
00321         PointT point = filtered.points[i];
00322 
00323         tf::Vector3 norm = tf::Vector3(point.normal[0], point.normal[1], point.normal[2]);
00324         double this_dot = norm.dot(tf::Vector3(1,0,0));
00325         normal_cost += 1-pow(cos(2*(acos(this_dot) - PIby4)),2);
00326 
00327 
00328         centroid += tf::Vector3(point.x, point.y, point.z);
00329 
00330         //Only use points for symmetry that are on a plane roughly perpendicular to gripper-X
00331         if( fabs(this_dot) > 0.707 )
00332         {
00333             if(point.z > 0 && point.y > 0) count[0] +=  1; //point.z + point.y;
00334             if(point.z < 0 && point.y > 0) count[1] +=  1; //-point.z + point.y;
00335             if(point.z < 0 && point.y < 0) count[2] +=  1; //-point.z - point.y;
00336             if(point.z > 0 && point.y < 0) count[3] +=  1; //point.z - point.y;
00337             Iyz += point.y * point.z;
00338         }
00339       }
00340 
00341       int max_count = 1;
00342       int min_count = 100000;
00343       for(int i = 0; i < 4; i++)
00344       {
00345         min_count = std::min(min_count, count[i]);
00346         max_count = std::max(max_count, count[i]);
00347       }
00348 
00349       fraction = (min_count / (double)max_count );
00350       centroid /= filtered.points.size();
00351       features.normal = normal_cost/(filtered.points.size() + 1);
00352       features.symmetry = (min_count / (double)max_count ) * ((points_enclosed > 20)?(1):(points_enclosed/20.0));
00353       Iyz *=100;
00354       features.Iyz = fabs(Iyz);
00355       if(debug_print)
00356       {
00357         ROS_INFO("Counts: % 4d % 4d % 4d % 4d, frac %.3f, Iyz %f", count[0], count[1], count[2], count[3], fraction, Iyz);
00358       }
00359     }
00360     else{
00361         centroid = default_centroid;
00362     }
00363 
00364     tf::Vector3 palm_center = DEFAULT_GRIPPER.palm_.frame.getOrigin() + tf::Vector3(DEFAULT_GRIPPER.palm_.dims.getX()/2 + 0.005, 0, 0);
00365     features.centroid = (centroid - palm_center).length();
00366 
00367     double quality = 0.0;
00368     if( weights_as_output_features )
00369     {
00370       weights = features;
00371     }
00372     else
00373     {
00374 
00375       quality =     weights.symmetry    * features.symmetry
00376                   + weights.Iyz         * features.Iyz
00377                   + weights.normal      * features.normal
00378                   + weights.point       * features.point
00379                   + weights.centroid    * features.centroid
00380                   + weights.collision   * features.collision;
00381     }
00382 
00383     double max_quality = weights.normal + weights.symmetry; // This is questionable
00384     double min_quality = 0;
00385     double score = std::max(std::min(quality, max_quality), min_quality)/max_quality;
00386 
00387     if(debug_print)
00388     {
00389 
00390         geometry_msgs::Transform msg;
00391         tf::transformTFToMsg(model->wrist_frame_, msg);
00392         //cerr << msg << endl;
00393         geometry_msgs::Vector3 msg2;
00394         geometry_msgs::Vector3 msg3;
00395         tf::vector3TFToMsg(centroid, msg2);
00396         tf::vector3TFToMsg(palm_center, msg3);
00397         //cerr << "Centroid: " << msg2;
00398         //cerr << "Palm center: " << msg3;
00399         //ROS_INFO("pos_error: %f", pos_error);
00400         ROS_INFO( "Quality = sym % 7.3f + iyz % 7.3f + nor % 7.3f + pnt % 7.3f + cen % 7.3f + col % 7.3f = % 7.3f -> score= % 6.4f",
00401                     weights.symmetry    * features.symmetry
00402                   , weights.Iyz         * features.Iyz
00403                   , weights.normal      * features.normal
00404                   , weights.point       * features.point
00405                   , weights.centroid    * features.centroid
00406                   , weights.collision   * features.collision
00407                   , quality
00408                   , score);
00409         //tf::Transform temp(tf::Quaternion(0,0,0,1), centroid);
00410         //br_.sendTransform(tf::StampedTransform(model->wrist_roll_frame*temp, now_, "r_wrist_roll_link", "cloud_centroid"));
00411     }
00412 
00413     model->score_ = score;
00414     return score;
00415 }
00416 
 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:54