gripper_model.cpp

Go to the documentation of this file.

#include <ros/ros.h>

#include <pcl_ros/transforms.h>

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

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

#include <object_manipulator/tools/shape_tools.h>


#include "gripper_model.h"
\
/*
#define PROF_ENABLED
#include <profiling/profiling.h>
*/

//PROF_DECLARE(TRANSFORM)
//PROF_DECLARE(BOX_FILTER)
//PROF_DECLARE(BOX_FILTER_GRIPPER)
//PROF_DECLARE(NORMALS)

GripperModel DEFAULT_GRIPPER;

void GripperModel::toPoseStamped(geometry_msgs::PoseStamped &ps)
{
  tf::pointTFToMsg(wrist_frame_.getOrigin(), ps.pose.position);
  tf::quaternionTFToMsg(wrist_frame_.getRotation(), ps.pose.orientation);
  ps.header.frame_id = this->header.frame_id;
  ps.header.stamp = this->header.stamp;
}


void GripperModel::transform(const geometry_msgs::Transform &T, const std::string &frame_id){
  tf::Transform tfT;
  tf::transformMsgToTF(T, tfT);
  transform(tfT, frame_id);
}

void GripperModel::transform(const tf::Transform &T, const std::string &frame_id){
    palm_.transform(T);
    l_finger_.transform(T);
    r_finger_.transform(T);
    space_.transform(T);
    tool_frame_= T*tool_frame_;
    extended_.transform(T);
    wrist_frame_ = T*wrist_frame_;
    bounding_box_.transform(T);
    roi_box_.transform(T);

    setFrameId(frame_id.c_str());
}

void GripperModel::setFrameId(const std::string &frame_id)
{
    if(!frame_id.empty())
    {
        header.frame_id = frame_id;
        palm_.header.frame_id = frame_id;
        l_finger_.header.frame_id = frame_id;
        r_finger_.header.frame_id = frame_id;
        space_.header.frame_id = frame_id;
        extended_.header.frame_id = frame_id;
        bounding_box_.header.frame_id = frame_id;
        roi_box_.header.frame_id = frame_id;
    }
}

void GripperModel::setStamp(const ros::Time &stamp)
{
    header.stamp = stamp;
    palm_.header.stamp = stamp;
    l_finger_.header.stamp = stamp;
    r_finger_.header.stamp = stamp;
    space_.header.stamp = stamp;
    extended_.header.stamp = stamp;
    bounding_box_.header.stamp = stamp;
    roi_box_.header.stamp = stamp;
}

void GripperModel::setHeader(const std_msgs::Header &header)
{
    this->setFrameId(header.frame_id.c_str());
    this->setStamp(header.stamp);
}

void GripperModel::reset( bool resetStateAndScore)
{
    //##return the xyz bounding box ranges for the parts of the gripper model placed at 3-list pos and 3x3 scipy matrix rot
    //#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
    //#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
    //#origin of gripper is at the ?_wrist_roll_link
    //def gripper_model(self):

    wrist_frame_.setOrigin(tf::Vector3(0,0,0));
    wrist_frame_.setRotation(tf::Quaternion(0,0,0,1));
    header.frame_id = "r_wrist_roll_link";

      // Safer model
    palm_ = Box(     tf::Pose(     tf::Quaternion(0,0,0,1),
                                tf::Vector3( (.04 + .138)/2, 0, 0 ) ),
                    tf::Vector3(0.138-0.04, 0.0755 + 0.0755, 0.026 + 0.026)  );
    l_finger_ = Box( tf::Pose(     tf::Quaternion(0,0,0,1),
                                    tf::Vector3( (0.185 + 0.138)/2, (0.0425 + 0.0755)/2, 0 ) ),
                    tf::Vector3( 0.185 - 0.138, 0.0755 - 0.0425, 0.0115 + 0.0115) );

    r_finger_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
                                tf::Vector3( (0.185 + 0.138)/2, -(0.0425 + 0.0755)/2, 0 )   ),
                    tf::Vector3( 0.185 - 0.138, 0.0755 - 0.0425, 0.0115 + 0.0115)  );

    space_ = Box(    tf::Pose(   tf::Quaternion(0,0,0,1),
                                tf::Vector3( (0.175 + .138)/2, 0, 0 ) ),
                    tf::Vector3(0.175-0.138, 0.015 + 0.015, 0.0075 + 0.0075) );

    extended_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
                                tf::Vector3( (0.175 + .138)/2, 0, 0 )  ),
                    tf::Vector3(0.175-0.138, 0.0405 + 0.0405, 0.02 + 0.02)   );

    bounding_box_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
                                    tf::Vector3( 0.1, 0, 0 )  ),
                        tf::Vector3(0.2, 0.2, 0.2)  );

      // Aggressive model
//    palm_ = Box(     tf::Pose(     tf::Quaternion(0,0,0,1),
//                                tf::Vector3( (.04 + .128)/2, 0, 0 ) ),
//                    tf::Vector3(0.128-0.04, 0.0755 + 0.0755, 0.026 + 0.026)  );

//    l_finger_ = Box( tf::Pose(     tf::Quaternion(0,0,0,1),
//                                    tf::Vector3( (0.185 + 0.138)/2, (0.0475 + 0.0755)/2, 0 ) ),
//                    tf::Vector3( 0.185 - 0.138, 0.0755 - 0.0475, 0.0115 + 0.0115) );

//    r_finger_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
//                                tf::Vector3( (0.185 + 0.138)/2, -(0.0475 + 0.0755)/2, 0 )   ),
//                    tf::Vector3( 0.185 - 0.138, 0.0755 - 0.0475, 0.0115 + 0.0115)  );

//    space_ = Box(    tf::Pose(   tf::Quaternion(0,0,0,1),
//                                tf::Vector3( (0.180 + .128)/2, 0, 0 ) ),
//                    tf::Vector3(0.180-0.128, 0.015 + 0.015, 0.0075 + 0.0075) );

//    extended_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
//                                tf::Vector3( (0.180 + .128)/2, 0, 0 )  ),
//                    tf::Vector3(0.180-0.128, 0.0405 + 0.0405, 0.02 + 0.02)   );

//    bounding_box_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
//                                    tf::Vector3( 0.1, 0, 0 )  ),
//                        tf::Vector3(0.2, 0.2, 0.2)  );

    roi_box_ = Box( tf::Pose(   tf::Quaternion(0,0,0,1),
                                    tf::Vector3( 0.25, 0, 0 )  ),
                    tf::Vector3(0.3, 0.18, 0.18)  );

    tool_frame_ = tf::Pose(   tf::Quaternion(0,0,0,1),
                             tf::Vector3(0.15, 0, 0)  );

    if( resetStateAndScore )
    {
        score_ = 0;
        state_.reset();
    }
}


void GripperModel::publishModel(ros::Publisher &pub, std::string ns, int id, ros::Duration lifetime, double opacity, int action)
{

    if(pub.getNumSubscribers() < 1)
        return;

    std_msgs::ColorRGBA red =         object_manipulator::msg::createColorMsg(0.8, 0.2, 0.2, opacity);
    std_msgs::ColorRGBA green =       object_manipulator::msg::createColorMsg(0.2, 1.0, 0.2, opacity);
    std_msgs::ColorRGBA light_blue =  object_manipulator::msg::createColorMsg(0.2, 0.7, 0.8, opacity);
    std_msgs::ColorRGBA gray =        object_manipulator:: msg::createColorMsg(0.5, 0.5, 0.5, opacity);

    visualization_msgs::MarkerArray array;
    visualization_msgs::Marker marker;

    marker.lifetime = lifetime;
    marker.header = this->header;
    marker.ns = "gripper_model_" + ns;
    marker.type = visualization_msgs::Marker::CUBE;
    marker.action = action;

    int num_markers = 6;

    marker.id = num_markers*id + 0;
    tf::poseTFToMsg(this->palm_.frame, marker.pose);
    tf::vector3TFToMsg(this->palm_.dims, marker.scale);
    marker.color = this->state_.collide_palm?(red):(gray);
    array.markers.push_back(marker);

    marker.id = num_markers*id + 1;
    tf::poseTFToMsg(this->l_finger_.frame, marker.pose);
    tf::vector3TFToMsg(this->l_finger_.dims, marker.scale);
    marker.color = this->state_.collide_l?(red):(gray);
    array.markers.push_back(marker);

    marker.id = num_markers*id + 2;
    tf::poseTFToMsg(this->r_finger_.frame, marker.pose);
    tf::vector3TFToMsg(this->r_finger_.dims, marker.scale);
    marker.color = this->state_.collide_r?(red):(gray);
    array.markers.push_back(marker);

    marker.id =num_markers*id + 3;
    tf::poseTFToMsg(this->space_.frame, marker.pose);
    tf::vector3TFToMsg(this->space_.dims, marker.scale);
    marker.color = this->state_.valid?(green):(gray);
    array.markers.push_back(marker);

    marker.id = num_markers*id + 4;
    tf::poseTFToMsg(this->extended_.frame, marker.pose);
    tf::vector3TFToMsg(this->extended_.dims, marker.scale);
    marker.color = light_blue;
    array.markers.push_back(marker);

    marker.id = num_markers*id + 5;
    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    tf::Pose text_frame = palm_.frame;
    text_frame.setOrigin(text_frame.getOrigin()
                         - text_frame.getBasis().getColumn(0)*palm_.dims.getX()/1.9
                         - text_frame.getBasis().getColumn(1)*palm_.dims.getY()/1.8);
    tf::poseTFToMsg(text_frame, marker.pose);
    marker.scale.z = 0.07;
    char text[10];
    sprintf(text, "% 2d", id);
    marker.text = text;
    marker.color = object_manipulator::msg::createColorMsg(0.8, 0.2, 0.2, 0.8);
    array.markers.push_back(marker);

    pub.publish(array);
}
// *****************************************************************************
double GripperModel::evaluateGrasp(const pcl::PointCloud<PointT> &cloud,
                     FeatureWeights &weights,
                     tf::Vector3 default_centroid,
                     bool debug_print,
                     bool weights_as_output_features)
{
    GripperModel* model = this;

    pcl::PointCloud<PointT> transformed;
    pcl::PointCloud<PointT> filtered;

    FeatureWeights features(0, 0, 0, 0, 0, 0);

    {
        //PROF_TIMER_FUNC(TRANSFORM)
        pcl_ros::transformPointCloudWithNormals(cloud, transformed, model->wrist_frame_.inverse());
        default_centroid = model->wrist_frame_.inverse()*default_centroid;
    }

    // ----------------------------------------------------------------- //
    // * Check for collisions wit the gripper boxes
    // ----------------------------------------------------------------- //

    int collisions = 0;
    {
        //PROF_TIMER_FUNC(BOX_FILTER_GRIPPER)
        model->state_.reset();

        box_filter(transformed, filtered, DEFAULT_GRIPPER.palm_, false, false );
        if(filtered.points.size() != 0) model->state_.set(GraspState::COLLIDE_PALM);
        collisions += filtered.points.size();

        box_filter(transformed, filtered, DEFAULT_GRIPPER.l_finger_, false, false );
        if(filtered.points.size() != 0) model->state_.set(GraspState::COLLIDE_L);
        collisions += filtered.points.size();

        box_filter(transformed, filtered, DEFAULT_GRIPPER.r_finger_, false, false );
        if(filtered.points.size() != 0) model->state_.set(GraspState::COLLIDE_R);
        collisions += filtered.points.size();

        box_filter(transformed, filtered, DEFAULT_GRIPPER.space_, false, false);
    }
    features.collision = collisions;

    // get points!
    int points_enclosed = filtered.points.size();
    features.point = pow(points_enclosed, 0.25);
    //int Iyz_min = 2;
    //features.Iyz = (points_enclosed > Iyz_min);

    if(!model->state_.inCollision()) {
      if(filtered.points.size() < 20)  // TODO: magic number!
        model->state_.set(GraspState::NO_OBJECT);
      else
        model->state_.set(GraspState::VALID);
    }

    // ----------------------------------------------------------------- //
    // * Use extended box for normals, symmetry features
    // ----------------------------------------------------------------- //
    box_filter(transformed, filtered, DEFAULT_GRIPPER.extended_, false, true );

    double normal_cost = 0;
    int count[4] = {0, 0, 0, 0};
    double PIby4 = M_PI/4;

    tf::Vector3 centroid(0, 0, 0);
    double fraction = 0;

    double Iyz = 0;

    if(filtered.points.size() > 0)
    {
      //PROF_TIMER_FUNC(NORMALS)
      for(unsigned int i = 0; i < filtered.points.size(); i++ )
      {
        PointT point = filtered.points[i];

        tf::Vector3 norm = tf::Vector3(point.normal[0], point.normal[1], point.normal[2]);
        double this_dot = norm.dot(tf::Vector3(1,0,0));
        normal_cost += 1-pow(cos(2*(acos(this_dot) - PIby4)),2);


        centroid += tf::Vector3(point.x, point.y, point.z);

        //Only use points for symmetry that are on a plane roughly perpendicular to gripper-X
        if( fabs(this_dot) > 0.707 )
        {
            if(point.z > 0 && point.y > 0) count[0] +=  1; //point.z + point.y;
            if(point.z < 0 && point.y > 0) count[1] +=  1; //-point.z + point.y;
            if(point.z < 0 && point.y < 0) count[2] +=  1; //-point.z - point.y;
            if(point.z > 0 && point.y < 0) count[3] +=  1; //point.z - point.y;
            Iyz += point.y * point.z;
        }
      }

      int max_count = 1;
      int min_count = 100000;
      for(int i = 0; i < 4; i++)
      {
        min_count = std::min(min_count, count[i]);
        max_count = std::max(max_count, count[i]);
      }

      fraction = (min_count / (double)max_count );
      centroid /= filtered.points.size();
      features.normal = normal_cost/(filtered.points.size() + 1);
      features.symmetry = (min_count / (double)max_count ) * ((points_enclosed > 20)?(1):(points_enclosed/20.0));
      Iyz *=100;
      features.Iyz = fabs(Iyz);
      if(debug_print)
      {
        ROS_INFO("Counts: % 4d % 4d % 4d % 4d, frac %.3f, Iyz %f", count[0], count[1], count[2], count[3], fraction, Iyz);
      }
    }
    else{
        centroid = default_centroid;
    }

    tf::Vector3 palm_center = DEFAULT_GRIPPER.palm_.frame.getOrigin() + tf::Vector3(DEFAULT_GRIPPER.palm_.dims.getX()/2 + 0.005, 0, 0);
    features.centroid = (centroid - palm_center).length();

    double quality = 0.0;
    if( weights_as_output_features )
    {
      weights = features;
    }
    else
    {

      quality =     weights.symmetry    * features.symmetry
                  + weights.Iyz         * features.Iyz
                  + weights.normal      * features.normal
                  + weights.point       * features.point
                  + weights.centroid    * features.centroid
                  + weights.collision   * features.collision;
    }

    double max_quality = weights.normal + weights.symmetry; // This is questionable
    double min_quality = 0;
    double score = std::max(std::min(quality, max_quality), min_quality)/max_quality;

    if(debug_print)
    {

        geometry_msgs::Transform msg;
        tf::transformTFToMsg(model->wrist_frame_, msg);
        //cerr << msg << endl;
        geometry_msgs::Vector3 msg2;
        geometry_msgs::Vector3 msg3;
        tf::vector3TFToMsg(centroid, msg2);
        tf::vector3TFToMsg(palm_center, msg3);
        //cerr << "Centroid: " << msg2;
        //cerr << "Palm center: " << msg3;
        //ROS_INFO("pos_error: %f", pos_error);
        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",
                    weights.symmetry    * features.symmetry
                  , weights.Iyz         * features.Iyz
                  , weights.normal      * features.normal
                  , weights.point       * features.point
                  , weights.centroid    * features.centroid
                  , weights.collision   * features.collision
                  , quality
                  , score);
        //tf::Transform temp(tf::Quaternion(0,0,0,1), centroid);
        //br_.sendTransform(tf::StampedTransform(model->wrist_roll_frame*temp, now_, "r_wrist_roll_link", "cloud_centroid"));
    }

    model->score_ = score;
    return score;
}