thestrut.cc

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

    isWalking = false;
    isKicking = false;
    isInterpolating = false;
    if (prevPointsSet == false) {
      prevPointsSet = true;
      // get start joint points from sensors...
      #ifdef ERS_7
        headJointPrevPoints[0] = sensorValues_[S_HEAD_TILT1];
        headJointPrevPoints[1] = sensorValues_[S_HEAD_PAN];
        headJointPrevPoints[2] = sensorValues_[S_HEAD_TILT2];
      #endif
      #ifdef ERS_210
        headJointPrevPoints[0] = sensorValues_[S_HEAD_TILT];
        headJointPrevPoints[1] = sensorValues_[S_HEAD_PAN];
        headJointPrevPoints[2] = sensorValues_[S_HEAD_ROLL];
      #endif

      bodyJointPrevPoints[0] = sensorValues_[S_RF_ROTATOR];
      bodyJointPrevPoints[1] = sensorValues_[S_RF_ABDUCTOR];
      bodyJointPrevPoints[2] = sensorValues_[S_RF_KNEE];
      bodyJointPrevPoints[3] = sensorValues_[S_LF_ROTATOR];
      bodyJointPrevPoints[4] = sensorValues_[S_LF_ABDUCTOR];
      bodyJointPrevPoints[5] = sensorValues_[S_LF_KNEE];

      bodyJointPrevPoints[6] = sensorValues_[S_RR_ROTATOR];
      bodyJointPrevPoints[7] = sensorValues_[S_RR_ABDUCTOR];
      bodyJointPrevPoints[8] = sensorValues_[S_RR_KNEE];
      bodyJointPrevPoints[9] = sensorValues_[S_LR_ROTATOR];
      bodyJointPrevPoints[10] = sensorValues_[S_LR_ABDUCTOR];
      bodyJointPrevPoints[11] = sensorValues_[S_LR_KNEE];
    }

    // do get up.
    motionType = 99;
    for (unsigned int k = 0; k < kicks.size(); k++) {
      if (kicks[k].id == motionType) {
        LocomotionCommand tslc;
        tslc.SetKick(motionType,true);isKicking=true;
        isWalking=false;
        isInterpolating=false;
        kickIndex = k;
        for (int i = 0; i < NUM_HEAD_JOINTS; i++) {
          headJointEndPoints[i] = DEG_TO_MICRO(kicks[kickIndex].frames[0][i]);
        }
        for (int i = 0; i < NUM_BODY_JOINTS; i++) {
          bodyJointEndPoints[i] = DEG_TO_MICRO(kicks[kickIndex].frames[0][i+3]);
        }
        bool kickUsesHead = true;
        if (ShouldInterpolate(kickUsesHead)) {
          isInterpolating = true;
          isKicking = false;
          interpolateHead = kickUsesHead;
          interpolationRate = DEG_TO_MICRO(0.6);
          // store the kick command that we were going to do. we will reinterpret it again once interpolation is complete !
          memcpy(&nextLocomotionCommand, &tslc, sizeof(LocomotionCommand));
        }
      }
    }
    cout << "TheStrut: Changed into a motionless state." << endl << flush;
  } else if (robotState_.GetState() == RobotState::ST_PLAYING || robotState_.GetState() == RobotState::ST_READY) {
    timeParameter = 0.0;
    lcq_.Clear();
    isWalking = false;
    isKicking = false;
    isInterpolating = false;
    cout << "TheStrut: Changed into moving state." << endl << flush;
  }
  if (motorPowerOn_) {
    IncJointsReady();
  }
}

// get next locomotion command from the queue
bool TheStrut::GetNextLocomotionCommand() {
  if (lcq_.Size() > 0) {
    LocomotionCommand lc = lcq_.Front();
    if (lc.motionType == LocomotionCommand::TP_WALK || lc.motionType == LocomotionCommand::TP_WALKWITHOUTFRONT || lc.motionType == LocomotionCommand::TP_WALKTURNKICK) {
//    if (((int)(timeParameter*10) == 0) || ((int)(timeParameter*10) ==  5) || ((int)(timeParameter*10) == 10)) {
      InterpretLocomotionCommand(&lc);
      return true;
//    }
    }
    if (lc.motionType == LocomotionCommand::TP_DONOTHING) {
      isWalking=false;
      isKicking=false;
      timeParameter=0;
      return false;
    }
    if (lc.motionType > 0) {
      // special case - can't grab in certain parts of step ..
      /*if (lc.motionType == LocomotionCommand::TP_GRAB && isWalking) {
        if (((int)(timeParameter*10) != 0) && ((int)(timeParameter*10) !=  5)) {
          return true;
        }
      }*/
      lc = lcq_.Dequeue();
      timeParameter=0.0;

      InterpretLocomotionCommand(&lc);
      return true;
    }
    if (lc.motionType == LocomotionCommand::TP_ENDKICK) {
      //cout << "Found end kick. Dequeueing." << endl << flush;
      lc = lcq_.Dequeue();
      isWalking=false;
      isKicking=false;
      timeParameter = 0.0;

      return false;
    }
  } else {
    // queue was empty. do nothing !
    isWalking=false;
    isKicking=false;
    motionType = -10;
    timeParameter=0;
    return false;
  }
  return true;
}

// get next place the head is meant to be
void TheStrut::GetNextHeadCommand() {
  HeadCommand hc = lcq_.GetHeadCommand();
  headTilt1 = hc.targetTilt1_;
  headPan = hc.targetPan_;
  headTilt2 = hc.targetTilt2_;
  allowHeadCommandInterpolation = hc.allowInterpolation;
}

void TheStrut::DoLocusScaling(double** origLocusPoints, double sl, double sh, double** finalLocusPoints, double* topPerim, double* botPerim) {
  double fMax = 0; double fMin = 0;
  double hMax = 0; double hMin = 0;
  for (int i = 0; i < NUM_LOCUS_POINTS; i++) {
    if (origLocusPoints[i][0] >= fMax) {fMax = origLocusPoints[i][0];}
    if (origLocusPoints[i][0] <= fMin) {fMin = origLocusPoints[i][0];}
    if (origLocusPoints[i][1] >= hMax) {hMax = origLocusPoints[i][1];}
    if (origLocusPoints[i][1] <= hMin) {hMin = origLocusPoints[i][1];}
  }

  double fCoordMultiplier = sl/fabs(fMax-fMin);
  double hCoordMultiplier = sh/fabs(hMax-hMin);
  *topPerim = 0;
  *botPerim = 0;
  finalLocusPoints[0][0] = (origLocusPoints[0][0])*fCoordMultiplier;
  finalLocusPoints[0][1] = (origLocusPoints[0][1])*hCoordMultiplier;

  double maxX = 0; double minX = 0; int maxXIndex = -1; int minXIndex = -1;

  for (int j = 1; j < NUM_LOCUS_POINTS; j++) {
    finalLocusPoints[j][0] = (origLocusPoints[j][0])*fCoordMultiplier;
    finalLocusPoints[j][1] = (origLocusPoints[j][1])*hCoordMultiplier;

    if (finalLocusPoints[j][0] >= maxX) {maxX = finalLocusPoints[j][0]; maxXIndex = j; }
    if (finalLocusPoints[j][0] <= minX) {minX = finalLocusPoints[j][0]; minXIndex = j; }
  }

  for (int j = 1; j < NUM_LOCUS_POINTS; j++) {
    if (j > minXIndex && j <= maxXIndex) {
      *topPerim += sqrt(pow((finalLocusPoints[j][0] - finalLocusPoints[j-1][0]),2.0)+pow((finalLocusPoints[j][1] - finalLocusPoints[j-1][1]),2.0));
    } else {
      *botPerim += sqrt(pow((finalLocusPoints[j][0] - finalLocusPoints[j-1][0]),2.0)+pow((finalLocusPoints[j][1] - finalLocusPoints[j-1][1]),2.0));
    }
  }
}

void TheStrut::LocusInterpolation(double** origLocusPoints, double topPerim, double botPerim, double** finalLocusPoints, int* nP) {

  double maxX = 0; double minX = 0; int maxXIndex = -1; int minXIndex = -1;
  for (int i = 0; i < NUM_LOCUS_POINTS-1; i++) {
    if (origLocusPoints[i][0] >= maxX) {maxX = origLocusPoints[i][0]; maxXIndex = i; }
    if (origLocusPoints[i][0] <= minX) {minX = origLocusPoints[i][0]; minXIndex = i; }
  }
  
    
  int npts = 0;
  int npbs = 0;

  double x = 0;
  double xInc = 0.001;
  double distanceSinceLastPoint = 0.0;
//double distanceBetweenPoints = perimeter/(double)(NUM_LOCUS_POINTS_INTERNAL-1);

  double distanceBetweenPointsTopStroke = topPerim / ((double)(NUM_LOCUS_POINTS_INTERNAL)/2.0);
  double distanceBetweenPointsBottomStroke = botPerim / ((double)(NUM_LOCUS_POINTS_INTERNAL)/2.0);
  double pFCoord = 0.0;
  double pHCoord = 0.0;
  bool firstRun = true;
  int k = 1;
  finalLocusPoints[0][0] = origLocusPoints[0][0];
  finalLocusPoints[0][1] = origLocusPoints[0][1];
  while (x < 1.0) {
    double exactPosition = x*(double)(NUM_LOCUS_POINTS-1);
    int prevPoint = (int)exactPosition;
    int nextPoint = prevPoint+1;
    double fraction = exactPosition-prevPoint;
    double fCoord = origLocusPoints[prevPoint][0]+(origLocusPoints[nextPoint][0]-origLocusPoints[prevPoint][0])*fraction;
    double hCoord = origLocusPoints[prevPoint][1]+(origLocusPoints[nextPoint][1]-origLocusPoints[prevPoint][1])*fraction;
    if (firstRun) {
      firstRun = false;
      distanceSinceLastPoint += sqrt(pow((fCoord-origLocusPoints[0][0]),2.0)+pow((hCoord-origLocusPoints[0][1]),2.0)); 
    } else {
      distanceSinceLastPoint += sqrt(pow((fCoord-pFCoord),2.0)+pow((hCoord-pHCoord),2.0)); 
    }
    double distanceBetweenPoints = 0.0;
    if (prevPoint >= minXIndex && prevPoint < maxXIndex) {
      distanceBetweenPoints = distanceBetweenPointsTopStroke;
    } else {
      distanceBetweenPoints = distanceBetweenPointsBottomStroke;
    }
    if (distanceSinceLastPoint >= distanceBetweenPoints && k < NUM_LOCUS_POINTS_INTERNAL) {
      if (prevPoint >= minXIndex && prevPoint < maxXIndex) {
        npts++;
      } else {
        npbs++;
      }
    
      finalLocusPoints[k][0] = fCoord;
      finalLocusPoints[k][1] = hCoord;
      k++;
      distanceSinceLastPoint = 0;
    }
    pFCoord = fCoord;
    pHCoord = hCoord;
    
    x+=xInc;
  }
  *nP = k;


  /*


  double x = 0;
  double xInc = 0.01;
  double distanceSinceLastPoint = 0.0;
  double distanceBetweenPoints = perimeter/(double)(NUM_LOCUS_POINTS_INTERNAL-1);
  double pFCoord = 0.0;
  double pHCoord = 0.0;
  bool firstRun = true;
  int k = 1;
  finalLocusPoints[0][0] = origLocusPoints[0][0];
  finalLocusPoints[0][1] = origLocusPoints[0][1];
  while (x < 1.0) {
    double exactPosition = x*(double)(NUM_LOCUS_POINTS-1);
    int prevPoint = (int)exactPosition;
    int nextPoint = prevPoint+1;
    double fraction = exactPosition-prevPoint;
    double fCoord = origLocusPoints[prevPoint][0]+(origLocusPoints[nextPoint][0]-origLocusPoints[prevPoint][0])*fraction;
    double hCoord = origLocusPoints[prevPoint][1]+(origLocusPoints[nextPoint][1]-origLocusPoints[prevPoint][1])*fraction;
    if (firstRun) {
      firstRun = false;
      distanceSinceLastPoint += sqrt(pow((fCoord-origLocusPoints[0][0]),2.0)+pow((hCoord-origLocusPoints[0][1]),2.0)); 
    } else {
      distanceSinceLastPoint += sqrt(pow((fCoord-pFCoord),2.0)+pow((hCoord-pHCoord),2.0)); 
    }
    if (distanceSinceLastPoint >= distanceBetweenPoints && k < NUM_LOCUS_POINTS_INTERNAL) {
      finalLocusPoints[k][0] = fCoord;
      finalLocusPoints[k][1] = hCoord;
      k++;
      distanceSinceLastPoint = 0;
    }
    pFCoord = fCoord;
    pHCoord = hCoord;
    
    x+=xInc;
  }
  *nP = k;
*/
}

void TheStrut::InterpretLocomotionCommand(LocomotionCommand* tslc, bool enableInterpolation) {
  motionType = tslc->motionType;
  lcq_.SetCurrentLocomotionCommand(*tslc);

  offsetWalk = tslc->offsetWalk;
  stepFrequency = tslc->stepFrequency;

  #ifdef ERS_7
  double bs = ABS(tslc->backStrideLength);
  bs = MIN(MAX(20.0,bs),120.0);
 // backMultiplier = 0.0089*(bs*bs) - 0.0754*bs + 0.594;

  double st = ABS(tslc->strafe);
  st = MIN(MAX(20.0,st),60.0);
  strafeMultiplier = 0.0002*(st*st) - 0.0193*st + 0.79;
  #endif

  double newForwardMultiplier = forwardMultiplier;
  //if (tslc->frontStrideLength < 70.0) newForwardMultiplier=0.8;
  frontStrideLength = tslc->frontStrideLength;
  backStrideLength = tslc->backStrideLength;

  if (backStrideLength >= 0) { // forwards multipliers
    frontForwardComponent = (frontStrideLength * newForwardMultiplier) / 2.0;
    backForwardComponent = (backStrideLength * newForwardMultiplier) / 2.0;

  } else { // backwards multipliers
    frontForwardComponent = (frontStrideLength * backMultiplier) / 2.0;
    backForwardComponent = (backStrideLength * backMultiplier) / 2.0;  
    if (tslc->strafe > 0) tslc->turn-=1.5;

  }

  frontSideComponent = (tslc->strafe * strafeMultiplier) / 2.0;
  backSideComponent = (tslc->strafe * strafeMultiplier) / 2.0;
  
  if (frontForwardComponent > -0.01)
    frontTurnComponent = tslc->turn*turnMultiplier;
  else
    frontTurnComponent = tslc->turn*turnMultiplier / 3;

  if (backForwardComponent > -0.01)
    backTurnComponent = tslc->turn*turnMultiplier*1.5;  // Craig I think this is crap (MQ) P.S I have no fi
  else 
    backTurnComponent = tslc->turn*turnMultiplier/3;

  if (motionType == LocomotionCommand::TP_SINGLEWALKWITHOUTFRONT || motionType == LocomotionCommand::TP_WALKWITHOUTFRONT) {
    frontTurnComponent = 0.0;
    backTurnComponent = tslc->turn*turnMultiplierWithoutFront;
  }

  frontHeight = tslc->frontHeight;
  frontStrideHeight = tslc->frontStrideHeight;
  frontForwardOffset = tslc->frontForwardOffset;
  frontSideOffset = tslc->frontSideOffset;

  backHeight = tslc->backHeight;
  backStrideHeight = tslc->backStrideHeight;
  backForwardOffset = tslc->backForwardOffset;
  backSideOffset = tslc->backSideOffset;

  turn = tslc->turn;
  strafe = tslc->strafe;

  if (tslc->odomTurn < -999.0) odomTurn = turn;
  else odomTurn = tslc->odomTurn;

  if (backStrideLength < 0 && tslc->strafe > 0 && turn == odomTurn) odomTurn+=1.5;

  locusType = tslc->locusType;

  if (locusType == LocomotionCommand::L_TRAPEZOID) {
    locusFunction = &TheStrut::CalculateComponents_TZ;
  } else if (locusType == LocomotionCommand::L_ELLIPSE) {
    locusFunction = &TheStrut::CalculateComponents_EL;
  } else if (locusType == LocomotionCommand::L_RECTANGLE) {
    locusFunction = &TheStrut::CalculateComponents_R;
  } else if (locusType != LocomotionCommand::L_ARBITRARY) {
    locusType = LocomotionCommand::L_TRAPEZOID;
    locusFunction = &TheStrut::CalculateComponents_TZ;
  }



  double sineTheta5 = sin(atan(119.4/115.0));
//  double cosineTheta5 = cos(atan(119.4/115.0));
  if (locusType == LocomotionCommand::L_ARBITRARY && motionType <= LocomotionCommand::TP_DONOTHING) {
    if (ABS(backForwardComponent) < 0.1 && (ABS(strafe) > 0 || ABS(turn) > 0)) {
      frontForwardComponent = 0.1;
      backForwardComponent = 0.1;
    }

    FR_numPoints = NUM_LOCUS_POINTS_INTERNAL;
    FL_numPoints = NUM_LOCUS_POINTS_INTERNAL;

    BR_numPoints = NUM_LOCUS_POINTS_INTERNAL;
    BL_numPoints = NUM_LOCUS_POINTS_INTERNAL;

    double FL_topPerimeter, FR_topPerimeter, BL_topPerimeter, BR_topPerimeter;
    double FL_botPerimeter, FR_botPerimeter, BL_botPerimeter, BR_botPerimeter;

    // This is for changing the walk on a turn kick
    double extraLeftFront = 1.0;
    double extraRightFront = 1.0;
    if (motionType == LocomotionCommand::TP_WALKTURNKICK || motionType == LocomotionCommand::TP_SINGLEWALKTURNKICK) {
      if (turn > 0) extraRightFront = 2.5;
      else extraLeftFront = 2.5;
    }

    DoLocusScaling(origFrontLocusPoints_, (frontForwardComponent*2.0*extraRightFront) + frontTurnComponent*sineTheta5, frontStrideHeight, FR_origLocusPoints, &FR_topPerimeter, &FR_botPerimeter);
    DoLocusScaling(origFrontLocusPoints_, (frontForwardComponent*2.0*extraLeftFront) - frontTurnComponent*sineTheta5, frontStrideHeight, FL_origLocusPoints, &FL_topPerimeter, &FL_botPerimeter);
    DoLocusScaling(origBackLocusPoints_, (backForwardComponent*2.0) + backTurnComponent*sineTheta5, backStrideHeight, BR_origLocusPoints, &BR_topPerimeter, &BR_botPerimeter);
    DoLocusScaling(origBackLocusPoints_, (backForwardComponent*2.0) - backTurnComponent*sineTheta5, backStrideHeight, BL_origLocusPoints, &BL_topPerimeter, &BL_botPerimeter);

    LocusInterpolation(FR_origLocusPoints, FR_topPerimeter, FR_botPerimeter, FR_locusPoints, &FR_numPoints);
    LocusInterpolation(FL_origLocusPoints, FL_topPerimeter, FL_botPerimeter, FL_locusPoints, &FL_numPoints);
    LocusInterpolation(BR_origLocusPoints, BR_topPerimeter, BR_botPerimeter, BR_locusPoints, &BR_numPoints);
    LocusInterpolation(BL_origLocusPoints, BL_topPerimeter, BL_botPerimeter, BL_locusPoints, &BL_numPoints);
  }

  // may wish to interpolate mid step (!). let's try it
/*if (1 && motionType == LocomotionCommand::TP_WALK && enableInterpolation) {
    double f = 0, s = 0, h = 0;
    double timeParameterOffset = 0.0;
    double ang1=0, ang2=0,ang3=0;
    CalculateComponents_A(&f, &s, &h, timeParameter+timeParameterOffset+0.5, FR_numPoints, FR_locusPoints, -frontSideComponent-frontTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, frontStrideHeight, frontForwardOffset, frontSideOffset, frontHeight);
    CalculateAngles_F(f, s, h, &ang1, &ang2, &ang3);
    bodyJointEndPoints[0] = RAD_TO_MICRO(ang2);
    bodyJointEndPoints[1] = RAD_TO_MICRO(ang1);
    bodyJointEndPoints[2] = RAD_TO_MICRO(ang3);

    CalculateComponents_A(&f, &s, &h, timeParameter+timeParameterOffset, FL_numPoints, FL_locusPoints, frontSideComponent+frontTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, frontStrideHeight, frontForwardOffset, frontSideOffset, frontHeight);
    CalculateAngles_F(f, s, h, &ang1, &ang2, &ang3);
    bodyJointEndPoints[3] = RAD_TO_MICRO(ang2);
    bodyJointEndPoints[4] = RAD_TO_MICRO(ang1);