Grasper.h

Go to the documentation of this file.

#ifndef _GRASPER_H
#define _GRASPER_H

#include "Shared/RobotInfo.h"
#if (defined(TGT_HAS_ARMS) || !defined(STRICT_TGT_MODEL)) && !defined(TGT_IS_AIBO)

#include "Behaviors/Nodes/ArmNode.h"
#include "Behaviors/Nodes/DynamicMotionSequenceNode.h"
#include "Behaviors/Nodes/PostMachine.h"
#include "Behaviors/Nodes/SpeechNode.h"
#include "Behaviors/Transitions/CompareTrans.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/PilotTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Behaviors/Transitions/TextMsgTrans.h"
#include "Crew/GrasperRequest.h"
#include "Crew/MapBuilderNode.h"
#include "Crew/MotionNodes.h"
#include "Crew/PilotNode.h"
#include "Events/GrasperEvent.h"
#include "Motion/IKSolver.h"

#if defined(TGT_IS_CALLIOPE5) || defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3) || defined(TGT_IS_MANTIS)
#  include "Planners/Manipulation/ShapeSpacePlanner3DR.h"
#else
#  include "Planners/Manipulation/ShapeSpacePlanner2DR.h"
#endif

/*
  TODO
  - Add code to validate Grasper requests and complain, e.g., if reach is missing an object
  - fix up the Fail/Success/Complete thing; do we need all those different classes?
  - plan failure is currently not being caught; the Grasper hangs
  - take out all the DEBUG printouts
 
  - Sweep
  - Test-suite
  - Calculate the offset of the gripper frame from wristYaw when projected in XY
*/

//! The Grasper is the member of the Tekkotsu @a Crew responsible for arm-based manipulation.

/*! The Grasper can make requests to the Pilot, MapBuilder, and Lookout.

  The quintessential Grasper operation is @a moveTo, which picks up an
  object, transports it to a destination, and deposits it there.
  Depending on the type of robot, arm, and gripper, and the locations of
  the object and destination point, a grasper request might involve
  moving the robot's body as well as the arm.

  The @a moveTo operation has six phases:

  @li @a Approach: Get the robot close to the object.  Then position the arm
  so it's ready to grab the object.

  @li @a Grasp: Close the fingers around the object and verify that it is
  under control of the gripper.  May involve moving the object and visual verification.

  @li @a Transport: Carry or drag the object to a location near the destination.  May
  involve lifting the object, or repositioning the arm prior to moving the body.

  @li @a Deliver: Place the object at its destination.

  @li @a Release: Let go of the object and verify that it is placed properly.

  @li @a Withdraw: retract the arm and/or move the body away from the object.

*/

class Grasper: public StateNode {
public:
#if defined(TGT_IS_CALLIOPE5)
  typedef ShapeSpacePlanner3DR<NumArmJoints-2> ArmPlanner;
  typedef PlannerObstacle3D PlannerObstacleG;
  static const unsigned int numPlannerJoints = NumArmJoints-2;
#elif defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
  typedef ShapeSpacePlanner3DR<NumArmJoints-1> ArmPlanner;
  typedef PlannerObstacle3D PlannerObstacleG;
  static const unsigned int numPlannerJoints = NumArmJoints-1;
#elif defined(TGT_IS_MANTIS)
  typedef ShapeSpacePlanner3DR<JointsPerFrLeg> ArmPlanner;
  typedef PlannerObstacle3D PlannerObstacleG;
  static const unsigned int numPlannerJoints = JointsPerFrLeg;
#elif defined(TGT_HAS_FINGERS)
  typedef ShapeSpacePlanner2DR<NumArmJoints-2> ArmPlanner;
  typedef PlannerObstacle2D PlannerObstacleG;
  static const unsigned int numPlannerJoints = NumArmJoints-2;
#elif defined(TGT_HAS_GRIPPER)
  typedef ShapeSpacePlanner2DR<NumArmJoints-1> ArmPlanner;
  typedef PlannerObstacle2D PlannerObstacleG;
  static const unsigned int numPlannerJoints = NumArmJoints-1;
#else
  typedef ShapeSpacePlanner2DR<NumArmJoints> ArmPlanner;
  typedef PlannerObstacle2D PlannerObstacleG;
  static const unsigned int numPlannerJoints = NumArmJoints;
#endif
  typedef ArmPlanner::NodeType_t NodeType_t;
  typedef NodeType_t::NodeValue_t NodeValue_t;

  typedef unsigned int GrasperVerbosity_t;
  static const GrasperVerbosity_t GVstart = 1<<0;
  static const GrasperVerbosity_t GVexecuteRequest   = 1<<1;
  static const GrasperVerbosity_t GVnumObstacles     = 1<<2;
  static const GrasperVerbosity_t GVexecutePath      = 1<<3;
  static const GrasperVerbosity_t GVcomputeGoals     = 1<<4;
  static const GrasperVerbosity_t GVsetJoint         = 1<<5;

private:
  //! Plan an approach or transport path for the body
  GenericRRTBase::PlannerResult2D planBodyPath(const Point &targPt, AngTwoPi approachOrientation,
                                               const fmat::Column<3> &baseOffset,
                                               Shape<AgentData> &pose, float radius, bool isFinalApproach);

  //**************** Nodes Used in Grasper Actions ****************
  //
  // A collection of nodes from which complex Grasper actions are built

  //! Plan body motion to approach the object
class PlanBodyApproach : public StateNode {
 public:
  PlanBodyApproach(const std::string &nodename="PlanBodyApproach") : StateNode(nodename) {}
    virtual void doStart();
    static float getBodyApproachRadius();
};

  //! Execute body motion to approach the object
class DoBodyApproach : public PilotNode {
 public:
  DoBodyApproach(const std::string &nodename="DoBodyApproach") : PilotNode(nodename,PilotTypes::goToShape) {}
    virtual void doStart();
};

  //! Re-acquire object on local map after moving the body
class FindObj : public MapBuilderNode {
 public:
  FindObj(const std::string &nodename="FindObj") : MapBuilderNode(nodename) {}
    virtual void doStart();
};

  //! Plan arm motion to get fingers around the object
class PlanArmApproach : public StateNode {
 public:
  PlanArmApproach(const std::string &nodename="PlanArmApproach") : StateNode(nodename) {}
    virtual void doStart();
};

  //! Execute arm motion to get fingers around the object
class DoArmApproach : public StateNode {
 public:
  DoArmApproach(const std::string &nodename="DoArmApproach") : StateNode(nodename) {}
#if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
    // For Calliope2SP: deploy the arm and set finger height first, then open fingers
  virtual void setup() {
   MoveArm *movearm1 = new MoveArm("movearm1",GrasperRequest::doApproach);
   addNode(movearm1);

   FingersApproach *fingersapproach1 = new FingersApproach("fingersapproach1");
   addNode(fingersapproach1);
   fingersapproach1->setMC(VRmixin::grasper->getArmId());

   PostMachineCompletion *postmachinecompletion1 = new PostMachineCompletion("postmachinecompletion1");
   addNode(postmachinecompletion1);

   startnode = movearm1;

   movearm1->addTransition(new CompletionTrans("completiontrans1",fingersapproach1));
   fingersapproach1->addTransition(new CompletionTrans("completiontrans2",postmachinecompletion1));
  }
#else
    // For Calliope5KP: open fingers and then move the arm to acquire the object
  virtual void setup() {
   FingersApproach *fingersapproach2 = new FingersApproach("fingersapproach2");
   addNode(fingersapproach2);
   fingersapproach2->setMC(VRmixin::grasper->getArmId());

   MoveArm *movearm2 = new MoveArm("movearm2",GrasperRequest::doApproach);
   addNode(movearm2);

   PostMachineCompletion *postmachinecompletion2 = new PostMachineCompletion("postmachinecompletion2");
   addNode(postmachinecompletion2);

   startnode = fingersapproach2;

   fingersapproach2->addTransition(new CompletionTrans("completiontrans3",movearm2));
   movearm2->addTransition(new CompletionTrans("completiontrans4",postmachinecompletion2));
  }
#endif
};

  //! Open fingers far enough to accomodate object
class FingersApproach : public ArmNode {
 public:
  FingersApproach(const std::string &nodename="FingersApproach") : ArmNode(nodename) {}
    virtual void doStart();
};

  //! For Calliope2SP: move body instead of arm to acquire the object
class DoBodyApproach2 : public PilotNode {
 public:
  DoBodyApproach2(const std::string &nodename="DoBodyApproach2") : PilotNode(nodename,PilotTypes::walk) {}
    virtual void doStart();
};

  //! For Calliope2SP: move body instead of arm to acquire the object
class DoBodyApproach3 : public PilotNode {
 public:
  DoBodyApproach3(const std::string &nodename="DoBodyApproach3") : PilotNode(nodename,PilotTypes::walk) {}
    virtual void doStart();
};
  
  //! Verify that object is grasped
class Verify : public VisualRoutinesStateNode {
 public:
  Verify(const std::string &nodename, bool _postGrasp) : VisualRoutinesStateNode(nodename), postGrasp(_postGrasp) {}
  bool postGrasp;  // cache the constructor's parameter
    //--------------------------------------New as of 4-18-14
    enum VerifyMethod {
      apriltag,
      cross,
      domino,
      gripperload
    };
    
  class VerifyDispatch : public StateNode {
   public:
    VerifyDispatch(const std::string &nodename="VerifyDispatch") : StateNode(nodename) {}
      virtual void doStart();
  };
        
  class GetAprilTag : public MapBuilderNode {
   public:
    GetAprilTag(const std::string &nodename="GetAprilTag") : MapBuilderNode(nodename,MapBuilderRequest::cameraMap) {}
      virtual void doStart();
  };

  class CheckAprilTag : public VisualRoutinesStateNode {
   public:
    CheckAprilTag(const std::string &nodename="CheckAprilTag") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart();
  };

  class GetDomino : public MapBuilderNode {
   public:
    GetDomino(const std::string &nodename="GetDomino") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
      virtual void doStart();
  };

  class CheckDomino : public VisualRoutinesStateNode {
   public:
    CheckDomino(const std::string &nodename="CheckDomino") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart();
  };
    
  class GetCross : public MapBuilderNode {
   public:
    GetCross(const std::string &nodename="GetCross") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
      virtual void doStart();
  };

  class CheckCross : public VisualRoutinesStateNode {
   public:
    CheckCross(const std::string &nodename="CheckCross") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart();
  };

  class CheckGripperLoad : public StateNode {
   public:
    CheckGripperLoad(const std::string &nodename="CheckGripperLoad") : StateNode(nodename) {}
      virtual void doStart();
  };

  class CheckUserVerify : public StateNode {
   public:
    CheckUserVerify(const std::string &nodename="CheckUserVerify") : StateNode(nodename) {}
      virtual void doStart();
  };

  virtual void setup() {
   startnode = new VerifyDispatch("startnode");
   addNode(startnode);

   PostMachineCompletion *postmachinecompletion3 = new PostMachineCompletion("postmachinecompletion3");
   addNode(postmachinecompletion3);

   LookAtGripper *lookApril = new LookAtGripper("lookApril");
   addNode(lookApril);

   GetAprilTag *getapriltag1 = new GetAprilTag("getapriltag1");
   addNode(getapriltag1);

   CheckAprilTag *checkapriltag1 = new CheckAprilTag("checkapriltag1");
   addNode(checkapriltag1);

   LookAtGripper *lookDomino = new LookAtGripper("lookDomino",100.0);
   addNode(lookDomino);

   GetDomino *getdomino1 = new GetDomino("getdomino1");
   addNode(getdomino1);

   CheckDomino *checkdomino1 = new CheckDomino("checkdomino1");
   addNode(checkdomino1);

   LookAtGripper *lookCross = new LookAtGripper("lookCross",100.0);
   addNode(lookCross);

   GetCross *getcross1 = new GetCross("getcross1");
   addNode(getcross1);

   CheckCross *checkcross1 = new CheckCross("checkcross1");
   addNode(checkcross1);

   CheckGripperLoad *checkLoad = new CheckGripperLoad("checkLoad");
   addNode(checkLoad);

   CheckUserVerify *checkUser = new CheckUserVerify("checkUser");
   addNode(checkUser);

   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans1",postmachinecompletion3,GrasperRequest::verifyNone));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans2",lookApril,GrasperRequest::verifyAprilTag));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans3",lookDomino,GrasperRequest::verifyDomino));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans4",lookCross,GrasperRequest::verifyCross));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans5",checkLoad,GrasperRequest::verifyLoad));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperVerifyStrategy_t>("signaltrans6",checkUser,GrasperRequest::verifyUser));
   lookApril->addTransition(new CompletionTrans("completiontrans5",getapriltag1));
   getapriltag1->addTransition(new CompletionTrans("completiontrans6",checkapriltag1));
   lookDomino->addTransition(new CompletionTrans("completiontrans7",getdomino1));
   getdomino1->addTransition(new CompletionTrans("completiontrans8",checkdomino1));
   lookCross->addTransition(new CompletionTrans("completiontrans9",getcross1));
   getcross1->addTransition(new CompletionTrans("completiontrans10",checkcross1));
  }
};

  //! Close fingers to grasp the object
class ArmGrasp : public ArmNode {
 public:
  ArmGrasp(const std::string &nodename="ArmGrasp") : ArmNode(nodename) {}
    virtual void doStart();
};

  //! Set or clear gripper pulsing to prevent load errors
class ArmPulse : public ArmNode {
 public:
  ArmPulse(const std::string &nodename, bool _activate) : ArmNode(nodename), activate(_activate) {}
  bool activate;  // cache the constructor's parameter
    virtual void doStart();
};

  //! Move the object to test that it was grasped successfully
class ArmNudge : public DynamicMotionSequenceNode {
 public:
  ArmNudge(const std::string &nodename="ArmNudge") : DynamicMotionSequenceNode(nodename) {}
    virtual void doStart();
};

  //! Raise the object for transport
class ArmRaise : public DynamicMotionSequenceNode {
 public:
  ArmRaise(const std::string &nodename="ArmRaise") : DynamicMotionSequenceNode(nodename) {}
    virtual void doStart();
};

  //! Plan body move to dropoff location
class PlanBodyTransport : public StateNode {
 public:
  PlanBodyTransport(const std::string &nodename="PlanBodyTransport") : StateNode(nodename) {}
    virtual void doStart();
};

  //! Execute body move for dropoff
class DoBodyTransport : public PilotNode {
 public:
  DoBodyTransport(const std::string &nodename="DoBodyTransport") : PilotNode(nodename,PilotTypes::goToShape) {}
    virtual void doStart();
};

  //! Plan arm move to dropoff location
class PlanArmDeliver : public StateNode {
 public:
  PlanArmDeliver(const std::string &nodename="PlanArmDeliver") : StateNode(nodename) {}
    virtual void doStart();
};

  //! Execute arm motion to deposit the object at its destination
class DoArmDeliver : public StateNode {
 public:
  DoArmDeliver(const std::string &nodename="DoArmDeliver") : StateNode(nodename) {}
#if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
    //Changed as of 4-28-2014
    
  virtual void setup() {
   MoveArm *movearm3 = new MoveArm("movearm3",GrasperRequest::doDeliver);
   addNode(movearm3);

   FingersApproach *fingersapproach3 = new FingersApproach("fingersapproach3");
   addNode(fingersapproach3);
   fingersapproach3->setMC(VRmixin::grasper->getArmId());

   PostMachineCompletion *postmachinecompletion4 = new PostMachineCompletion("postmachinecompletion4");
   addNode(postmachinecompletion4);

   startnode = movearm3;

   movearm3->addTransition(new CompletionTrans("completiontrans11",fingersapproach3));
   fingersapproach3->addTransition(new CompletionTrans("completiontrans12",postmachinecompletion4));
  }
#else
    // Move arm to destination
  virtual void setup() {
   FingersApproach *fingersapproach4 = new FingersApproach("fingersapproach4");
   addNode(fingersapproach4);
   fingersapproach4->setMC(VRmixin::grasper->getArmId());

   MoveArm *movearm4 = new MoveArm("movearm4",GrasperRequest::doDeliver);
   addNode(movearm4);

   PostMachineCompletion *postmachinecompletion5 = new PostMachineCompletion("postmachinecompletion5");
   addNode(postmachinecompletion5);

   startnode = fingersapproach4;

   fingersapproach4->addTransition(new CompletionTrans("completiontrans13",movearm4));
   movearm4->addTransition(new CompletionTrans("completiontrans14",postmachinecompletion5));
  }
#endif
};

  //! Release the object
class ReleaseArm : public ArmNode {
 public:
  ReleaseArm(const std::string &nodename="ReleaseArm") : ArmNode(nodename) {}
    virtual void doStart();
};

  //! DoWithdraw from the object
class DoWithdraw : public PilotNode {
 public:
  DoWithdraw(const std::string &nodename="DoWithdraw") : PilotNode(nodename,PilotTypes::walk) {}
    virtual void doStart();
};

  //==== MOTION ====
  
  //! Moves the body

  //! Executes an arm movement with time delays estimated for hand/eye system
class MoveArm : public DynamicMotionSequenceNode {
 public:
  MoveArm(const std::string &nodename, GrasperRequest::GrasperPathType_t _pa) : DynamicMotionSequenceNode(nodename), pa(_pa) {}
  GrasperRequest::GrasperPathType_t pa;  // cache the constructor's parameter
    virtual void doStart();
    void executeMove(const std::vector<NodeValue_t>& path);
    unsigned int advTime(const NodeValue_t& confDif) {
      float maxDiff = abs(confDif[0]);
      unsigned int jointsToMove = 
#ifdef TGT_IS_MANTIS
        JointsPerFrLeg
#else
        JointsPerArm
#endif
        ;
      for (unsigned int i = 1; i < jointsToMove; i++) {
        if (confDif[i] > maxDiff)
          maxDiff = abs(confDif[i]);
      }
      unsigned int time = (unsigned int)( maxDiff/(M_PI/6)*1500 );
      return max((unsigned)11, (unsigned int)(time * curReq->armTimeFactor));
    }
};

  /*
  //! Grasps or releases an object from the gripper if the gripper is manipulable
class Grasp : public StateNode {
 public:
  Grasp(const std::string &nodename, bool _closeGripper=true) : StateNode(nodename), closeGripper(_closeGripper) {}
  bool closeGripper;  // cache the constructor's parameter
  virtual void setup() {
     startnode = new StateNode("startnode");
     addNode(startnode);

     StateNode *split = new StateNode("split");
     addNode(split);

     OpenTheGripper *openTheGripper = new OpenTheGripper("openTheGripper");
     addNode(openTheGripper);

     PostMachineCompletion *postmachinecompletion6 = new PostMachineCompletion("postmachinecompletion6");
     addNode(postmachinecompletion6);

     CloseTheGripper *closeTheGripper = new CloseTheGripper("closeTheGripper");
     addNode(closeTheGripper);

     PostMachineCompletion *postmachinecompletion7 = new PostMachineCompletion("postmachinecompletion7");
     addNode(postmachinecompletion7);

     SignalGraspError *failed = new SignalGraspError("failed",GrasperRequest::badMove);
     addNode(failed);

     startnode->addTransition(new NullTrans("nulltrans1",split));
     split->addTransition(new CompareTrans<bool>("comparetrans1",openTheGripper,&closeGripper,CompareTrans<bool>::EQ,true));
     split->addTransition(new CompareTrans<bool>("comparetrans2",closeTheGripper,&closeGripper,CompareTrans<bool>::EQ,false));
     openTheGripper->addTransition(new CompletionTrans("completiontrans15",postmachinecompletion6));
     closeTheGripper->addTransition(new CompletionTrans("completiontrans16",postmachinecompletion7));
  }
};
  */

  //! Moves the arm to the rest state
class Rest : public StateNode {
 public:
  Rest(const std::string &nodename="Rest") : StateNode(nodename) {}
  class GetRestType : public StateNode {
   public:
    GetRestType(const std::string &nodename="GetRestType") : StateNode(nodename) {}
    virtual void doStart() {
        postStateSignal<GrasperRequest::GrasperRestType_t>(curReq->restType);
    }
  };

  class GetOpenGripperFlag : public StateNode {
   public:
    GetOpenGripperFlag(const std::string &nodename="GetOpenGripperFlag") : StateNode(nodename) {}
    virtual void doStart() {
        postStateSignal<bool>(curReq->openGripperOnRest);
    }
  };

    virtual void setup() {
   startnode = new GetRestType("startnode");
   addNode(startnode);

   MoveArm *restArm = new MoveArm("restArm",GrasperRequest::doRelease);
   addNode(restArm);

   MoveArm *restToOpen = new MoveArm("restToOpen",GrasperRequest::doRelease);
   addNode(restToOpen);

   GetOpenGripperFlag *open = new GetOpenGripperFlag("open");
   addNode(open);

   OpenTheGripper *openthegripper1 = new OpenTheGripper("openthegripper1");
   addNode(openthegripper1);

   PostMachineCompletion *succeeded = new PostMachineCompletion("succeeded");
   addNode(succeeded);

   SignalGraspError *failed = new SignalGraspError("failed",GrasperRequest::badMove);
   addNode(failed);

   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans7",succeeded,GrasperRequest::stationary));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans8",restArm,GrasperRequest::settleArm));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans9",restToOpen,GrasperRequest::settleBodyAndArm));
   restArm->addTransition(new CompletionTrans("completiontrans17",succeeded));
   restArm->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans10",failed,StateNode::failureSignal));
   restToOpen->addTransition(new CompletionTrans("completiontrans18",open));
   restToOpen->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans11",failed,StateNode::failureSignal));
   open->addTransition(new SignalTrans<bool>("signaltrans12",succeeded,false));
   open->addTransition(new SignalTrans<bool>("signaltrans13",openthegripper1,true));
   openthegripper1->addTransition(new CompletionTrans("completiontrans19",succeeded));
    }
};

  //! Set joint to specified angle
class SetJoint : public ArmNode {
 public:
  SetJoint(const std::string &nodename, string _jointName, float _storedVal=0, float _speed=0.4f) : ArmNode(nodename), jointName(_jointName), storedVal(_storedVal), speed(_speed) {}
  string jointName;  // cache the constructor's parameter
  float storedVal;  // cache the constructor's parameter
  float speed;  // cache the constructor's parameter
    void moveJoint(float value);  
    virtual void doStart() {
      if (const DataEvent<float> *dev = dynamic_cast<const DataEvent<float>*>(event)) {
        float receivedVal = dev->getData();
        moveJoint(receivedVal);
      } else {
        moveJoint(storedVal);
      }
    }
};

  //! Open the gripper
class OpenTheGripper : public StateNode {
 public:
  OpenTheGripper(const std::string &nodename="OpenTheGripper") : StateNode(nodename) {}
  virtual void setup() {
#if defined(TGT_HAS_GRIPPER)
     startnode = new StateNode("startnode");
     addNode(startnode);

     SetJoint *openLeft = new SetJoint("openLeft","ARM:gripperLeft", outputRanges[capabilities.findFrameOffset("ARM:gripperLeft")][0]);
     addNode(openLeft);
     openLeft->setMC(VRmixin::grasper->getArmId());

     SetJoint *openRight = new SetJoint("openRight","ARM:gripperRight", outputRanges[capabilities.findFrameOffset("ARM:gripperRight")][1]);
     addNode(openRight);
     openRight->setMC(VRmixin::grasper->getArmId());

     PostMachineCompletion *postmachinecompletion8 = new PostMachineCompletion("postmachinecompletion8");
     addNode(postmachinecompletion8);

     NullTrans *nulltrans2 = new NullTrans("nulltrans2",openLeft);
     nulltrans2->addDestination(openRight);
     startnode->addTransition(nulltrans2);

     CompletionTrans *completiontrans20 = new CompletionTrans("completiontrans20",postmachinecompletion8);
     openLeft->addTransition(completiontrans20);
     openRight->addTransition(completiontrans20);

#endif
  }
};

  //! Close the gripper
class CloseTheGripper : public StateNode {
 public:
  CloseTheGripper(const std::string &nodename="CloseTheGripper") : StateNode(nodename) {}
  virtual void setup() {
#if defined(TGT_HAS_GRIPPER)
     startnode = new StateNode("startnode");
     addNode(startnode);

     SetJoint *closeLeft = new SetJoint("closeLeft","ARM:gripperLeft", outputRanges[capabilities.findFrameOffset("ARM:gripperLeft")][1]);
     addNode(closeLeft);
     closeLeft->setMC(VRmixin::grasper->getArmId());

     SetJoint *closeRight = new SetJoint("closeRight","ARM:gripperRight", outputRanges[capabilities.findFrameOffset("ARM:gripperRight")][0]);
     addNode(closeRight);
     closeRight->setMC(VRmixin::grasper->getArmId());

     PostMachineCompletion *postmachinecompletion9 = new PostMachineCompletion("postmachinecompletion9");
     addNode(postmachinecompletion9);

     NullTrans *nulltrans3 = new NullTrans("nulltrans3",closeLeft);
     nulltrans3->addDestination(closeRight);
     startnode->addTransition(nulltrans3);

     CompletionTrans *completiontrans21 = new CompletionTrans("completiontrans21",postmachinecompletion9);
     closeLeft->addTransition(completiontrans21);
     closeRight->addTransition(completiontrans21);

#endif
  }
};

  // ==== OLD VERSION'S PLANNING NODES ====

  //! Plan a constrained path
class PathPlanConstrained : public StateNode {
 public:
  PathPlanConstrained(const std::string &nodename="PathPlanConstrained") : StateNode(nodename) {}
    virtual void doStart();
};

  //! Plan a path to the rest state
class PathPlanToRest : public StateNode {
 public:
  PathPlanToRest(const std::string &nodename="PathPlanToRest") : StateNode(nodename) {}
    virtual void doStart();
};

  //==== POSTING OF EVENTS ====

class IfRequestIs : public StateNode {
 public:
  IfRequestIs(const std::string &nodename, GrasperRequest::GrasperRequestType_t _rt) : StateNode(nodename), rt(_rt) {}
  GrasperRequest::GrasperRequestType_t rt;  // cache the constructor's parameter
  virtual void doStart() {
    if ( curReq->requestType == rt )
      postStateSuccess();
    else
      postStateCompletion();
  }
};

  //! Signal the failure of a grasp sub-statemachine via a signalTrans
class SignalGraspError : public StateNode {
 public:
  SignalGraspError(const std::string &nodename, GraspError _e=GrasperRequest::noError) : StateNode(nodename), e(_e) {}
  GraspError e;  // cache the constructor's parameter
  virtual void doStart() {
    if (const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event))
      postParentSignal<GraspError>(datev->getData());
    else
      postParentSignal<GraspError>(e);
    VRmixin::refreshSketchWorld();
  }
};

  //! Report successful completion of the grasper request via a GrasperEvent
class GrasperSucceeded : public StateNode {
 public:
  GrasperSucceeded(const std::string &nodename="GrasperSucceeded") : StateNode(nodename) {}
  virtual void doStart() {
    GrasperEvent ev(true, curReq->requestType, GrasperRequest::noError, (size_t)curReq->requestingBehavior);
    switch (curReq->populateEventPathWith) {
    case GrasperRequest::doApproach:
      ev.path = curReq->approachPath;
      break;
    case GrasperRequest::doDeliver:
      ev.path = curReq->deliverPath;
      break;
    case GrasperRequest::doRelease:
      ev.path = curReq->releasePath;
      break;
    case GrasperRequest::noPath:
    default:
      break;
    }
    erouter->postEvent(ev);
  }
};

  //! Report failure of the grasper request via a GrasperEvent
class GrasperFailed : public StateNode {
 public:
  GrasperFailed(const std::string &nodename, GraspError _err=GrasperRequest::badMove) : StateNode(nodename), err(_err) {}
  GraspError err;  // cache the constructor's parameter
  virtual void doStart() {
    GraspError e(err);
    if (const DataEvent<GraspError> *datev = dynamic_cast<const DataEvent<GraspError>*>(event))
      e = datev->getData();
    GrasperEvent myEvent(false, curReq->requestType, e, (size_t)curReq->requestingBehavior);
    if (e == GrasperRequest::pickUpUnreachable || e == GrasperRequest::dropOffUnreachable) {
      // myEvent.suggestedRobotLocation = VRmixin::grasper->getDesiredRobotLocation();
      // myEvent.suggestedLookAtPoint = VRmixin::grasper->getDesiredLookAtPoint();
    }
    std::cout << "GrasperFailed posting " << myEvent.getDescription() << std::endl;
    erouter->postEvent(myEvent);
    VRmixin::refreshSketchWorld();
  }
};

  //! Move on to the next grasper request
class NextRequest : public StateNode {
 public:
  NextRequest(const std::string &nodename="NextRequest") : StateNode(nodename) {}
  virtual void doStart() {
    delete curReq;
    curReq = NULL;
    VRmixin::grasper->executeRequest();
    stop();
  }
};

  //**************** End of Grasper Actions ****************
  

  StateNode *startmain_;  //!< The main reach-grasp-move-release-rest sequence

public:
      
  Grasper(): StateNode(), startmain_(), requests(), idCounter(0), armId(MotionManager::invalid_MC_ID)
  {
    curReq = NULL;
  }
  
  virtual void setup() {
    SharedObject<ArmMC> arm_mc;
    armId = motman->addPersistentMotion(arm_mc);
 startnode = new StateNode("startnode");
 addNode(startnode);

 StateNode *startmain = new StateNode("startmain");
 addNode(startmain);

 ParkArm *parkArm = new ParkArm("parkArm");
 addNode(parkArm);

 PlanBodyApproach *planBodyApproach = new PlanBodyApproach("planBodyApproach");
 addNode(planBodyApproach);

 DoBodyApproach *doBodyApproach = new DoBodyApproach("doBodyApproach");
 addNode(doBodyApproach);

 FindObj *findobj = new FindObj("findobj");
 addNode(findobj);

 PlanArmApproach *planArmApproach = new PlanArmApproach("planArmApproach");
 addNode(planArmApproach);

 DoArmApproach *doArmApproach = new DoArmApproach("doArmApproach");
 addNode(doArmApproach);

 DoBodyApproach2 *doBodyApproach2 = new DoBodyApproach2("doBodyApproach2");
 addNode(doBodyApproach2);

 DoBodyApproach3 *doBodyApproach3 = new DoBodyApproach3("doBodyApproach3");
 addNode(doBodyApproach3);

 Verify *verify1 = new Verify("verify1",false);
 addNode(verify1);

 IfRequestIs *verify1a = new IfRequestIs("verify1a",GrasperRequest::reach);
 addNode(verify1a);

 ArmGrasp *armGrasp = new ArmGrasp("armGrasp");
 addNode(armGrasp);
 armGrasp->setMC(VRmixin::grasper->getArmId());

 ArmPulse *armPulse = new ArmPulse("armPulse",true);
 addNode(armPulse);
 armPulse->setMC(VRmixin::grasper->getArmId());

 ArmNudge *armNudge = new ArmNudge("armNudge");
 addNode(armNudge);

 Verify *verify2 = new Verify("verify2",true);
 addNode(verify2);

 IfRequestIs *verify2a = new IfRequestIs("verify2a",GrasperRequest::grasp);
 addNode(verify2a);

 ArmRaise *armRaise = new ArmRaise("armRaise");
 addNode(armRaise);

 PlanBodyTransport *planBodyTransport = new PlanBodyTransport("planBodyTransport");
 addNode(planBodyTransport);

 DoBodyTransport *doBodyTransport = new DoBodyTransport("doBodyTransport");
 addNode(doBodyTransport);

 PlanArmDeliver *planArmDeliver = new PlanArmDeliver("planArmDeliver");
 addNode(planArmDeliver);

 DoArmDeliver *doArmDeliver = new DoArmDeliver("doArmDeliver");
 addNode(doArmDeliver);

 ReleaseArm *releaseArm = new ReleaseArm("releaseArm");
 addNode(releaseArm);
 releaseArm->setMC(VRmixin::grasper->getArmId());

 Verify *verify3 = new Verify("verify3",true);
 addNode(verify3);

 DoWithdraw *doWithdraw = new DoWithdraw("doWithdraw");
 addNode(doWithdraw);

 SpeechNode *pilotFailed = new SpeechNode("pilotFailed","Pilot failed.  What to do now???");
 addNode(pilotFailed);

 GrasperSucceeded *succeeded = new GrasperSucceeded("succeeded");
 addNode(succeeded);

 GrasperFailed *failed = new GrasperFailed("failed");
 addNode(failed);

 NextRequest *next = new NextRequest("next");
 addNode(next);

 startmain->addTransition(new NullTrans("nulltrans4",parkArm));
 parkArm->addTransition(new CompletionTrans("completiontrans22",planBodyApproach));
 planBodyApproach->addTransition(new CompletionTrans("completiontrans23",doBodyApproach));
 planBodyApproach->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans14",planArmApproach,StateNode::failureSignal));
 planBodyApproach->addTransition(new SignalTrans<GraspError>("signaltrans15",failed));
 doBodyApproach->addTransition(new PilotTrans("pilottrans1",findobj,PilotTypes::noError));
 doBodyApproach->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans16",planArmApproach,StateNode::failureSignal));
 doBodyApproach->addTransition(new PilotTrans("pilottrans2",pilotFailed));
 findobj->addTransition(new CompletionTrans("completiontrans24",planArmApproach));
 findobj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans17",planArmApproach,StateNode::failureSignal));
 planArmApproach->addTransition(new CompletionTrans("completiontrans25",doArmApproach));
 planArmApproach->addTransition(new SignalTrans<GraspError>("signaltrans18",failed));
 doArmApproach->addTransition(new CompletionTrans("completiontrans26",doBodyApproach2));
 doBodyApproach2->addTransition(new PilotTrans("pilottrans3",doBodyApproach3,PilotTypes::noError));
 doBodyApproach2->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans19",failed,StateNode::failureSignal));
 doBodyApproach2->addTransition(new PilotTrans("pilottrans4",pilotFailed));
 doBodyApproach3->addTransition(new CompletionTrans("completiontrans27",verify1));
 doBodyApproach3->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans20",verify1,StateNode::failureSignal));
 verify1->addTransition(new CompletionTrans("completiontrans28",verify1a));
 verify1->addTransition(new SignalTrans<GraspError>("signaltrans21",failed));
 verify1a->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans22",succeeded,StateNode::successSignal));
 verify1a->addTransition(new CompletionTrans("completiontrans29",armGrasp));
 armGrasp->addTransition(new CompletionTrans("completiontrans30",verify2));
 armPulse->addTransition(new CompletionTrans("completiontrans31",armNudge));
 armNudge->addTransition(new CompletionTrans("completiontrans32",verify2));
 verify2->addTransition(new CompletionTrans("completiontrans33",verify2a));
 verify2->addTransition(new SignalTrans<GraspError>("signaltrans23",failed));
 verify2a->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans24",succeeded,StateNode::successSignal));
 verify2a->addTransition(new CompletionTrans("completiontrans34",armRaise));
 armRaise->addTransition(new CompletionTrans("completiontrans35",planBodyTransport));
 planBodyTransport->addTransition(new CompletionTrans("completiontrans36",doBodyTransport));
 planBodyTransport->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans25",planArmDeliver,StateNode::failureSignal));
 doBodyTransport->addTransition(new CompletionTrans("completiontrans37",planArmDeliver));
 planArmDeliver->addTransition(new CompletionTrans("completiontrans38",doArmDeliver));
 doArmDeliver->addTransition(new CompletionTrans("completiontrans39",releaseArm));
 releaseArm->addTransition(new CompletionTrans("completiontrans40",verify3));
 verify3->addTransition(new CompletionTrans("completiontrans41",doWithdraw));
 verify3->addTransition(new SignalTrans<GraspError>("signaltrans26",failed));
 doWithdraw->addTransition(new CompletionTrans("completiontrans42",succeeded));
 succeeded->addTransition(new NullTrans("nulltrans5",next));
 failed->addTransition(new NullTrans("nulltrans6",next));

  startmain_ = startmain;
  }

  virtual void preStart() {
    StateNode::preStart();
    if ( verbosity & GVstart ) {
      cout << "Grasper starting up" << endl;
    }
    while ( !requests.empty() ) {
      delete requests.front();
      requests.pop();
    }
  }
  
  //! User-level interface for queueing a request to the Grasper
  unsigned int executeRequest(const GrasperRequest& req, BehaviorBase* requestingBehavior);

  //! Executes the next pending request, i.e., the one at the front of the queue
  void executeRequest();

  //! Dispatches on the request type, running the appropriate state machine; called by executeRequest()
  void dispatch();
  void doStop();
  
  MotionManager::MC_ID getArmId() const { return armId; }

  //! Gets current state of robot's arm from the end effector given
  /* If no joint is given, pulls values from WorldState */
  static void getCurrentState(NodeValue_t &current, KinematicJoint* endEffector=NULL);
  
  //! Generates potential goal states around a point, rotating the approach orientation
  void computeGoalStates(IKSolver::Point &toPt,
       std::vector<std::pair<float, float> >& rangesX,
                         std::vector<std::pair<float, float> >& rangesY,
       std::vector<std::pair<float, float> >& rangesZ,
                         float resolution,
       std::vector<NodeValue_t>& goals,
       const IKSolver::Point &offset);

  //! Helper function for computeGoalStates
  void checkGoalCandidate(const IKSolver::Point &offset, const IKSolver::Rotation &ori, 
        KinematicJoint* effector, const IKSolver::Point &position, std::vector<NodeValue_t>& goals);

protected:
  //  Point desiredRobotLocation; //! When an object or target is out of range, this will hold the desired robot's location in order to manipulate
  //  Point desiredLookAtPoint; //! When an object or target is out of range, this will hold the desired robot's orientation in order to manipulate
  
  std::queue<GrasperRequest*> requests; //!< Queue of pending Grasper requests
  unsigned int idCounter;               //!< Counter for assigning a unique id to each Grasper request
  MotionManager::MC_ID armId;   //!< id for shared ArmMC

public:
  static GrasperRequest* curReq;        //!< The request itself
  static GrasperVerbosity_t verbosity;

private:
  Grasper(const Grasper& o);  //!< Copy constructor; do not use
  void operator=(const Grasper& o); //!< Assignment operator; do not use
};

#else   // not TGT_HAS_ARMS
class Grasper: public StateNode {
public:
  unsigned int executeRequest(const GrasperRequest& req, BehaviorBase* requestingBehavior) {
    std::cout << "Warning:  cannot execute Grasper request: robot has no arms!" << std::endl;
    return 0;
  }
};
#endif

#endif