approach_lift_grasp.h

Go to the documentation of this file.

/*********************************************************************
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  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.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  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 _APPROACH_LIFT_GRASP_
#define _APPROACH_LIFT_GRASP_

#include <ros/ros.h>

#include <object_manipulation_msgs/GraspResult.h>
#include <object_manipulation_msgs/PickupGoal.h>

#include <trajectory_msgs/JointTrajectory.h>

#include "object_manipulator/tools/grasp_marker_publisher.h"

namespace object_manipulator {

struct GraspExecutionInfo {
  trajectory_msgs::JointTrajectory approach_trajectory_; 
  trajectory_msgs::JointTrajectory lift_trajectory_; 
  object_manipulation_msgs::GraspResult result_;
  int marker_id_;
};

// ---------------------------- Definitions ---------------------------------

class GraspTester {
protected:
  virtual void testGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                         const manipulation_msgs::Grasp &grasp,
                         GraspExecutionInfo &execution_info) = 0;  

  GraspMarkerPublisher *marker_publisher_;

  boost::function<void(size_t)> feedback_function_;

  boost::function<bool()> interrupt_function_;
public:
  GraspTester() : marker_publisher_(NULL) {}

  virtual void testGrasps(const object_manipulation_msgs::PickupGoal &pickup_goal,
                          const std::vector<manipulation_msgs::Grasp> &grasps,
                          std::vector<GraspExecutionInfo> &execution_info,
                          bool return_on_first_hit);

  void setMarkerPublisher(GraspMarkerPublisher *pub){marker_publisher_ = pub;}

  void setFeedbackFunction(boost::function<void(size_t)> f){feedback_function_ = f;}

  void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}

  object_manipulation_msgs::GraspResult Result(int result_code, bool continuation)
  {
    object_manipulation_msgs::GraspResult result;
    result.result_code = result_code;
    result.continuation_possible = continuation;
    return result;
  }
};

class GraspPerformer {
protected:
  virtual void performGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                            const manipulation_msgs::Grasp &grasp,
                            GraspExecutionInfo &execution_info) = 0;

  GraspMarkerPublisher *marker_publisher_;

  boost::function<void(size_t)> feedback_function_;

  boost::function<bool()> interrupt_function_;
public:
  GraspPerformer() : marker_publisher_(NULL) {}

  virtual void performGrasps(const object_manipulation_msgs::PickupGoal &pickup_goal,
                             const std::vector<manipulation_msgs::Grasp> &grasps,
                             std::vector<GraspExecutionInfo> &execution_info);

  void setMarkerPublisher(GraspMarkerPublisher *pub){marker_publisher_ = pub;}

  void setFeedbackFunction(boost::function<void(size_t)> f){feedback_function_ = f;}

  void setInterruptFunction(boost::function<bool()> f){interrupt_function_ = f;}

  object_manipulation_msgs::GraspResult Result(int result_code, bool continuation)
  {
    object_manipulation_msgs::GraspResult result;
    result.result_code = result_code;
    result.continuation_possible = continuation;
    return result;
  }
};

// ------------------------------ Grasp Testers ----------------------------------

class GraspTesterWithApproach : public GraspTester
{
protected:
  virtual void testGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                         const manipulation_msgs::Grasp &grasp,
                         GraspExecutionInfo &execution_info);  

  object_manipulation_msgs::GraspResult 
  getInterpolatedIKForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
                           const manipulation_msgs::Grasp &grasp,
                           const std::vector<double> &grasp_joint_angles,
                           GraspExecutionInfo &execution_info);
  
  virtual arm_navigation_msgs::OrderedCollisionOperations 
  collisionOperationsForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
                             const manipulation_msgs::Grasp &grasp);
  
  virtual std::vector<arm_navigation_msgs::LinkPadding> 
  linkPaddingForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
                     const manipulation_msgs::Grasp &grasp);  

  object_manipulation_msgs::GraspResult 
  getInterpolatedIKForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                            const manipulation_msgs::Grasp &grasp,
                            GraspExecutionInfo &execution_info);
  
  virtual arm_navigation_msgs::OrderedCollisionOperations 
  collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                              const manipulation_msgs::Grasp &grasp);
  
  virtual std::vector<arm_navigation_msgs::LinkPadding> 
  linkPaddingForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                      const manipulation_msgs::Grasp &grasp);

  static float EPS;
};

class UnsafeGraspTester : public GraspTesterWithApproach
{
protected:
  virtual arm_navigation_msgs::OrderedCollisionOperations 
  collisionOperationsForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
                             const manipulation_msgs::Grasp &grasp);

  virtual arm_navigation_msgs::OrderedCollisionOperations 
  collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                              const manipulation_msgs::Grasp &grasp);
};

// ---------------------------- Grasp Performers ---------------------------------

class StandardGraspPerformer : public GraspPerformer {
protected:
  virtual void performGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                            const manipulation_msgs::Grasp &grasp,
                            GraspExecutionInfo &execution_info);

  virtual object_manipulation_msgs::GraspResult 
  approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                   const manipulation_msgs::Grasp &grasp,
                   GraspExecutionInfo &execution_info);
  
  virtual object_manipulation_msgs::GraspResult 
  lift(const object_manipulation_msgs::PickupGoal &pickup_goal,
       const manipulation_msgs::Grasp &grasp,
       GraspExecutionInfo &execution_info);

  virtual object_manipulation_msgs::GraspResult 
  retreat(const object_manipulation_msgs::PickupGoal &pickup_goal,
          const manipulation_msgs::Grasp &grasp,
          GraspExecutionInfo &execution_info); 
};

class ReactiveGraspPerformer : public StandardGraspPerformer {
protected:

  virtual object_manipulation_msgs::GraspResult 
  nonReactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
                  const manipulation_msgs::Grasp &grasp,
                  GraspExecutionInfo &execution_info);
  
  virtual object_manipulation_msgs::GraspResult 
  reactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
               const manipulation_msgs::Grasp &grasp,
               GraspExecutionInfo &execution_info);  
  
  virtual object_manipulation_msgs::GraspResult 
  lift(const object_manipulation_msgs::PickupGoal &pickup_goal,
       const manipulation_msgs::Grasp &grasp,
       GraspExecutionInfo &execution_info);  
  
  virtual object_manipulation_msgs::GraspResult 
  approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                   const manipulation_msgs::Grasp &grasp,
                   GraspExecutionInfo &execution_info);  
};



class UnsafeGraspPerformer : public ReactiveGraspPerformer {
protected:
  virtual object_manipulation_msgs::GraspResult 
  approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
                   const manipulation_msgs::Grasp &grasp,
                   GraspExecutionInfo &execution_info);  
};

}

#endif