GrasperRequest.cc

Go to the documentation of this file.

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

#if defined(TGT_HANDEYE) or defined(TGT_CHIARA) or defined(TGT_CHIARA2)
#  include "Crew/CBracketGrasperPredicate.h"
#endif
#include "Crew/GrasperRequest.h"

GrasperRequest::GrasperRequest(GrasperRequestType_t _type) :
  objectPlannerObstacle(NULL),
  requestType(_type),
  restType(GrasperRequest::stationary),
  armRestState(),
  setVerbosity(0),
  clearVerbosity(0),
  effectorOffset(),
  rrtMaxIterations(2000),
  rrtInflation(10),
  rrtInterpolationStep(),
  armTimeFactor(1.0f),
  openGripperOnRest(false),
  object(), 
  targetLocation(), 
  gripperAngleRangesX(),
  gripperAngleRangesY(),
  gripperAngleRangesZ(),
  maxNumberOfAngles(1000000000),
  angleResolution((float)M_PI/12),
  sweepObjects(), 
  envObstacles(), 
  plannerObstacles(),
  moveConstrainedPath(),
  moveFreePath(),
  disengagePath(),
  predicate(NULL),
  displayPath(false),
  displayTree(false),
  requestingBehavior(),
  verbosity(0),
  populateEventPathWith(noPath),
  requestID(0)
{ 
#if defined(TGT_HANDEYE) or defined(TGT_CHIARA) or defined(TGT_CHIARA2)
  predicate = new CBracketGrasperPredicate<NumArmJoints>();
  effectorOffset = RobotInfo::GripperFrameOffset;
#endif
#if defined(TGT_CHIARA) or defined(TGT_CHIARA2)
  restType = GrasperRequest::settleBodyAndArm;
#endif
  for (unsigned int i = 0; i < NumArmJoints; i++) {
    armRestState[i] = 0.0f;
    rrtInterpolationStep[i] = M_PI / 180;
  }
}

GrasperRequest::GrasperRequest(const GrasperRequest &req) :
  objectPlannerObstacle(req.objectPlannerObstacle),
  requestType(req.requestType),
  restType(req.restType),
  armRestState(req.armRestState),
  setVerbosity(req.setVerbosity), clearVerbosity(req.clearVerbosity),
  effectorOffset(req.effectorOffset),
  rrtMaxIterations(req.rrtMaxIterations),
  rrtInflation(req.rrtInflation),
  rrtInterpolationStep(req.rrtInterpolationStep),
  armTimeFactor(req.armTimeFactor),
  openGripperOnRest(req.openGripperOnRest),
  object(req.object), 
  targetLocation(req.targetLocation), 
  gripperAngleRangesX(req.gripperAngleRangesX),
  gripperAngleRangesY(req.gripperAngleRangesY),
  gripperAngleRangesZ(req.gripperAngleRangesZ),
  maxNumberOfAngles(req.maxNumberOfAngles),
  angleResolution(req.angleResolution),
  sweepObjects(req.sweepObjects), 
  envObstacles(req.envObstacles), 
  plannerObstacles(req.plannerObstacles),
  moveConstrainedPath(req.moveConstrainedPath),
  moveFreePath(req.moveFreePath),
  disengagePath(req.disengagePath),
  predicate(req.predicate),
  displayPath(req.displayPath),
  displayTree(req.displayTree),
  requestingBehavior(req.requestingBehavior),
  verbosity(req.verbosity),
  populateEventPathWith(req.populateEventPathWith),
  requestID(req.requestID)
{}

GrasperRequest::~GrasperRequest() {
  for (unsigned i = 0; i < plannerObstacles.size(); i++)
    delete plannerObstacles[i];
  plannerObstacles.clear();
  // Don't also delete objectPlannerObstacle because its value was included in plannerObstacles
}

GrasperRequest::GrasperErrorType_t GrasperRequest::validateRequest() {
  // If grasper action requires an object, make sure that one was supplied
  switch ( requestType ) {
  case checkGraspable:
  case checkMovable:
  case computeMove:
  case grasp:
  case computeReach:
  case reach:
  case touch:
  case moveTo:
    if ( ! object.isValid() ) {
      cout << "GrasperRequest of this type requires a valid 'object' field. " << endl;
      return invalidRequest;
    }
    break;
  default:
    break;
  }

  // If grasper action requires a taget location, make sure that one was supplied
  switch ( requestType ) {
  case checkMovable:
  case computeMove:
  case grasp:
  case computeReach:
  case reach:
  case moveTo:
    if ( ! targetLocation.isValid() ) {
      cout << "GrasperRequest of this type requires a valid 'targetLocation' field. " << endl;
      return invalidRequest;
    }
    break;
  default:
    break;
  }

  if (armTimeFactor < 0) {
    cout << "Grasper request has armTimeFactor < 0: changing to 1." << endl;
    armTimeFactor = 1;
  }

  return noError;
}

#endif