コード例 #1
0
/*! Attempts to move the element from its current pose to the new pose
  in \a newTran in a single step. If the final pose if collision-free
  it is done. If not, it interpolates to find the exact moment of contact.
  Returns \a false if the interpolation fails.
*/
bool WorldElement::jumpTo(transf newTran, CollisionReport *contactReport)
{
  int i, numCols;
  bool success;
  CollisionReport colReport;
  transf lastTran = getTran();
  if (setTran(newTran) == FAILURE) { return false; }
  //we are only interested in collisions involving this body
  std::vector<Body *> interestList;
  //a robot will place all of its links in here
  getBodyList(&interestList);
  contactReport->clear();
  while (1) {
    numCols = myWorld->getCollisionReport(&colReport, &interestList);
    if (!numCols) {
      return true;
    }

#ifdef GRASPITDBG
    for (i = 0; i < numCols; i++) {
      std::cerr << colReport[i].first->getName().latin1() << " -- "
                << colReport[i].second->getName().latin1() << std::endl;
    }
    DBGP("I am " << myName.latin1());
#endif

    success = interpolateTo(lastTran, getTran(), colReport);
    if (!success) {
      return false;
    }
    contactReport->clear();
    for (i = 0; i < numCols; i++) {
      if (myWorld->getDist(colReport[i].first, colReport[i].second) < Contact::THRESHOLD) {
        contactReport->push_back(colReport[i]);
      }
    }
  }
}
コード例 #2
0
ファイル: worldElement.cpp プロジェクト: CURG/graspit_handop
/*!
  Moves the element from its current pose to the new pose specified by \a tr.
  This motion is performed in several steps such that the translation
  between each step does not exceed \a translStepSize and the angle of
  rotation does not exceed \a rotStepSize (expressed in radians).  The
  intermediate poses are determined using linear interpolation for the
  translation and spherical linear interpolation for the rotation.  If
  a collision is encountered during the motion, the point of first contact
  is determined and the element is left in that position.  This function
  returns false if a collision was encountered (or contacts prevented the motion)
  or true if no collisions were encountered and the move was completed.
*/
bool
WorldElement::moveTo(transf &newTran,double translStepSize, double rotStepSize)
{
    bool moveFinished = false;
    transf origTran,nextTran,motion;
    Quaternion nextRotation;
    vec3 nextTranslation;
    double percentComplete,moveIncrement,translationLength;
    double angle;
    vec3 axis;
    bool success;

    CollisionReport contactReport;

    //DBGP("moveTo called");

    origTran = getTran();
    /*
    	std::cout << "WorldElement origTran: " << origTran.translation().x() << " " <<
    		origTran.translation().y() << " " <<
    		origTran.translation().z() << " " <<
    		origTran.rotation().w << " " <<
    		origTran.rotation().x << " " <<
    		origTran.rotation().y << " " <<
    		origTran.rotation().z << " " << "\n";
    */
    //calculate the difference
    translationLength = (newTran.translation() - origTran.translation()).len();
    nextRotation = newTran.rotation() * origTran.rotation().inverse();
    nextRotation.ToAngleAxis(angle,axis);

    moveIncrement = 1.0;
    if (translationLength != 0.0) {
        if (translStepSize == ONE_STEP)
            moveIncrement = 1.0;
        else
            moveIncrement = MIN(moveIncrement, translStepSize / translationLength);
    }
    if (angle != 0.0) {
        if (rotStepSize == ONE_STEP)
            moveIncrement = MIN(moveIncrement, 1.0);
        else
            moveIncrement = MIN(moveIncrement, rotStepSize / angle);
    }

    // check contacts
    nextTranslation = (1.0-moveIncrement)*origTran.translation() + moveIncrement*newTran.translation();
    nextRotation = Quaternion::Slerp(moveIncrement,origTran.rotation(), newTran.rotation());
    nextTran = transf(nextRotation,nextTranslation);
    motion = nextTran * getTran().inverse();

    if (contactsPreventMotion(motion)) {
        DBGP("contacts prevent motion")
        return false;
    }

    percentComplete = 0.0;
    while (!moveFinished) {
        percentComplete += moveIncrement;
        if (percentComplete >= 1.0) {
            percentComplete = 1.0;
            moveFinished = true;
        }

        nextTranslation = (1.0-percentComplete)*origTran.translation() + percentComplete*newTran.translation();
        nextRotation = Quaternion::Slerp(percentComplete,origTran.rotation(), newTran.rotation());
        nextTran = transf(nextRotation,nextTranslation);
        /*
        		std::cout << "moveTo NextTran: " << nextTran.translation().x() << " " <<
        			nextTran.translation().y() << " " <<
        			nextTran.translation().z() << " " <<
        			nextTran.rotation().w << " " <<
        			nextTran.rotation().x << " " <<
        			nextTran.rotation().y << " " <<
        			nextTran.rotation().z << " " << "\n";
        */
        success = jumpTo(nextTran, &contactReport);
        if (!success || contactReport.size() != 0) {
            moveFinished = true;
        }
    }

    if (!success) {
        DBGA("JumpTo error, stopping execution. Object " << myName.latin1() << " in thread "
             << getWorld()->getCollisionInterface()->getThreadId());
    } else {
        myWorld->findContacts(contactReport);
    }

    if (contactReport.size() != 0) return false;
    return true;
}