Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

GrasperRequest.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_GrasperRequest_h_
00003 #define INCLUDED_GrasperRequest_h_
00004 
00005 #include "Behaviors/StateNode.h"
00006 #include "DualCoding/ShapeAgent.h"
00007 
00008 #include "PilotRequest.h"
00009 #include "MapBuilderRequest.h"
00010 
00011 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3) || defined(TGT_IS_CALLIOPE5) || defined(TGT_IS_MANTIS)
00012 #  include "Planners/Manipulation/ShapeSpacePlanner3DR.h"
00013 #else
00014 #  include "Planners/Manipulation/ShapeSpacePlanner2DR.h"
00015 #endif
00016 
00017 using namespace std;
00018 using namespace DualCoding;
00019 
00020 //! Request to the @a Grasper to manipulate something
00021 
00022 class GrasperRequest {
00023 public:
00024 #if defined(TGT_IS_CALLIOPE5)
00025   typedef ShapeSpacePlanner3DR<NumArmJoints-2> Planner;
00026   typedef PlannerObstacle3D PlannerObstacleG;
00027   static const unsigned int numPlannerJoints = NumArmJoints-2;
00028 #elif defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00029   typedef ShapeSpacePlanner3DR<NumArmJoints-1> Planner;
00030   typedef PlannerObstacle3D PlannerObstacleG;
00031   static const unsigned int numPlannerJoints = NumArmJoints-1;
00032 #elif defined(TGT_IS_MANTIS)
00033   typedef ShapeSpacePlanner3DR<JointsPerFrLeg> Planner;
00034   typedef PlannerObstacle3D PlannerObstacleG;
00035   static const unsigned int numPlannerJoints = JointsPerFrLeg;
00036 #elif defined(TGT_HAS_FINGERS)
00037   typedef ShapeSpacePlanner2DR<NumArmJoints-2> Planner;
00038   typedef PlannerObstacle2D PlannerObstacleG;
00039   static const unsigned int numPlannerJoints = NumArmJoints-2;
00040 #elif defined(TGT_HAS_GRIPPER)
00041   typedef ShapeSpacePlanner2DR<NumArmJoints-1> Planner;
00042   typedef PlannerObstacle2D PlannerObstacleG;
00043   static const unsigned int numPlannerJoints = NumArmJoints-1;
00044 #else
00045   typedef ShapeSpacePlanner2DR<NumArmJoints> Planner;
00046   typedef PlannerObstacle2D PlannerObstacleG;
00047   static const unsigned int numPlannerJoints = NumArmJoints;
00048 #endif
00049 
00050   typedef Planner::NodeType_t NodeType_t;
00051   typedef NodeType_t::NodeValue_t NodeValue_t;
00052   
00053 private:
00054   friend class Grasper;
00055   
00056   typedef unsigned int GrasperVerbosity_t;
00057   
00058 public:
00059   
00060   //! What we're asking the @a Grasper to do
00061   enum GrasperRequestType_t {
00062     checkGraspable, //!< Check whether @a object is graspable; don't actually move
00063     checkMovable, //!< Check whether @a object can be moved to @a targetLocation; don't actually move
00064     checkRestable,  //!< Check whether @a the arm can be moved to the resting position; don't actually move
00065     computeReach, //!< Compute and return a path for the arm to reach to a point
00066     reach,    //!< Move the arm to @a targetLocation, then rest
00067     grasp,    //!< Approach and grasp @a object, then rest the arm
00068     touch,    //!< Grasp the @a object but don't rest the arm afterwards
00069     release,    //!< Release and disengage from whatever the arm is holding
00070     moveTo,   //!< Move @a object to @a targetLocation
00071     rest,   //!< Move the arm (and possibly the body) to a resting position
00072     //todo
00073     sweep,    //!< Sweep arm along an arc (to clear a region of the workspace)
00074     computeMove,  //!< Compute and return a path for moving @a object to @a targetLocation
00075     computeRest   //!< Compute and return a path for moving the arm to the resting position
00076   };
00077   
00078   //! What error is the Grasper going to return
00079   enum GrasperErrorType_t {
00080     noError = 0,
00081     someError,    //!< Generic error will match anything but noError in a GrasperTrans
00082     invalidRequest, 
00083     noGraspState, //!< IK cannot get the gripper around the object
00084     noGraspPath,  //!< No unobstructed path to the grasp configuration
00085     lostObject,   //!< Lost sight of the object
00086     noDeliverState,
00087     noDeliverPath,
00088     noRestPath,
00089     badGrasp,
00090     badMove,
00091     pickUpUnreachable,
00092     dropOffUnreachable
00093   };
00094   
00095   //! The grasp strategy to use
00096   enum GraspStrategy_t {
00097     unconstrainedGrasp,   //!< Any orientation
00098     sideGrasp,            //!< Side approach: fingers parallel to the ground
00099     overheadGrasp         //!< Overhead approach: fingers perpendicular to the ground
00100   };
00101 
00102   //! What path are we talking about, e.g., to return in the @a GrasperEvent
00103   enum GrasperPathType_t {
00104     noPath, //!< Don't return any path
00105     doApproach, //!< Return the unconstrained path to @a object
00106     doDeliver,  //!< Return the constrained path to move @a object to @a targetLocation
00107     doRelease //!< Return the path to disengage from the object and move the arm to the rest position
00108   };
00109   
00110   //! 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
00111   enum GrasperRestType_t {
00112     stationary,
00113     settleArm,
00114     settleBodyAndArm
00115   };
00116   
00117   //! How to verify a grasp
00118   enum GrasperVerifyStrategy_t {
00119     verifyNone, //!< Don't try to verify the grasp,
00120     verifyAprilTag, //!< Check for an AprilTag in the gripper
00121     verifyDomino, //!< Check for a domino in the gripper
00122     verifyCross, //!< Check for a cross in the gripper
00123     verifyLoad, //!< Verify by gripper load signal
00124     verifyUser //!< User will supply a verify function
00125   };
00126 
00127   //! Constructor
00128   GrasperRequest(GrasperRequestType_t _type);
00129   
00130   //! Copy constructor
00131   GrasperRequest(const GrasperRequest &req);
00132   
00133   //! Validate the GrasperRequest and return an error code if there is a problem
00134   GrasperErrorType_t validateRequest();
00135   
00136   //! The type of this Grasper request
00137   GrasperRequestType_t requestType;
00138   
00139   //! Type of grasp strategy to use
00140   GraspStrategy_t graspStrategy;
00141 
00142   //! The rest type for this request
00143   GrasperRestType_t restType;
00144   
00145   //! The rest arm configuration 
00146   NodeValue_t armRestState;
00147   
00148   GrasperVerbosity_t setVerbosity;   //!< Verbosity elements to force on
00149   GrasperVerbosity_t clearVerbosity;   //!< Verbosity elements to force off
00150   
00151   //RRT PARAMETERS
00152 
00153   //! Offset of the end effector in the Kinematic chain to use (typically GripperFrameOffset)
00154   unsigned int effectorOffset;
00155 
00156   //! The maximum number of iterations the RRT should undergo
00157   unsigned int rrtMaxIterations;
00158 
00159   //! The amount to inflate obstacles for safe planning (mm)
00160   float rrtInflation;
00161 
00162   //! The amount to move each joint while extending
00163   NodeValue_t rrtInterpolationStep;
00164   
00165   //! Factor to change how fast arm movements will be executed
00166   float armTimeFactor;
00167   
00168   //! Whether to open the gripper upon resting
00169   bool openGripperOnRest;
00170   
00171   //! The object to manipulate
00172   ShapeRoot object;
00173   
00174   //! Object-dependent enumerated type, e.g., "high end" or "low end" for a domino
00175   unsigned int objectFeature;
00176 
00177   //! Points on the object where a grasp could be made (relative to its centroid); currently unused
00178   std::vector<Point> objectGraspPoints;
00179 
00180   //! Orientation with which to approach the object
00181   AngTwoPi approachOrientation;
00182 
00183   //! Grasp verify strategy
00184   GrasperVerifyStrategy_t verifyStrategy;
00185 
00186   //! User-supplied function to verify object has been grasped
00187   bool (*verifyGraspFunction)(ShapeRoot&);
00188   
00189   //! Where to place the object
00190   ShapeRoot targetLocation;
00191   
00192   //! Orientation at which to place the object
00193   AngTwoPi targetOrientation;
00194 
00195   //! If true, the Grasper can issue Pilot requests to move the body
00196   bool allowBodyMotion;
00197 
00198   //! Desired grip pressure (units are robot-dependent)
00199   float gripPressure;
00200 
00201   //! Vector of ranges of egocentric angles in which the gripper is allowed to end up
00202   std::vector<std::pair<float, float> > gripperAngleRangesX;
00203   
00204   //! Vector of ranges of egocentric angles in which the gripper is allowed to end up
00205   std::vector<std::pair<float, float> > gripperAngleRangesY;
00206   
00207   //! Vector of ranges of egocentric angles in which the gripper is allowed to end up
00208   std::vector<std::pair<float, float> > gripperAngleRangesZ;
00209   
00210   //! Maximum number of angles to allow the gripper to have in angle ranges
00211   unsigned int maxNumberOfAngles;
00212   
00213   //! Resolution of angles to use within gripperAngleRanges
00214   float angleResolution;
00215   
00216   //! vector of objects to be swept
00217   std::vector<ShapeRoot> sweepObjects;
00218   
00219   //! List of obstacles in the environment
00220   std::vector<ShapeRoot> envObstacles;
00221   
00222   //! The desired predicate to use during planning for @a moveConstrainedPath
00223   AdmissibilityPredicate<NodeType_t> *predicate;
00224   
00225   //! What kind of path to return with the GrasperEvent
00226   GrasperPathType_t populateEventPathWith;
00227   
00228   //! Template to use for Pilot requests for body motion
00229   PilotRequest pilotreq;
00230 
00231   //! MapBuilder request to reacquire the object after motion
00232   MapBuilderRequest *mapreq;
00233 
00234   //! If true, the planned path is displayed in the shape space
00235   bool displayPath;
00236   
00237   //! If true, the RRT search tree is displayed in the shape space
00238   bool displayTree;
00239   
00240 private:
00241   //! Robot's desired location and heading for reaching to an object
00242   Shape<AgentData> approachPose;
00243 
00244   //! Robot's desired location and heading for transporting the object
00245   Shape<AgentData> transportPose;
00246 
00247   //! Robot's desired location and heading for withdrawing
00248   Shape<AgentData> withdrawPose;
00249 
00250   //! The "approach" arm path, which is not predicate-constrained
00251   std::vector<NodeValue_t> approachPath;
00252 
00253   //! The predicate-constrained arm path for moving an object
00254   std::vector<NodeValue_t> deliverPath;
00255 
00256   //! The "release" arm path, which is not predicate-constrained
00257   std::vector<NodeValue_t> releasePath;
00258   
00259   //! The Behavior making this request; used for posting grasper events
00260   BehaviorBase* requestingBehavior;
00261   
00262   //! Verbosity value computed by Grasper::executeRequest
00263   GrasperVerbosity_t verbosity;
00264   
00265 protected:
00266   unsigned int requestID;
00267   GrasperRequest& operator=(const GrasperRequest&); //!< don't call this
00268 };
00269 
00270 typedef GrasperRequest::GrasperErrorType_t GraspError;
00271 
00272 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:41 2016 by Doxygen 1.6.3