simple_grasps.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 // Author: Dave Coleman
00036 // Desc:   Generates grasps for a cube
00037 
00038 #ifndef MOVEIT_SIMPLE_GRASPS__MOVEIT_SIMPLE_GRASPS_H_
00039 #define MOVEIT_SIMPLE_GRASPS__MOVEIT_SIMPLE_GRASPS_H_
00040 
00041 // ROS
00042 #include <ros/ros.h>
00043 
00044 // TF
00045 #include <tf_conversions/tf_eigen.h>
00046 
00047 // Msgs
00048 #include <geometry_msgs/PoseArray.h>
00049 
00050 // MoveIt
00051 #include <moveit_msgs/Grasp.h>
00052 #include <moveit/macros/deprecation.h>
00053 
00054 // Eigen
00055 #include <Eigen/Core>
00056 #include <Eigen/Geometry>
00057 #include <eigen_conversions/eigen_msg.h>
00058 
00059 // Visualization
00060 #include <moveit_visual_tools/visual_tools.h>
00061 
00062 // C++
00063 #include <math.h>
00064 #define _USE_MATH_DEFINES
00065 
00066 #include <moveit_simple_grasps/grasp_data.h>
00067 
00068 namespace moveit_simple_grasps
00069 {
00070 
00071 static const double RAD2DEG = 57.2957795;
00072 
00073 // Grasp axis orientation
00074 enum grasp_axis_t {X_AXIS, Y_AXIS, Z_AXIS};
00075 enum grasp_direction_t {UP, DOWN};
00076 enum grasp_rotation_t {FULL, HALF};
00077 
00078 // Class
00079 class SimpleGrasps
00080 {
00081 private:
00082 
00083   // class for publishing stuff to rviz
00084   moveit_visual_tools::VisualToolsPtr visual_tools_;
00085 
00086   // Transform from frame of box to global frame
00087   Eigen::Affine3d object_global_transform_;
00088 
00089   // Display more output both in console and in Rviz (with arrows and markers)
00090   bool verbose_;
00091 
00092 public:
00093   EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles). With GCC, this is done with a attribute ((aligned(16))).
00094 
00098   SimpleGrasps(moveit_visual_tools::VisualToolsPtr rviz_tools, bool verbose = false);
00099 
00103   ~SimpleGrasps();
00104 
00108   MOVEIT_DEPRECATED bool generateAllGrasps(const geometry_msgs::Pose& object_pose, const GraspData& grasp_data,
00109     std::vector<moveit_msgs::Grasp>& possible_grasps)
00110   {
00111     generateBlockGrasps(object_pose, grasp_data, possible_grasps);
00112 
00113     return true;
00114   }
00115 
00123   bool generateBlockGrasps(const geometry_msgs::Pose& object_pose, const GraspData& grasp_data,
00124     std::vector<moveit_msgs::Grasp>& possible_grasps);
00125 
00140   bool generateAxisGrasps(
00141     const geometry_msgs::Pose& object_pose,
00142     grasp_axis_t axis,
00143     grasp_direction_t direction,
00144     grasp_rotation_t rotation,
00145     double hand_roll,
00146     const GraspData& grasp_data,
00147     std::vector<moveit_msgs::Grasp>& possible_grasps);
00148 
00155   static geometry_msgs::PoseStamped getPreGraspPose(const moveit_msgs::Grasp &grasp, const std::string &ee_parent_link);
00156 
00161   MOVEIT_DEPRECATED static void printObjectGraspData(const GraspData& data)
00162   {
00163     ROS_INFO_STREAM_NAMED("grasp","ROBOT GRASP DATA DEBUG OUTPUT ---------------------");
00164     ROS_INFO_STREAM_NAMED("grasp","Base Link: " << data.base_link_);
00165     ROS_INFO_STREAM_NAMED("grasp","EE Parent Link: " << data.ee_parent_link_);
00166     ROS_INFO_STREAM_NAMED("grasp","Grasp Depth: " << data.grasp_depth_);
00167     ROS_INFO_STREAM_NAMED("grasp","Angle Resolution: " << data.angle_resolution_);
00168     ROS_INFO_STREAM_NAMED("grasp","Approach Retreat Desired Dist: " << data.approach_retreat_desired_dist_);
00169     ROS_INFO_STREAM_NAMED("grasp","Approach Retreat Min Dist: " << data.approach_retreat_min_dist_);
00170     ROS_INFO_STREAM_NAMED("grasp","Pregrasp Posture: \n" << data.pre_grasp_posture_);
00171     ROS_INFO_STREAM_NAMED("grasp","Grasp Posture: \n" << data.grasp_posture_);
00172     ROS_INFO_STREAM_NAMED("grasp","---------------------------------------------------\n");
00173   }
00174 
00175 }; // end of class
00176 
00177 typedef boost::shared_ptr<SimpleGrasps> SimpleGraspsPtr;
00178 typedef boost::shared_ptr<const SimpleGrasps> SimpleGraspsConstPtr;
00179 
00180 } // namespace
00181 
00182 #endif


moveit_simple_grasps
Author(s): Dave Coleman
autogenerated on Fri Aug 28 2015 11:36:01