inter.cc

该文件是包含了机器人足球比赛中的整个系统的代码

#include "TheStrut.h"


// checks if we need to interpolate to a position or not (ie, are we close enough already ?)
bool TheStrut::ShouldInterpolate(bool includeHead) {
  bool shouldWeInterpolate = false;
  for (int i = 0; i < NUM_BODY_JOINTS; i++) {
    if (ABS(bodyJointPrevPoints[i]-bodyJointEndPoints[i]) >= interpolationError) {
      int requiredFrames = (long) (((double)bodyJointEndPoints[i]-(double)bodyJointPrevPoints[i])/((double)interpolationRate));
      if (requiredFrames > 1) shouldWeInterpolate = true;
    }
  }
  if (includeHead) {
    for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
      if (ABS(headJointPrevPoints[i]-headJointEndPoints[i]) >= interpolationError) {
        shouldWeInterpolate = true;
      }
    }
  }
  if (shouldWeInterpolate == false) {
    return false;
  }
  return true;
}

// Send actual interpolation commands to joints
void TheStrut::DoInterpolate() {
  int bodyRegionID = FindFreeBodyRegionID();
  int headRegionID = -1;
  if (interpolateHead)
    headRegionID=FindFreeHeadRegionID();

  if (bodyRegionID != -1 && ((headRegionID != -1) || (interpolateHead == false))) {
    bool stillInterpolating = false;

    long currentHeadPoints[NUM_HEAD_JOINTS];
    long currentBodyPoints[NUM_BODY_JOINTS];

    // Check if we're close enough to end position to stop interpolation
    // Also determines exactly how many joint frames are required to do the interpolation.
    // Could use ShouldInterpolate, but it doesn't do the second bit (determine number of frames)
    int numFrames = 1;
    for (int i = 0; i < NUM_BODY_JOINTS; i++) {
      OCommandData* jointData = bodyVec[bodyRegionID]->GetData(i);
      bodyValue[i] = (OJointCommandValue2*)jointData->value;

      currentBodyPoints[i] = bodyJointPrevPoints[i];

      int requiredFrames = (long) (((double)bodyJointEndPoints[i]-(double)currentBodyPoints[i])/((double)interpolationRate));
      if (requiredFrames > numFrames) {
        numFrames = requiredFrames;
      }

      if (ABS(currentBodyPoints[i]-bodyJointEndPoints[i]) >= interpolationError) {
        stillInterpolating = true;
      }
    }
    if (interpolateHead) {
      for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
        OCommandData* jointData = headVec[headRegionID]->GetData(i);
        headValue[i] = (OJointCommandValue2*)jointData->value;

        int requiredFrames = (long) (((double)headJointEndPoints[i]-(double)currentHeadPoints[i])/((double)interpolationRate));
        if (requiredFrames > numFrames) {
          numFrames = requiredFrames;
        }

        currentHeadPoints[i] = headJointPrevPoints[i];
        if (ABS(currentHeadPoints[i]-headJointEndPoints[i]) >= interpolationError) {
          stillInterpolating = true;
        }
      }
    }

    // apparently we're close enough to end interpolation. kill it!
    if (stillInterpolating == false || numFrames == 1) {
//    cout << "Cancelled interpolation 1 - " << stillInterpolating << ", nf - " << numFrames << endl << flush;

      FinishInterpolation();
      return;
    }

    // limit number of joint frames that we can send
    if (numFrames > maxInterFrames) numFrames = maxInterFrames;
    // set numframes info to joints.
    for (int i = 0; i < NUM_BODY_JOINTS; i++) {
      OCommandInfo* jointInfo = bodyVec[bodyRegionID]->GetInfo(i);
      jointInfo->numFrames = numFrames;
    }
    if (interpolateHead) {
      for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
        OCommandInfo* jointInfo = headVec[headRegionID]->GetInfo(i);
        jointInfo->numFrames = numFrames;
      }
    }


    // ok we're not close enough yet, so we have to interpolate for a bit.

    // minimum time step for an interpolation is now 0.008*numFrames seconds
    
    // also there's no real point in one joint reaching the end point faster, as we'll still have to
    // wait for the others... this means we need not recalculate delta for every frame.

    // after we send this joint data, we *may* have finished up interpolation. we might as well
    // check whether this is the case here to avoid any latency issues
    stillInterpolating = false;
 
    long jointPosition = 0;
    long delta = 0;
    for (int i = 0; i < NUM_BODY_JOINTS; i++) {
      // we calculate a delta, which is how far we'll move this joint each frame (frame = 0.08s)
      // jointposition holds the joint position as it is interpolated towards the end point.
      jointPosition = currentBodyPoints[i];
      delta = (long) (((double)bodyJointEndPoints[i]-(double)jointPosition)/(double)numFrames);

      // here we limit the delta value to keep it below our maximum angular velocity.
      if (delta > interpolationRate) delta = interpolationRate;
      if (delta < -interpolationRate) delta = -interpolationRate;
      for (int j = 0; j < numFrames; j++) {
        // increment current joint angle by delta
        jointPosition+=delta;
        // set joint position
        SetBodyJoint(&bodyValue[i][j].value, jointPosition, i);
      }
      // check if the last frame is still a long way from the destired angle... if so, we'll have to
      // interpolate again when we can send more joint data...
      if (ABS(jointPosition-bodyJointEndPoints[i]) >= interpolationError) stillInterpolating = true;
    }

    // as above, but optional - head may or may not be interpolated...
    if (interpolateHead) {
      for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
        jointPosition = currentHeadPoints[i];
        delta = (long) (((double)headJointEndPoints[i]-(double)jointPosition)/(double)numFrames);

        if (delta > interpolationRate) delta = interpolationRate;
        if (delta < -interpolationRate) delta = -interpolationRate;

        for (int j = 0; j < numFrames; j++) {
          jointPosition+=delta;
          SetHeadJoint(&headValue[i][j].value, jointPosition, i);
        }
        if (ABS(jointPosition-headJointEndPoints[i]) >= interpolationError) stillInterpolating = true;
      }
    }
    sbjCommandVector->SetData(cmdBodyRegion[bodyRegionID]);
    if (interpolateHead) {
      sbjCommandVector->SetData(cmdHeadRegion[headRegionID]);
    }

    if (stillInterpolating == false) {
//    cout << "Cancelled interpolation 2" << endl << flush;
      FinishInterpolation();
    }
  }
}

void TheStrut::FinishInterpolation() {
  // hooray!
  isInterpolating = false;
  InterpretLocomotionCommand(&nextLocomotionCommand, false);
}