rotating_hand.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Andreas ten Pas
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef ROTATING_HAND_H
#define ROTATING_HAND_H

#include <Eigen/Dense>
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <vector>

#include <agile_grasp/antipodal.h>
#include <agile_grasp/finger_hand.h>
#include <agile_grasp/grasp_hypothesis.h>

class RotatingHand
{
public:

  RotatingHand() : tolerant_antipodal_(true), cam_source_(-1) { }

        RotatingHand(bool tolerant_antipodal, int cam_source) 
    : tolerant_antipodal_(tolerant_antipodal), cam_source_(cam_source)
        {       }

        RotatingHand(const Eigen::VectorXd& camera_origin_left, const Eigen::VectorXd& camera_origin_right,
                        const FingerHand& finger_hand, bool tolerant_antipodal, int cam_source);

        void transformPoints(const Eigen::Matrix3Xd& points, const Eigen::Vector3d& normal,
                        const Eigen::Vector3d& axis, const Eigen::Matrix3Xd& normals, const Eigen::VectorXi& points_cam_source,
                        double hand_height);
        
        std::vector<GraspHypothesis> evaluateHand(double init_bite, const Eigen::Vector3d& sample, bool use_antipodal);
                          
  const Eigen::Matrix<double, 3, 2>& getCams() const 
  {
    return cams_;
  }
  
  const Eigen::VectorXi& getPointsCamSource() const 
  {
    return points_cam_source_;
  }
  
  int getCamSource() const
  {
    return cam_source_;
  }


private:

  int cam_source_; 
        Eigen::Matrix<double, 3, 2> cams_; 
        Eigen::VectorXd angles_; 
        Eigen::Matrix3Xd points_; 
        Eigen::Vector3d hand_axis_; 
        Eigen::Matrix3d frame_; 
        Eigen::Matrix3Xd normals_; 
        Eigen::VectorXi points_cam_source_;      
  FingerHand finger_hand_; 
  
  bool tolerant_antipodal_; 
};

#endif