00001 #include "Shared/RobotInfo.h"
00002 #if (defined(TGT_HAS_ARMS) || !defined(STRICT_TGT_MODEL)) && !defined(TGT_IS_AIBO)
00003
00004 #if defined(TGT_HANDEYE) or defined(TGT_CHIARA) or defined(TGT_CHIARA2)
00005 # include "Crew/CBracketGrasperPredicate.h"
00006 #endif
00007 #include "Crew/GrasperRequest.h"
00008
00009 GrasperRequest::GrasperRequest(GrasperRequestType_t _type) :
00010 requestType(_type),
00011 graspStrategy(sideGrasp),
00012 restType(GrasperRequest::stationary),
00013 armRestState(),
00014 setVerbosity(0),
00015 clearVerbosity(0),
00016 effectorOffset(),
00017 rrtMaxIterations(2000),
00018 rrtInflation(10),
00019 rrtInterpolationStep(),
00020 armTimeFactor(1.0f),
00021 openGripperOnRest(false),
00022 object(),
00023 objectFeature(), objectGraspPoints(),
00024 approachOrientation(std::numeric_limits<float>::quiet_NaN()),
00025 verifyStrategy(verifyNone), verifyGraspFunction(NULL),
00026 targetLocation(),
00027 targetOrientation(std::numeric_limits<float>::quiet_NaN()),
00028 allowBodyMotion(true),
00029 gripPressure(
00030 #ifdef TGT_IS_CALLIOPE
00031 -280
00032 #else
00033 0
00034 #endif
00035 ),
00036 gripperAngleRangesX(),
00037 gripperAngleRangesY(),
00038 gripperAngleRangesZ(),
00039 maxNumberOfAngles(100),
00040 angleResolution((float)M_PI/12),
00041 sweepObjects(),
00042 envObstacles(),
00043 predicate(NULL),
00044 populateEventPathWith(noPath),
00045 pilotreq(PilotTypes::goToShape),
00046 mapreq(NULL),
00047 displayPath(false),
00048 displayTree(false),
00049 approachPose(),
00050 transportPose(),
00051 withdrawPose(),
00052 approachPath(),
00053 deliverPath(),
00054 releasePath(),
00055 requestingBehavior(),
00056 verbosity(0),
00057 requestID(0)
00058 {
00059 #ifdef TGT_HANDEYE
00060 allowBodyMotion = false;
00061 #endif
00062 #if defined(TGT_HANDEYE) or defined(TGT_CHIARA) or defined(TGT_CHIARA2)
00063 predicate = new CBracketGrasperPredicate<NumArmJoints>();
00064 effectorOffset = RobotInfo::GripperFrameOffset;
00065 #endif
00066 #if defined(TGT_CHIARA) or defined(TGT_CHIARA2)
00067 restType = GrasperRequest::settleBodyAndArm;
00068 #endif
00069 #if defined(TGT_HAS_GRIPPER)
00070 effectorOffset = RobotInfo::GripperFrameOffset;
00071 #endif
00072 for (unsigned int i = 0; i < numPlannerJoints; i++) {
00073 armRestState[i] = 0.0f;
00074 rrtInterpolationStep[i] = M_PI / 180;
00075 }
00076 #ifdef TGT_IS_CALLIOPE5
00077 std::pair<float,float> noSlop(0,0);
00078 gripperAngleRangesX.push_back(noSlop);
00079 gripperAngleRangesY.push_back(noSlop);
00080 gripperAngleRangesZ.push_back(noSlop);
00081 #endif
00082 }
00083
00084 GrasperRequest::GrasperRequest(const GrasperRequest &req) :
00085 requestType(req.requestType),
00086 graspStrategy(req.graspStrategy),
00087 restType(req.restType),
00088 armRestState(req.armRestState),
00089 setVerbosity(req.setVerbosity), clearVerbosity(req.clearVerbosity),
00090 effectorOffset(req.effectorOffset),
00091 rrtMaxIterations(req.rrtMaxIterations),
00092 rrtInflation(req.rrtInflation),
00093 rrtInterpolationStep(req.rrtInterpolationStep),
00094 armTimeFactor(req.armTimeFactor),
00095 openGripperOnRest(req.openGripperOnRest),
00096 object(req.object),
00097 objectFeature(req.objectFeature), objectGraspPoints(req.objectGraspPoints),
00098 approachOrientation(req.approachOrientation),
00099 verifyStrategy(req.verifyStrategy), verifyGraspFunction(req.verifyGraspFunction),
00100 targetLocation(req.targetLocation),
00101 targetOrientation(req.targetOrientation),
00102 allowBodyMotion(req.allowBodyMotion),
00103 gripPressure(req.gripPressure),
00104 gripperAngleRangesX(req.gripperAngleRangesX),
00105 gripperAngleRangesY(req.gripperAngleRangesY),
00106 gripperAngleRangesZ(req.gripperAngleRangesZ),
00107 maxNumberOfAngles(req.maxNumberOfAngles),
00108 angleResolution(req.angleResolution),
00109 sweepObjects(req.sweepObjects),
00110 envObstacles(req.envObstacles),
00111 predicate(req.predicate),
00112 populateEventPathWith(req.populateEventPathWith),
00113 pilotreq(req.pilotreq),
00114 mapreq(req.mapreq),
00115 displayPath(req.displayPath),
00116 displayTree(req.displayTree),
00117 approachPose(req.approachPose),
00118 transportPose(req.transportPose),
00119 withdrawPose(req.withdrawPose),
00120 approachPath(req.approachPath),
00121 deliverPath(req.deliverPath),
00122 releasePath(req.releasePath),
00123 requestingBehavior(req.requestingBehavior),
00124 verbosity(req.verbosity),
00125 requestID(req.requestID)
00126 {}
00127
00128 GrasperRequest::GrasperErrorType_t GrasperRequest::validateRequest() {
00129
00130 switch ( requestType ) {
00131 case checkGraspable:
00132 case checkMovable:
00133 case computeMove:
00134 case grasp:
00135 case computeReach:
00136 case reach:
00137 case touch:
00138 case moveTo:
00139 if ( ! object.isValid() ) {
00140 cout << "GrasperRequest of this type requires a valid 'object' field. " << endl;
00141 return invalidRequest;
00142 }
00143 break;
00144 default:
00145 break;
00146 }
00147
00148
00149 switch ( verifyStrategy ) {
00150 case verifyUser:
00151 if ( verifyGraspFunction == NULL ) {
00152 cout << "GrasperRequest with verifyStrategy of 'verifyUser' requires a verifyGraspFunction" << endl;
00153 return invalidRequest;
00154 }
00155
00156
00157 default:
00158 break;
00159 }
00160
00161
00162 switch ( requestType ) {
00163 case checkMovable:
00164 case computeMove:
00165 case computeReach:
00166 case moveTo:
00167 if ( ! targetLocation.isValid() ) {
00168 cout << "GrasperRequest of this type requires a valid 'targetLocation' field. " << endl;
00169 return invalidRequest;
00170 }
00171 break;
00172 default:
00173 break;
00174 }
00175
00176 if (armTimeFactor < 0) {
00177 cout << "Grasper request has armTimeFactor < 0: changing to 1." << endl;
00178 armTimeFactor = 1;
00179 }
00180
00181 return noError;
00182 }
00183
00184 #endif