rotating_hand.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/rotating_hand.h>
00002 
00003 
00004 RotatingHand::RotatingHand(const Eigen::VectorXd& camera_origin_left,
00005         const Eigen::VectorXd& camera_origin_right, const FingerHand& finger_hand, bool tolerant_antipodal,
00006         int cam_source) :
00007                 finger_hand_(finger_hand), tolerant_antipodal_(tolerant_antipodal), cam_source_(cam_source)
00008 {
00009         cams_.col(0) = camera_origin_left;
00010         cams_.col(1) = camera_origin_right;
00011 
00012         // generate hand orientations
00013         int num_orientations = 8;
00014         Eigen::VectorXd angles = Eigen::VectorXd::LinSpaced(num_orientations + 1, -1.0 * M_PI, M_PI);
00015         angles_ = angles.block(0, 0, 1, num_orientations);
00016 }
00017 
00018 
00019 void RotatingHand::transformPoints(const Eigen::Matrix3Xd& points, const Eigen::Vector3d& normal,
00020         const Eigen::Vector3d& axis, const Eigen::Matrix3Xd& normals, const Eigen::VectorXi& points_cam_source,
00021         double hand_height)
00022 {
00023         // rotate points into hand frame
00024         hand_axis_ = axis;
00025         frame_ << normal, normal.cross(axis), axis;
00026         points_ = frame_.transpose() * points;
00027 //  std::cout << "transformPoints(): " << std::endl;
00028 //  std::cout << " points: " << points.rows() << " x " << points.cols() << std::endl;
00029 //  std::cout << " frame_: " << frame_.rows() << " x " << frame_.cols() << std::endl;
00030 //  std::cout << frame_ << std::endl;
00031 //  std::cout << " points_: " << points_.rows() << " x " << points_.cols() << std::endl;
00032 
00033         normals_ = frame_.transpose() * normals;
00034         points_cam_source_ = points_cam_source;
00035 
00036         // crop points by hand_height
00037         std::vector<int> indices(points_.size());
00038         int k = 0;
00039 //  std::cout << "h.pts: " << points_.cols() << ", -1.0 * hand_height: " << -1.0 * hand_height << "\n";
00040         for (int i = 0; i < points_.cols(); i++)
00041         {
00042 //    std::cout << "i: " << i << " " << points_.col(i).transpose() << "\n";
00043 
00044                 if (points_(2, i) > -1.0 * hand_height && points_(2, i) < hand_height)
00045                 {
00046 //      std::cout << " hand_height i: " << i << " " << points_(2, i) << "\n";
00047 //      indices.push_back(i);
00048                         indices[k] = i;
00049                         k++;
00050                 }
00051         }
00052 //  indices[k+1] = -1;
00053 //
00055 //
00059 
00060         Eigen::Matrix3Xd points_cropped(3, k);
00061         Eigen::Matrix3Xd normals_cropped(3, k);
00062         Eigen::VectorXi points_cam_source_cropped(k);
00063         for (int i = 0; i < k; i++)
00064         {
00065                 points_cropped.col(i) = points_.col(indices[i]);
00066                 normals_cropped.col(i) = normals_.col(indices[i]);
00067                 points_cam_source_cropped(i) = points_cam_source_(indices[i]);
00068         }
00069 
00070 //  std::cout << normals_cropped.col(0) << std::endl;
00071 
00072         points_ = points_cropped;
00073         normals_ = normals_cropped;
00074         points_cam_source_ = points_cam_source_cropped;
00075 }
00076 
00077 
00078 std::vector<GraspHypothesis> RotatingHand::evaluateHand(double init_bite, const Eigen::Vector3d& sample,
00079         bool use_antipodal)
00080 {
00081         // initialize hands to zero
00082         int n = angles_.size();
00083 
00084         std::vector<GraspHypothesis> grasp_list;
00085 
00086         for (int i = 0; i < n; i++)
00087         {
00088                 // rotate points into <angles_(i)> orientation
00089                 Eigen::Matrix3d rot;
00090                 rot << cos(angles_(i)), -1.0 * sin(angles_(i)), 0.0, sin(angles_(i)), cos(angles_(i)), 0.0, 0.0, 0.0, 1.0;
00091                 Eigen::Matrix3Xd points_rot = rot * points_;
00092                 Eigen::Matrix3Xd normals_rot = rot * normals_;
00093                 Eigen::Vector3d x, y;
00094                 x << 1.0, 0.0, 0.0;
00095                 y << 0.0, 1.0, 0.0;
00096                 Eigen::Vector3d approach = frame_ * rot.transpose() * y;
00097 
00098                 // reject hand if it is not within 90 degrees of one of the camera sources
00099                 if (approach.dot(cams_.col(0)) > 0 && approach.dot(cams_.col(1)) > 0)
00100                 {
00101                         continue;
00102                 }
00103 
00104                 Eigen::Vector3d binormal = frame_ * rot.transpose() * x;
00105 
00106                 // calculate free hand placements
00107                 finger_hand_.setPoints(points_rot);
00108                 finger_hand_.evaluateFingers(init_bite);
00109                 finger_hand_.evaluateHand();
00110 
00111                 if (finger_hand_.getHand().sum() > 0)
00112                 {
00113                         // find deepest hand
00114                         finger_hand_.deepenHand(init_bite, finger_hand_.getHandDepth());
00115                         finger_hand_.evaluateGraspParameters(init_bite);
00116 
00117                         // save grasp parameters
00118                         Eigen::Vector3d surface, bottom;
00119                         surface << finger_hand_.getGraspSurface(), 0.0;
00120                         bottom << finger_hand_.getGraspBottom(), 0.0;
00121                         surface = frame_ * rot.transpose() * surface;
00122                         bottom = frame_ * rot.transpose() * bottom;
00123 
00124                         // save data for learning
00125                         std::vector<int> points_in_box_indices;
00126                         for (int j = 0; j < points_rot.cols(); j++)
00127                         {
00128                                 if (points_rot(1, j) < finger_hand_.getBackOfHand() + finger_hand_.getHandDepth())
00129                                         points_in_box_indices.push_back(j);
00130                         }
00131 
00132                         Eigen::Matrix3Xd points_in_box(3, points_in_box_indices.size());
00133                         Eigen::Matrix3Xd normals_in_box(3, points_in_box_indices.size());
00134                         Eigen::VectorXi cam_source_in_box(points_in_box_indices.size());
00135 
00136                         for (int j = 0; j < points_in_box_indices.size(); j++)
00137                         {
00138                                 points_in_box.col(j) = points_rot.col(points_in_box_indices[j]) - surface;
00139                                 normals_in_box.col(j) = normals_rot.col(points_in_box_indices[j]);
00140                                 cam_source_in_box(j) = points_cam_source_(points_in_box_indices[j]);
00141                         }
00142 
00143                         std::vector<int> indices_cam1;
00144                         std::vector<int> indices_cam2;
00145                         for (int j = 0; j < points_in_box.cols(); j++)
00146                         {
00147                                 if (cam_source_in_box(j) == 0)
00148                                         indices_cam1.push_back(j);
00149                                 else if (cam_source_in_box(j) == 1)
00150                                         indices_cam2.push_back(j);
00151                         }
00152 
00153                         surface += sample;
00154                         bottom += sample;
00155 
00156                         GraspHypothesis grasp(hand_axis_, approach, binormal, bottom, surface, finger_hand_.getGraspWidth(),
00157                                 points_in_box, indices_cam1, indices_cam2, cam_source_);
00158 
00159                         if (use_antipodal) // if we need to evaluate whether the grasp is antipodal
00160                         {
00161                                 Antipodal antipodal(normals_in_box);
00162                                 int antipodal_type = antipodal.evaluateGrasp(20, 20);
00163                                 if (antipodal_type == Antipodal::FULL_GRASP)
00164                                 {
00165                                         grasp.setHalfAntipodal(true);
00166                                         grasp.setFullAntipodal(true);
00167                                 }
00168                                 else if (antipodal_type == Antipodal::HALF_GRASP)
00169                                         grasp.setHalfAntipodal(true);
00170                         }
00171 
00172                         grasp_list.push_back(grasp);
00173                 }
00174         }
00175 
00176         return grasp_list;
00177 }


agile_grasp
Author(s): Andreas ten Pas
autogenerated on Sat Jun 8 2019 20:08:27