moveit_blocks.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, CU 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 CU 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 
00040 // ROS
00041 #include <ros/ros.h>
00042 
00043 // MoveIt!
00044 #include <moveit/move_group_interface/move_group.h>
00045 
00046 // Grasp generation
00047 #include <moveit_simple_grasps/simple_grasps.h>
00048 #include <moveit_simple_grasps/grasp_data.h>
00049 #include <moveit_visual_tools/visual_tools.h> // simple tool for showing graspsp
00050 
00051 namespace moveit_simple_grasps
00052 {
00053 
00054 static const double BLOCK_SIZE = 0.04;
00055 
00056 struct MetaBlock
00057 {
00058   std::string name;
00059   geometry_msgs::Pose start_pose;
00060   geometry_msgs::Pose goal_pose;
00061 };
00062 
00063 class MoveItBlocks
00064 {
00065 public:
00066 
00067   // grasp generator
00068   moveit_simple_grasps::SimpleGraspsPtr simple_grasps_;
00069 
00070   moveit_visual_tools::VisualToolsPtr visual_tools_;
00071 
00072   // data for generating grasps
00073   moveit_simple_grasps::GraspData grasp_data_;
00074 
00075   // our interface with MoveIt
00076   boost::scoped_ptr<move_group_interface::MoveGroup> move_group_;
00077 
00078   // which arm are we using
00079   std::string ee_group_name_;
00080   std::string planning_group_name_;
00081 
00082 
00083   ros::NodeHandle nh_;
00084 
00085   // settings
00086   bool auto_reset_;
00087   int auto_reset_sec_;
00088   int pick_place_count_; // tracks how many pick_places have run 
00089 
00090   MoveItBlocks()
00091     : auto_reset_(false),
00092       auto_reset_sec_(4),
00093       pick_place_count_(0),
00094       nh_("~")
00095   {
00096     ROS_INFO_STREAM_NAMED("moveit_blocks","Starting MoveIt Blocks");
00097 
00098     // Get arm info from param server
00099     nh_.param("ee_group_name", ee_group_name_, std::string("unknown"));
00100     nh_.param("planning_group_name", planning_group_name_, std::string("unknown"));
00101 
00102     ROS_INFO_STREAM_NAMED("moveit_blocks","End Effector: " << ee_group_name_);
00103     ROS_INFO_STREAM_NAMED("moveit_blocks","Planning Group: " << planning_group_name_);
00104       
00105     // Create MoveGroup for one of the planning groups
00106     move_group_.reset(new move_group_interface::MoveGroup(planning_group_name_));
00107     move_group_->setPlanningTime(30.0);
00108 
00109     // Load grasp generator
00110     if (!grasp_data_.loadRobotGraspData(nh_, ee_group_name_))
00111       ros::shutdown();
00112 
00113     // Load the Robot Viz Tools for publishing to rviz
00114     visual_tools_.reset(new moveit_visual_tools::VisualTools( grasp_data_.base_link_));
00115     visual_tools_->setFloorToBaseHeight(-0.9);
00116     visual_tools_->loadEEMarker(grasp_data_.ee_group_, planning_group_name_);
00117 
00118     simple_grasps_.reset(new moveit_simple_grasps::SimpleGrasps(visual_tools_));
00119 
00120     // Let everything load
00121     ros::Duration(1.0).sleep();
00122 
00123     // Do it.
00124     startRoutine();
00125   }
00126 
00127   bool startRoutine()
00128   {
00129     // Debug - calculate and output table surface dimensions
00130     /*
00131     if( false )
00132     {
00133       double y_min, y_max, x_min, x_max;
00134       getTableWidthRange(y_min, y_max);
00135       getTableDepthRange(x_min, x_max);
00136       ROS_INFO_STREAM_NAMED("table","Blocks width range: " << y_min << " <= y <= " << y_max);
00137       ROS_INFO_STREAM_NAMED("table","Blocks depth range: " << x_min << " <= x <= " << x_max);
00138     }
00139     */
00140 
00141     // Create start block positions (hard coded)
00142     std::vector<MetaBlock> blocks;
00143     double block_y = 0.1;
00144     double block_x = 0.35;
00145     // Flip the side of the table the blocks are on depending on which arm we are using
00146     //if( arm_.compare("right") == 0 )
00147     //  block_y *= -1;
00148     blocks.push_back( createStartBlock(block_x,       block_y, "Block1") );
00149     blocks.push_back( createStartBlock(block_x+0.1,   block_y, "Block2") );
00150     blocks.push_back( createStartBlock(block_x+0.2,   block_y, "Block3") );
00151 
00152     // The goal for each block is simply translating them on the y axis
00153     for (std::size_t i = 0; i < blocks.size(); ++i)
00154     {
00155       blocks[i].goal_pose = blocks[i].start_pose;
00156       blocks[i].goal_pose.position.y += 0.2;
00157     }
00158 
00159     // Show grasp visualizations or not
00160     visual_tools_->setMuted(false);
00161 
00162     // Create the walls and tables
00163     //createEnvironment(visual_tools_);
00164 
00165     // --------------------------------------------------------------------------------------------------------
00166     // Repeat pick and place forever
00167     while(ros::ok())
00168     {
00169       // --------------------------------------------------------------------------------------------
00170       // Re-add all blocks
00171       for (std::size_t i = 0; i < blocks.size(); ++i)
00172       {
00173         resetBlock(blocks[i]);
00174       }
00175 
00176       // Place on left side, then back on right side
00177       for (std::size_t side = 0; side < 2; ++side)
00178       {
00179 
00180         // Do for all blocks
00181         for (std::size_t block_id = 0; block_id < blocks.size(); ++block_id)
00182         {
00183           // Pick -------------------------------------------------------------------------------------
00184           while(ros::ok())
00185           {
00186             ROS_INFO_STREAM_NAMED("pick_place","Picking '" << blocks[block_id].name << "'");
00187 
00188             // Visualize the block we are about to pick
00189             visual_tools_->publishBlock( blocks[block_id].start_pose, moveit_visual_tools::BLUE, BLOCK_SIZE);
00190 
00191             if( !pick(blocks[block_id].start_pose, blocks[block_id].name) )
00192             {
00193               ROS_ERROR_STREAM_NAMED("pick_place","Pick failed.");
00194 
00195               // Ask user if we should try again
00196               if( !promptUser() )
00197                 exit(0);
00198 
00199               // Retry
00200               resetBlock(blocks[block_id]);
00201             }
00202             else
00203             {
00204               ROS_INFO_STREAM_NAMED("pick_place","Done with pick ---------------------------");
00205               break;
00206             }
00207           }
00208           
00209           // Place -------------------------------------------------------------------------------------
00210           while(ros::ok())
00211           {
00212             ROS_INFO_STREAM_NAMED("pick_place","Placing '" << blocks[block_id].name << "'");
00213 
00214             // Publish goal block location
00215             visual_tools_->publishBlock( blocks[block_id].goal_pose, moveit_visual_tools::BLUE, BLOCK_SIZE);
00216 
00217             if( !place(blocks[block_id].goal_pose, blocks[block_id].name) )
00218             {
00219               ROS_ERROR_STREAM_NAMED("pick_place","Place failed.");
00220 
00221               // Determine if the attached collision body as already been removed, in which case
00222               // we can ignore the failure and just resume picking
00223               /*
00224                 if( !move_group_->hasAttachedObject(blocks[block_id].name) )
00225                 {
00226                 ROS_WARN_STREAM_NAMED("pick_place","Collision object already detached, so auto resuming pick place.");
00227 
00228                 // Ask user if we should try again
00229                 if( !promptUser() )
00230                 break; // resume picking
00231                 }
00232               */
00233 
00234               // Ask user if we should try again
00235               if( !promptUser() )
00236                 exit(0);
00237             }
00238             else
00239             {
00240               ROS_INFO_STREAM_NAMED("pick_place","Done with place ----------------------------");
00241               break;
00242             }
00243           } // place retry loop
00244 
00245           // Swap this block's start and end pose so that we can then move them back to position
00246           geometry_msgs::Pose temp = blocks[block_id].start_pose;
00247           blocks[block_id].start_pose = blocks[block_id].goal_pose;
00248           blocks[block_id].goal_pose = temp;
00249 
00250           pick_place_count_++;
00251 
00252         } // loop through 3 blocks
00253 
00254         ROS_INFO_STREAM_NAMED("pick_place","Finished picking and placing " << blocks.size() 
00255           << " blocks. Total pick_places: " << pick_place_count_);
00256 
00257       } // place on both sides of table
00258 
00259       // Ask user if we should repeat
00260       //if( !promptUser() )
00261       //  break;
00262       ros::Duration(1.0).sleep();
00263 
00264     }
00265 
00266     // Everything worked!
00267     return true;
00268   }
00269 
00270   void resetBlock(MetaBlock block)
00271   {
00272     // Remove attached object
00273     visual_tools_->cleanupACO(block.name);
00274 
00275     // Remove collision object
00276     visual_tools_->cleanupCO(block.name);
00277 
00278     // Add the collision block
00279     visual_tools_->publishCollisionBlock(block.start_pose, block.name, BLOCK_SIZE);
00280   }
00281 
00282   MetaBlock createStartBlock(double x, double y, const std::string name)
00283   {
00284     MetaBlock start_block;
00285     start_block.name = name;
00286 
00287     // Position
00288     start_block.start_pose.position.x = x;
00289     start_block.start_pose.position.y = y;
00290     start_block.start_pose.position.z = BLOCK_SIZE / 2.0; //getTableHeight(-0.9);
00291 
00292     // Orientation
00293     double angle = 0; // M_PI / 1.5;
00294     Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
00295     start_block.start_pose.orientation.x = quat.x();
00296     start_block.start_pose.orientation.y = quat.y();
00297     start_block.start_pose.orientation.z = quat.z();
00298     start_block.start_pose.orientation.w = quat.w();
00299 
00300     return start_block;
00301   }
00302 
00303   bool pick(const geometry_msgs::Pose& block_pose, std::string block_name)
00304   {
00305     std::vector<moveit_msgs::Grasp> possible_grasps;
00306 
00307     // Pick grasp
00308     simple_grasps_->generateBlockGrasps( block_pose, grasp_data_, possible_grasps );
00309 
00310     // Visualize them
00311     //visual_tools_->publishAnimatedGrasps(possible_grasps, grasp_data_.ee_parent_link_);
00312     visual_tools_->publishGrasps(possible_grasps, grasp_data_.ee_parent_link_);
00313 
00314     // Prevent collision with table
00315     //move_group_->setSupportSurfaceName(SUPPORT_SURFACE3_NAME);
00316 
00317     // Allow blocks to be touched by end effector
00318     {
00319       // an optional list of obstacles that we have semantic information about and that can be touched/pushed/moved in the course of grasping
00320       std::vector<std::string> allowed_touch_objects;
00321       allowed_touch_objects.push_back("Block1");
00322       allowed_touch_objects.push_back("Block2");
00323       allowed_touch_objects.push_back("Block3");
00324       allowed_touch_objects.push_back("Block4");
00325 
00326       // Add this list to all grasps
00327       for (std::size_t i = 0; i < possible_grasps.size(); ++i)
00328       {
00329         possible_grasps[i].allowed_touch_objects = allowed_touch_objects;
00330       }
00331     }
00332 
00333     //ROS_INFO_STREAM_NAMED("","Grasp 0\n" << possible_grasps[0]);
00334 
00335     return move_group_->pick(block_name, possible_grasps);
00336   }
00337 
00338   bool place(const geometry_msgs::Pose& goal_block_pose, std::string block_name)
00339   {
00340     ROS_WARN_STREAM_NAMED("place","Placing '"<< block_name << "'");
00341 
00342     std::vector<moveit_msgs::PlaceLocation> place_locations;
00343 
00344     // Re-usable datastruct
00345     geometry_msgs::PoseStamped pose_stamped;
00346     pose_stamped.header.frame_id = grasp_data_.base_link_;
00347     pose_stamped.header.stamp = ros::Time::now();
00348 
00349     // Create 360 degrees of place location rotated around a center
00350     for (double angle = 0; angle < 2*M_PI; angle += M_PI/2)
00351     {
00352       pose_stamped.pose = goal_block_pose;
00353 
00354       // Orientation
00355       Eigen::Quaterniond quat(Eigen::AngleAxis<double>(double(angle), Eigen::Vector3d::UnitZ()));
00356       pose_stamped.pose.orientation.x = quat.x();
00357       pose_stamped.pose.orientation.y = quat.y();
00358       pose_stamped.pose.orientation.z = quat.z();
00359       pose_stamped.pose.orientation.w = quat.w();
00360 
00361       // Create new place location
00362       moveit_msgs::PlaceLocation place_loc;
00363 
00364       place_loc.place_pose = pose_stamped;
00365 
00366       visual_tools_->publishBlock( place_loc.place_pose.pose, moveit_visual_tools::BLUE, BLOCK_SIZE);
00367 
00368       // Approach
00369       moveit_msgs::GripperTranslation pre_place_approach;
00370       pre_place_approach.direction.header.stamp = ros::Time::now();
00371       pre_place_approach.desired_distance = grasp_data_.approach_retreat_desired_dist_; // The distance the origin of a robot link needs to travel
00372       pre_place_approach.min_distance = grasp_data_.approach_retreat_min_dist_; // half of the desired? Untested.
00373       pre_place_approach.direction.header.frame_id = grasp_data_.base_link_;
00374       pre_place_approach.direction.vector.x = 0;
00375       pre_place_approach.direction.vector.y = 0;
00376       pre_place_approach.direction.vector.z = -1; // Approach direction (negative z axis)  // TODO: document this assumption
00377       place_loc.pre_place_approach = pre_place_approach;
00378 
00379       // Retreat
00380       moveit_msgs::GripperTranslation post_place_retreat;
00381       post_place_retreat.direction.header.stamp = ros::Time::now();
00382       post_place_retreat.desired_distance = grasp_data_.approach_retreat_desired_dist_; // The distance the origin of a robot link needs to travel
00383       post_place_retreat.min_distance = grasp_data_.approach_retreat_min_dist_; // half of the desired? Untested.
00384       post_place_retreat.direction.header.frame_id = grasp_data_.base_link_;
00385       post_place_retreat.direction.vector.x = 0;
00386       post_place_retreat.direction.vector.y = 0;
00387       post_place_retreat.direction.vector.z = 1; // Retreat direction (pos z axis)
00388       place_loc.post_place_retreat = post_place_retreat;
00389 
00390       // Post place posture - use same as pre-grasp posture (the OPEN command)
00391       place_loc.post_place_posture = grasp_data_.pre_grasp_posture_;
00392 
00393       place_locations.push_back(place_loc);
00394     }
00395 
00396     // Prevent collision with table
00397     //move_group_->setSupportSurfaceName(SUPPORT_SURFACE3_NAME);
00398 
00399     move_group_->setPlannerId("RRTConnectkConfigDefault");
00400 
00401     return move_group_->place(block_name, place_locations);
00402   }
00403 
00404   bool promptUser()
00405   {
00406     // Make sure ROS is still with us
00407     if( !ros::ok() )
00408       return false;
00409 
00410     if( auto_reset_ )
00411     {
00412       ROS_INFO_STREAM_NAMED("pick_place","Auto-retrying in " << auto_reset_sec_ << " seconds");
00413       ros::Duration(auto_reset_sec_).sleep();
00414     }
00415     else
00416     {
00417       ROS_INFO_STREAM_NAMED("pick_place","Retry? (y/n)");
00418       char input; // used for prompting yes/no
00419       std::cin >> input;
00420       if( input == 'n' )
00421         return false;
00422     }
00423     return true;
00424   }
00425 
00426 };
00427 
00428 } //namespace


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