walk.cc

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

#include "TheStrut.h"
#include "../Globals.h"
#include <math.h>
// see the genius explanations in UNSW's 2000 report.
// Bernhard's omnidirectional walk paper is useful too.
void TheStrut::CalculateAngles_F(double f, double s, double h, double *ang1, double *ang2, double *ang3) {
#ifdef ERS_7
   double l1=69.5; double l2=76.896944; double l3=7.8; double lsh=130;
#endif  
#ifdef ERS_210
   double l1=64; double l2=68.41286429; double l3=12.8; double lsh=119;
#endif

  double Pi = PI;

  double theta4=asin((backHeight-frontHeight)/lsh);  // essentially just angle of body. lsh is fixed by dimensions of robot
  double z=s;
  double y=h*cos(theta4)-f*sin(theta4);
  double x=h*sin(theta4)+f*cos(theta4);


//l1, l2, l3 are constants. rename!
//x, y, z contains leg locus positions.
//x-y-z to theta1,2,3 transformation
  double temp1=l1*l1+2*l3*l3+l2*l2-x*x-y*y-z*z;
  double temp2=2*sqrt((l1*l1+l3*l3)*(l2*l2+l3*l3));
  double theta3=2*Pi-atan(l1/l3)-atan(l2/l3)-acos(temp1/temp2);
  double tempu=2*l3*sin(theta3/2)*sin(theta3/2)+l2*sin(theta3);
  double tempv=l1+2*l3*sin(theta3/2)*cos(theta3/2)+l2*cos(theta3);
  double theta1=asin(z/tempv);
  temp1=y*tempv*cos(theta1)+tempu*x;
  temp2=tempu*tempu+tempv*tempv*cos(theta1)*cos(theta1);
  double theta2=acos(temp1/temp2);
  if(ABS(x-tempv*cos(theta1)*sin(theta2)-tempu*cos(theta2))>.001) theta2*=-1;
  
  *ang1 = theta1;
  *ang2 = theta2;
  *ang3 = theta3;
}

void TheStrut::CalculateAngles_B(double f, double s, double h, double *ang1, double *ang2, double *ang3) {
#ifdef ERS_7
  double l1=69.5; double l3=7.8; double lsh=130; double l4=79.409948999; // l3 was 9.0
#endif
#ifdef ERS_210
  double l1=64; double l3=12.8; double lsh=119; double l4=75.92865072;
#endif

  double Pi = PI;


  double theta4=asin((backHeight-frontHeight)/lsh);  // essentially just angle of body. lsh is fixed by dimensions of robot
  double z=s;
  double y=h*cos(theta4)-f*sin(theta4);
  double x=h*sin(theta4)+f*cos(theta4);



//l1, l2, l3 are constants. rename these!
//x, y, z contains leg locus positions.
//x-y-z to theta1,2,3 transformation

  double temp1=l1*l1+2*l3*l3+l4*l4-x*x-y*y-z*z;
  double temp2=2*sqrt((l1*l1+l3*l3)*(l4*l4+l3*l3));
  double theta3=2*Pi-atan(l1/l3)-atan(l4/l3)-acos(temp1/temp2);
  double tempu=2*l3*sin(theta3/2)*sin(theta3/2)+l4*sin(theta3);
  double tempv=l1+2*l3*sin(theta3/2)*cos(theta3/2)+l4*cos(theta3);
  double theta1=asin(z/tempv);
  temp1=y*tempv*cos(theta1)-tempu*x;
  temp2=tempu*tempu+tempv*tempv*cos(theta1)*cos(theta1);
  double theta2=acos(temp1/temp2);
  if(ABS(-x-tempv*cos(theta1)*sin(theta2)-tempu*cos(theta2))>.001) theta2*=-1;
 
  *ang1 = theta1;
  *ang2 = theta2;
  *ang3 = theta3;
}

void TheStrut::CalculateComponents_EL(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;

  if (timeParameter < 0.25 - verticalStrokeTime) {
    *f = frontMin*(timeParameter*4.0);
    *s = sideMin*(timeParameter*4.0);
    *h = sh/2.0;
  } else if (timeParameter < 0.25) {
    *f = frontMin*(timeParameter*4.0);
    *s = sideMin*(timeParameter*4.0);
    *h = sh/2.0 - (timeParameter-(0.25-verticalStrokeTime))/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 - 4.0*(timeParameter-0.75)*frontMax;
    *s = sideMax - 4.0*(timeParameter-0.75)*sideMax;
    *h = -sh/2.0 + (timeParameter-0.75)/verticalStrokeTime*sh;
  } else if (timeParameter <= 1.0) {
    *f = frontMax - 4.0*(timeParameter-0.75)*frontMax;
    *s = sideMax - 4.0*(timeParameter-0.75)*sideMax;
    *h = sh/2.0;
  }

  if (timeParameter < 0.25 || timeParameter > 0.75) {
    *h = + ((sh))*cos((timeParameter)*2*PI) - sh/2.0;
  } else {
    *h = -((sh)/2.0);
  }
  *f = *f + lfo;
  *h = *h + lh;
  *s = *s + lso;
}



/********************************************************************/

void TheStrut::CalculateComponents_TZ(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;

  if (timeParameter < 0.25 - verticalStrokeTime) {
    *f = frontMin*(timeParameter*4.0);
    *s = sideMin*(timeParameter*4.0);
    *h = sh/2.0;
  } else if (timeParameter < 0.25) {
    *f = frontMin*(timeParameter*4.0);
    *s = sideMin*(timeParameter*4.0);
    *h = sh/2.0 - (timeParameter-(0.25-verticalStrokeTime))/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 - 4.0*(timeParameter-0.75)*frontMax;
    *s = sideMax - 4.0*(timeParameter-0.75)*sideMax;
    *h = -sh/2.0 + (timeParameter-0.75)/verticalStrokeTime*sh;
  } else if (timeParameter <= 1.0) {
    *f = frontMax - 4.0*(timeParameter-0.75)*frontMax;
    *s = sideMax - 4.0*(timeParameter-0.75)*sideMax;
    *h = sh/2.0;
  }

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

/********************************************************************/


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

  double sideMax = (sMax);
  double sideMin = -(sMax);
  double sideLength = (sideMax-sideMin);

  double maxX = 0; double minX = 0; int maxXIndex = -1; int minXIndex = -1;
  for (int i = 0; i < numPoints; i++) {
    if (locusPoints[i][0] >= maxX) {maxX = locusPoints[i][0]; maxXIndex = i; }
    if (locusPoints[i][0] <= minX) {minX = locusPoints[i][0]; minXIndex = i; }
  }

  double exactPosition = timeParameter*(double)(numPoints-1);
  int prevPoint = (int)exactPosition;
  int nextPoint = prevPoint+1;
  
  double fraction = exactPosition-prevPoint;
  double fCoord = locusPoints[prevPoint][0]+(locusPoints[nextPoint][0]-locusPoints[prevPoint][0])*fraction;
  double hCoord = locusPoints[prevPoint][1]+(locusPoints[nextPoint][1]-locusPoints[prevPoint][1])*fraction;

  double sideFirstTime = (double)minXIndex/((double)numPoints-1.0);
  double sideLastTime  = (double)maxXIndex/((double)numPoints-1.0);
  if (sideLastTime < sideFirstTime) {
    double temp = sideFirstTime;
    sideFirstTime = sideLastTime;
    sideLastTime = temp;
  }
/*
hack to stop speed imbalance on big turns
look at after aus open 2004!
  double timeDiff = ABS(sideFirstTime-sideLastTime);
  if (timeDiff < 0.3) {