object_manipulator: object_manipulator: approach_lift

00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 #ifndef _APPROACH_LIFT_GRASP_
00035 #define _APPROACH_LIFT_GRASP_
00036 
00037 #include <ros/ros.h>
00038 
00039 #include <object_manipulation_msgs/GraspResult.h>
00040 #include <object_manipulation_msgs/PickupGoal.h>
00041 
00042 #include <trajectory_msgs/JointTrajectory.h>
00043 
00044 #include "object_manipulator/tools/grasp_marker_publisher.h"
00045 
00046 namespace object_manipulator {
00047 
00050 struct GraspExecutionInfo {
00051   trajectory_msgs::JointTrajectory approach_trajectory_; 
00052   trajectory_msgs::JointTrajectory lift_trajectory_; 
00053   object_manipulation_msgs::GraspResult result_;
00054   int marker_id_;
00055 };
00056 
00057 // ---------------------------- Definitions ---------------------------------
00058 
00061 class GraspTester {
00062 protected:
00064   virtual void testGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00065                          const object_manipulation_msgs::Grasp &grasp,
00066                          GraspExecutionInfo &execution_info) = 0;  
00067 
00069   GraspMarkerPublisher *marker_publisher_;
00070 
00073   boost::function<void(size_t)> feedback_function_;
00074 
00076   boost::function<bool()> interrupt_function_;
00077 public:
00078   GraspTester() : marker_publisher_(NULL) {}
00079 
00081   virtual void testGrasps(const object_manipulation_msgs::PickupGoal &pickup_goal,
00082                           const std::vector<object_manipulation_msgs::Grasp> &grasps,
00083                           std::vector<GraspExecutionInfo> &execution_info,
00084                           bool return_on_first_hit);
00085 
00087   void setMarkerPublisher(GraspMarkerPublisher *pub){marker_publisher_ = pub;}
00088 
00090   void setFeedbackFunction(boost::function<void(size_t)> f){feedback_function_ = f;}
00091 
00093   void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}
00094 
00096   object_manipulation_msgs::GraspResult Result(int result_code, bool continuation)
00097   {
00098     object_manipulation_msgs::GraspResult result;
00099     result.result_code = result_code;
00100     result.continuation_possible = continuation;
00101     return result;
00102   }
00103 };
00104 
00107 class GraspPerformer {
00108 protected:
00110   virtual void performGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00111                             const object_manipulation_msgs::Grasp &grasp,
00112                             GraspExecutionInfo &execution_info) = 0;
00113 
00115   GraspMarkerPublisher *marker_publisher_;
00116 
00119   boost::function<void(size_t)> feedback_function_;
00120 
00122   boost::function<bool()> interrupt_function_;
00123 public:
00124   GraspPerformer() : marker_publisher_(NULL) {}
00125 
00127   virtual void performGrasps(const object_manipulation_msgs::PickupGoal &pickup_goal,
00128                              const std::vector<object_manipulation_msgs::Grasp> &grasps,
00129                              std::vector<GraspExecutionInfo> &execution_info);
00130 
00132   void setMarkerPublisher(GraspMarkerPublisher *pub){marker_publisher_ = pub;}
00133 
00135   void setFeedbackFunction(boost::function<void(size_t)> f){feedback_function_ = f;}
00136 
00138   void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}
00139 
00141   object_manipulation_msgs::GraspResult Result(int result_code, bool continuation)
00142   {
00143     object_manipulation_msgs::GraspResult result;
00144     result.result_code = result_code;
00145     result.continuation_possible = continuation;
00146     return result;
00147   }
00148 };
00149 
00150 // ------------------------------ Grasp Testers ----------------------------------
00151 
00154 class GraspTesterWithApproach : public GraspTester
00155 {
00156 protected:
00157   virtual void testGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00158                          const object_manipulation_msgs::Grasp &grasp,
00159                          GraspExecutionInfo &execution_info);  
00160 
00162   object_manipulation_msgs::GraspResult 
00163   getInterpolatedIKForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00164                            const object_manipulation_msgs::Grasp &grasp,
00165                            const std::vector<double> &grasp_joint_angles,
00166                            GraspExecutionInfo &execution_info);
00167   
00169   virtual arm_navigation_msgs::OrderedCollisionOperations 
00170   collisionOperationsForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00171                              const object_manipulation_msgs::Grasp &grasp);
00172   
00174   virtual std::vector<arm_navigation_msgs::LinkPadding> 
00175   linkPaddingForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00176                      const object_manipulation_msgs::Grasp &grasp);  
00177 
00179   object_manipulation_msgs::GraspResult 
00180   getInterpolatedIKForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00181                             const object_manipulation_msgs::Grasp &grasp,
00182                             GraspExecutionInfo &execution_info);
00183   
00185   virtual arm_navigation_msgs::OrderedCollisionOperations 
00186   collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00187                               const object_manipulation_msgs::Grasp &grasp);
00188   
00190   virtual std::vector<arm_navigation_msgs::LinkPadding> 
00191   linkPaddingForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00192                       const object_manipulation_msgs::Grasp &grasp);
00193 
00195   static float EPS;
00196 };
00197 
00199 class UnsafeGraspTester : public GraspTesterWithApproach
00200 {
00201 protected:
00203   virtual arm_navigation_msgs::OrderedCollisionOperations 
00204   collisionOperationsForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00205                              const object_manipulation_msgs::Grasp &grasp);
00206 
00208   virtual arm_navigation_msgs::OrderedCollisionOperations 
00209   collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00210                               const object_manipulation_msgs::Grasp &grasp);
00211 };
00212 
00213 // ---------------------------- Grasp Performers ---------------------------------
00214 
00216 class StandardGraspPerformer : public GraspPerformer {
00217 protected:
00218   virtual void performGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00219                             const object_manipulation_msgs::Grasp &grasp,
00220                             GraspExecutionInfo &execution_info);
00221 
00222   virtual object_manipulation_msgs::GraspResult 
00223   approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00224                    const object_manipulation_msgs::Grasp &grasp,
00225                    GraspExecutionInfo &execution_info);
00226   
00227   virtual object_manipulation_msgs::GraspResult 
00228   lift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00229        const object_manipulation_msgs::Grasp &grasp,
00230        GraspExecutionInfo &execution_info);
00231 
00232   virtual object_manipulation_msgs::GraspResult 
00233   retreat(const object_manipulation_msgs::PickupGoal &pickup_goal,
00234           const object_manipulation_msgs::Grasp &grasp,
00235           GraspExecutionInfo &execution_info); 
00236 };
00237 
00240 class ReactiveGraspPerformer : public StandardGraspPerformer {
00241 protected:
00242 
00244   virtual object_manipulation_msgs::GraspResult 
00245   nonReactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00246                   const object_manipulation_msgs::Grasp &grasp,
00247                   GraspExecutionInfo &execution_info);
00248   
00250   virtual object_manipulation_msgs::GraspResult 
00251   reactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00252                const object_manipulation_msgs::Grasp &grasp,
00253                GraspExecutionInfo &execution_info);  
00254   
00255   virtual object_manipulation_msgs::GraspResult 
00256   lift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00257        const object_manipulation_msgs::Grasp &grasp,
00258        GraspExecutionInfo &execution_info);  
00259   
00260   virtual object_manipulation_msgs::GraspResult 
00261   approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00262                    const object_manipulation_msgs::Grasp &grasp,
00263                    GraspExecutionInfo &execution_info);  
00264 };
00265 
00266 
00268 
00281 class UnsafeGraspPerformer : public ReactiveGraspPerformer {
00282 protected:
00283   virtual object_manipulation_msgs::GraspResult 
00284   approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00285                    const object_manipulation_msgs::Grasp &grasp,
00286                    GraspExecutionInfo &execution_info);  
00287 };
00288 
00289 }
00290 
00291 #endif
approach_lift_grasp.h
