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/StateNode.h"
#include "Behaviors/Nodes/DynamicMotionSequenceNode.h"
#include "Behaviors/Nodes/PostMachine.h"
#include "Behaviors/Transitions/CompareTrans.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Crew/GrasperRequest.h"
#include "Crew/MotionNodes.h"
#include "Events/GrasperEvent.h"

/*
  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
*/

class Grasper: public StateNode {
public:
#ifdef TGT_CALLIOPE
  typedef ShapeSpacePlanner3DR<NumArmJoints> Planner;
  typedef Planner::NodeType_t NodeType_t;
  typedef PlannerObstacle3D PlannerObstacle;
#else
  typedef ShapeSpacePlanner2DR<NumArmJoints> Planner;
  typedef Planner::NodeType_t NodeType_t;
  typedef PlannerObstacle2D PlannerObstacle;
#endif
  typedef NodeType_t::NodeValue_t NodeValue_t;
  
  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:
  //**************** Nodes Used in Grasper Actions ****************
  //
  // A collection of nodes from which complex Grasper actions are built

  //! Forward the request type through a SignalTrans
class ForwardRequestType : public StateNode {
 public:
  ForwardRequestType(const std::string &nodename="ForwardRequestType") : StateNode(nodename) {}
    virtual void doStart();
};

  //! Forward the target's centroid through a SignalTrans
class ForwardTargetLocation : public StateNode {
 public:
  ForwardTargetLocation(const std::string &nodename="ForwardTargetLocation") : StateNode(nodename) {}
  virtual void doStart() {
    postStateSignal<Point>(curReq->targetLocation->getCentroid());
  }
};

  // ==== PLANNING NODES ====

  //! Plan an unconstrained path
class PathPlanUnconstrained : public StateNode {
 public:
  PathPlanUnconstrained(const std::string &nodename="PathPlanUnconstrained") : StateNode(nodename) {}
    virtual void doStart();
};

  //! 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();
};

  //! Runs all appropriate planners based on the requested action: move OBJECT to TARGET and then REST
class Plan : public StateNode {
 public:
  Plan(const std::string &nodename="Plan") : StateNode(nodename) {}
  virtual void setup() {
     startnode = new ForwardRequestType("startnode");
     addNode(startnode);

     PathPlanUnconstrained *objectUncon = new PathPlanUnconstrained("objectUncon");
     addNode(objectUncon);

     PathPlanUnconstrained *objectUnconCont = new PathPlanUnconstrained("objectUnconCont");
     addNode(objectUnconCont);

     PathPlanConstrained *targetCon = new PathPlanConstrained("targetCon");
     addNode(targetCon);

     PathPlanToRest *restPlan = new PathPlanToRest("restPlan");
     addNode(restPlan);

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

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

     startnode = startnode;

     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans1",objectUncon,GrasperRequest::grasp));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans2",objectUncon,GrasperRequest::checkGraspable));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans3",objectUncon,GrasperRequest::reach));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans4",objectUncon,GrasperRequest::computeReach));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans5",objectUncon,GrasperRequest::touch));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans6",targetCon,GrasperRequest::release));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans7",objectUnconCont,GrasperRequest::moveTo));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans8",objectUnconCont,GrasperRequest::checkMovable));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans9",restPlan,GrasperRequest::rest));
     startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRequestType_t>("signaltrans10",restPlan,GrasperRequest::checkRestable));
     objectUncon->addTransition(new CompletionTrans("completiontrans1",restPlan));
     objectUnconCont->addTransition(new CompletionTrans("completiontrans2",targetCon));
     targetCon->addTransition(new CompletionTrans("completiontrans3",restPlan));
     restPlan->addTransition(new CompletionTrans("completiontrans4",postmachinecompletion1));

     SignalTrans<GraspError> *signaltrans11 = new SignalTrans<GraspError>("signaltrans11",grasperfailed1);
     objectUncon->addTransition(signaltrans11);
     objectUnconCont->addTransition(signaltrans11);
     targetCon->addTransition(signaltrans11);
     restPlan->addTransition(signaltrans11);

  }
};

  //==== MOTION ====
  
  //! Executes an arm movement
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]);
      for (unsigned int i = 1; i < NumArmJoints; 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 *postmachinecompletion2 = new PostMachineCompletion("postmachinecompletion2");
     addNode(postmachinecompletion2);

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

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

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

     startnode = startnode;

     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("completiontrans5",postmachinecompletion2));
     closeTheGripper->addTransition(new CompletionTrans("completiontrans6",postmachinecompletion3));
  }
};

  //! 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::disengage);
   addNode(restArm);

   MoveArm *restToOpen = new MoveArm("restToOpen", GrasperRequest::disengage);
   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 = startnode;

   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans12",succeeded,GrasperRequest::stationary));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans13",restArm,GrasperRequest::settleArm));
   startnode->addTransition(new SignalTrans<GrasperRequest::GrasperRestType_t>("signaltrans14",restToOpen,GrasperRequest::settleBodyAndArm));
   restArm->addTransition(new CompletionTrans("completiontrans7",succeeded));
   restArm->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans15",failed,StateNode::failureSignal));
   restToOpen->addTransition(new CompletionTrans("completiontrans8",open));
   restToOpen->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans16",failed,StateNode::failureSignal));
   open->addTransition(new SignalTrans<bool>("signaltrans17",succeeded,false));
   open->addTransition(new SignalTrans<bool>("signaltrans18",openthegripper1,true));
   openthegripper1->addTransition(new CompletionTrans("completiontrans9",succeeded));
    }
};

  //! 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);

     SetJoint *openRight = new SetJoint("openRight", "ARM:gripperRight", outputRanges[capabilities.findFrameOffset("ARM:gripperRight")][1]);
     addNode(openRight);

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

     startnode = startnode;

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

     CompletionTrans *completiontrans10 = new CompletionTrans("completiontrans10",postmachinecompletion4);
     openLeft->addTransition(completiontrans10);
     openRight->addTransition(completiontrans10);

#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);

     SetJoint *closeRight = new SetJoint("closeRight", "ARM:gripperRight", outputRanges[capabilities.findFrameOffset("ARM:gripperRight")][0]);
     addNode(closeRight);

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

     startnode = startnode;

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

     CompletionTrans *completiontrans11 = new CompletionTrans("completiontrans11",postmachinecompletion5);
     closeLeft->addTransition(completiontrans11);
     closeRight->addTransition(completiontrans11);

#endif
  }
};

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

  //! 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);
  }
};

  //! 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::moveFree:
      ev.path = curReq->moveFreePath;
      break;
    case GrasperRequest::moveConstrained:
      ev.path = curReq->moveConstrainedPath;
      break;
    case GrasperRequest::disengage:
      ev.path = curReq->disengagePath;
      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();
    }
    erouter->postEvent(myEvent);
  }
};

  //! 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 *checkRoot;   //!< The state machine entrance for any check* request
  StateNode *graspRoot;   //!< The state machine for only acquiring and grasping
  StateNode *releaseRoot;   //!< The state machine for only releasing
  StateNode *moveRoot;    //!< The state machine to move an object to the targetLocation
  StateNode *reachRoot;   //!< The state machine for acquiring
  StateNode *touchRoot;   //!< The state machine for performing the touch action
  StateNode *restRoot;    //!< The state machine to execute resting
  StateNode *computeReachRoot;

public:
      
  Grasper(): StateNode(),
       checkRoot(), graspRoot(), releaseRoot(), moveRoot(), reachRoot(), 
       touchRoot(), restRoot(), computeReachRoot(),
             // desiredRobotLocation(), desiredLookAtPoint(),
       requests(), idCounter(0)
  {
    curReq = NULL;
  }
  
  virtual void setup() {
 startnode = new StateNode("startnode");
 addNode(startnode);

 Plan *check_Plan = new Plan("check_Plan");
 addNode(check_Plan);

 Plan *reach_Plan = new Plan("reach_Plan");
 addNode(reach_Plan);

 MoveArm *reach_Move = new MoveArm("reach_Move", GrasperRequest::moveFree);
 addNode(reach_Move);

 Rest *reach_Rest = new Rest("reach_Rest");
 addNode(reach_Rest);

 Plan *grasp_Plan = new Plan("grasp_Plan");
 addNode(grasp_Plan);

 MoveArm *grasp_Move = new MoveArm("grasp_Move", GrasperRequest::moveFree);
 addNode(grasp_Move);

 Grasp *grasp_Grasp = new Grasp("grasp_Grasp", false);
 addNode(grasp_Grasp);

 Rest *grasp_Rest = new Rest("grasp_Rest");
 addNode(grasp_Rest);

 Plan *release_Plan = new Plan("release_Plan");
 addNode(release_Plan);

 MoveArm *release_Move = new MoveArm("release_Move", GrasperRequest::moveConstrained);
 addNode(release_Move);

 Grasp *release_Grasp = new Grasp("release_Grasp", true);
 addNode(release_Grasp);

 Rest *release_Rest = new Rest("release_Rest");
 addNode(release_Rest);

 Plan *moveTo_Plan = new Plan("moveTo_Plan");
 addNode(moveTo_Plan);

 MoveArm *moveTo_MoveA = new MoveArm("moveTo_MoveA", GrasperRequest::moveFree);
 addNode(moveTo_MoveA);

 Grasp *moveTo_GraspO = new Grasp("moveTo_GraspO", false);
 addNode(moveTo_GraspO);

 MoveArm *moveTo_MoveB = new MoveArm("moveTo_MoveB", GrasperRequest::moveConstrained);
 addNode(moveTo_MoveB);

 Grasp *moveTo_GraspC = new Grasp("moveTo_GraspC", true);
 addNode(moveTo_GraspC);

 Rest *moveTo_Rest = new Rest("moveTo_Rest");
 addNode(moveTo_Rest);

 Plan *touch_Plan = new Plan("touch_Plan");
 addNode(touch_Plan);

 MoveArm *touch_Move = new MoveArm("touch_Move", GrasperRequest::moveFree);
 addNode(touch_Move);

 Grasp *touch_Grasp = new Grasp("touch_Grasp", false);
 addNode(touch_Grasp);

 Plan *rest_Plan = new Plan("rest_Plan");
 addNode(rest_Plan);

 Rest *rest_Rest = new Rest("rest_Rest");
 addNode(rest_Rest);

 Plan *computeReach_Plan = new Plan("computeReach_Plan");
 addNode(computeReach_Plan);

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

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

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

 startnode = startnode;

 check_Plan->addTransition(new CompletionTrans("completiontrans12",succeeded));
 check_Plan->addTransition(new SignalTrans<GraspError>("signaltrans19",failed));
 reach_Plan->addTransition(new CompletionTrans("completiontrans13",reach_Move));
 reach_Move->addTransition(new CompletionTrans("completiontrans14",reach_Rest));
 reach_Rest->addTransition(new CompletionTrans("completiontrans15",succeeded));
 reach_Plan->addTransition(new SignalTrans<GraspError>("signaltrans20",failed));
 reach_Move->addTransition(new SignalTrans<GraspError>("signaltrans21",failed));
 grasp_Plan->addTransition(new CompletionTrans("completiontrans16",grasp_Move));
 grasp_Move->addTransition(new CompletionTrans("completiontrans17",grasp_Grasp));
 grasp_Grasp->addTransition(new CompletionTrans("completiontrans18",grasp_Rest));
 grasp_Rest->addTransition(new CompletionTrans("completiontrans19",succeeded));
 grasp_Plan->addTransition(new SignalTrans<GraspError>("signaltrans22",failed));
 grasp_Move->addTransition(new SignalTrans<GraspError>("signaltrans23",failed));
 grasp_Grasp->addTransition(new SignalTrans<GraspError>("signaltrans24",failed));
 release_Plan->addTransition(new CompletionTrans("completiontrans20",release_Move));
 release_Move->addTransition(new CompletionTrans("completiontrans21",release_Grasp));
 release_Grasp->addTransition(new CompletionTrans("completiontrans22",release_Rest));
 release_Rest->addTransition(new CompletionTrans("completiontrans23",succeeded));
 release_Plan->addTransition(new SignalTrans<GraspError>("signaltrans25",failed));
 release_Move->addTransition(new SignalTrans<GraspError>("signaltrans26",failed));
 release_Grasp->addTransition(new SignalTrans<GraspError>("signaltrans27",failed));
 moveTo_Plan->addTransition(new CompletionTrans("completiontrans24",moveTo_MoveA));
 moveTo_MoveA->addTransition(new CompletionTrans("completiontrans25",moveTo_GraspO));
 moveTo_GraspO->addTransition(new CompletionTrans("completiontrans26",moveTo_MoveB));
 moveTo_MoveB->addTransition(new CompletionTrans("completiontrans27",moveTo_GraspC));
 moveTo_GraspC->addTransition(new CompletionTrans("completiontrans28",moveTo_Rest));
 moveTo_Rest->addTransition(new CompletionTrans("completiontrans29",succeeded));
 moveTo_Plan->addTransition(new SignalTrans<GraspError>("signaltrans28",failed));
 moveTo_MoveA->addTransition(new SignalTrans<GraspError>("signaltrans29",failed));
 moveTo_GraspO->addTransition(new SignalTrans<GraspError>("signaltrans30",failed));
 moveTo_MoveB->addTransition(new SignalTrans<GraspError>("signaltrans31",failed));
 moveTo_GraspC->addTransition(new SignalTrans<GraspError>("signaltrans32",failed));
 touch_Plan->addTransition(new CompletionTrans("completiontrans30",touch_Move));
 touch_Move->addTransition(new CompletionTrans("completiontrans31",touch_Grasp));
 touch_Grasp->addTransition(new CompletionTrans("completiontrans32",succeeded));
 touch_Plan->addTransition(new SignalTrans<GraspError>("signaltrans33",failed));
 touch_Move->addTransition(new SignalTrans<GraspError>("signaltrans34",failed));
 touch_Grasp->addTransition(new SignalTrans<GraspError>("signaltrans35",failed));
 rest_Plan->addTransition(new CompletionTrans("completiontrans33",rest_Rest));
 rest_Rest->addTransition(new CompletionTrans("completiontrans34",succeeded));
 rest_Plan->addTransition(new SignalTrans<GraspError>("signaltrans36",failed));
 rest_Rest->addTransition(new SignalTrans<GraspError>("signaltrans37",failed));
 computeReach_Plan->addTransition(new CompletionTrans("completiontrans35",succeeded));
 computeReach_Plan->addTransition(new SignalTrans<GraspError>("signaltrans38",failed));
 succeeded->addTransition(new NullTrans("nulltrans4",next));
 failed->addTransition(new NullTrans("nulltrans5",next));

    checkRoot = check_Plan;
    reachRoot = reach_Plan;
    graspRoot = grasp_Plan;
    releaseRoot = release_Plan;
    moveRoot = moveTo_Plan;
    touchRoot = touch_Plan;
    restRoot = rest_Plan;
    computeReachRoot = computeReach_Plan;
  }

  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();
  
  //  Point& getDesiredRobotLocation() { return desiredRobotLocation; }
  //  Point& getDesiredLookAtPoint() { return desiredLookAtPoint; }
  
  //! Converts the manipulated object to a planner obstacle and adds it to the planner obstacles list
  void appendObjectToObstacles() {
    if (!curReq->object.isValid()) return;
    if (curReq->objectPlannerObstacle == NULL) {
      PlannerObstacle::convertShapeToPlannerObstacle(curReq->object, 0, curReq->plannerObstacles);
      curReq->objectPlannerObstacle = curReq->plannerObstacles.back();
    } else {
      curReq->plannerObstacles.push_back(curReq->objectPlannerObstacle);
    }
    if (!curReq->moveConstrainedPath.empty()) {
#ifdef TGT_CALLIOPE
      fmat::Column<3> newPos = fmat::pack(curReq->targetLocation->getCentroid().coordX(),
                                          curReq->targetLocation->getCentroid().coordY(),
                                          curReq->targetLocation->getCentroid().coordZ());
#else
      fmat::Column<2> newPos = fmat::pack(curReq->targetLocation->getCentroid().coordX(),
                                          curReq->targetLocation->getCentroid().coordY());
#endif
      curReq->objectPlannerObstacle->updatePosition(newPos);
    }
  }
  
  //! Removes the manipulated object's planner obstacle from the planner obstacles list
  void removeTargetFromObstacles() {
    if (!curReq->object.isValid()) return;

    std::vector<PlannerObstacle*>::iterator obj = std::find(curReq->plannerObstacles.begin(), curReq->plannerObstacles.end(), curReq->objectPlannerObstacle);
    if (obj != curReq->plannerObstacles.end())
      curReq->plannerObstacles.erase(obj);
    delete curReq->objectPlannerObstacle;
    curReq->objectPlannerObstacle = NULL;
  }
  
  //! 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);
  
  //! Computes all goal states around a point and rotating the approach orientation
  void computeGoalStates(Point& toPt, std::vector<std::pair<float, float> >& rangesX,
                         std::vector<std::pair<float, float> >& rangesY, std::vector<std::pair<float, float> >& rangesZ,
                         float res, std::vector<NodeValue_t>& goals, fmat::Column<3> offset);

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

  //! Converts all environmental obstacles to planner obstacles
  void convertObstacles();
  
  std::queue<GrasperRequest*> requests; //!< Queue of pending Grasper requests
  unsigned int idCounter;               //!< Counter for assigning a unique id to each Grasper request

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