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
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
00029 Point agentpt = VRmixin::theAgent->getCentroid();
00030 Point vec = targetPt - agentpt;
00031 float dist = vec.xyNorm();
00032 GET_SHAPE(worldBounds, PolygonData, VRmixin::worldShS);
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
00043 if ( dist >= radius ) {
00044
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
00050
00051
00052
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
00059
00060 if (result.code != GenericRRTBase::SUCCESS) {
00061
00062
00063 if ( result.code == GenericRRTBase::END_COLLIDES ) {
00064 VRmixin::robotObstaclesPt = targetPt - Point(baseOffset,allocentric);
00065 VRmixin::robotObstaclesOri = AngTwoPi(0);
00066 } else {
00067
00068 VRmixin::robotObstaclesPt = agentpt;
00069 VRmixin::robotObstaclesOri = VRmixin::theAgent->getOrientation();
00070 }
00071 planner.addObstaclesToShapeSpace(VRmixin::worldShS);
00072 return result;
00073 }
00074
00075
00076 VRmixin::robotObstaclesPt = agentpt;
00077 VRmixin::robotObstaclesOri = VRmixin::theAgent->getOrientation();
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092 finalValue = pathResult.back();
00093 } else {
00094
00095
00096
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
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
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
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;
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);
00181
00182
00183 }
00184 if ( curReq->object->getType() == naughtDataType ) {
00185 baseOffset[0] += ShapeRootTypeConst(curReq->object, NaughtData)->getRadius() + bodyRadius;
00186 curReq->object->setObstacle(false);
00187
00188
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);
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
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);
00208 baseOffset = fmat::pack(bodyRadius*1.25, 0, 0);
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
00215 }
00216
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
00222 if ( result.code != GenericRRTBase::SUCCESS ) {
00223
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
00243 curReq->approachPose = approachPose;
00244 postStateCompletion();
00245 }
00246
00247 void Grasper::DoBodyApproach::doStart() {
00248 if ( ! curReq->approachPose.isValid() ) {
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);
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
00296 if ( curReq->object->getType() == cylinderDataType )
00297 target[2] += ShapeRootTypeConst(curReq->object, CylinderData)->getHeight() * 0.25;
00298 else if ( curReq->object->getType() == naughtDataType )
00299 target[2] += ShapeRootTypeConst(curReq->object, NaughtData)->getHeight() * 0.25;
00300 else if ( curReq->object->getType() == brickDataType )
00301 target[2] += ShapeRootTypeConst(curReq->object, BrickData)->getCentroid().coordZ() * 0.5;
00302
00303 PostureEngine pe;
00304 pe.solveLinkPosition(target, GripperFrameOffset, fmat::ZERO3);
00305
00306 NodeValue_t armpoint;
00307 armpoint[0] = 0;
00308 armpoint[1] = pe.getOutputCmd(ArmShoulderOffset).value;
00309 curReq->approachPath.push_back(armpoint);
00310 postStateCompletion();
00311 #elif defined(TGT_IS_CALLIOPE5)
00312
00313
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;
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
00357
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
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
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
00431
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
00450
00451
00452
00453
00454
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
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
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
00526
00527
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
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
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 )
00589 radius = ShapeRootTypeConst(curReq->object, CylinderData)->getRadius();
00590 else if ( curReq->object->getType() == naughtDataType )
00591 radius = ShapeRootTypeConst(curReq->object, NaughtData)->getRadius();
00592 else if ( curReq->object->getType() == dominoDataType )
00593 radius = 30;
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;
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
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);
00615
00616 #elif defined(TGT_IS_CALLIOPE3)
00617 getMC()->openGripper(0.5);
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
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;
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
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
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
00716 postParentCompletion();
00717 return;
00718 }
00719 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00720 if(state->sensors[GPSXOffset] != 0.0){
00721
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) {
00730
00731 postParentSignal<GraspError>(GrasperRequest::badGrasp);
00732 return;
00733 }
00734 #endif
00735
00736
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
00751 getMC()->setGripperSpeed(0.5);
00752 getMC()->requestGripperLoad(curReq->gripPressure);
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);
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);
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);
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;
00844 }
00845
00846 void Grasper::PlanArmDeliver::doStart() {
00847 #if defined(TGT_IS_CALLIOPE2) || defined(TGT_IS_CALLIOPE3)
00848
00849
00850
00851
00852
00853
00854
00855 fmat::Column<3> target = curReq->object->getCentroid().getCoords();
00856 if ( curReq->object->getType() == cylinderDataType )
00857 target[2] += ShapeRootTypeConst(curReq->object, CylinderData)->getHeight() * 0.25;
00858 else if ( curReq->object->getType() == naughtDataType )
00859 target[2] += ShapeRootTypeConst(curReq->object, NaughtData)->getHeight() * 0.25;
00860 else if ( curReq->object->getType() == brickDataType ||
00861 curReq->object->getType() == dominoDataType || curReq->object->getType() == crossDataType )
00862 target[2] += ShapeRootTypeConst(curReq->object, BrickData)->getCentroid().coordZ() * 0.5;
00863
00864 PostureEngine pe;
00865 pe.solveLinkPosition(target, GripperFrameOffset, fmat::ZERO3);
00866
00867 NodeValue_t armpoint;
00868 armpoint[0] = 0;
00869 armpoint[1] = pe.getOutputCmd(ArmShoulderOffset).value;
00870
00871 curReq->deliverPath.push_back(armpoint);
00872
00873 postStateCompletion();
00874 #elif defined(TGT_IS_CALLIOPE5)
00875
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
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
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
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
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
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
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
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
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 ¤t, 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
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 }
01209 factorY++;
01210 }
01211 factorX++;
01212 }
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), 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
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
01247
01248
01249
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
01299
01300
01301
01302
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
01322
01323
01324
01325
01326
01327
01328
01329
01330
01331
01332
01333
01334
01335
01336
01337
01338
01339
01340
01341
01342
01343
01344
01345
01346
01347
01348
01349
01350
01351
01352
01353
01354
01355
01356
01357
01358
01359
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