Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Grasper.cc

Go to the documentation of this file.
00001 #include "Shared/RobotInfo.h"
00002 #if (defined(TGT_HAS_ARMS) || !defined(STRICT_TGT_MODEL)) && !defined(TGT_IS_AIBO)
00003 
00004 #include "Crew/Grasper.h"
00005 #include "DualCoding/ShapeCross.h"
00006 #include "DualCoding/ShapeNaught.h"
00007 #include "DualCoding/VRmixin.h"
00008 #include "Motion/IKSolver.h"
00009 #include "Planners/Navigation/ShapeSpacePlannerXYTheta.h"
00010 #include "Shared/mathutils.h"
00011 
00012 using namespace DualCoding;
00013 using namespace mathutils;
00014 
00015 //**** These constants are for the Chiara chess gripper.
00016 float openLeftGripperVal = -0.10f;
00017 float closedLeftGripperVal = -0.25f;
00018 float openRightGripperVal = 0.07f;
00019 float closedRightGripperVal = 0.25f;
00020 
00021 GrasperRequest* Grasper::curReq = NULL;
00022 Grasper::GrasperVerbosity_t Grasper::verbosity = -1U;
00023 
00024 GenericRRTBase::PlannerResult2D
00025 Grasper::planBodyPath(const Point &targetPt, AngTwoPi approachOrientation,
00026                       const fmat::Column<3> &baseOffset, 
00027                       Shape<AgentData> &pose, float radius, bool isFinalApproach = true) {
00028   //Declare function members for calculation
00029   Point agentpt = VRmixin::theAgent->getCentroid();       //Define where the robot is currently
00030   Point vec = targetPt - agentpt; 
00031   float dist = vec.xyNorm();                              //Find the distance between those two points
00032   GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS); //Get the bounds of the world
00033   
00034   ShapeSpacePlannerXYTheta planner(VRmixin::worldShS, worldBounds, curReq->pilotreq.obstacleInflation);
00035   GenericRRTBase::PlannerResult2D result;
00036   std::vector<ShapeSpacePlannerXYTheta::NodeValue_t> pathResult;
00037   std::vector<ShapeSpacePlannerXYTheta::NodeType_t> *treeStartResult = new std::vector<ShapeSpacePlannerXYTheta::NodeType_t>;
00038   std::vector<ShapeSpacePlannerXYTheta::NodeType_t> *treeEndResult = new std::vector<ShapeSpacePlannerXYTheta::NodeType_t>;
00039   Point pIn, pOut, approachPoint;
00040   ShapeSpacePlannerXYTheta::NodeValue_t finalValue;
00041   
00042   //If the robot is outside the desired radius...
00043   if ( dist >= radius ) {
00044     //Plan the path to the target and store the result in the variable of the same name
00045     result = planner.planPath(agentpt, baseOffset, 0, targetPt, 
00046                               VRmixin::theAgent->getOrientation(),
00047                               isFinalApproach ? curReq->targetOrientation : approachOrientation,
00048                               curReq->pilotreq.maxRRTIterations, &pathResult, treeStartResult, treeEndResult);
00049     //curReq->***Orientation orients the robot and (only by extension) the object to be placed (if we're using a Calliope2SP)
00050     
00051     // begin debug---------------------------------------------------------------------------------------------------
00052     // std::cout << "showing trees size " << treeStartResult->size() << " " << treeEndResult->size() << std::endl;
00053     NEW_SHAPE(plannerTrees, GraphicsData, new GraphicsData(VRmixin::worldShS));
00054     planner.plotTree(*treeStartResult, plannerTrees, rgb(0,0,0));
00055     planner.plotTree(*treeEndResult, plannerTrees, rgb(0,192,0));
00056     delete treeStartResult;
00057     delete treeEndResult;
00058     // end debug-----------------------------------------------------------------------------------------------------
00059     
00060     if (result.code != GenericRRTBase::SUCCESS) {
00061       //If the RRT did not succeed, determine why:
00062       
00063       if ( result.code == GenericRRTBase::END_COLLIDES ) {
00064         VRmixin::robotObstaclesPt = targetPt - Point(baseOffset,allocentric);
00065         VRmixin::robotObstaclesOri = AngTwoPi(0);
00066       } else {
00067       // this means we encountered a START_COLLIDES or MAX_ITER
00068         VRmixin::robotObstaclesPt = agentpt;
00069         VRmixin::robotObstaclesOri = VRmixin::theAgent->getOrientation();
00070       }
00071       planner.addObstaclesToShapeSpace(VRmixin::worldShS);
00072       return result;
00073     }
00074     
00075     // If we were successful...
00076     VRmixin::robotObstaclesPt = agentpt;
00077     VRmixin::robotObstaclesOri = VRmixin::theAgent->getOrientation();
00078     /*
00079     // Find the path segment that crosses the circle
00080     std::vector<ShapeSpacePlannerXYTheta::NodeValue_t>::const_iterator it = pathResult.end();
00081     while ( it-- != pathResult.begin() ) {
00082       pOut = Point(it->first.first, it->first.second);
00083       float pdist = (pOut - targpt).xyNorm();
00084       if ( pdist >= radius ) {
00085   // point pOut is outside the circle; next point will be inside
00086   ++it;
00087   pIn = Point(it->first.first, it->first.second);
00088   break;
00089       }
00090     }
00091     */
00092     finalValue = pathResult.back();
00093   } else {  // If the dist to the target is < the desired radius
00094     
00095     // Must get outside the circle.  Use present heading and back up
00096     // if necessary.  Start by making a dummy path segment.
00097     pIn = agentpt;
00098     float orient = VRmixin::theAgent->getOrientation();
00099     pOut = Point(pIn.coordX() - 2*radius*cos(orient),
00100                  pIn.coordY() - 2*radius*sin(orient));
00101     finalValue = ShapeSpacePlannerXYTheta::
00102 			NodeValue_t(agentpt.coordX() - (2*radius-dist)*cos(orient),
00103                   agentpt.coordY() - (2*radius-dist)*sin(orient),
00104                   orient);
00105   }
00106   /*
00107   // Find the line equation for the path segment
00108   std::pair<float,float> lineEq = LineData(VRmixin::worldShS,pOut,pIn).lineEquation_mb();
00109   float m = lineEq.first;
00110   float b = lineEq.second;
00111   // Now find the point on the path segment that crosses the circle.
00112   // This point simultaneously satisfies the line equation y=m*x+b and
00113   // the constraint x^2+y^2 = r^2.  Solution: replace y with mx+b in
00114   // the circle equation, yielding a quadratic in x.  Solve for x in
00115   // terms of the constants m, b, and r.  There can be two solutions.
00116   float xo = targpt.coordX(), yo = targpt.coordY();
00117   float root = sqrt(-b*b - 2*b*m*xo + 2*b*yo - m*m*xo*xo + m*m*radius*radius + 2*m*xo*yo + radius*radius - yo*yo);
00118   float x1 = ( root - b*m + m*yo + xo) / (m*m+1);  // First solution
00119   float x2 = (-root - b*m + m*yo + xo) / (m*m+1);  // Second solution
00120   // Choose the correct solution.
00121   float x;
00122   if ( dist > radius )
00123     x = ((x1 >= pOut.coordX() && x1 <= pIn.coordX()) || (x1 >= pIn.coordX() && x1 <= pOut.coordX())) ? x1 : x2;
00124   else // we're inside the circle
00125     x = ((agentpt-Point(x1,m*x1+b)).xyNorm() < radius) ? x1 : x2;
00126   // Create an agent pose at the desired point, and save it in curReq
00127   float y = m*x + b;
00128     */
00129   if ( std::isnan(finalValue.x) ) {
00130     std::cout << " finalValue.x is nan " << endl;
00131     finalValue.x = agentpt.coordX();
00132     finalValue.y = agentpt.coordY();
00133   }
00134   if ( std::isnan(float(finalValue.theta)) ) {
00135     std::cout << " finalValue.theta is nan " << endl;
00136     finalValue.theta = 0.0;
00137   }
00138   Point posePoint(finalValue.x,finalValue.y,0,allocentric);
00139   //AngTwoPi poseHeading((pIn-pOut).atanYX());  // maintain the segment's heading to assure obstacle clearance
00140   pose->setCentroidPt(posePoint);
00141   pose->setOrientation(finalValue.theta);
00142   pose->setColor(rgb(0,255,0));
00143   pose->setObstacle(false);
00144   return result;
00145 }
00146 
00147 float Grasper::PlanBodyApproach::getBodyApproachRadius() {
00148 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00149   const float radius = 250;
00150 #elif defined(TGT_IS_CALLIOPE5)
00151   const float radius = 640;
00152 #else
00153   const float radius = 500;  // a "reasonable" value for unknown robot type
00154 #endif
00155   return radius;
00156 }
00157 
00158 void Grasper::PlanBodyApproach::doStart() {
00159   if ( ! curReq->allowBodyMotion ) {
00160     postStateFailure();
00161     return;
00162   }
00163   float bodyRadius = getBodyApproachRadius();
00164   std::cout << "*** bodyRadius = " << bodyRadius << std::endl;
00165   fmat::Column<3> baseOffset = fmat::pack(bodyRadius,0,0);
00166   Point targPoint = curReq->object->getCentroid();
00167   AngTwoPi orient = curReq->approachOrientation;
00168 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00169   PostureEngine pe;
00170   pe.setOutputCmd(ArmBaseOffset,0.0);
00171   pe.setOutputCmd(ArmShoulderOffset,0.0);
00172   baseOffset = pe.getPosition(GripperFrameOffset);
00173 #else
00174   std::cout << "*** Grasper::PlanBodyApproach::doStart doesn't support this robot type." << std::endl;
00175 #endif
00176   cout << "*** targPoint=" << targPoint << "   orient=" << orient
00177        << "   baseOffset=" << baseOffset << "   bodyRadius=" << bodyRadius << endl;
00178   if ( curReq->object->getType() == cylinderDataType ) {
00179     baseOffset[0] += ShapeRootTypeConst(curReq->object, CylinderData)->getRadius() + bodyRadius;
00180     curReq->object->setObstacle(false);  // *** THIS IS WRONG; can cause collision if approachOrientation specified
00181     // *** Need to rethink path planning to properly take into account the object's radius and grasp point, obstacle inflation,
00182     // *** minimum required distance for apporach (misnamed as bodyRadius), etc.
00183   }
00184    if ( curReq->object->getType() == naughtDataType ) {
00185     baseOffset[0] += ShapeRootTypeConst(curReq->object, NaughtData)->getRadius() + bodyRadius;
00186     curReq->object->setObstacle(false);  // *** THIS IS WRONG; can cause collision if approachOrientation specified
00187     // *** Need to rethink path planning to properly take into account the object's radius and grasp point, obstacle inflation,
00188     // *** minimum required distance for apporach (misnamed as bodyRadius), etc.
00189    } 
00190    else if ( curReq->object->getType() == dominoDataType ) {
00191     orient = std::numeric_limits<float>::quiet_NaN();
00192     ShapeRootTypeConst(curReq->object, DominoData)->
00193       computeGraspPoint((DominoData::ObjectFeature)curReq->objectFeature, targPoint, orient);
00194     baseOffset = fmat::pack(bodyRadius*1.25, 0, 0);  // **** HACK for now
00195     fmat::Column<3> offsetTarget = targPoint.getCoords() - fmat::rotationZ(orient) * baseOffset;
00196     cout << "*** targPoint=" << targPoint << "   orient=" << orient 
00197          << "   offsetTarget=" << offsetTarget << endl;
00198     targPoint.setCoords(offsetTarget);
00199     bodyRadius = 0;
00200     // we want to put the gripper, not the base, at the offset target point
00201    }
00202    else if ( curReq->object->getType() == crossDataType ) {
00203     orient = std::numeric_limits<float>::quiet_NaN();
00204     std::vector<Point> graspPoints = ShapeRootTypeConst(curReq->object, CrossData)->computeGraspPoints();
00205     targPoint = graspPoints[0];
00206     orient = (targPoint - curReq->object->getCentroid()).atanYX();
00207     curReq->object->setObstacle(false);  // *** THIS IS WRONG; can cause collision if approachOrientation specified
00208     baseOffset = fmat::pack(bodyRadius*1.25, 0, 0);  // **** HACK for now
00209     fmat::Column<3> offsetTarget = targPoint.getCoords() - fmat::rotationZ(orient) * baseOffset;
00210     cout << "*** targPoint=" << targPoint << "   orient=" << orient << "   centroid=" << curReq->object->getCentroid()  
00211          << "   offsetTarget=" << offsetTarget << endl;
00212     targPoint.setCoords(offsetTarget);
00213     bodyRadius = 0;
00214     // we want to put the gripper, not the base, at the offset target point
00215   } 
00216   //Plan the body path
00217   NEW_SHAPE(approachPose, AgentData, new AgentData(VRmixin::theAgent.getData()));
00218   GenericRRTBase::PlannerResult2D result =
00219     VRmixin::grasper->planBodyPath(targPoint, orient, baseOffset, approachPose, bodyRadius, false);
00220 
00221   //Check the result of that planning:
00222   if ( result.code != GenericRRTBase::SUCCESS ) {
00223     //If we did not succeed, try to explain why before returning
00224     switch ( result.code ) {
00225     case GenericRRTBase::START_COLLIDES:
00226       std::cout << "Grasper approach planning failed: start state " << result.movingObstacle->toString()
00227     << " is in collision with " << result.collidingObstacle->toString() << ".\n";
00228       break;
00229     case GenericRRTBase::END_COLLIDES:
00230       std::cout << "Grasper approach planning failed: no collision-free path to " << curReq->object 
00231     << " due to " << result.collidingObstacle->toString() << ".\n";
00232       break;
00233     case GenericRRTBase::MAX_ITER:
00234       std::cout << "Grasper approach path planning gave up after " << curReq->pilotreq.maxRRTIterations << " iterations.\n";
00235       break;
00236     default: break;
00237     }
00238     postStateSignal<GraspError>(GrasperRequest::noGraspPath);
00239     return;
00240   }
00241 
00242   //If we did succeed, continue with the rest of the request
00243   curReq->approachPose = approachPose;
00244   postStateCompletion();
00245 }
00246 
00247 void Grasper::DoBodyApproach::doStart() {
00248   if ( ! curReq->approachPose.isValid() ) {   // should always be valid
00249     std::cout << "Error: Grasper::ReachBody invoked with no valid approachPose!" << std::endl;
00250     cancelThisRequest();
00251     return;
00252   }
00253   if ( curReq->pilotreq.targetShape.isValid() )
00254     pilotreq = curReq->pilotreq;
00255   else {
00256     pilotreq = PilotRequest(PilotTypes::goToShape);
00257     pilotreq.targetShape = curReq->approachPose;
00258     pilotreq.targetHeading = curReq->approachPose->getOrientation();
00259   }
00260 }
00261 
00262 void Grasper::FindObj::doStart() {
00263   if ( curReq->mapreq != NULL ) {
00264     mapreq = *curReq->mapreq;
00265   }
00266   else {
00267     mapreq = MapBuilderRequest(MapBuilderRequest::worldMap);
00268     mapreq.addAttributes(curReq->object);
00269     mapreq.addAllMinBlobAreas(1000); // *** stopgap for cylinders (and naughts)
00270     mapreq.worldTargets.push(new LookoutSearchRequest(curReq->object));
00271   }
00272 }
00273 
00274 void Grasper::PlanArmApproach::doStart() {
00275 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00276   fmat::Column<3> target;
00277   if ( curReq->object->getType() == dominoDataType ) {
00278     Point targPoint;
00279     AngTwoPi orient;
00280     ShapeRootTypeConst(curReq->object, DominoData)->
00281       computeGraspPoint((DominoData::ObjectFeature)curReq->objectFeature, targPoint, orient);
00282     target = targPoint.getCoords();
00283   }
00284   else if ( curReq->object->getType() == crossDataType ) {
00285     Point targPoint;
00286     AngTwoPi orient;
00287     std::vector<Point> graspPoints = ShapeRootTypeConst(curReq->object, CrossData)->
00288       computeGraspPoints();
00289     targPoint = graspPoints[0];
00290     orient = (targPoint - curReq->object->getCentroid()).atanYX();
00291     target = targPoint.getCoords();
00292   } 
00293   else
00294     target = curReq->object->getCentroid().getCoords();
00295   // Set shoulder position to 3/4 of the height of the target shape.
00296   if ( curReq->object->getType() == cylinderDataType ) // grasp cylinders at 75% of their height
00297     target[2] += ShapeRootTypeConst(curReq->object, CylinderData)->getHeight() * 0.25;
00298   else if ( curReq->object->getType() == naughtDataType ) // grasp naughts, like cylinders, at 75% of their height
00299     target[2] += ShapeRootTypeConst(curReq->object, NaughtData)->getHeight() * 0.25;
00300   else if ( curReq->object->getType() == brickDataType ) // If the object to grasp is a brick-type object
00301     target[2] += ShapeRootTypeConst(curReq->object, BrickData)->getCentroid().coordZ() * 0.5;
00302   // cout << "PlanReachArm: target at: " << target << endl; 
00303   PostureEngine pe;
00304   pe.solveLinkPosition(target, GripperFrameOffset, fmat::ZERO3);
00305   //cout << "PlanReachArm: GripperFrame is at " << pe.getPosition(GripperFrameOffset) << endl;
00306   NodeValue_t armpoint;
00307   armpoint[0] = 0;  // arm base
00308   armpoint[1] = pe.getOutputCmd(ArmShoulderOffset).value;
00309   curReq->approachPath.push_back(armpoint);
00310   postStateCompletion();
00311 #elif defined(TGT_IS_CALLIOPE5)
00312   // plan Calliope5 arm reach
00313   // assume there is a shape in local space that matches our world space object; fail if not
00314   std::cout << "PlanArmApproach for 5KP" << std::endl;
00315   ShapeRoot objlocal = find_if(VRmixin::localShS, IsLastMatch(curReq->object));
00316   if ( ! objlocal.isValid() ) {
00317     std::cout << "Grasper can' fitnd " << curReq->object << " (last match id " << curReq->object->getLastMatchId()
00318         << ") in local shape space!" << std::endl;
00319     postStateSignal<GraspError>(GrasperRequest::lostObject);
00320     return;
00321   }
00322   objlocal->setObstacle(true);
00323   std::vector<NodeValue_t> endStates;
00324 
00325   fmat::Column<3> toPtCent = objlocal->getCentroid().getCoords();
00326   fmat::Column<3> toPtArmRef = toPtCent - kine->getPosition(ArmBaseOffset);
00327   fmat::Column<3> toPtArmRefNormal = toPtArmRef / toPtArmRef.norm();
00328   fmat::Column<3> toPtOff = toPtArmRefNormal * 125;  // pick a point 90 mm short of the object
00329   fmat::Column<3> toPtShort = toPtArmRef - toPtOff;
00330   IKSolver::Point toPt(kine->getPosition(ArmBaseOffset) + toPtShort);
00331   VRmixin::grasper->computeGoalStates(toPt,
00332               curReq->gripperAngleRangesX,
00333               curReq->gripperAngleRangesY,
00334               curReq->gripperAngleRangesZ,
00335               curReq->angleResolution,
00336               endStates, IKSolver::Point(fmat::ZERO3));
00337   for (uint i = 0; i < endStates.size(); i++) {
00338     cout << "state " << i << ": ";
00339   for (uint j = 0; j < 5; j++)
00340     cout << endStates[i][j] << ", ";
00341   cout << endl;
00342   }
00343   if ( endStates.empty() ) {
00344   cout << "failed to find end state" << endl;
00345     postStateSignal<GraspError>(GrasperRequest::pickUpUnreachable);
00346     return;
00347   }
00348   NodeValue_t startSt;
00349   Grasper::getCurrentState(startSt);
00350   
00351   GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
00352   Grasper::ArmPlanner planner(VRmixin::localShS, worldBounds, curReq->rrtInflation, curReq->effectorOffset);
00353   std::vector<NodeType_t> *treeStartResult = curReq->displayTree ? new std::vector<NodeType_t> : NULL;
00354   std::vector<NodeType_t> *treeEndResult = curReq->displayTree ? new std::vector<NodeType_t> : NULL;
00355   fmat::Transform t = fmat::Transform();
00356   //t.translation() = VRmixin::theAgent->getCentroid().coords;
00357   //t.rotation() = fmat::rotationZ(VRmixin::theAgent->getOrientation());
00358   Grasper::ArmPlanner::PlannerResult result;
00359   for (uint i = 0; i < endStates.size(); i++) {
00360     result = planner.planPath(startSt, endStates.front(), curReq->rrtInterpolationStep,
00361          t, curReq->rrtMaxIterations,
00362          &(curReq->approachPath), treeStartResult, treeEndResult);
00363     if (result.code == GenericRRTBase::SUCCESS)
00364     break;
00365     switch ( result.code ) {
00366     case GenericRRTBase::SUCCESS:
00367       break;
00368     case GenericRRTBase::START_COLLIDES:
00369       std::cout << "Arm path planning failed: start state is in collision.\n";
00370       break;
00371     case GenericRRTBase::END_COLLIDES:
00372       std::cout << "Arm path planning failed: end state is in collision.\n";
00373       break;
00374     case GenericRRTBase::MAX_ITER:
00375       std::cout << "Arm path planning failed: too many iterations.\n";
00376       break;
00377     }
00378     if (result.code == GenericRRTBase::START_COLLIDES || result.code == GenericRRTBase::END_COLLIDES) {
00379     std::cout << "Moving part: " << std::endl << "=================" << std::endl
00380         << result.movingObstacle->toString() << std::endl;
00381     std::cout << "collided with: " << std::endl << "=================" << std::endl
00382         << result.collidingObstacle->toString() << std::endl;
00383     std::cout << "=================" << std::endl;
00384   }
00385   }
00386   switch ( result.code ) {
00387     case GenericRRTBase::SUCCESS:
00388       std::cout << "PlanArmApproach succeeded" << std::endl;
00389       break;
00390     case GenericRRTBase::START_COLLIDES:
00391       std::cout << "Arm path planning failed: start state is in collision.\n";
00392       break;
00393     case GenericRRTBase::END_COLLIDES:
00394       std::cout << "Arm path planning failed: end state is in collision.\n";
00395       break;
00396     case GenericRRTBase::MAX_ITER:
00397       std::cout << "Arm path planning failed: too many iterations.\n";
00398       break;
00399   }
00400   
00401   if (result.code == GenericRRTBase::START_COLLIDES || result.code == GenericRRTBase::END_COLLIDES) {
00402     std::cout << "Obstacle: " << std::endl << "=================" << std::endl
00403         << result.movingObstacle->toString() << std::endl;
00404     std::cout << "collided with: " << std::endl << "=================" << std::endl
00405         << result.collidingObstacle->toString() << std::endl;
00406     std::cout << "=================" << std::endl;
00407   }
00408   
00409   // Display the tree if requested
00410   if ( curReq->displayTree && result.code != GenericRRTBase::START_COLLIDES && GenericRRTBase::END_COLLIDES ) {
00411     NEW_SHAPE(plannerTrees, GraphicsData, new GraphicsData(VRmixin::worldShS));
00412     planner.plotTree(*treeStartResult, plannerTrees, rgb(0,0,0));
00413     planner.plotTree(*treeEndResult, plannerTrees, rgb(0,192,0));
00414     delete treeStartResult;
00415     delete treeEndResult;
00416   }
00417   
00418   // Display the path if requested
00419   if ( curReq->displayPath && result.code == GenericRRTBase::SUCCESS ) {
00420     NEW_SHAPE(plannedPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
00421     planner.plotPath(curReq->approachPath, plannedPath, rgb(0,0,255));
00422   } 
00423   
00424   if (result.code != GenericRRTBase::SUCCESS) {
00425     planner.addObstaclesToShapeSpace(VRmixin::localShS);
00426     planner.addObstaclesToShapeSpace(VRmixin::worldShS, VRmixin::mapBuilder->localToWorldMatrix);
00427     postStateSignal<GraspError>(GrasperRequest::noGraspPath);
00428     return;
00429   }
00430   // We planned a path to the gate point.  Now add one more step to
00431   // get to the actual target point.
00432   KinematicJoint *effector = kine->getKinematicJoint(curReq->effectorOffset)->cloneBranch();
00433   IKSolver& solver = effector->getIK();
00434   float const positionMostImportant = 1.0f;
00435   float const orientationLeastImportant = 0.5f;
00436   IKSolver::Point goalPos(objlocal->getCentroid().getCoords());
00437   bool reached = solver.solve(IKSolver::Point(fmat::ZERO3), IKSolver::Rotation(fmat::Quaternion::aboutX(-M_PI/2)),
00438             *effector, goalPos, positionMostImportant, IKSolver::Parallel(0,0,1), orientationLeastImportant);
00439   NodeValue_t goalState;
00440   Grasper::getCurrentState(goalState,effector);
00441   curReq->approachPath.push_back(goalState);
00442   delete effector;
00443   postStateCompletion();
00444   return;
00445 #elif defined(TGT_HANDEYE)
00446   std::vector<NodeValue_t> endStates;
00447   Point toPt = curReq->object->getCentroid();
00448   /*
00449   VRmixin::grasper->computeGoalStates(toPt,
00450               curReq->gripperAngleRangesX,
00451               curReq->gripperAngleRangesY,
00452               curReq->gripperAngleRangesZ,
00453                                       curReq->angleResolution,
00454               endStates, IKSolver::Point(fmat::ZERO3));
00455   */
00456   if ( endStates.empty() ) {
00457     postStateSignal<GraspError>(GrasperRequest::pickUpUnreachable);
00458     return;
00459   }
00460 
00461   NodeValue_t startSt;
00462   Grasper::getCurrentState(startSt);
00463   
00464   GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
00465   Grasper::ArmPlanner planner(VRmixin::worldShS, worldBounds, curReq->rrtInflation, curReq->effectorOffset);
00466   std::vector<NodeType_t> *treeStartResult = curReq->displayTree ? new std::vector<NodeType_t> : NULL;
00467   std::vector<NodeType_t> *treeEndResult = curReq->displayTree ? new std::vector<NodeType_t> : NULL;
00468   fmat::Transform t;
00469   t.translation() = VRmixin::theAgent->getCentroid().coords;
00470   t.rotation() = fmat::rotationZ(VRmixin::theAgent->getOrientation());
00471   Grasper::ArmPlanner::PlannerResult result =
00472     planner.planPath(startSt, endStates.front(), curReq->rrtInterpolationStep,
00473          t, curReq->rrtMaxIterations,
00474          &(curReq->approachPath), treeStartResult, treeEndResult);
00475   
00476   switch ( result.code ) {
00477     case GenericRRTBase::SUCCESS:
00478       std::cout << "Plan Unconstrained succeeded" << std::endl;
00479       break;
00480     case GenericRRTBase::START_COLLIDES:
00481       std::cout << "Arm path planning failed: start state is in collision.\n";
00482       break;
00483     case GenericRRTBase::END_COLLIDES:
00484       std::cout << "Arm path planning failed: end state is in collision.\n";
00485       break;
00486     case GenericRRTBase::MAX_ITER:
00487       std::cout << "Arm path planning failed: too many iterations.\n";
00488       break;
00489   }
00490   
00491   if (result.code == GenericRRTBase::START_COLLIDES || result.code == GenericRRTBase::END_COLLIDES) {
00492     std::cout << "Obstacle: " << std::endl << "=================" << std::endl
00493         << result.movingObstacle->toString() << std::endl;
00494     std::cout << "collided with: " << std::endl << "=================" << std::endl
00495         << result.collidingObstacle->toString() << std::endl;
00496     std::cout << "=================" << std::endl;
00497   }
00498   
00499   // Display the tree if requested
00500   if ( curReq->displayTree && result.code != GenericRRTBase::START_COLLIDES && GenericRRTBase::END_COLLIDES ) {
00501     NEW_SHAPE(plannerTrees, GraphicsData, new GraphicsData(VRmixin::worldShS));
00502     planner.plotTree(*treeStartResult, plannerTrees, rgb(0,0,0));
00503     planner.plotTree(*treeEndResult, plannerTrees, rgb(0,192,0));
00504     delete treeStartResult;
00505     delete treeEndResult;
00506   }
00507   
00508   // Display the path if requested
00509   if ( curReq->displayPath && result.code == GenericRRTBase::SUCCESS ) {
00510     NEW_SHAPE(plannedPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
00511     planner.plotPath(curReq->approachPath, plannedPath, rgb(0,0,255));
00512   } 
00513   
00514   if (result.code != GenericRRTBase::SUCCESS) {
00515     std::cout << "PlanArmApproach failed" << std::endl;
00516     postStateSignal<GraspError>(GrasperRequest::noGraspPath);
00517     return;
00518   }
00519   postStateCompletion();
00520 #endif
00521 }
00522 
00523 void Grasper::DoBodyApproach2::doStart() {
00524 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00525   // We're at the approach gate but perhaps not pointed quite correctly.
00526   // Determine how much to rotate the body to point the gripper at the target.
00527   // Target should have been left in localShS by FindObj
00528   curReq->object->setObstacle(false);
00529   ShapeRoot objlocal = find_if(VRmixin::localShS, IsLastMatch(curReq->object));
00530   if ( ! objlocal.isValid() ) {
00531     std::cout << "Can't find local shape that matches " << curReq->object
00532               << " loc=" << curReq->object->getCentroid()
00533               << "(lastmatch " << curReq->object->getLastMatchId() << ") !" << std::endl;
00534     SHAPEROOTVEC_ITERATE(VRmixin::localShS, element) {
00535       std::cout << "   " << element << " loc=" << element->getCentroid() << std::endl;
00536     } END_ITERATE;
00537     cancelThisRequest();
00538                                                                                                                   return;
00539   }
00540   fmat::Column<3> objPos = objlocal->getCentroid().getCoords();
00541   float objDist = objlocal->getCentroid().xyNorm();
00542   float distsq = objDist * objDist;
00543   fmat::Column<3> gripperPos = kine->linkToBase(GripperFrameOffset).translation();
00544 /*
00545   fmat::Column<3> shoulderPos = kine->linkToBase(ArmShoulderOffset).translation();
00546   fmat::Column<3> armVec = gripperPos - shoulderPos;
00547   armVec = armVec / armVec.norm() * objDist;
00548   float pa = (armVec[0]*armVec[0] + armVec[1]*armVec[1]);
00549   float pb = 2 * (shoulderPos[0]*armVec[0] + shoulderPos[1]*armVec[1]);
00550   float pc = shoulderPos[0]*shoulderPos[0] + shoulderPos[1]*shoulderPos[1] - distsq;
00551   float k1 = (-pb + sqrt(pb*pb - 4*pa*pc)) / (2*pa);
00552   fmat::Column<3> shoulderProj = shoulderPos + k1*armVec;
00553   cout << "pa=" << pa << " pb=" << pb << " pc=" << pc << " k1=" << k1 << " shoulderProj=" << shoulderProj << endl;
00554   objPos[2] = shoulderProj[2] = 0;
00555   float dot = fmat::dotProduct(objPos, shoulderProj) / distsq;
00556   float theta1 = acos(dot);
00557   if ( shoulderProj[1] > objPos[1] )
00558     theta1 *= -1;
00559   std::cout << "objPos=" << objPos << "  shoulderPos=" << shoulderPos
00560             << "  shoulderProj=" << shoulderProj
00561             << "  theta1=" << theta1*180/M_PI << " deg." << std::endl;
00562 */
00563   fmat::Column<3> ahead = fmat::pack(1,0,0);
00564   float pa = 1;
00565   float pb = 2 * gripperPos[0];
00566   float pc = gripperPos[0]*gripperPos[0] + gripperPos[1]*gripperPos[1] - distsq;
00567   float k1 = (-pb + sqrt(pb*pb - 4*pa*pc)) / (2*pa);
00568   fmat::Column<3> gripperProj = gripperPos + k1 * ahead;
00569   objPos[2] = gripperProj[2] = 0;
00570   float dot = fmat::dotProduct(objPos, gripperProj) / distsq;
00571   float theta1 = acos(dot);
00572   if ( gripperProj[1] > objPos[1] )
00573     theta1 *= -1;
00574   std::cout << "objPos=" << objPos << "  gripperPos=" << gripperPos << "  gripperProj=" << gripperProj
00575     << "  theta1=" << theta1*180/M_PI << " deg." << std::endl;
00576   pilotreq.da = theta1;
00577   pilotreq.turnSpeed = 0.25;
00578 #else
00579   cancelThisRequest();
00580 #endif
00581 }
00582 
00583 void Grasper::DoBodyApproach3::doStart() {
00584 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00585   ShapeRoot objlocal = find_if(VRmixin::localShS, IsLastMatch(curReq->object));
00586   float dist = objlocal->getCentroid().xyNorm();
00587   float radius = 0;
00588   if ( curReq->object->getType() == cylinderDataType ) // subtract half the cylinder radius (heuristic; should really consider finger length)
00589     radius = ShapeRootTypeConst(curReq->object, CylinderData)->getRadius();
00590   else if ( curReq->object->getType() == naughtDataType ) // subtract half the naught radius, like cylinders, (heuristic; should really consider finger length)
00591     radius = ShapeRootTypeConst(curReq->object, NaughtData)->getRadius();
00592   else if ( curReq->object->getType() == dominoDataType )
00593     radius = 30; // *** HACK FOR DOMINOES
00594   else if ( curReq->object->getType() == crossDataType )
00595     radius = ShapeRootTypeConst(curReq->object, CrossData)->getArmSemiLength();
00596   else if ( curReq->object->getType() == brickDataType ){
00597     cout << "Trying to approach a brick\n" ;
00598   }
00599   float gripperXDisp = kine->linkToBase(GripperFrameOffset).translation()[0];
00600   int const gripperCenterToPalm = 10; // fudge factor for 2SP gripper
00601   pilotreq.dx = dist - gripperXDisp - radius + gripperCenterToPalm;
00602   pilotreq.forwardSpeed = 100;
00603 #else
00604   cancelThisRequest();
00605 #endif
00606 }
00607 
00608 void Grasper::FingersApproach::doStart() {
00609   // Should calculate finger distance based on object width.
00610 #if defined(TGT_IS_CALLIOPE2)
00611   if ( curReq->object->getType() == dominoDataType  || curReq->object->getType() == crossDataType )
00612     getMC()->openGripper(0.55);
00613   else
00614     getMC()->openGripper(0.8);    //Changed as of 4-7-14
00615   //getMC()->requestGripperLoad(0);
00616 #elif defined(TGT_IS_CALLIOPE3)
00617   getMC()->openGripper(0.5); // just a guess for now
00618 #elif defined(TGT_IS_CALLIOPE5)
00619   getMC()->openGripper(0.8f);
00620 #else
00621   std::cout << "Error: Grasper::FingersApproach undefined for this robot model." << std::endl;
00622 #endif
00623 }
00624 //------------------------------Verify methods-------------------
00625 
00626 void Grasper::Verify::VerifyDispatch::doStart() {
00627   postStateSignal<GrasperRequest::GrasperVerifyStrategy_t>(curReq->verifyStrategy);
00628 }
00629 
00630 void Grasper::Verify::GetAprilTag::doStart() {
00631   mapreq.setAprilTagFamily();
00632 }
00633 
00634 void Grasper::Verify::CheckAprilTag::doStart() {
00635     Point camcenter(VRmixin::camSkS.getWidth()/2, VRmixin::camSkS.getHeight()/2);
00636     float matchTolerance = 0.5;  // tag distance from center must be < 50% of camera width
00637     NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(VRmixin::camShS));
00638     SHAPEVEC_ITERATE(tags, AprilTagData, tag) {
00639       float offcenter = (tag->getCentroid() - camcenter).xyNorm();
00640       if ( offcenter/VRmixin::camSkS.getWidth() < matchTolerance ) {
00641         postParentCompletion();
00642         return;
00643       }
00644     } END_ITERATE;
00645     std::cout << "Grasper failed to find AprilTag to verify grasp object." << std::endl;
00646     postParentSignal<GraspError>(GrasperRequest::badGrasp);
00647 }
00648 
00649 void Grasper::Verify::GetDomino::doStart() {
00650   mapreq.addAttributes(curReq->object);
00651 }
00652 
00653 void Grasper::Verify::GetCross::doStart() {
00654   mapreq.addAttributes(curReq->object);
00655 }
00656 
00657 void Grasper::Verify::CheckDomino::doStart() {
00658   fmat::Column<3> gripperPos;
00659 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00660   gripperPos = kine->linkToBase(GripperFrameOffset).translation();
00661 #endif
00662   NEW_SHAPEVEC(dominoes, DominoData, select_type<DominoData>(VRmixin::localShS));
00663   // ******* SHOULD USE DOMINO GRASP POINT, NOT CENTROID *********
00664   SHAPEVEC_ITERATE(dominoes, DominoData, domino) {
00665     Point targPoint;
00666     AngTwoPi orient;
00667     domino->computeGraspPoint((DominoData::ObjectFeature)curReq->objectFeature, targPoint, orient);
00668     fmat::Column<3> diff = targPoint.getCoords() - gripperPos;
00669     cout << "*** Verify domino grasp point at " << targPoint
00670          << ", gripper at " << gripperPos << "  diff=" << diff << endl;
00671     if ( diff[0] > -25.0 && diff[0] < 0.75*domino->getLength() && fabs(diff[1]) < domino->getWidth() ) {
00672       postParentCompletion();
00673       return;
00674     }
00675   } END_ITERATE;
00676   if ( dominoes.empty() )
00677     cout << "*** Couldn't find a domino in the gripper." << endl;
00678   else
00679     cout << "*** Domino found but not within gripper." << endl;
00680   postParentSignal<GraspError>(GrasperRequest::badGrasp);
00681 }
00682 
00683 void Grasper::Verify::CheckCross::doStart() {
00684   fmat::Column<3> gripperPos;
00685 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00686   gripperPos = kine->linkToBase(GripperFrameOffset).translation();
00687 #endif
00688   NEW_SHAPEVEC(crosses, CrossData, select_type<CrossData>(VRmixin::localShS));
00689   // ******* SHOULD USE CROSS ARM GRASP POINT, NOT CENTROID *********
00690   SHAPEVEC_ITERATE(crosses, CrossData, cross) {
00691     Point targPoint;
00692     AngTwoPi orient;
00693     std::vector<Point> graspPoints = cross->computeGraspPoints();
00694     for (unsigned int i = 0; i < 4; i++){
00695       targPoint = graspPoints[i];
00696       orient = (targPoint - curReq->object->getCentroid()).atanYX();
00697       fmat::Column<3> diff = targPoint.getCoords() - gripperPos;
00698       cout << "*** Verify cross grasp point at " << targPoint
00699            << ", gripper at " << gripperPos << "  diff=" << diff << endl;
00700       if ( diff[0] > -25.0 && diff[0] < 0.75*cross->getArmSemiLength() && fabs(diff[1]) < cross->getArmWidth() ) {
00701         postParentCompletion();
00702         return;
00703       }
00704     }
00705   } END_ITERATE;
00706   if ( crosses.empty() )
00707     cout << "*** Couldn't find a cross in the gripper." << endl;
00708   else
00709     cout << "*** Cross found but not within gripper." << endl;
00710   postParentSignal<GraspError>(GrasperRequest::badGrasp);
00711 }
00712 
00713 void Grasper::Verify::CheckGripperLoad::doStart() {
00714   if (getAncestor<Verify>()->postGrasp == false) {
00715     //Note: Please update localspace here.
00716     postParentCompletion(); //If we're calling this before we've tried to grasp an object,
00717     return;                 // we should exit the node, because we have no load to check
00718   } 
00719 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00720   if(state->sensors[GPSXOffset] != 0.0){
00721     //If this block is true, we're in Mirage and need to ignore this check
00722     postParentCompletion();
00723     return;
00724   } 
00725 #endif
00726 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00727   std::cout << "*** CheckGripperLoad: pidduty=" << int(state->pidduties[GripperOffset]*1023)
00728             << "  gripPressure=" << curReq->gripPressure << std::endl;
00729   if (int(state->pidduties[GripperOffset]*1023) > curReq->gripPressure) { // getMC()->getDesiredLoad()
00730     //If we are supposed to have something in our arms, and we have an invalid load, post failure
00731     postParentSignal<GraspError>(GrasperRequest::badGrasp);
00732     return;
00733   }
00734 #endif
00735 
00736   // If the grasp was valid, post completion
00737   postParentCompletion();
00738 }
00739 
00740 void Grasper::Verify::CheckUserVerify::doStart() {
00741   if ( (*curReq->verifyGraspFunction)(curReq->object) )
00742     postParentCompletion();
00743   else
00744     postParentSignal<GraspError>(GrasperRequest::badGrasp);
00745 }
00746 
00747 //-------------------------------------------------------------------
00748 
00749 void Grasper::ArmGrasp::doStart() {
00750   // should compute gripper position based on object widths
00751   getMC()->setGripperSpeed(0.5);
00752   getMC()->requestGripperLoad(curReq->gripPressure);  //Changed as of 4-7-2014 to the new method in ArmMC
00753 }
00754 
00755 void Grasper::ArmPulse::doStart() {
00756   if ( activate )
00757     getMC()->setGripperPulse(2000,125);
00758   else
00759     getMC()->clearGripperPulse();
00760   postStateCompletion();
00761 }
00762 
00763 void Grasper::ArmNudge::doStart() {
00764 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00765   fmat::Column<3> gpos = kine->getPosition(GripperFrameOffset);
00766   fmat::Column<3> offset = fmat::pack(0, -100, -30);  // lower the gripper so we can see the AprilTag
00767   fmat::Column<3> newpos = gpos + offset;
00768   PostureEngine pe;
00769   pe.solveLinkPosition(newpos, GripperFrameOffset, fmat::ZERO3);
00770   getMC()->advanceTime(1000);
00771   getMC()->setOutputCmd(ArmBaseOffset, pe.getOutputCmd(ArmBaseOffset));
00772   getMC()->setOutputCmd(ArmShoulderOffset, pe.getOutputCmd(ArmShoulderOffset));
00773 #endif
00774 }
00775 
00776 void Grasper::ArmRaise::doStart() {
00777 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00778   fmat::Column<3> gpos = kine->getPosition(GripperFrameOffset);
00779   fmat::Column<3> offset = fmat::pack(0, 0, 80);  // raise the gripper so we can transport the object
00780   fmat::Column<3> newpos = gpos + offset;
00781   PostureEngine pe;
00782   pe.solveLinkPosition(newpos, GripperFrameOffset, fmat::ZERO3);
00783   getMC()->advanceTime(1000);
00784   getMC()->setOutputCmd(ArmBaseOffset, pe.getOutputCmd(ArmBaseOffset));
00785   getMC()->setOutputCmd(ArmShoulderOffset, pe.getOutputCmd(ArmShoulderOffset));
00786 
00787   getMC()->advanceTime(2000);   //ArmBaseOffset, ArmShoulderOffset
00788   getMC()->setOutputCmd(ArmBaseOffset, pe.getOutputCmd(ArmBaseOffset));
00789   getMC()->setOutputCmd(ArmShoulderOffset, pe.getOutputCmd(ArmShoulderOffset));
00790 #endif
00791 }
00792 
00793 void Grasper::PlanBodyTransport::doStart() {
00794   if ( ! curReq->allowBodyMotion ) {
00795     postStateFailure();
00796     return;
00797   }
00798   NEW_SHAPE(transportPose, AgentData, new AgentData(VRmixin::theAgent.getData()));
00799 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00800   const float radius = 1;
00801 #elif defined(TGT_CALLIOPE5)
00802   const float radius = 540;
00803 #else
00804   const float radius = 1;
00805 #endif
00806 #ifdef TGT_HAS_ARMS
00807   fmat::Column<3> baseOffset = kine->getPosition(GripperFrameOffset);
00808 #else
00809   fmat::Column<3> baseOffset = fmat::ZERO3;
00810 #endif
00811   curReq->targetLocation->setObstacle(false);
00812   AngTwoPi orient = std::numeric_limits<float>::quiet_NaN();
00813   GenericRRTBase::PlannerResult2D result = 
00814     VRmixin::grasper->planBodyPath(curReq->targetLocation->getCentroid(), orient, baseOffset, transportPose, radius, true);
00815   if ( result.code != GenericRRTBase::SUCCESS ) {
00816     switch ( result.code ) {
00817     case GenericRRTBase::START_COLLIDES:
00818       std::cout << "Grasper transport planning failed: start state " << result.movingObstacle->toString()
00819     << " is in collision with " << result.collidingObstacle->toString() << ".\n";
00820       break;
00821     case GenericRRTBase::END_COLLIDES:
00822       std::cout << "Grasper transport planning failed: pose at " << curReq->object
00823     << " collides with " << result.collidingObstacle->toString() << ".\n";
00824       break;
00825     case GenericRRTBase::MAX_ITER:
00826       std::cout << "Grasper transport planning failed: no path found after "
00827     << curReq->pilotreq.maxRRTIterations << " RRT iterations.\n";
00828       break;
00829     default: break;
00830     }
00831     transportPose.deleteShape();
00832     postStateSignal<GraspError>(GrasperRequest::noGraspPath);
00833     return;
00834   }
00835   curReq->transportPose = transportPose;
00836   postStateCompletion();
00837 }
00838 
00839 void Grasper::DoBodyTransport::doStart() {
00840   pilotreq.turnSpeed = 0.25;
00841   pilotreq.targetShape = curReq->transportPose;
00842   pilotreq.targetHeading = curReq->transportPose->getOrientation();
00843   pilotreq.allowBackwardMotion = false; // doesn't handle baseOffset properly?  check this.
00844 }
00845 
00846 void Grasper::PlanArmDeliver::doStart() {
00847 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00848   //Changed as of 4-28-2014
00849   
00850   //Here, we want to lower our object to the ground before we release
00851   //Like in PlanArmApproach...
00852 
00853   // Sets shoulder position based on the centroid of the target shape -- 
00854   // we usually want to be at 3/4 of the height of the target shape.
00855   fmat::Column<3> target = curReq->object->getCentroid().getCoords();
00856   if ( curReq->object->getType() == cylinderDataType ) // grasp cylinders at 75% of their height
00857     target[2] += ShapeRootTypeConst(curReq->object, CylinderData)->getHeight() * 0.25;
00858   else if ( curReq->object->getType() == naughtDataType ) // grasp naughts, like cylinders, at 75% of their height
00859     target[2] += ShapeRootTypeConst(curReq->object, NaughtData)->getHeight() * 0.25;
00860   else if ( curReq->object->getType() == brickDataType || // If the object to grasp is a brick-type object, like a domino...
00861             curReq->object->getType() == dominoDataType  || curReq->object->getType() == crossDataType )
00862     target[2] += ShapeRootTypeConst(curReq->object, BrickData)->getCentroid().coordZ() * 0.5;
00863   // cout << "PlanReachArm: target at: " << target << endl; 
00864   PostureEngine pe;
00865   pe.solveLinkPosition(target, GripperFrameOffset, fmat::ZERO3);
00866   //cout << "PlanReachArm: GripperFrame is at " << pe.getPosition(GripperFrameOffset) << endl;
00867   NodeValue_t armpoint;
00868   armpoint[0] = 0;  // arm base
00869   armpoint[1] = pe.getOutputCmd(ArmShoulderOffset).value;
00870   //This time, however, we set the deliver path instead of the approach path
00871   curReq->deliverPath.push_back(armpoint);
00872   
00873   postStateCompletion();
00874 #elif defined(TGT_IS_CALLIOPE5)
00875   // plan arm motion to drop off the object
00876 #endif
00877 }
00878 
00879 void Grasper::ReleaseArm::doStart() {
00880     getMC()->openGripper(0.8);
00881 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00882     getMC()->setJointValue(ArmShoulderOffset-ArmOffset, 0);
00883 #endif
00884   // change the object's location to the gripper's location
00885 #ifdef TGT_HAS_ARMS
00886   fmat::Column<3> gripperPos = VRmixin::mapBuilder->localToWorldMatrix * 
00887     kine->baseToLocal() * kine->linkToBase(GripperFrameOffset).translation();
00888 #else
00889   fmat::Column<3> gripperPos = VRmixin::theAgent->getCentroid().coords;
00890 #endif
00891   Point objPos = curReq->object->getCentroid();
00892   objPos.setCoords(gripperPos[0], gripperPos[1], objPos.coordZ());
00893   curReq->object->setPosition(objPos);
00894 }
00895 
00896 void Grasper::DoWithdraw::doStart() {
00897   pilotreq.dx = -100;
00898 }
00899 
00900 
00901 //================ OLD STUFF FROM HANDEYE ================
00902 
00903 void Grasper::PathPlanConstrained::doStart() {
00904   std::vector<NodeValue_t> endStates;
00905   
00906   if (!curReq->targetLocation.isValid()) {
00907     postStateSignal<GraspError>(GrasperRequest::someError);
00908     return;
00909   }
00910   
00911   IKSolver::Point toPt(curReq->object->getCentroid().getCoords());
00912   VRmixin::grasper->computeGoalStates(toPt, curReq->gripperAngleRangesX, curReq->gripperAngleRangesY, curReq->gripperAngleRangesZ,
00913                                       curReq->angleResolution, endStates, IKSolver::Point(fmat::ZERO3));
00914   if (endStates.empty()) {
00915     std::cout << "PLAN - pickUpUnreachable" << std::endl;
00916     postStateSignal<GraspError>(GrasperRequest::pickUpUnreachable);
00917     return;
00918   }
00919   
00920   NodeValue_t startSt;
00921   if (curReq->approachPath.empty())
00922     Grasper::getCurrentState(startSt);
00923   else
00924     startSt = curReq->approachPath.back();
00925   
00926   GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
00927   ArmPlanner planner(VRmixin::worldShS, worldBounds, curReq->rrtInflation,
00928          curReq->effectorOffset, curReq->predicate);
00929   
00930   std::vector<NodeType_t> *treeStartResult = NULL;
00931   std::vector<NodeType_t> *treeEndResult = NULL;
00932   fmat::Transform t;
00933   t.translation() = VRmixin::theAgent->getCentroid().coords;
00934   t.rotation() = fmat::rotationZ(VRmixin::theAgent->getOrientation());
00935   ArmPlanner::PlannerResult result =
00936     planner.planPath(startSt, endStates.front(), curReq->rrtInterpolationStep,
00937          t, curReq->rrtMaxIterations,
00938          &(curReq->approachPath), treeStartResult, treeEndResult);
00939   
00940   switch ( result.code ) {
00941     case GenericRRTBase::SUCCESS:
00942       std::cout << "Plan Constrained succeeded" << std::endl;
00943       postStateCompletion();
00944       return;
00945     case GenericRRTBase::START_COLLIDES:
00946       std::cout << "Navigation path planning failed: start state is in collision.\n";
00947       break;
00948     case GenericRRTBase::END_COLLIDES:
00949       std::cout << "Navigation path planning failed: end state is in collision.\n";
00950       break;
00951     case GenericRRTBase::MAX_ITER:
00952       std::cout << "Navigation path planning failed: too many iterations.\n";
00953       break;
00954   }
00955   
00956   if (result.code == GenericRRTBase::START_COLLIDES || result.code == GenericRRTBase::END_COLLIDES) {
00957     std::cout << "Obstacle: " << std::endl << "=================" << std::endl << result.movingObstacle->toString() << std::endl;
00958     std::cout << "collided with: " <<std::endl << "=================" << std::endl << result.collidingObstacle->toString() << std::endl;
00959     std::cout << "=================" << std::endl;
00960   }
00961   
00962   // Display the tree if requested
00963   if ( curReq->displayTree && result.code != GenericRRTBase::START_COLLIDES && GenericRRTBase::END_COLLIDES ) {
00964     NEW_SHAPE(plannerTrees, GraphicsData, new GraphicsData(VRmixin::worldShS));
00965     planner.plotTree(*treeStartResult, plannerTrees, rgb(0,0,0));
00966     planner.plotTree(*treeEndResult, plannerTrees, rgb(0,192,0));
00967     delete treeStartResult;
00968     delete treeEndResult;
00969   }
00970   
00971   // Display the path if requested
00972   if ( curReq->displayPath && result.code == GenericRRTBase::SUCCESS ) {
00973     NEW_SHAPE(plannedPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
00974     planner.plotPath(curReq->approachPath, plannedPath, rgb(0,0,255));
00975   } 
00976   
00977   if (result.code == GenericRRTBase::SUCCESS)
00978     postStateCompletion();
00979   else {
00980     std::cout << "PLANNER FAILURE - CONSTRAINED" << std::endl;
00981     postStateSignal<GraspError>(GrasperRequest::noGraspPath);
00982   }
00983 }
00984 
00985 void Grasper::PathPlanToRest::doStart() {
00986   NodeValue_t startSt;
00987   NodeValue_t endSt;
00988   
00989   switch(curReq->restType) {
00990     case GrasperRequest::stationary:
00991       postStateCompletion();
00992       return;
00993     case GrasperRequest::settleArm:
00994     case GrasperRequest::settleBodyAndArm:
00995       for(unsigned i = 0; i < NumArmJoints; i++)
00996         endSt[i] = curReq->armRestState[i];
00997       break;
00998   }
00999   
01000   if (!curReq->deliverPath.empty())
01001     startSt = curReq->deliverPath.back();
01002   else if (!curReq->approachPath.empty())
01003     startSt = curReq->approachPath.back();
01004   else
01005     Grasper::getCurrentState(startSt);
01006   
01007   GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
01008   ArmPlanner planner(VRmixin::worldShS, worldBounds, curReq->rrtInflation, curReq->effectorOffset, NULL);
01009   
01010   std::vector<NodeType_t> *treeStartResult = NULL;
01011   std::vector<NodeType_t> *treeEndResult = NULL;
01012   fmat::Transform t;
01013   t.translation() = VRmixin::theAgent->getCentroid().coords;
01014   t.rotation() = fmat::rotationZ(VRmixin::theAgent->getOrientation());
01015   ArmPlanner::PlannerResult result =
01016     planner.planPath(startSt, endSt, curReq->rrtInterpolationStep,
01017          t, curReq->rrtMaxIterations,
01018          &(curReq->releasePath), treeStartResult, treeEndResult);
01019   switch ( result.code ) {
01020     case GenericRRTBase::SUCCESS:
01021       std::cout << "Plan disengage succeeded" << std::endl;
01022       postStateCompletion();
01023       return;
01024     case GenericRRTBase::START_COLLIDES:
01025       std::cout << "Navigation path planning failed: start state is in collision.\n";
01026       break;
01027     case GenericRRTBase::END_COLLIDES:
01028       std::cout << "Navigation path planning failed: end state is in collision.\n";
01029       break;
01030     case GenericRRTBase::MAX_ITER:
01031       std::cout << "Navigation path planning failed: too many iterations.\n";
01032       break;
01033   }
01034   
01035   if (result.code == GenericRRTBase::START_COLLIDES || result.code == GenericRRTBase::END_COLLIDES) {
01036     std::cout << "Obstacle: " << std::endl << "=================" << std::endl << result.movingObstacle->toString() << std::endl;
01037     std::cout << "collided with: " <<std::endl << "=================" << std::endl << result.collidingObstacle->toString() << std::endl;
01038     std::cout << "=================" << std::endl;
01039   }
01040   
01041   // Display the tree if requested
01042   if ( curReq->displayTree && result.code != GenericRRTBase::START_COLLIDES && GenericRRTBase::END_COLLIDES ) {
01043     NEW_SHAPE(plannerTrees, GraphicsData, new GraphicsData(VRmixin::worldShS));
01044     planner.plotTree(*treeStartResult, plannerTrees, rgb(0,0,0));
01045     planner.plotTree(*treeEndResult, plannerTrees, rgb(0,192,0));
01046     delete treeStartResult;
01047     delete treeEndResult;
01048   }
01049   
01050   // Display the path if requested
01051   if ( curReq->displayPath && result.code == GenericRRTBase::SUCCESS ) {
01052     NEW_SHAPE(plannedPath, GraphicsData, new GraphicsData(VRmixin::worldShS));
01053     planner.plotPath(curReq->releasePath, plannedPath, rgb(0,0,255));
01054   } 
01055   
01056   if (result.code == GenericRRTBase::SUCCESS)
01057     postStateCompletion();
01058   else {
01059     std::cout << "PLANNER FAILURE - ARM_REST" << std::endl;
01060     postStateSignal<GraspError>(GrasperRequest::noGraspPath);
01061   }
01062 }
01063 
01064 void Grasper::MoveArm::doStart() {
01065   switch(pa) {
01066     case GrasperRequest::doApproach:
01067       if ( curReq->verbosity & GVexecutePath )
01068         std::cout << "Grasper: executing arm reach path" << std::endl;
01069       executeMove(curReq->approachPath);
01070       break;
01071     case GrasperRequest::doDeliver:
01072       if ( curReq->verbosity & GVexecutePath )
01073         std::cout << "Grasper: moving along constrained path" << std::endl;
01074       executeMove(curReq->deliverPath);
01075       break;
01076     case GrasperRequest::doRelease:
01077       if ( curReq->verbosity & GVexecutePath )
01078         std::cout << "Grasper: executing arm release path" << std::endl;
01079       executeMove(curReq->releasePath);
01080       break;
01081     case GrasperRequest::noPath:
01082       break;
01083   }
01084 }
01085 
01086 void Grasper::MoveArm::executeMove(const std::vector<NodeValue_t>& path) {
01087 #ifdef TGT_HAS_ARMS
01088   MMAccessor<DynamicMotionSequence> move_acc = getMC();
01089   move_acc->clear();
01090   move_acc->setTime(1000);
01091   for(unsigned int p = 0; p < path.size(); p++) {
01092     for(unsigned int j = 0; j < numPlannerJoints; j++) {
01093       if (kine->getKinematicJoint(ArmOffset+j)->isMobile() == false)
01094         continue;
01095       move_acc->setOutputCmd(ArmOffset+j, (float)path[p][j]);
01096       //cout << "MoveArm path[" << p << "][" << j << "] = " << path[p][j] << endl;
01097     }
01098     if (p < path.size()-1) {
01099       NodeValue_t confDif;
01100       for(unsigned i = 0; i < numPlannerJoints; i++)
01101         confDif[i] = path[p][i] - path[p+1][i];
01102       // std::cout << "advTime = " << advTime(confDif) << std::endl;
01103       move_acc->advanceTime( advTime(confDif) );
01104     }
01105   }
01106   
01107   move_acc->play();
01108 #endif
01109 }
01110 
01111 void Grasper::SetJoint::moveJoint(float value) {
01112   unsigned int offset = 10;
01113   if(jointName == "ARM:shldr")
01114     offset = 0;
01115   else if(jointName == "ARM:elbow")
01116     offset = 1;
01117   else if(jointName == "ARM:wristYaw")
01118     offset = 2;
01119   else if(RobotInfo::NumArmJoints > 3) {
01120     if(jointName == "ARM:wristPitch")
01121       offset = 3;
01122     else if(jointName == "ARM:wristRoll" || jointName == "ARM:gripperLeft")
01123       offset = 4;
01124     else if(jointName == "ARM:gripper" || jointName == "ARM:gripperRight")
01125       offset = 5;
01126     else {
01127       postStateCompletion();
01128       return;
01129     }
01130   }
01131   else {
01132     postStateCompletion();
01133     return;
01134   }
01135   if ( (curReq ? curReq->verbosity : verbosity) & GVsetJoint )
01136     cout << "Setting " << jointName << " to " << value << endl;
01137   getMC()->takeSnapshot();
01138   getMC()->setMaxSpeed(offset, speed);   
01139   getMC()->setJointValue(offset, value);
01140 }
01141 
01142 void Grasper::getCurrentState(NodeValue_t &current, KinematicJoint* endEffector) {
01143   const KinematicJoint* joint = !endEffector ? kine->getKinematicJoint(curReq->effectorOffset) : endEffector;
01144   for (unsigned int j = numPlannerJoints; j > 0; j--) {
01145     while (joint->isMobile() == false) joint = joint->getParent();
01146     current[j-1] = joint->getQ();
01147     joint = joint->getParent();
01148   }
01149 }
01150 
01151 void Grasper::computeGoalStates(IKSolver::Point &toPt,
01152         std::vector<std::pair<float, float> > &rangesX,
01153                                 std::vector<std::pair<float, float> > &rangesY,
01154         std::vector<std::pair<float, float> > &rangesZ,
01155                                 float resolution, std::vector<NodeValue_t>& goals, const IKSolver::Point &offset) {
01156   // std::cout << "computGoalStates " << toPt << " resolution=" << resolution << std::endl;
01157   KinematicJoint* effector = kine->getKinematicJoint(curReq->effectorOffset)->cloneBranch();
01158   goals.clear();
01159   if (resolution == 0)
01160     resolution = M_PI/2;
01161   
01162   if (rangesX.empty())
01163     rangesX.push_back(pair<float, float>(0, 2*M_PI));
01164   if (rangesY.empty())
01165     rangesY.push_back(pair<float, float>(0, 2*M_PI));
01166   if (rangesZ.empty())
01167     rangesZ.push_back(pair<float, float>(0, 2*M_PI));
01168   
01169   int factorX = 0;
01170   bool keepGoing;
01171   do {
01172     keepGoing = false;
01173     for(unsigned rX = 0; rX < rangesX.size(); rX++) {
01174       float thetaX = resolution * factorX;
01175       float midX = (rangesX[rX].first + rangesX[rX].second)/2;
01176       if (thetaX > rangesX[rX].second - midX)
01177         continue;
01178 
01179       int factorY = 0;
01180       for (unsigned rY = 0; rY < rangesY.size(); rY++) {
01181         float thetaY = resolution * factorY;
01182         float midY = (rangesY[rY].first + rangesY[rY].second)/2;
01183         if (thetaY > rangesY[rY].second - midY)
01184           continue;
01185 
01186         int factorZ = 0;
01187         for (unsigned rZ = 0; rZ < rangesZ.size(); rZ++) {
01188           float thetaZ = resolution * factorZ;
01189           float midZ = (rangesZ[rZ].first + rangesZ[rZ].second)/2;
01190           if (thetaZ > rangesZ[rZ].second - midZ)
01191             continue;
01192           
01193           keepGoing = true;
01194           fmat::Quaternion oriPlus = fmat::Quaternion::fromMatrix(fmat::rotationZ(midZ + thetaZ) * 
01195                   fmat::rotationY(midY + thetaY) *
01196                   fmat::rotationX(midX + thetaX));
01197     checkGoalCandidate(offset, IKSolver::Rotation(oriPlus), effector, toPt, goals);
01198     if ( factorX > 0 ||  factorY > 0 || factorZ > 0 ) {
01199       fmat::Quaternion oriMinus = fmat::Quaternion::fromMatrix(fmat::rotationZ(midZ - thetaZ) *
01200                      fmat::rotationY(midY - thetaY) * 
01201                      fmat::rotationX(midX - thetaX));
01202       checkGoalCandidate(offset, IKSolver::Rotation(oriMinus), effector, toPt, goals);
01203     }
01204 
01205           if ( goals.size() >= curReq->maxNumberOfAngles )
01206             keepGoing = false;
01207           factorZ++;
01208         } // Z
01209         factorY++;
01210       } // Y
01211       factorX++;
01212     } // X
01213   } while (keepGoing);
01214   if ( curReq->verbosity & GVcomputeGoals )
01215     std::cout << "Grasper found " << goals.size() << " potential goal states." << std::endl;
01216 }
01217 
01218 void Grasper::checkGoalCandidate(const IKSolver::Point &offset, const IKSolver::Rotation &ori, 
01219          KinematicJoint *effector, const IKSolver::Point &position, std::vector<NodeValue_t>& goals) {
01220   IKSolver& solver = effector->getIK();
01221   float const positionMostImportant = 1.0f;
01222   float const orientationLeastImportant = 0.5f;
01223   bool reached = solver.solve(offset, IKSolver::Rotation(fmat::Quaternion::aboutX(-M_PI/2)), *effector,
01224             position, positionMostImportant, IKSolver::Parallel(0,0,1)/*ori*/, orientationLeastImportant);
01225   std::cout << "checkGoalCandidate: reached = " << reached << std::endl;
01226   if (true||reached) {
01227     NodeValue_t endSt;
01228     KinematicJoint *joint = effector;
01229     for (int j = numPlannerJoints-1; j >= 0; j--) {
01230       while (joint->isMobile() == false)
01231   joint = joint->getParent();
01232       endSt[j] = joint->getQ();
01233       joint = joint->getParent();
01234     }
01235     switch ( curReq->graspStrategy ) {
01236     case GrasperRequest::unconstrainedGrasp:
01237       break;
01238     case GrasperRequest::sideGrasp:
01239 #ifdef TGT_IS_CALLIOPE5
01240       //      endSt[WristRotateOffset-ArmOffset] = M_PI/2 * (endSt[WristRotateOffset-ArmOffset] > 0 ? 1 : -1 );
01241       std:: cout << "wristrot " << WristRotateOffset << " value " << endSt[WristRotateOffset-ArmOffset] << std::endl;
01242 #endif
01243       break;
01244     case GrasperRequest::overheadGrasp:
01245 #ifdef TGT_IS_CALLIOPE5
01246       // Set wrist joint to complement elbow and shoulder joints,
01247       // making fingers perpendicular to the ground to the extent this
01248       // is compatible with the joint limits.
01249       // *** THIS IS WRONG *** It changes the position of GripperFrame; not a legal solution.
01250       float wristDesired = -M_PI - endSt[ArmShoulderOffset-ArmOffset] - endSt[ArmElbowOffset-ArmOffset];
01251       float wristAchievable = std::max(wristDesired, outputRanges[ArmWristOffset][0]);
01252       endSt[ArmWristOffset-ArmOffset] = wristAchievable;
01253 #endif
01254       break;
01255     }
01256     goals.push_back(endSt);
01257   }
01258 }
01259 
01260 unsigned int Grasper::executeRequest(const GrasperRequest& req, BehaviorBase* requestingBehavior) {
01261   VRmixin::requireCrew("Grasper");
01262   GrasperRequest *newReq = new GrasperRequest(req);
01263   newReq->requestingBehavior = requestingBehavior;
01264   unsigned int reqID = ++idCounter;
01265   newReq->requestID = reqID;
01266   GraspError e = newReq->validateRequest();
01267   if ( e != GrasperRequest::noError ) {
01268     std::cout << "*** Grasper received invalid grasp request, id=" << reqID << std::endl;
01269     GrasperEvent myEvent(false, newReq->requestType, e, (size_t)newReq->requestingBehavior);
01270     erouter->postEvent(myEvent);
01271   }
01272   else {
01273     requests.push(newReq);
01274     executeRequest();
01275   }
01276   return reqID;
01277 }
01278 
01279 void Grasper::executeRequest() {
01280   if ( curReq != NULL )
01281     return;
01282   else if ( requests.empty() )
01283     return;
01284   curReq = requests.front();
01285   requests.pop();
01286   
01287   curReq->verbosity = (verbosity & ~curReq->clearVerbosity) | curReq->setVerbosity;
01288   if ( curReq->verbosity & GVexecuteRequest ) {
01289     std::cout << "Grasper now executing request " << curReq->requestID;
01290     if ( curReq->object.isValid() )
01291       std::cout << " object=" << curReq->object;
01292     if ( curReq->targetLocation.isValid() )
01293       std::cout << " targetLocation=" << curReq->targetLocation
01294                 << " : " << curReq->targetLocation->getCentroid();
01295     std::cout << std::endl;
01296   }
01297   /*
01298   if (curReq->object.isValid() && curReq->object->getSpace().getRefFrameType() == VRmixin::worldShS.getRefFrameType())
01299     curReq->object = VRmixin::mapBuilder->importWorldToLocal(curReq->object);
01300   
01301   if (curReq->targetLocation.isValid() && curReq->targetLocation->getSpace().getRefFrameType() == VRmixin::worldShS.getRefFrameType())
01302     curReq->targetLocation = VRmixin::mapBuilder->importWorldToLocal(curReq->targetLocation);
01303   */
01304   {
01305     GET_SHAPE(approachPose, AgentData, VRmixin::worldShS);
01306     GET_SHAPE(transportPose, AgentData, VRmixin::worldShS);
01307     GET_SHAPE(withdrawPose, AgentData, VRmixin::worldShS);
01308     GET_SHAPE(plannerTrees, GraphicsData, VRmixin::worldShS);
01309     approachPose.deleteShape();
01310     transportPose.deleteShape();
01311     withdrawPose.deleteShape();
01312     plannerTrees.deleteShape();
01313   }
01314 
01315   dispatch();
01316 }
01317 
01318 void Grasper::dispatch() {
01319   startmain_->start();
01320   /*
01321   switch( curReq->requestType ) {
01322     case GrasperRequest::reach:
01323       reachRoot->start();
01324       break;
01325     case GrasperRequest::computeReach:
01326       if ( curReq->populateEventPathWith == GrasperRequest::noPath )
01327         curReq->populateEventPathWith = GrasperRequest::moveFree;
01328       computeReachRoot->start();
01329       break;
01330     case GrasperRequest::grasp:
01331       graspRoot->start();
01332       break;
01333     case GrasperRequest::checkGraspable:
01334       checkRoot->start();
01335       break;
01336     case GrasperRequest::release:
01337       releaseRoot->start();
01338       break;
01339     case GrasperRequest::touch:
01340       touchRoot->start();
01341       break;
01342     case GrasperRequest::moveTo:
01343       moveRoot->start();
01344       break;
01345     case GrasperRequest::checkMovable:
01346       checkRoot->start();
01347       break;
01348     case GrasperRequest::rest:
01349       restRoot->start();
01350       break;
01351     case GrasperRequest::checkRestable:
01352       checkRoot->start();
01353       break;
01354     case GrasperRequest::sweep:
01355       std::cout << "Grasper functionality " << curReq->requestType << " not implemented"<< std::endl;
01356       break;
01357     default:
01358       std::cout << "Unidentified requestType" << std::endl;
01359       break;
01360   }
01361   */
01362 }
01363 
01364 void Grasper::doStop() {
01365   std::cout << "Stopping Grasper sub-machines" << std::endl;
01366   startmain_->stop();
01367   motman->removeMotion(armId);
01368   armId = MotionManager::invalid_MC_ID;
01369 }
01370 
01371 #endif // TGT_HAS_ARMS

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