walk.cc

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

    sideFirstTime = sideFirstTime - 0.15;
    sideLastTime = sideLastTime + 0.15;
  }
*/
  *s = 0;

/*
  trapezoid lookalike
  if (timeParameter < 0.25) {
    *s = sideMin*(timeParameter)*(1.0/0.25);
  } else if (timeParameter < 0.5) {
    *s = sideMin - sideMin*(timeParameter-0.25)*(1.0/0.25);
  } else if (timeParameter < 0.75) {
    *s = sideMax*(timeParameter-0.5)*(1.0/0.25);
  } else if (timeParameter <= 1.0) {
    *s = sideMax - sideMax*(timeParameter-0.75)*(1.0/0.25);
  }
*/
// general approach
  if (timeParameter < sideFirstTime) {
    *s = sideMin*(timeParameter-0.0)*(1.0/(sideFirstTime-0.0));
  } else if (timeParameter < sideLastTime) {
    *s = sideMin + sideLength * (timeParameter-sideFirstTime)*(1.0/(sideLastTime-sideFirstTime));
  } else if (timeParameter <= 1.0) {
    *s = sideMax - sideMax*(timeParameter-sideLastTime)*(1.0/(1.0-sideLastTime));
  }


  *f = fCoord;
  *h = hCoord;

  *f = *f + lfo;
  *h = *h + lh;
  *s = *s + lso;
}


void TheStrut::CalculateComponents_R(double *f, double *s, double *h, double timeParameter, double fMax, double sMax, double bSMax, double sh, double lfo, double lso, double lh) {
  if (timeParameter > 1.0) timeParameter -= 1.0;

  double frontMax = fMax;
  double frontMin = -fMax;

  double sideMax = sMax;
  double sideMin = -sMax;

  double frontLength = frontMax-frontMin;
  double sideLength = sideMax-sideMin;

  double backSideMax = bSMax;
  double backSideMin = -bSMax;
  double backSideLength = backSideMax - backSideMin;
  double avgSideLength = (ABS(backSideLength)+ABS(sideLength))/2.0;


  // vertical component / ground component * 0.5. essentially, give equal time to travel vertically as for horizontal.
  // this is justifiable mathematically, but maybe we could spend less time going upwards ?
  if (ABS(frontLength) < 0.001 && ABS(sideLength) < 0.001) {
    frontLength = 0.001;
    sideLength = 0.001;
  }
  double vstm = verticalStrokeTimeMultiplier;
  if (motionType == LocomotionCommand::TP_WALKWITHOUTFRONT || motionType == LocomotionCommand::TP_SINGLEWALKWITHOUTFRONT) {
    vstm = 0.1;
  }
  double verticalStrokeTime = ABS(sh) / sqrt(frontLength*frontLength + avgSideLength*avgSideLength)*0.5*vstm;
  if (verticalStrokeTime < 0.0001) verticalStrokeTime = 0.0001;

  double horizHalfStrokeTime = 0.25-verticalStrokeTime;
  if (timeParameter < horizHalfStrokeTime) {
    *f = frontMin*(timeParameter*(1/horizHalfStrokeTime));
    *s = sideMin*(timeParameter*(1/horizHalfStrokeTime));
    *h = sh/2.0;
  } else if (timeParameter < 0.25) {
    *f = frontMin;
    *s = sideMin;
    *h = sh/2.0 - (timeParameter-horizHalfStrokeTime)/verticalStrokeTime*sh;
  } else if (timeParameter < 0.75) {
    *f = frontMin + (frontLength)*(timeParameter-0.25)*2.0;
    *s = sideMin + (sideLength)*(timeParameter-0.25)*2.0;
    *h = -sh/2.0;
  } else if (timeParameter < 0.75 + verticalStrokeTime) {
    *f = frontMax;
    *s = sideMax;
    *h = -sh/2.0 + (timeParameter-0.75)/verticalStrokeTime*sh;
  } else if (timeParameter <= 1.0) {
    *f = frontMax - 1/(0.25-verticalStrokeTime)*(timeParameter-(0.75+verticalStrokeTime))*frontMax;
    *s = sideMax - 1/(0.25-verticalStrokeTime)*(timeParameter-(0.75+verticalStrokeTime))*sideMax;
    *h = sh/2.0;
  }

  *f = *f + lfo;
  *h = *h + lh;
  *s = *s + lso;
}


void TheStrut::DoWalk() {
  inDefaultStance = false;
  double timeParameterChange = 0.0;
  int regionID = FindFreeBodyRegionID();

  if (regionID != -1) {
    if (motionType >= 0 && motionType <=2) traction.DoTractionSensing(stepFrequency,timeParameter,backStrideLength,turn,strafe);
    double ang1;
    double ang2;
    double ang3;

    double timeIncrement = (stepFrequency)/125.0;
    int numFrames = (int)((1.0-timeParameter)/timeIncrement);
    if (numFrames > maxWalkFrames) numFrames = maxWalkFrames;

    if (numFrames <= 0) {
      timeParameter = 0;
      motionType = -1;
      if (isDebugStall) {
        cout << "numFrames was <= 0. leaving dowalk()" << endl << flush;
      }
      return;
    }

    for (int i = 0; i < NUM_BODY_JOINTS; i++) {
      OCommandData* jointData = bodyVec[regionID]->GetData(i);
      bodyValue[i] = (OJointCommandValue2*)jointData->value;

      OCommandInfo* jointInfo = bodyVec[regionID]->GetInfo(i);
      jointInfo->numFrames = numFrames;
    }
    
    double timeParameterOffset = 0.0; // ie, don't actually start walk at timeParameter 0.
                                       // use timeparameter 0.25.. this is top of ellipse !
    // ok, we want to 'reverse' the walk so we can make left and right identical... 
    if (offsetWalk) timeParameterOffset = 0.5;

    double s; // sideways
    double h; // height
    double f; // forwards
    for (int j = 0; j < numFrames; j++) {
      // one frame = 0.008seconds (125hz). max frames we can fill at once = 16. So we fill joints for at most 0.128s
      timeParameter += timeIncrement;
      timeParameterChange += timeIncrement;

      if (timeParameter+timeIncrement >= 1) { // step completed.
        lcq_.IncrementNumSteps();
//      cout << lcq_.GetNumSteps() << endl << flush;
        timeParameter = 0;
        motionType = -1;
        continue;
      }

      double sineTheta5 = sin(atan(119.4/115.0));
      double cosineTheta5 = cos(atan(119.4/115.0));

      if (locusType == LocomotionCommand::L_TRAPEZOID || locusType == LocomotionCommand::L_ELLIPSE || locusType == LocomotionCommand::L_RECTANGLE) {
        //ledOn_[0] = false;
        //ledOn_[3] = false;

        // front right limb
        (*this.*locusFunction)(&f, &s, &h, timeParameter+timeParameterOffset+0.5, frontForwardComponent+frontTurnComponent*sineTheta5, -frontSideComponent-frontTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, frontStrideHeight, frontForwardOffset, frontSideOffset, frontHeight);
        CalculateAngles_F(f, s, h, &ang1, &ang2, &ang3);
        SetBodyJoint(&bodyValue[0][j].value, RAD_TO_MICRO(ang2), 0);
        SetBodyJoint(&bodyValue[1][j].value, RAD_TO_MICRO(ang1), 1);
        SetBodyJoint(&bodyValue[2][j].value, RAD_TO_MICRO(ang3), 2);

        // front left limb
        (*this.*locusFunction)(&f, &s, &h, timeParameter+timeParameterOffset, frontForwardComponent-frontTurnComponent*sineTheta5, frontSideComponent+frontTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, frontStrideHeight, frontForwardOffset, frontSideOffset, frontHeight);
        CalculateAngles_F(f, s, h, &ang1, &ang2, &ang3);
        SetBodyJoint(&bodyValue[3][j].value, RAD_TO_MICRO(ang2), 3);
        SetBodyJoint(&bodyValue[4][j].value, RAD_TO_MICRO(ang1), 4);
        SetBodyJoint(&bodyValue[5][j].value, RAD_TO_MICRO(ang3), 5);

        // back right limb
        (*this.*locusFunction)(&f, &s, &h, timeParameter+timeParameterOffset, backForwardComponent+backTurnComponent*sineTheta5, -backSideComponent + backTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, backStrideHeight, backForwardOffset, backSideOffset, backHeight);
        CalculateAngles_B(f, s, h, &ang1, &ang2, &ang3);
        SetBodyJoint(&bodyValue[6][j].value, RAD_TO_MICRO(ang2), 6);
        SetBodyJoint(&bodyValue[7][j].value, RAD_TO_MICRO(ang1), 7);
        SetBodyJoint(&bodyValue[8][j].value, RAD_TO_MICRO(ang3), 8);

        // back left limb
        (*this.*locusFunction)(&f, &s, &h, timeParameter+timeParameterOffset+0.5, backForwardComponent-backTurnComponent*sineTheta5, backSideComponent-backTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, backStrideHeight, backForwardOffset, backSideOffset, backHeight);
        CalculateAngles_B(f, s, h, &ang1, &ang2, &ang3);
        SetBodyJoint(&bodyValue[9][j].value, RAD_TO_MICRO(ang2), 9);
        SetBodyJoint(&bodyValue[10][j].value, RAD_TO_MICRO(ang1), 10);
        SetBodyJoint(&bodyValue[11][j].value, RAD_TO_MICRO(ang3), 11);
      } else if (locusType == LocomotionCommand::L_ARBITRARY) {
        //ledOn_[0] = true;
        //ledOn_[3] = true;
         // front right limb
        double extraLeftFront = 0.0;
        double extraRightFront = 0.0;
    
        if (motionType == LocomotionCommand::TP_WALKTURNKICK || motionType == LocomotionCommand::TP_SINGLEWALKTURNKICK) {
          if (turn > 0) extraRightFront = 40.0;
          else extraLeftFront = 40.0;
        }

        CalculateComponents_A(&f, &s, &h, timeParameter+timeParameterOffset+0.5, FR_numPoints, FR_locusPoints, -frontSideComponent-frontTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, frontStrideHeight, frontForwardOffset+extraRightFront, frontSideOffset, frontHeight);
        CalculateAngles_F(f, s, h, &ang1, &ang2, &ang3);
        SetBodyJoint(&bodyValue[0][j].value, RAD_TO_MICRO(ang2), 0);
        SetBodyJoint(&bodyValue[1][j].value, RAD_TO_MICRO(ang1), 1);
        SetBodyJoint(&bodyValue[2][j].value, RAD_TO_MICRO(ang3), 2);

        // front left limb
        CalculateComponents_A(&f, &s, &h, timeParameter+timeParameterOffset, FL_numPoints, FL_locusPoints, frontSideComponent+frontTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, frontStrideHeight, frontForwardOffset+extraLeftFront, frontSideOffset, frontHeight);
        CalculateAngles_F(f, s, h, &ang1, &ang2, &ang3);
        SetBodyJoint(&bodyValue[3][j].value, RAD_TO_MICRO(ang2), 3);
        SetBodyJoint(&bodyValue[4][j].value, RAD_TO_MICRO(ang1), 4);
        SetBodyJoint(&bodyValue[5][j].value, RAD_TO_MICRO(ang3), 5);

        // back right limb
        CalculateComponents_A(&f, &s, &h, timeParameter+timeParameterOffset, BR_numPoints, BR_locusPoints, -backSideComponent + backTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, backStrideHeight, backForwardOffset, backSideOffset, backHeight);
        CalculateAngles_B(f, s, h, &ang1, &ang2, &ang3);
        SetBodyJoint(&bodyValue[6][j].value, RAD_TO_MICRO(ang2), 6);
        SetBodyJoint(&bodyValue[7][j].value, RAD_TO_MICRO(ang1), 7);
        SetBodyJoint(&bodyValue[8][j].value, RAD_TO_MICRO(ang3), 8);

        // back left limb
        CalculateComponents_A(&f, &s, &h, timeParameter+timeParameterOffset+0.5, BL_numPoints, BL_locusPoints, backSideComponent-backTurnComponent*cosineTheta5, backSideComponent+backTurnComponent*cosineTheta5, backStrideHeight, backForwardOffset, backSideOffset, backHeight);
        CalculateAngles_B(f, s, h, &ang1, &ang2, &ang3);
        SetBodyJoint(&bodyValue[9][j].value, RAD_TO_MICRO(ang2), 9);
        SetBodyJoint(&bodyValue[10][j].value, RAD_TO_MICRO(ang1), 10);
        SetBodyJoint(&bodyValue[11][j].value, RAD_TO_MICRO(ang3), 11);
      }
    }
    // odometry data
    LocomotionData ld;
    // 1.5 due to gait structure
    ld.deltaForward = backStrideLength * timeParameterChange * 0.1;  // 0.1 to convert from mm to cm
    ld.deltaLeft = strafe/10.0 * timeParameterChange * 1.0;
    ld.deltaTurn = DEG_TO_RAD(odomTurn) * timeParameterChange * 1.0;
    
    lcq_.AddLocomotionData(ld);

    sbjCommandVector->SetData(cmdBodyRegion[regionID]);
  }
}