Go to the documentation of this file.00001 #include <agile_grasp/antipodal.h>
00002
00003 const int Antipodal::NO_GRASP = 0;
00004 const int Antipodal::HALF_GRASP = 1;
00005 const int Antipodal::FULL_GRASP = 2;
00006
00007 Antipodal::Antipodal(const Eigen::Matrix3Xd& normals) : normals_(normals)
00008 {
00009
00010 }
00011
00012 int Antipodal::evaluateGrasp(double thresh_half, double thresh_full)
00013 {
00014 int num_thresh = 6;
00015 int grasp = 0;
00016 double cos_thresh = cos(thresh_half * M_PI / 180.0);
00017 int numl = 0;
00018 int numr = 0;
00019 Eigen::Vector3d l, r;
00020 l << -1, 0, 0;
00021 r << 1, 0, 0;
00022 bool is_half_grasp = false;
00023 bool is_full_grasp = false;
00024
00025
00026 for (int i = 0; i < normals_.cols(); i++)
00027 {
00028 if (l.dot(normals_.col(i)) > cos_thresh)
00029 {
00030 numl++;
00031 if (numl > num_thresh)
00032 {
00033 is_half_grasp = true;
00034 break;
00035 }
00036 }
00037
00038 if (r.dot(normals_.col(i)) > cos_thresh)
00039 {
00040 numr++;
00041 if (numr > num_thresh)
00042 {
00043 is_half_grasp = true;
00044 break;
00045 }
00046 }
00047 }
00048
00049
00050 cos_thresh = cos(thresh_full * M_PI / 180.0);
00051 numl = 0;
00052 numr = 0;
00053
00054 for (int i = 0; i < normals_.cols(); i++)
00055 {
00056
00057 if (l.dot(normals_.col(i)) > cos_thresh)
00058 {
00059 numl++;
00060
00061 if (numl > num_thresh && numr > num_thresh)
00062 {
00063 is_full_grasp = true;
00064 break;
00065 }
00066 }
00067
00068 if (r.dot(normals_.col(i)) > cos_thresh)
00069 {
00070 numr++;
00071
00072 if (numl > num_thresh && numr > num_thresh)
00073 {
00074 is_full_grasp = true;
00075 break;
00076 }
00077 }
00078 }
00079
00080 if (is_full_grasp)
00081 return FULL_GRASP;
00082 else if (is_half_grasp)
00083 return HALF_GRASP;
00084
00085 return NO_GRASP;
00086 }