grasp_data.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L.
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 Univ of CO, Boulder, PAL Robotics, S.L.
00018  *     nor the names of its contributors may be used to endorse or
00019  *     promote products derived from this software without specific
00020  *     prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  */
00035 
00036 /* Authors: Bence Magyar, Dave Coleman
00037    Description: Data class used by the grasp generator.
00038 */
00039 
00040 #include <moveit_simple_grasps/grasp_data.h>
00041 
00042 // Eigen
00043 #include <Eigen/Core>
00044 #include <Eigen/Geometry>
00045 #include <eigen_conversions/eigen_msg.h>
00046 
00047 // C++
00048 #include <math.h>
00049 #define _USE_MATH_DEFINES
00050 
00051 namespace moveit_simple_grasps
00052 {
00053 GraspData::GraspData() :
00054   // Fill in default values where possible:
00055   base_link_("/base_link"),
00056   grasp_depth_(0.12),
00057   angle_resolution_(16),
00058   approach_retreat_desired_dist_(0.6),
00059   approach_retreat_min_dist_(0.4),
00060   object_size_(0.04)
00061 {}
00062 
00063 bool GraspData::loadRobotGraspData(const ros::NodeHandle& nh, const std::string& end_effector)
00064 {
00065   std::vector<std::string> joint_names;
00066   std::vector<double> pre_grasp_posture; // todo: remove all underscore post-fixes
00067   std::vector<double> grasp_posture;
00068   std::vector<double> grasp_pose_to_eef;
00069   std::vector<double> grasp_pose_to_eef_rotation;
00070   double pregrasp_time_from_start;
00071   double grasp_time_from_start;
00072   std::string end_effector_name;
00073   std::string end_effector_parent_link;
00074 
00075   // Load a param
00076   if (!nh.hasParam("base_link"))
00077   {
00078     ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `base_link` missing from rosparam server. Did you load your end effector's configuration yaml file? Searching in namespace: " << nh.getNamespace());
00079     return false;
00080   }
00081   nh.getParam("base_link", base_link_);
00082 
00083   // Search within the sub-namespace of this end effector name
00084   ros::NodeHandle child_nh(nh, end_effector);
00085 
00086   // Load a param
00087   if (!child_nh.hasParam("pregrasp_time_from_start"))
00088   {
00089     ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `pregrasp_time_from_start` missing from rosparam server. Did you load your end effector's configuration yaml file? Searching in namespace: " << child_nh.getNamespace());
00090     return false;
00091   }
00092   child_nh.getParam("pregrasp_time_from_start", pregrasp_time_from_start);
00093 
00094   // Load a param
00095   if (!child_nh.hasParam("grasp_time_from_start"))
00096   {
00097     ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `grasp_time_from_start` missing from rosparam server. Did you load your end effector's configuration yaml file?");
00098     return false;
00099   }
00100   child_nh.getParam("grasp_time_from_start", grasp_time_from_start);
00101 
00102   // Load a param
00103   if (!child_nh.hasParam("end_effector_name"))
00104   {
00105     ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `end_effector_name` missing from rosparam server. Did you load your end effector's configuration yaml file?");
00106     return false;
00107   }
00108   child_nh.getParam("end_effector_name", end_effector_name);
00109 
00110   // Load a param
00111   if (!child_nh.hasParam("end_effector_parent_link"))
00112   {
00113     ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `end_effector_parent_link` missing from rosparam server. Did you load your end effector's configuration yaml file?");
00114     return false;
00115   }
00116   child_nh.getParam("end_effector_parent_link", end_effector_parent_link);
00117 
00118   // Load a param
00119   if (!child_nh.hasParam("joints"))
00120   {
00121     ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `joints` missing from rosparam server. Did you load your end effector's configuration yaml file?");
00122     return false;
00123   }
00124   XmlRpc::XmlRpcValue joint_list;
00125   child_nh.getParam("joints", joint_list);
00126   if (joint_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
00127     for (int32_t i = 0; i < joint_list.size(); ++i)
00128     {
00129       ROS_ASSERT(joint_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
00130       joint_names.push_back(static_cast<std::string>(joint_list[i]));
00131     }
00132   else
00133     ROS_ERROR_STREAM_NAMED("temp","joint list type is not type array???");
00134 
00135   if(child_nh.hasParam("pregrasp_posture"))
00136   {
00137     XmlRpc::XmlRpcValue preg_posture_list;
00138     child_nh.getParam("pregrasp_posture", preg_posture_list);
00139     ROS_ASSERT(preg_posture_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00140     for (int32_t i = 0; i < preg_posture_list.size(); ++i)
00141     {
00142       ROS_ASSERT(preg_posture_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00143       pre_grasp_posture.push_back(static_cast<double>(preg_posture_list[i]));
00144     }
00145   }
00146 
00147   ROS_ASSERT(child_nh.hasParam("grasp_posture"));
00148   XmlRpc::XmlRpcValue grasp_posture_list;
00149   child_nh.getParam("grasp_posture", grasp_posture_list);
00150   ROS_ASSERT(grasp_posture_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00151   for (int32_t i = 0; i < grasp_posture_list.size(); ++i)
00152   {
00153     ROS_ASSERT(grasp_posture_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00154     grasp_posture.push_back(static_cast<double>(grasp_posture_list[i]));
00155   }
00156 
00157   ROS_ASSERT(child_nh.hasParam("grasp_pose_to_eef"));
00158   XmlRpc::XmlRpcValue g_to_eef_list;
00159   child_nh.getParam("grasp_pose_to_eef", g_to_eef_list);
00160   ROS_ASSERT(g_to_eef_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00161   for (int32_t i = 0; i < g_to_eef_list.size(); ++i)
00162   {
00163     // Cast to double OR int
00164     if (g_to_eef_list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble)
00165     {
00166       if (g_to_eef_list[i].getType() != XmlRpc::XmlRpcValue::TypeInt )
00167       {
00168         ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `grasp_pose_to_eef` wrong data type - int or double required.");
00169         return false;
00170       }
00171       else
00172         grasp_pose_to_eef.push_back(static_cast<int>(g_to_eef_list[i]));
00173     }
00174     else
00175       grasp_pose_to_eef.push_back(static_cast<double>(g_to_eef_list[i]));
00176   }
00177 
00178   ROS_ASSERT(child_nh.hasParam("grasp_pose_to_eef_rotation"));
00179   XmlRpc::XmlRpcValue g_to_eef_rotation_list;
00180   child_nh.getParam("grasp_pose_to_eef_rotation", g_to_eef_rotation_list);
00181   ROS_ASSERT(g_to_eef_rotation_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
00182   for (int32_t i = 0; i < g_to_eef_rotation_list.size(); ++i)
00183   {
00184     // Cast to double OR int
00185     if (g_to_eef_rotation_list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble)
00186     {
00187       if (g_to_eef_rotation_list[i].getType() != XmlRpc::XmlRpcValue::TypeInt )
00188       {
00189         ROS_ERROR_STREAM_NAMED("grasp_data_loader","Grasp configuration parameter `grasp_pose_to_eef_rotation` wrong data type - int or double required.");
00190         return false;
00191       }
00192       else
00193         grasp_pose_to_eef_rotation.push_back(static_cast<int>(g_to_eef_rotation_list[i]));
00194     }
00195     else
00196       grasp_pose_to_eef_rotation.push_back(static_cast<double>(g_to_eef_rotation_list[i]));
00197   }
00198 
00199   // -------------------------------
00200   // Convert generic grasp pose to this end effector's frame of reference, approach direction for short
00201 
00202   // Orientation
00203   ROS_ASSERT(grasp_pose_to_eef_rotation.size() == 3);
00204   Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(grasp_pose_to_eef_rotation[1]), Eigen::Vector3d::UnitY())); // turn on Z axis
00205   // TODO: rotate for roll and yaw also, not just pitch (unit y)
00206   // but i don't need that feature right now and it might be tricky
00207   grasp_pose_to_eef_pose_.orientation.x = quat.x();
00208   grasp_pose_to_eef_pose_.orientation.y = quat.y();
00209   grasp_pose_to_eef_pose_.orientation.z = quat.z();
00210   grasp_pose_to_eef_pose_.orientation.w = quat.w();
00211 
00212   // Position // approach vector?
00213   ROS_ASSERT(grasp_pose_to_eef.size() == 3);
00214   grasp_pose_to_eef_pose_.position.x = grasp_pose_to_eef[0];
00215   grasp_pose_to_eef_pose_.position.y = grasp_pose_to_eef[1];
00216   grasp_pose_to_eef_pose_.position.z = grasp_pose_to_eef[2];
00217 
00218   // -------------------------------
00219   // Create pre-grasp posture if specified
00220   if(!pre_grasp_posture.empty())
00221   {
00222     pre_grasp_posture_.header.frame_id = base_link_;
00223     pre_grasp_posture_.header.stamp = ros::Time::now();
00224     // Name of joints:
00225     pre_grasp_posture_.joint_names = joint_names;
00226     // Position of joints
00227     pre_grasp_posture_.points.resize(1);
00228     pre_grasp_posture_.points[0].positions = pre_grasp_posture;
00229     pre_grasp_posture_.points[0].time_from_start = ros::Duration(pregrasp_time_from_start);
00230   }
00231   // -------------------------------
00232   // Create grasp posture
00233   grasp_posture_.header.frame_id = base_link_;
00234   grasp_posture_.header.stamp = ros::Time::now();
00235   // Name of joints:
00236   grasp_posture_.joint_names = joint_names;
00237   // Position of joints
00238   grasp_posture_.points.resize(1);
00239   grasp_posture_.points[0].positions = grasp_posture;
00240   grasp_posture_.points[0].time_from_start = ros::Duration(grasp_time_from_start);
00241 
00242   // -------------------------------
00243   // SRDF Info
00244   ee_parent_link_ = end_effector_parent_link;
00245   ee_group_ = end_effector_name;
00246 
00247   // -------------------------------
00248   // Nums
00249   approach_retreat_desired_dist_ = 0.2; // 0.3;
00250   approach_retreat_min_dist_ = 0.06;
00251   // distance from center point of object to end effector
00252   grasp_depth_ = 0.06;// in negative or 0 this makes the grasps on the other side of the object! (like from below)
00253 
00254   // generate grasps at PI/angle_resolution increments
00255   angle_resolution_ = 16; //TODO parametrize this, or move to action interface
00256 
00257   // Debug
00258   //moveit_simple_grasps::SimpleGrasps::printObjectGraspData(grasp_data);
00259 
00260   return true;
00261 }
00262 
00263 void GraspData::print()
00264 {
00265   ROS_WARN_STREAM_NAMED("grasp_data","Debug Grasp Data variable values:");
00266   std::cout << "grasp_pose_to_eef_pose_: \n" <<grasp_pose_to_eef_pose_<<std::endl;
00267   std::cout << "pre_grasp_posture_: \n" <<pre_grasp_posture_<<std::endl;
00268   std::cout << "grasp_posture_: \n" <<grasp_posture_<<std::endl;
00269   std::cout << "base_link_: " <<base_link_<<std::endl;
00270   std::cout << "ee_parent_link_: " <<ee_parent_link_<<std::endl;
00271   std::cout << "ee_group_: " <<ee_group_<<std::endl;
00272   std::cout << "grasp_depth_: " <<grasp_depth_<<std::endl;
00273   std::cout << "angle_resolution_: " <<angle_resolution_<<std::endl;
00274   std::cout << "approach_retreat_desired_dist_: " <<approach_retreat_desired_dist_<<std::endl;
00275   std::cout << "approach_retreat_min_dist_: " <<approach_retreat_min_dist_<<std::endl;
00276   std::cout << "object_size_: " <<object_size_<<std::endl;
00277 }
00278 
00279 } // namespace


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