GrasperRequest.h

Go to the documentation of this file.

//-*-c++-*-
#ifndef INCLUDED_GrasperRequest_h_
#define INCLUDED_GrasperRequest_h_

#include "Behaviors/StateNode.h"
#include "DualCoding/ShapeRoot.h"
#include "Planners/Manipulation/ShapeSpacePlanner2DR.h"
#include "Planners/Manipulation/ShapeSpacePlanner3DR.h"

using namespace std;
using namespace DualCoding;

//! Request to the @a Grasper to manipulate something

class GrasperRequest {
public:
#ifdef TGT_CALLIOPE
  typedef ShapeSpacePlanner3DR<NumArmJoints> Planner;
  typedef Planner::NodeType_t NodeType_t;
  typedef PlannerObstacle3D PlannerObstacle;
#else
  typedef ShapeSpacePlanner2DR<NumArmJoints> Planner;
  typedef Planner::NodeType_t NodeType_t;
  typedef PlannerObstacle2D PlannerObstacle;
#endif
  typedef NodeType_t::NodeValue_t NodeValue_t;
  
private:
  friend class Grasper;
  
  typedef unsigned int GrasperVerbosity_t;
  
  //! The planner obstacle used when we treat the manipulating object as an obstacle to the planner
  PlannerObstacle* objectPlannerObstacle;
  
public:
  
  //! What we're asking the @a Grasper to do
  enum GrasperRequestType_t {
    checkGraspable, //!< Check whether @a object is graspable; don't actually move
    checkMovable, //!< Check whether @a object can be moved to @a targetLocation; don't actually move
    checkRestable,  //!< Check whether @a the arm can be moved to the resting position; don't actually move
    computeReach, //!< Compute and return a path for the arm to reach to a point
    reach,    //!< Move the arm to @a targetLocation, then rest
    grasp,    //!< Approach and grasp @o object, then rest the arm
    touch,    //!< Grasp the @a object but don't rest the arm afterwards
    release,    //!< Release and disengage from whatever the arm is holding
    moveTo,   //!< Move @a object to @a targetLocation
    rest,   //!< Move the arm (and possibly the body) to a resting position
    //todo
    sweep,    //!< Sweep arm along an arc (to clear a region of the workspace)
    computeMove,  //!< Compute and return a path for moving @a object to @a targetLocation
    computeRest   //!< Compute and return a path for moving the arm to the resting position
  };
  
  //! What error is the Grasper going to return
  enum GrasperErrorType_t {
    noError = 0,
    someError,    //!< Generic error will match anything but noError in a GrasperTrans
    invalidRequest, 
    noGraspState,
    noGraspPath,
    noTargetState,
    noTargetPath,
    noRestPath,
    badGrasp,
    badMove,
    pickUpUnreachable,
    dropOffUnreachable
  };
  
  //! What path we want to return in the @a GrasperEvent
  enum GrasperPathType_t {
    noPath,   //!< Don't return any path
    moveFree,   //!< Return the unconstrained path to acquire @a object
    moveConstrained,  //!< Return the constrained path to move @a object to @a targetLocation
    disengage   //!< Return the path to disengage from the object and move the arm to the rest position
  };
  
  //! The kind of rest state to reach. stationary means none, settleArm moves the arm to the arm configuration in @a armRestState, settleBodyAndArm does settleArm and resets the ground plane
  enum GrasperRestType_t {
    stationary,
    settleArm,
    settleBodyAndArm
  };
  
  ~GrasperRequest();
  
  //! Constructor
  GrasperRequest(GrasperRequestType_t _type);
  
  //! Copy constructor
  GrasperRequest(const GrasperRequest &req);
  
  //! Validate the GrasperRequest and return an error code if there is a problem
  GrasperErrorType_t validateRequest();
  
  //! The type of this Grasper request
  GrasperRequestType_t requestType;
  
  //! The rest type for this request
  GrasperRestType_t restType;
  
  //! The rest arm configuration 
  NodeValue_t armRestState;
  
  GrasperVerbosity_t setVerbosity;   //!< Verbosity elements to force on
  GrasperVerbosity_t clearVerbosity;   //!< Verbosity elements to force off
  
  //RRT PARAMETERS
  //! Offset of the end effector in the Kinematic chain to use (typically GripperFrameOffset)
  unsigned int effectorOffset;
  //! The maximum number of iterations the RRT should undergo
  unsigned int rrtMaxIterations;
  //! The amount to inflate obstacles for safe planning (mm)
  float rrtInflation;
  //! The amount to move each joint while extending
  NodeValue_t rrtInterpolationStep;
  
  //! Factor to change how fast arm movements will be executed
  float armTimeFactor;
  
  //! Whether to open the gripper upon resting
  bool openGripperOnRest;
  
  //! The object to manipulate
  ShapeRoot object;
  
  //! Where to place the object
  ShapeRoot targetLocation;
  
  //! Vector of ranges of egocentric angles in which the gripper is allowed to end up
  std::vector<std::pair<float, float> > gripperAngleRangesX;
  
  //! Vector of ranges of egocentric angles in which the gripper is allowed to end up
  std::vector<std::pair<float, float> > gripperAngleRangesY;
  
  //! Vector of ranges of egocentric angles in which the gripper is allowed to end up
  std::vector<std::pair<float, float> > gripperAngleRangesZ;
  
  //! Maximum number of angles to allow the gripper to have in angle ranges
  int maxNumberOfAngles;
  
  //! Resolution of angles to use within gripperAngleRanges
  float angleResolution;
  
  //! vector of objects to be swept
  std::vector<ShapeRoot> sweepObjects;
  
  //! List of obstacles in the environment
  std::vector<ShapeRoot> envObstacles;
  
  //! The list of obstacles for the planner to use
  std::vector<PlannerObstacle*> plannerObstacles;
  
  //! The predicate-constrained path
  std::vector<NodeValue_t> moveConstrainedPath;
  //! The "acquire" path which is not predicate-constrained
  std::vector<NodeValue_t> moveFreePath;
  //! The "disengage" path which is not predicate-constrained
  std::vector<NodeValue_t> disengagePath;
  
  //! The desired predicate to use during planning for @a moveConstrainedPath
  AdmissibilityPredicate<NodeType_t> *predicate;
  
  //! If true, the planned path is displayed in the shape space
  bool displayPath;
  
  //! If true, the RRT search tree is displayed in the shape space
  bool displayTree;
  
  //! The Behavior making this request; used for posting grasper events
  BehaviorBase* requestingBehavior;
  
  //! Verbosity value computed by Grasper::executeRequest
  GrasperVerbosity_t verbosity;
  
  GrasperPathType_t populateEventPathWith;
  
protected:
  unsigned int requestID;
  GrasperRequest& operator=(const GrasperRequest&); //!< don't call this
};

typedef GrasperRequest::GrasperErrorType_t GraspError;

#endif