Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Grasper.h

Go to the documentation of this file.
00001 #ifndef _GRASPER_H
00002 #define _GRASPER_H
00003 
00004 #include "Shared/RobotInfo.h"
00005 #if (defined(TGT_HAS_ARMS) || !defined(STRICT_TGT_MODEL)) && !defined(TGT_IS_AIBO)
00006 
00007 #include "Behaviors/Nodes/ArmNode.h"
00008 #include "Behaviors/Nodes/DynamicMotionSequenceNode.h"
00009 #include "Behaviors/Nodes/PostMachine.h"
00010 #include "Behaviors/Nodes/SpeechNode.h"
00011 #include "Behaviors/Transitions/CompareTrans.h"
00012 #include "Behaviors/Transitions/CompletionTrans.h"
00013 #include "Behaviors/Transitions/NullTrans.h"
00014 #include "Behaviors/Transitions/PilotTrans.h"
00015 #include "Behaviors/Transitions/SignalTrans.h"
00016 #include "Behaviors/Transitions/TextMsgTrans.h"
00017 #include "Crew/GrasperRequest.h"
00018 #include "Crew/MapBuilderNode.h"
00019 #include "Crew/MotionNodes.h"
00020 #include "Crew/PilotNode.h"
00021 #include "Events/GrasperEvent.h"
00022 #include "Motion/IKSolver.h"
00023 
00024 #if defined(TGT_IS_CALLIOPE5) || defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3) || defined(TGT_IS_MANTIS)
00025 #  include "Planners/Manipulation/ShapeSpacePlanner3DR.h"
00026 #else
00027 #  include "Planners/Manipulation/ShapeSpacePlanner2DR.h"
00028 #endif
00029 
00030 /*
00031   TODO
00032   - Add code to validate Grasper requests and complain, e.g., if reach is missing an object
00033   - fix up the Fail/Success/Complete thing; do we need all those different classes?
00034   - plan failure is currently not being caught; the Grasper hangs
00035   - take out all the DEBUG printouts
00036  
00037   - Sweep
00038   - Test-suite
00039   - Calculate the offset of the gripper frame from wristYaw when projected in XY
00040 */
00041 
00042 //! The Grasper is the member of the Tekkotsu @a Crew responsible for arm-based manipulation.
00043 
00044 /*! The Grasper can make requests to the Pilot, MapBuilder, and Lookout.
00045 
00046   The quintessential Grasper operation is @a moveTo, which picks up an
00047   object, transports it to a destination, and deposits it there.
00048   Depending on the type of robot, arm, and gripper, and the locations of
00049   the object and destination point, a grasper request might involve
00050   moving the robot's body as well as the arm.
00051 
00052   The @a moveTo operation has six phases:
00053 
00054   @li @a Approach: Get the robot close to the object.  Then position the arm
00055   so it's ready to grab the object.
00056 
00057   @li @a Grasp: Close the fingers around the object and verify that it is
00058   under control of the gripper.  May involve moving the object and visual verification.
00059 
00060   @li @a Transport: Carry or drag the object to a location near the destination.  May
00061   involve lifting the object, or repositioning the arm prior to moving the body.
00062 
00063   @li @a Deliver: Place the object at its destination.
00064 
00065   @li @a Release: Let go of the object and verify that it is placed properly.
00066 
00067   @li @a Withdraw: retract the arm and/or move the body away from the object.
00068 
00069 */
00070 
00071 class Grasper: public StateNode {
00072 public:
00073 #if defined(TGT_IS_CALLIOPE5)
00074   typedef ShapeSpacePlanner3DR<NumArmJoints-2> ArmPlanner;
00075   typedef PlannerObstacle3D PlannerObstacleG;
00076   static const unsigned int numPlannerJoints = NumArmJoints-2;
00077 #elif defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00078   typedef ShapeSpacePlanner3DR<NumArmJoints-1> ArmPlanner;
00079   typedef PlannerObstacle3D PlannerObstacleG;
00080   static const unsigned int numPlannerJoints = NumArmJoints-1;
00081 #elif defined(TGT_IS_MANTIS)
00082   typedef ShapeSpacePlanner3DR<JointsPerFrLeg> ArmPlanner;
00083   typedef PlannerObstacle3D PlannerObstacleG;
00084   static const unsigned int numPlannerJoints = JointsPerFrLeg;
00085 #elif defined(TGT_HAS_FINGERS)
00086   typedef ShapeSpacePlanner2DR<NumArmJoints-2> ArmPlanner;
00087   typedef PlannerObstacle2D PlannerObstacleG;
00088   static const unsigned int numPlannerJoints = NumArmJoints-2;
00089 #elif defined(TGT_HAS_GRIPPER)
00090   typedef ShapeSpacePlanner2DR<NumArmJoints-1> ArmPlanner;
00091   typedef PlannerObstacle2D PlannerObstacleG;
00092   static const unsigned int numPlannerJoints = NumArmJoints-1;
00093 #else
00094   typedef ShapeSpacePlanner2DR<NumArmJoints> ArmPlanner;
00095   typedef PlannerObstacle2D PlannerObstacleG;
00096   static const unsigned int numPlannerJoints = NumArmJoints;
00097 #endif
00098   typedef ArmPlanner::NodeType_t NodeType_t;
00099   typedef NodeType_t::NodeValue_t NodeValue_t;
00100 
00101   typedef unsigned int GrasperVerbosity_t;
00102   static const GrasperVerbosity_t GVstart = 1<<0;
00103   static const GrasperVerbosity_t GVexecuteRequest   = 1<<1;
00104   static const GrasperVerbosity_t GVnumObstacles     = 1<<2;
00105   static const GrasperVerbosity_t GVexecutePath      = 1<<3;
00106   static const GrasperVerbosity_t GVcomputeGoals     = 1<<4;
00107   static const GrasperVerbosity_t GVsetJoint         = 1<<5;
00108 
00109 private:
00110   //! Plan an approach or transport path for the body
00111   GenericRRTBase::PlannerResult2D planBodyPath(const Point &targPt, AngTwoPi approachOrientation,
00112                                                const fmat::Column<3> &baseOffset,
00113                                                Shape<AgentData> &pose, float radius, bool isFinalApproach);
00114 
00115   //**************** Nodes Used in Grasper Actions ****************
00116   //
00117   // A collection of nodes from which complex Grasper actions are built
00118 
00119   //! Plan body motion to approach the object
00120 class PlanBodyApproach : public StateNode {
00121  public:
00122   PlanBodyApproach(const std::string &nodename="PlanBodyApproach") : StateNode(nodename) {}
00123     virtual void doStart();
00124     static float getBodyApproachRadius();
00125 };
00126 
00127   //! Execute body motion to approach the object
00128 class DoBodyApproach : public PilotNode {
00129  public:
00130   DoBodyApproach(const std::string &nodename="DoBodyApproach") : PilotNode(nodename,PilotTypes::goToShape) {}
00131     virtual void doStart();
00132 };
00133 
00134   //! Re-acquire object on local map after moving the body
00135 class FindObj : public MapBuilderNode {
00136  public:
00137   FindObj(const std::string &nodename="FindObj") : MapBuilderNode(nodename) {}
00138     virtual void doStart();
00139 };
00140 
00141   //! Plan arm motion to get fingers around the object
00142 class PlanArmApproach : public StateNode {
00143  public:
00144   PlanArmApproach(const std::string &nodename="PlanArmApproach") : StateNode(nodename) {}
00145     virtual void doStart();
00146 };
00147 
00148   //! Execute arm motion to get fingers around the object
00149 class DoArmApproach : public StateNode {
00150  public:
00151   DoArmApproach(const std::string &nodename="DoArmApproach") : StateNode(nodename) {}
00152 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00153     // For Calliope2SP: deploy the arm and set finger height first, then open fingers
00154   virtual void setup() {
00155    MoveArm *movearm1 = new MoveArm("movearm1",GrasperRequest::doApproach);
00156    addNode(movearm1);
00157 
00158    FingersApproach *fingersapproach1 = new FingersApproach("fingersapproach1");
00159    addNode(fingersapproach1);
00160    fingersapproach1->setMC(VRmixin::grasper->getArmId());
00161 
00162    PostMachineCompletion *postmachinecompletion1 = new PostMachineCompletion("postmachinecompletion1");
00163    addNode(postmachinecompletion1);
00164 
00165    startnode = movearm1;
00166 
00167    movearm1->addTransition(new CompletionTrans("completiontrans1",fingersapproach1));
00168    fingersapproach1->addTransition(new CompletionTrans("completiontrans2",postmachinecompletion1));
00169   }
00170 #else
00171     // For Calliope5KP: open fingers and then move the arm to acquire the object
00172   virtual void setup() {
00173    FingersApproach *fingersapproach2 = new FingersApproach("fingersapproach2");
00174    addNode(fingersapproach2);
00175    fingersapproach2->setMC(VRmixin::grasper->getArmId());
00176 
00177    MoveArm *movearm2 = new MoveArm("movearm2",GrasperRequest::doApproach);
00178    addNode(movearm2);
00179 
00180    PostMachineCompletion *postmachinecompletion2 = new PostMachineCompletion("postmachinecompletion2");
00181    addNode(postmachinecompletion2);
00182 
00183    startnode = fingersapproach2;
00184 
00185    fingersapproach2->addTransition(new CompletionTrans("completiontrans3",movearm2));
00186    movearm2->addTransition(new CompletionTrans("completiontrans4",postmachinecompletion2));
00187   }
00188 #endif
00189 };
00190 
00191   //! Open fingers far enough to accomodate object
00192 class FingersApproach : public ArmNode {
00193  public:
00194   FingersApproach(const std::string &nodename="FingersApproach") : ArmNode(nodename) {}
00195     virtual void doStart();
00196 };
00197 
00198   //! For Calliope2SP: move body instead of arm to acquire the object
00199 class DoBodyApproach2 : public PilotNode {
00200  public:
00201   DoBodyApproach2(const std::string &nodename="DoBodyApproach2") : PilotNode(nodename,PilotTypes::walk) {}
00202     virtual void doStart();
00203 };
00204 
00205   //! For Calliope2SP: move body instead of arm to acquire the object
00206 class DoBodyApproach3 : public PilotNode {
00207  public:
00208   DoBodyApproach3(const std::string &nodename="DoBodyApproach3") : PilotNode(nodename,PilotTypes::walk) {}
00209     virtual void doStart();
00210 };
00211   
00212   //! Verify that object is grasped
00213 class Verify : public VisualRoutinesStateNode {
00214  public:
00215   Verify(const std::string &nodename, bool _postGrasp) : VisualRoutinesStateNode(nodename), postGrasp(_postGrasp) {}
00216   bool postGrasp;  // cache the constructor's parameter
00217     //--------------------------------------New as of 4-18-14
00218     enum VerifyMethod {
00219       apriltag,
00220       cross,
00221       domino,
00222       gripperload
00223     };
00224     
00225   class VerifyDispatch : public StateNode {
00226    public:
00227     VerifyDispatch(const std::string &nodename="VerifyDispatch") : StateNode(nodename) {}
00228       virtual void doStart();
00229   };
00230         
00231   class GetAprilTag : public MapBuilderNode {
00232    public:
00233     GetAprilTag(const std::string &nodename="GetAprilTag") : MapBuilderNode(nodename,MapBuilderRequest::cameraMap) {}
00234       virtual void doStart();
00235   };
00236 
00237   class CheckAprilTag : public VisualRoutinesStateNode {
00238    public:
00239     CheckAprilTag(const std::string &nodename="CheckAprilTag") : VisualRoutinesStateNode(nodename) {}
00240       virtual void doStart();
00241   };
00242 
00243   class GetDomino : public MapBuilderNode {
00244    public:
00245     GetDomino(const std::string &nodename="GetDomino") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
00246       virtual void doStart();
00247   };
00248 
00249   class CheckDomino : public VisualRoutinesStateNode {
00250    public:
00251     CheckDomino(const std::string &nodename="CheckDomino") : VisualRoutinesStateNode(nodename) {}
00252       virtual void doStart();
00253   };
00254     
00255   class GetCross : public MapBuilderNode {
00256    public:
00257     GetCross(const std::string &nodename="GetCross") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
00258       virtual void doStart();
00259   };
00260 
00261   class CheckCross : public VisualRoutinesStateNode {
00262    public:
00263     CheckCross(const std::string &nodename="CheckCross") : VisualRoutinesStateNode(nodename) {}
00264       virtual void doStart();
00265   };
00266 
00267   class CheckGripperLoad : public StateNode {
00268    public:
00269     CheckGripperLoad(const std::string &nodename="CheckGripperLoad") : StateNode(nodename) {}
00270       virtual void doStart();
00271   };
00272 
00273   class CheckUserVerify : public StateNode {
00274    public:
00275     CheckUserVerify(const std::string &nodename="CheckUserVerify") : StateNode(nodename) {}
00276       virtual void doStart();
00277   };
00278 
00279   virtual void setup() {
00280    startnode = new VerifyDispatch("startnode");
00281    addNode(startnode);
00282 
00283    PostMachineCompletion *postmachinecompletion3 = new PostMachineCompletion("postmachinecompletion3");
00284    addNode(postmachinecompletion3);
00285 
00286    LookAtGripper *lookApril = new LookAtGripper("lookApril");
00287    addNode(lookApril);
00288 
00289    GetAprilTag *getapriltag1 = new GetAprilTag("getapriltag1");
00290    addNode(getapriltag1);
00291 
00292    CheckAprilTag *checkapriltag1 = new CheckAprilTag("checkapriltag1");
00293    addNode(checkapriltag1);
00294 
00295    LookAtGripper *lookDomino = new LookAtGripper("lookDomino",100.0);
00296    addNode(lookDomino);
00297 
00298    GetDomino *getdomino1 = new GetDomino("getdomino1");
00299    addNode(getdomino1);
00300 
00301    CheckDomino *checkdomino1 = new CheckDomino("checkdomino1");
00302    addNode(checkdomino1);
00303 
00304    LookAtGripper *lookCross = new LookAtGripper("lookCross",100.0);
00305    addNode(lookCross);
00306 
00307    GetCross *getcross1 = new GetCross("getcross1");
00308    addNode(getcross1);
00309 
00310    CheckCross *checkcross1 = new CheckCross("checkcross1");
00311    addNode(checkcross1);
00312 
00313    CheckGripperLoad *checkLoad = new CheckGripperLoad("checkLoad");
00314    addNode(checkLoad);
00315 
00316    CheckUserVerify *checkUser = new CheckUserVerify("checkUser");
00317    addNode(checkUser);
00318 
00319    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans1",postmachinecompletion3,GrasperRequest::verifyNone));
00320    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans2",lookApril,GrasperRequest::verifyAprilTag));
00321    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans3",lookDomino,GrasperRequest::verifyDomino));
00322    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans4",lookCross,GrasperRequest::verifyCross));
00323    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans5",checkLoad,GrasperRequest::verifyLoad));
00324    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans6",checkUser,GrasperRequest::verifyUser));
00325    lookApril->addTransition(new CompletionTrans("completiontrans5",getapriltag1));
00326    getapriltag1->addTransition(new CompletionTrans("completiontrans6",checkapriltag1));
00327    lookDomino->addTransition(new CompletionTrans("completiontrans7",getdomino1));
00328    getdomino1->addTransition(new CompletionTrans("completiontrans8",checkdomino1));
00329    lookCross->addTransition(new CompletionTrans("completiontrans9",getcross1));
00330    getcross1->addTransition(new CompletionTrans("completiontrans10",checkcross1));
00331   }
00332 };
00333 
00334   //! Close fingers to grasp the object
00335 class ArmGrasp : public ArmNode {
00336  public:
00337   ArmGrasp(const std::string &nodename="ArmGrasp") : ArmNode(nodename) {}
00338     virtual void doStart();
00339 };
00340 
00341   //! Set or clear gripper pulsing to prevent load errors
00342 class ArmPulse : public ArmNode {
00343  public:
00344   ArmPulse(const std::string &nodename, bool _activate) : ArmNode(nodename), activate(_activate) {}
00345   bool activate;  // cache the constructor's parameter
00346     virtual void doStart();
00347 };
00348 
00349   //! Move the object to test that it was grasped successfully
00350 class ArmNudge : public DynamicMotionSequenceNode {
00351  public:
00352   ArmNudge(const std::string &nodename="ArmNudge") : DynamicMotionSequenceNode(nodename) {}
00353     virtual void doStart();
00354 };
00355 
00356   //! Raise the object for transport
00357 class ArmRaise : public DynamicMotionSequenceNode {
00358  public:
00359   ArmRaise(const std::string &nodename="ArmRaise") : DynamicMotionSequenceNode(nodename) {}
00360     virtual void doStart();
00361 };
00362 
00363   //! Plan body move to dropoff location
00364 class PlanBodyTransport : public StateNode {
00365  public:
00366   PlanBodyTransport(const std::string &nodename="PlanBodyTransport") : StateNode(nodename) {}
00367     virtual void doStart();
00368 };
00369 
00370   //! Execute body move for dropoff
00371 class DoBodyTransport : public PilotNode {
00372  public:
00373   DoBodyTransport(const std::string &nodename="DoBodyTransport") : PilotNode(nodename,PilotTypes::goToShape) {}
00374     virtual void doStart();
00375 };
00376 
00377   //! Plan arm move to dropoff location
00378 class PlanArmDeliver : public StateNode {
00379  public:
00380   PlanArmDeliver(const std::string &nodename="PlanArmDeliver") : StateNode(nodename) {}
00381     virtual void doStart();
00382 };
00383 
00384   //! Execute arm motion to deposit the object at its destination
00385 class DoArmDeliver : public StateNode {
00386  public:
00387   DoArmDeliver(const std::string &nodename="DoArmDeliver") : StateNode(nodename) {}
00388 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00389     //Changed as of 4-28-2014
00390     
00391   virtual void setup() {
00392    MoveArm *movearm3 = new MoveArm("movearm3",GrasperRequest::doDeliver);
00393    addNode(movearm3);
00394 
00395    FingersApproach *fingersapproach3 = new FingersApproach("fingersapproach3");
00396    addNode(fingersapproach3);
00397    fingersapproach3->setMC(VRmixin::grasper->getArmId());
00398 
00399    PostMachineCompletion *postmachinecompletion4 = new PostMachineCompletion("postmachinecompletion4");
00400    addNode(postmachinecompletion4);
00401 
00402    startnode = movearm3;
00403 
00404    movearm3->addTransition(new CompletionTrans("completiontrans11",fingersapproach3));
00405    fingersapproach3->addTransition(new CompletionTrans("completiontrans12",postmachinecompletion4));
00406   }
00407 #else
00408     // Move arm to destination
00409   virtual void setup() {
00410    FingersApproach *fingersapproach4 = new FingersApproach("fingersapproach4");
00411    addNode(fingersapproach4);
00412    fingersapproach4->setMC(VRmixin::grasper->getArmId());
00413 
00414    MoveArm *movearm4 = new MoveArm("movearm4",GrasperRequest::doDeliver);
00415    addNode(movearm4);
00416 
00417    PostMachineCompletion *postmachinecompletion5 = new PostMachineCompletion("postmachinecompletion5");
00418    addNode(postmachinecompletion5);
00419 
00420    startnode = fingersapproach4;
00421 
00422    fingersapproach4->addTransition(new CompletionTrans("completiontrans13",movearm4));
00423    movearm4->addTransition(new CompletionTrans("completiontrans14",postmachinecompletion5));
00424   }
00425 #endif
00426 };
00427 
00428   //! Release the object
00429 class ReleaseArm : public ArmNode {
00430  public:
00431   ReleaseArm(const std::string &nodename="ReleaseArm") : ArmNode(nodename) {}
00432     virtual void doStart();
00433 };
00434 
00435   //! DoWithdraw from the object
00436 class DoWithdraw : public PilotNode {
00437  public:
00438   DoWithdraw(const std::string &nodename="DoWithdraw") : PilotNode(nodename,PilotTypes::walk) {}
00439     virtual void doStart();
00440 };
00441 
00442   //==== MOTION ====
00443   
00444   //! Moves the body
00445 
00446   //! Executes an arm movement with time delays estimated for hand/eye system
00447 class MoveArm : public DynamicMotionSequenceNode {
00448  public:
00449   MoveArm(const std::string &nodename, GrasperRequest::GrasperPathType_t _pa) : DynamicMotionSequenceNode(nodename), pa(_pa) {}
00450   GrasperRequest::GrasperPathType_t pa;  // cache the constructor's parameter
00451     virtual void doStart();
00452     void executeMove(const std::vector<NodeValue_t>& path);
00453     unsigned int advTime(const NodeValue_t& confDif) {
00454       float maxDiff = abs(confDif[0]);
00455       unsigned int jointsToMove = 
00456 #ifdef TGT_IS_MANTIS
00457         JointsPerFrLeg
00458 #else
00459         JointsPerArm
00460 #endif
00461         ;
00462       for (unsigned int i = 1; i < jointsToMove; i++) {
00463         if (confDif[i] > maxDiff)
00464           maxDiff = abs(confDif[i]);
00465       }
00466       unsigned int time = (unsigned int)( maxDiff/(M_PI/6)*1500 );
00467       return max((unsigned)11, (unsigned int)(time * curReq->armTimeFactor));
00468     }
00469 };
00470 
00471   /*
00472   //! Grasps or releases an object from the gripper if the gripper is manipulable
00473 class Grasp : public StateNode {
00474  public:
00475   Grasp(const std::string &nodename, bool _closeGripper=true) : StateNode(nodename), closeGripper(_closeGripper) {}
00476   bool closeGripper;  // cache the constructor's parameter
00477   virtual void setup() {
00478      startnode = new StateNode("startnode");
00479      addNode(startnode);
00480 
00481      StateNode *split = new StateNode("split");
00482      addNode(split);
00483 
00484      OpenTheGripper *openTheGripper = new OpenTheGripper("openTheGripper");
00485      addNode(openTheGripper);
00486 
00487      PostMachineCompletion *postmachinecompletion6 = new PostMachineCompletion("postmachinecompletion6");
00488      addNode(postmachinecompletion6);
00489 
00490      CloseTheGripper *closeTheGripper = new CloseTheGripper("closeTheGripper");
00491      addNode(closeTheGripper);
00492 
00493      PostMachineCompletion *postmachinecompletion7 = new PostMachineCompletion("postmachinecompletion7");
00494      addNode(postmachinecompletion7);
00495 
00496      SignalGraspError *failed = new SignalGraspError("failed",GrasperRequest::badMove);
00497      addNode(failed);
00498 
00499      startnode->addTransition(new NullTrans("nulltrans1",split));
00500      split->addTransition(new CompareTrans<bool>("comparetrans1",openTheGripper,&closeGripper,CompareTrans<bool>::EQ,true));
00501      split->addTransition(new CompareTrans<bool>("comparetrans2",closeTheGripper,&closeGripper,CompareTrans<bool>::EQ,false));
00502      openTheGripper->addTransition(new CompletionTrans("completiontrans15",postmachinecompletion6));
00503      closeTheGripper->addTransition(new CompletionTrans("completiontrans16",postmachinecompletion7));
00504   }
00505 };
00506   */
00507 
00508   //! Moves the arm to the rest state
00509 class Rest : public StateNode {
00510  public:
00511   Rest(const std::string &nodename="Rest") : StateNode(nodename) {}
00512   class GetRestType : public StateNode {
00513    public:
00514     GetRestType(const std::string &nodename="GetRestType") : StateNode(nodename) {}
00515     virtual void doStart() {
00516         postStateSignal<GrasperRequest::GrasperRestType_t>(curReq->restType);
00517     }
00518   };
00519 
00520   class GetOpenGripperFlag : public StateNode {
00521    public:
00522     GetOpenGripperFlag(const std::string &nodename="GetOpenGripperFlag") : StateNode(nodename) {}
00523     virtual void doStart() {
00524         postStateSignal<bool>(curReq->openGripperOnRest);
00525     }
00526   };
00527 
00528     virtual void setup() {
00529    startnode = new GetRestType("startnode");
00530    addNode(startnode);
00531 
00532    MoveArm *restArm = new MoveArm("restArm",GrasperRequest::doRelease);
00533    addNode(restArm);
00534 
00535    MoveArm *restToOpen = new MoveArm("restToOpen",GrasperRequest::doRelease);
00536    addNode(restToOpen);
00537 
00538    GetOpenGripperFlag *open = new GetOpenGripperFlag("open");
00539    addNode(open);
00540 
00541    OpenTheGripper *openthegripper1 = new OpenTheGripper("openthegripper1");
00542    addNode(openthegripper1);
00543 
00544    PostMachineCompletion *succeeded = new PostMachineCompletion("succeeded");
00545    addNode(succeeded);
00546 
00547    SignalGraspError *failed = new SignalGraspError("failed",GrasperRequest::badMove);
00548    addNode(failed);
00549 
00550    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans7",succeeded,GrasperRequest::stationary));
00551    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans8",restArm,GrasperRequest::settleArm));
00552    startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans9",restToOpen,GrasperRequest::settleBodyAndArm));
00553    restArm->addTransition(new CompletionTrans("completiontrans17",succeeded));
00554    restArm->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans10",failed,StateNode::failureSignal));
00555    restToOpen->addTransition(new CompletionTrans("completiontrans18",open));
00556    restToOpen->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans11",failed,StateNode::failureSignal));
00557    open->addTransition(new SignalTrans<bool>("signaltrans12",succeeded,false));
00558    open->addTransition(new SignalTrans<bool>("signaltrans13",openthegripper1,true));
00559    openthegripper1->addTransition(new CompletionTrans("completiontrans19",succeeded));
00560     }
00561 };
00562 
00563   //! Set joint to specified angle
00564 class SetJoint : public ArmNode {
00565  public:
00566   SetJoint(const std::string &nodename, string _jointName, float _storedVal=0, float _speed=0.4f) : ArmNode(nodename), jointName(_jointName), storedVal(_storedVal), speed(_speed) {}
00567   string jointName;  // cache the constructor's parameter
00568   float storedVal;  // cache the constructor's parameter
00569   float speed;  // cache the constructor's parameter
00570     void moveJoint(float value);  
00571     virtual void doStart() {
00572       if (const DataEvent<float> *dev = dynamic_cast<const DataEvent<float>*>(event)) {
00573         float receivedVal = dev->getData();
00574         moveJoint(receivedVal);
00575       } else {
00576         moveJoint(storedVal);
00577       }
00578     }
00579 };
00580 
00581   //! Open the gripper
00582 class OpenTheGripper : public StateNode {
00583  public:
00584   OpenTheGripper(const std::string &nodename="OpenTheGripper") : StateNode(nodename) {}
00585   virtual void setup() {
00586 #if defined(TGT_HAS_GRIPPER)
00587      startnode = new StateNode("startnode");
00588      addNode(startnode);
00589 
00590      SetJoint *openLeft = new SetJoint("openLeft","ARM:gripperLeft", outputRanges[capabilities.findFrameOffset("ARM:gripperLeft")][0]);
00591      addNode(openLeft);
00592      openLeft->setMC(VRmixin::grasper->getArmId());
00593 
00594      SetJoint *openRight = new SetJoint("openRight","ARM:gripperRight", outputRanges[capabilities.findFrameOffset("ARM:gripperRight")][1]);
00595      addNode(openRight);
00596      openRight->setMC(VRmixin::grasper->getArmId());
00597 
00598      PostMachineCompletion *postmachinecompletion8 = new PostMachineCompletion("postmachinecompletion8");
00599      addNode(postmachinecompletion8);
00600 
00601      NullTrans *nulltrans2 = new NullTrans("nulltrans2",openLeft);
00602      nulltrans2->addDestination(openRight);
00603      startnode->addTransition(nulltrans2);
00604 
00605      CompletionTrans *completiontrans20 = new CompletionTrans("completiontrans20",postmachinecompletion8);
00606      openLeft->addTransition(completiontrans20);
00607      openRight->addTransition(completiontrans20);
00608 
00609 #endif
00610   }
00611 };
00612 
00613   //! Close the gripper
00614 class CloseTheGripper : public StateNode {
00615  public:
00616   CloseTheGripper(const std::string &nodename="CloseTheGripper") : StateNode(nodename) {}
00617   virtual void setup() {
00618 #if defined(TGT_HAS_GRIPPER)
00619      startnode = new StateNode("startnode");
00620      addNode(startnode);
00621 
00622      SetJoint *closeLeft = new SetJoint("closeLeft","ARM:gripperLeft", outputRanges[capabilities.findFrameOffset("ARM:gripperLeft")][1]);
00623      addNode(closeLeft);
00624      closeLeft->setMC(VRmixin::grasper->getArmId());
00625 
00626      SetJoint *closeRight = new SetJoint("closeRight","ARM:gripperRight", outputRanges[capabilities.findFrameOffset("ARM:gripperRight")][0]);
00627      addNode(closeRight);
00628      closeRight->setMC(VRmixin::grasper->getArmId());
00629 
00630      PostMachineCompletion *postmachinecompletion9 = new PostMachineCompletion("postmachinecompletion9");
00631      addNode(postmachinecompletion9);
00632 
00633      NullTrans *nulltrans3 = new NullTrans("nulltrans3",closeLeft);
00634      nulltrans3->addDestination(closeRight);
00635      startnode->addTransition(nulltrans3);
00636 
00637      CompletionTrans *completiontrans21 = new CompletionTrans("completiontrans21",postmachinecompletion9);
00638      closeLeft->addTransition(completiontrans21);
00639      closeRight->addTransition(completiontrans21);
00640 
00641 #endif
00642   }
00643 };
00644 
00645   // ==== OLD VERSION'S PLANNING NODES ====
00646 
00647   //! Plan a constrained path
00648 class PathPlanConstrained : public StateNode {
00649  public:
00650   PathPlanConstrained(const std::string &nodename="PathPlanConstrained") : StateNode(nodename) {}
00651     virtual void doStart();
00652 };
00653 
00654   //! Plan a path to the rest state
00655 class PathPlanToRest : public StateNode {
00656  public:
00657   PathPlanToRest(const std::string &nodename="PathPlanToRest") : StateNode(nodename) {}
00658     virtual void doStart();
00659 };
00660 
00661   //==== POSTING OF EVENTS ====
00662 
00663 class IfRequestIs : public StateNode {
00664  public:
00665   IfRequestIs(const std::string &nodename, GrasperRequest::GrasperRequestType_t _rt) : StateNode(nodename), rt(_rt) {}
00666   GrasperRequest::GrasperRequestType_t rt;  // cache the constructor's parameter
00667   virtual void doStart() {
00668     if ( curReq->requestType == rt )
00669       postStateSuccess();
00670     else
00671       postStateCompletion();
00672   }
00673 };
00674 
00675   //! Signal the failure of a grasp sub-statemachine via a signalTrans
00676 class SignalGraspError : public StateNode {
00677  public:
00678   SignalGraspError(const std::string &nodename, GraspError _e=GrasperRequest::noError) : StateNode(nodename), e(_e) {}
00679   GraspError e;  // cache the constructor's parameter
00680   virtual void doStart() {
00681     if (const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event))
00682       postParentSignal<GraspError>(datev->getData());
00683     else
00684       postParentSignal<GraspError>(e);
00685     VRmixin::refreshSketchWorld();
00686   }
00687 };
00688 
00689   //! Report successful completion of the grasper request via a GrasperEvent
00690 class GrasperSucceeded : public StateNode {
00691  public:
00692   GrasperSucceeded(const std::string &nodename="GrasperSucceeded") : StateNode(nodename) {}
00693   virtual void doStart() {
00694     GrasperEvent ev(true, curReq->requestType, GrasperRequest::noError, (size_t)curReq->requestingBehavior);
00695     switch (curReq->populateEventPathWith) {
00696     case GrasperRequest::doApproach:
00697       ev.path = curReq->approachPath;
00698       break;
00699     case GrasperRequest::doDeliver:
00700       ev.path = curReq->deliverPath;
00701       break;
00702     case GrasperRequest::doRelease:
00703       ev.path = curReq->releasePath;
00704       break;
00705     case GrasperRequest::noPath:
00706     default:
00707       break;
00708     }
00709     erouter->postEvent(ev);
00710   }
00711 };
00712 
00713   //! Report failure of the grasper request via a GrasperEvent
00714 class GrasperFailed : public StateNode {
00715  public:
00716   GrasperFailed(const std::string &nodename, GraspError _err=GrasperRequest::badMove) : StateNode(nodename), err(_err) {}
00717   GraspError err;  // cache the constructor's parameter
00718   virtual void doStart() {
00719     GraspError e(err);
00720     if (const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event))
00721       e = datev->getData();
00722     GrasperEvent myEvent(false, curReq->requestType, e, (size_t)curReq->requestingBehavior);
00723     if (e == GrasperRequest::pickUpUnreachable || e == GrasperRequest::dropOffUnreachable) {
00724       // myEvent.suggestedRobotLocation = VRmixin::grasper->getDesiredRobotLocation();
00725       // myEvent.suggestedLookAtPoint = VRmixin::grasper->getDesiredLookAtPoint();
00726     }
00727     std::cout << "GrasperFailed posting " << myEvent.getDescription() << std::endl;
00728     erouter->postEvent(myEvent);
00729     VRmixin::refreshSketchWorld();
00730   }
00731 };
00732 
00733   //! Move on to the next grasper request
00734 class NextRequest : public StateNode {
00735  public:
00736   NextRequest(const std::string &nodename="NextRequest") : StateNode(nodename) {}
00737   virtual void doStart() {
00738     delete curReq;
00739     curReq = NULL;
00740     VRmixin::grasper->executeRequest();
00741     stop();
00742   }
00743 };
00744 
00745   //**************** End of Grasper Actions ****************
00746   
00747 
00748   StateNode *startmain_;  //!< The main reach-grasp-move-release-rest sequence
00749 
00750 public:
00751       
00752   Grasper(): StateNode(), startmain_(), requests(), idCounter(0), armId(MotionManager::invalid_MC_ID)
00753   {
00754     curReq = NULL;
00755   }
00756   
00757   virtual void setup() {
00758     SharedObject<ArmMC> arm_mc;
00759     armId = motman->addPersistentMotion(arm_mc);
00760  startnode = new StateNode("startnode");
00761  addNode(startnode);
00762 
00763  StateNode *startmain = new StateNode("startmain");
00764  addNode(startmain);
00765 
00766  ParkArm *parkArm = new ParkArm("parkArm");
00767  addNode(parkArm);
00768 
00769  PlanBodyApproach *planBodyApproach = new PlanBodyApproach("planBodyApproach");
00770  addNode(planBodyApproach);
00771 
00772  DoBodyApproach *doBodyApproach = new DoBodyApproach("doBodyApproach");
00773  addNode(doBodyApproach);
00774 
00775  FindObj *findobj = new FindObj("findobj");
00776  addNode(findobj);
00777 
00778  PlanArmApproach *planArmApproach = new PlanArmApproach("planArmApproach");
00779  addNode(planArmApproach);
00780 
00781  DoArmApproach *doArmApproach = new DoArmApproach("doArmApproach");
00782  addNode(doArmApproach);
00783 
00784  DoBodyApproach2 *doBodyApproach2 = new DoBodyApproach2("doBodyApproach2");
00785  addNode(doBodyApproach2);
00786 
00787  DoBodyApproach3 *doBodyApproach3 = new DoBodyApproach3("doBodyApproach3");
00788  addNode(doBodyApproach3);
00789 
00790  Verify *verify1 = new Verify("verify1",false);
00791  addNode(verify1);
00792 
00793  IfRequestIs *verify1a = new IfRequestIs("verify1a",GrasperRequest::reach);
00794  addNode(verify1a);
00795 
00796  ArmGrasp *armGrasp = new ArmGrasp("armGrasp");
00797  addNode(armGrasp);
00798  armGrasp->setMC(VRmixin::grasper->getArmId());
00799 
00800  ArmPulse *armPulse = new ArmPulse("armPulse",true);
00801  addNode(armPulse);
00802  armPulse->setMC(VRmixin::grasper->getArmId());
00803 
00804  ArmNudge *armNudge = new ArmNudge("armNudge");
00805  addNode(armNudge);
00806 
00807  Verify *verify2 = new Verify("verify2",true);
00808  addNode(verify2);
00809 
00810  IfRequestIs *verify2a = new IfRequestIs("verify2a",GrasperRequest::grasp);
00811  addNode(verify2a);
00812 
00813  ArmRaise *armRaise = new ArmRaise("armRaise");
00814  addNode(armRaise);
00815 
00816  PlanBodyTransport *planBodyTransport = new PlanBodyTransport("planBodyTransport");
00817  addNode(planBodyTransport);
00818 
00819  DoBodyTransport *doBodyTransport = new DoBodyTransport("doBodyTransport");
00820  addNode(doBodyTransport);
00821 
00822  PlanArmDeliver *planArmDeliver = new PlanArmDeliver("planArmDeliver");
00823  addNode(planArmDeliver);
00824 
00825  DoArmDeliver *doArmDeliver = new DoArmDeliver("doArmDeliver");
00826  addNode(doArmDeliver);
00827 
00828  ReleaseArm *releaseArm = new ReleaseArm("releaseArm");
00829  addNode(releaseArm);
00830  releaseArm->setMC(VRmixin::grasper->getArmId());
00831 
00832  Verify *verify3 = new Verify("verify3",true);
00833  addNode(verify3);
00834 
00835  DoWithdraw *doWithdraw = new DoWithdraw("doWithdraw");
00836  addNode(doWithdraw);
00837 
00838  SpeechNode *pilotFailed = new SpeechNode("pilotFailed","Pilot failed.  What to do now???");
00839  addNode(pilotFailed);
00840 
00841  GrasperSucceeded *succeeded = new GrasperSucceeded("succeeded");
00842  addNode(succeeded);
00843 
00844  GrasperFailed *failed = new GrasperFailed("failed");
00845  addNode(failed);
00846 
00847  NextRequest *next = new NextRequest("next");
00848  addNode(next);
00849 
00850  startmain->addTransition(new NullTrans("nulltrans4",parkArm));
00851  parkArm->addTransition(new CompletionTrans("completiontrans22",planBodyApproach));
00852  planBodyApproach->addTransition(new CompletionTrans("completiontrans23",doBodyApproach));
00853  planBodyApproach->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans14",planArmApproach,StateNode::failureSignal));
00854  planBodyApproach->addTransition(new SignalTrans<GraspError>("signaltrans15",failed));
00855  doBodyApproach->addTransition(new PilotTrans("pilottrans1",findobj,PilotTypes::noError));
00856  doBodyApproach->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans16",planArmApproach,StateNode::failureSignal));
00857  doBodyApproach->addTransition(new PilotTrans("pilottrans2",pilotFailed));
00858  findobj->addTransition(new CompletionTrans("completiontrans24",planArmApproach));
00859  findobj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans17",planArmApproach,StateNode::failureSignal));
00860  planArmApproach->addTransition(new CompletionTrans("completiontrans25",doArmApproach));
00861  planArmApproach->addTransition(new SignalTrans<GraspError>("signaltrans18",failed));
00862  doArmApproach->addTransition(new CompletionTrans("completiontrans26",doBodyApproach2));
00863  doBodyApproach2->addTransition(new PilotTrans("pilottrans3",doBodyApproach3,PilotTypes::noError));
00864  doBodyApproach2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans19",failed,StateNode::failureSignal));
00865  doBodyApproach2->addTransition(new PilotTrans("pilottrans4",pilotFailed));
00866  doBodyApproach3->addTransition(new CompletionTrans("completiontrans27",verify1));
00867  doBodyApproach3->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans20",verify1,StateNode::failureSignal));
00868  verify1->addTransition(new CompletionTrans("completiontrans28",verify1a));
00869  verify1->addTransition(new SignalTrans<GraspError>("signaltrans21",failed));
00870  verify1a->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans22",succeeded,StateNode::successSignal));
00871  verify1a->addTransition(new CompletionTrans("completiontrans29",armGrasp));
00872  armGrasp->addTransition(new CompletionTrans("completiontrans30",verify2));
00873  armPulse->addTransition(new CompletionTrans("completiontrans31",armNudge));
00874  armNudge->addTransition(new CompletionTrans("completiontrans32",verify2));
00875  verify2->addTransition(new CompletionTrans("completiontrans33",verify2a));
00876  verify2->addTransition(new SignalTrans<GraspError>("signaltrans23",failed));
00877  verify2a->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans24",succeeded,StateNode::successSignal));
00878  verify2a->addTransition(new CompletionTrans("completiontrans34",armRaise));
00879  armRaise->addTransition(new CompletionTrans("completiontrans35",planBodyTransport));
00880  planBodyTransport->addTransition(new CompletionTrans("completiontrans36",doBodyTransport));
00881  planBodyTransport->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans25",planArmDeliver,StateNode::failureSignal));
00882  doBodyTransport->addTransition(new CompletionTrans("completiontrans37",planArmDeliver));
00883  planArmDeliver->addTransition(new CompletionTrans("completiontrans38",doArmDeliver));
00884  doArmDeliver->addTransition(new CompletionTrans("completiontrans39",releaseArm));
00885  releaseArm->addTransition(new CompletionTrans("completiontrans40",verify3));
00886  verify3->addTransition(new CompletionTrans("completiontrans41",doWithdraw));
00887  verify3->addTransition(new SignalTrans<GraspError>("signaltrans26",failed));
00888  doWithdraw->addTransition(new CompletionTrans("completiontrans42",succeeded));
00889  succeeded->addTransition(new NullTrans("nulltrans5",next));
00890  failed->addTransition(new NullTrans("nulltrans6",next));
00891 
00892   startmain_ = startmain;
00893   }
00894 
00895   virtual void preStart() {
00896     StateNode::preStart();
00897     if ( verbosity & GVstart ) {
00898       cout << "Grasper starting up" << endl;
00899     }
00900     while ( !requests.empty() ) {
00901       delete requests.front();
00902       requests.pop();
00903     }
00904   }
00905   
00906   //! User-level interface for queueing a request to the Grasper
00907   unsigned int executeRequest(const GrasperRequest& req, BehaviorBase* requestingBehavior);
00908 
00909   //! Executes the next pending request, i.e., the one at the front of the queue
00910   void executeRequest();
00911 
00912   //! Dispatches on the request type, running the appropriate state machine; called by executeRequest()
00913   void dispatch();
00914   void doStop();
00915   
00916   MotionManager::MC_ID getArmId() const { return armId; }
00917 
00918   //! Gets current state of robot's arm from the end effector given
00919   /* If no joint is given, pulls values from WorldState */
00920   static void getCurrentState(NodeValue_t &current, KinematicJoint* endEffector=NULL);
00921   
00922   //! Generates potential goal states around a point, rotating the approach orientation
00923   void computeGoalStates(IKSolver::Point &toPt,
00924        std::vector<std::pair<float, float> >& rangesX,
00925                          std::vector<std::pair<float, float> >& rangesY,
00926        std::vector<std::pair<float, float> >& rangesZ,
00927                          float resolution,
00928        std::vector<NodeValue_t>& goals,
00929        const IKSolver::Point &offset);
00930 
00931   //! Helper function for computeGoalStates
00932   void checkGoalCandidate(const IKSolver::Point &offset, const IKSolver::Rotation &ori, 
00933         KinematicJoint* effector, const IKSolver::Point &position, std::vector<NodeValue_t>& goals);
00934 
00935 protected:
00936   //  Point desiredRobotLocation; //! When an object or target is out of range, this will hold the desired robot's location in order to manipulate
00937   //  Point desiredLookAtPoint; //! When an object or target is out of range, this will hold the desired robot's orientation in order to manipulate
00938   
00939   std::queue<GrasperRequest*> requests; //!< Queue of pending Grasper requests
00940   unsigned int idCounter;               //!< Counter for assigning a unique id to each Grasper request
00941   MotionManager::MC_ID armId;   //!< id for shared ArmMC
00942 
00943 public:
00944   static GrasperRequest* curReq;        //!< The request itself
00945   static GrasperVerbosity_t verbosity;
00946 
00947 private:
00948   Grasper(const Grasper& o);  //!< Copy constructor; do not use
00949   void operator=(const Grasper& o); //!< Assignment operator; do not use
00950 };
00951 
00952 #else   // not TGT_HAS_ARMS
00953 class Grasper: public StateNode {
00954 public:
00955   unsigned int executeRequest(const GrasperRequest& req, BehaviorBase* requestingBehavior) {
00956     std::cout << "Warning:  cannot execute Grasper request: robot has no arms!" << std::endl;
00957     return 0;
00958   }
00959 };
00960 #endif
00961 
00962 #endif

DualCoding 5.1CVS
Generated Mon May 9 04:56:26 2016 by Doxygen 1.6.3