handle.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/handle.h>
00002 
00003 Handle::Handle(const std::vector<GraspHypothesis>& hand_list, const std::vector<int>& inliers) :
00004                 hand_list_(hand_list), inliers_(inliers)
00005 {
00006         setAxis();
00007         setDistAlongHandle();
00008         setGraspVariables();
00009         setGraspWidth();
00010 }
00011 
00012 void Handle::setAxis()
00013 {
00014         Eigen::Matrix3Xd axis_mat(3, inliers_.size());
00015         for (int i = 0; i < inliers_.size(); i++)
00016         {
00017                 axis_mat.col(i) = hand_list_[inliers_[i]].getAxis();
00018         }
00019 
00020         Eigen::EigenSolver<Eigen::Matrix3d> eigen_solver(axis_mat * axis_mat.transpose());
00021         Eigen::Vector3d eigen_values = eigen_solver.eigenvalues().real();
00022         Eigen::Matrix3d eigen_vectors = eigen_solver.eigenvectors().real();
00023         int max_idx;
00024         eigen_values.maxCoeff(&max_idx);
00025         axis_ = eigen_vectors.col(max_idx);
00026 }
00027 
00028 void Handle::setDistAlongHandle()
00029 {
00030         dist_along_handle_.resize(inliers_.size());
00031         for (int i = 0; i < inliers_.size(); i++)
00032         {
00033                 dist_along_handle_(i) = axis_.transpose()
00034                                 * hand_list_[inliers_[i]].getGraspBottom();
00035         }
00036 }
00037 
00038 void Handle::setGraspVariables()
00039 {
00040         double center_dist = (dist_along_handle_.maxCoeff() + dist_along_handle_.minCoeff()) / 2.0;
00041         double min_dist = 10000000;
00042         int min_idx = -1;
00043         for (int i = 0; i < dist_along_handle_.size(); i++)
00044         {
00045 //              std::cout << "dist_along_handle_: " << dist_along_handle_(i) << "\n";
00046                 double dist = fabs(dist_along_handle_(i) - center_dist);
00047                 if (dist < min_dist)
00048                 {
00049                         min_dist = dist;
00050                         min_idx = i;
00051                 }
00052         }
00053 //      std::cout << "min_idx: " << min_idx << "\n";
00054         center_ = hand_list_[inliers_[min_idx]].getGraspBottom();
00055         approach_ = hand_list_[inliers_[min_idx]].getApproach();
00056         hands_center_ = hand_list_[inliers_[min_idx]].getGraspSurface();
00057         binormal_ = approach_.cross(axis_);
00058 //      std::cout << "center_: " << center_.transpose() << std::endl;
00059 //      std::cout << "approach_: " << approach_.transpose() << std::endl;
00060 //      std::cout << "axis_: " << axis_.transpose() << std::endl;
00061 //      std::cout << "hands_center_: " << hands_center_.transpose() << std::endl;
00062 //      std::cout << std::endl;
00063 }
00064 
00065 void Handle::setGraspWidth()
00066 {
00067         width_ = 0.0;
00068         for (int i = 0; i < inliers_.size(); i++)
00069         {
00070                 width_ += hand_list_[inliers_[i]].getGraspWidth();
00071         }
00072         width_ /= (double) inliers_.size();
00073 }
00074 


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