Пример #1
0
std::vector<double>
darwinop_kinematics_inverse_leg(
               Transform trLeg,
               int leg, double)
{
  std::vector<double> qLeg(6);
  bool left = (leg == LEG_LEFT); // Left leg

  Transform trInvLeg = inv(trLeg);
  //  printTransform(trInvLeg);
  //  printTransform(trLeg);

  // Hip Offset vector in Torso frame
  double xHipOffset[3];
  if (left) {
    xHipOffset[0] = 0;
    xHipOffset[1] = hipOffsetY;
    xHipOffset[2] = -hipOffsetZ;
  }
  else {
    xHipOffset[0] = 0;
    xHipOffset[1] = -hipOffsetY;
    xHipOffset[2] = -hipOffsetZ;
  }

  // Hip Offset in Leg frame
  double xLeg[3];
  for (int i = 0; i < 3; i++)
    xLeg[i] = xHipOffset[i];
  trInvLeg.apply(xLeg);
  xLeg[2] -= footHeight;

  // Knee pitch
  double dLeg = xLeg[0]*xLeg[0] + xLeg[1]*xLeg[1] + xLeg[2]*xLeg[2];

  double cKnee = .5*(dLeg-dTibia*dTibia-dThigh*dThigh)/(dTibia*dThigh);
  if (cKnee > 1) cKnee = 1;
  if (cKnee < -1) cKnee = -1;
  double kneePitch = acos(cKnee);

  // Angle pitch and roll
  double ankleRoll = atan2(xLeg[1], xLeg[2]);
  double lLeg = sqrt(dLeg);
  if (lLeg < 1e-16) lLeg = 1e-16;
  double pitch0 = asin(dThigh*sin(kneePitch)/lLeg);
  double anklePitch = asin(-xLeg[0]/lLeg) - pitch0;

  Transform rHipT = trLeg;
  rHipT = rHipT.rotateX(-ankleRoll).rotateY(-anklePitch-kneePitch);

  double hipYaw = atan2(-rHipT(0,1), rHipT(1,1));
  double hipRoll = asin(rHipT(2,1));
  double hipPitch = atan2(-rHipT(2,0), rHipT(2,2));

  // Need to compensate for KneeOffsetX:
  qLeg[0] = hipYaw;
  qLeg[1] = hipRoll;
  qLeg[2] = hipPitch-aThigh;
  qLeg[3] = kneePitch+aThigh+aTibia;
  qLeg[4] = anklePitch-aTibia;
  qLeg[5] = ankleRoll;
  return qLeg;
}
Пример #2
0
std::vector<double>
nao_kinematics_inverse_leg(
			   Transform trLeg,
			   int leg,
			   double hipYawPitch)
{
  std::vector<double> qLeg(6);
  bool left = (leg == LEG_LEFT); // Left leg
  bool calcHipYawPitch = (hipYawPitch > 0);

  Transform trInvLeg = inv(trLeg);
  //  printTransform(trInvLeg);
  //  printTransform(trLeg);

  // Hip Offset vector in Torso frame
  double xHipOffset[3];
  if (left) {
    xHipOffset[0] = 0;
    xHipOffset[1] = hipOffsetY;
    xHipOffset[2] = -hipOffsetZ;
  }
  else {
    xHipOffset[0] = 0;
    xHipOffset[1] = -hipOffsetY;
    xHipOffset[2] = -hipOffsetZ;
  }

  // Hip Offset in Leg frame
  double xLeg[3];
  for (int i = 0; i < 3; i++)
    xLeg[i] = xHipOffset[i];
  trInvLeg.apply(xLeg);
  xLeg[2] -= footHeight;

  // Knee pitch
  double dLeg = xLeg[0]*xLeg[0] + xLeg[1]*xLeg[1] + xLeg[2]*xLeg[2];
  double cKnee = .5*(dLeg -tibiaLength*tibiaLength -thighLength*thighLength)/
    (tibiaLength*thighLength);
  if (cKnee > 1) cKnee = 1;
  if (cKnee < -1) cKnee = -1;
  double kneePitch = acos(cKnee);

  // Angle pitch and roll
  double ankleRoll = atan2(xLeg[1], xLeg[2]);
  double lLeg = sqrt(dLeg);
  if (lLeg < 1e-16) lLeg = 1e-16;
  double pitch0 = asin(thighLength*sin(kneePitch)/lLeg);
  double anklePitch = asin(-xLeg[0]/lLeg) - pitch0;

  if (calcHipYawPitch) {
    // Use hip rotation to calculate HipYawPitch
    Transform rHipT = trLeg;
    rHipT = rHipT.rotateX(-ankleRoll).rotateY(-anklePitch-kneePitch);
    if (left)
      hipYawPitch = atan2(SQRT2*rHipT(0,1),
			  rHipT(1,1)+rHipT(2,1));
    else
      hipYawPitch = atan2(-SQRT2*rHipT(0,1),
			  rHipT(1,1)-rHipT(2,1));
    //  printf("hipYawPitch = %g\n", hipYawPitch);
  }

  double xAnkle[3];
  xAnkle[0] = 0;
  xAnkle[1] = 0;
  xAnkle[2] = footHeight;
  trLeg.apply(xAnkle);
  for (int i = 0; i < 3; i++) {
    xAnkle[i] -= xHipOffset[i];
  }

  if (left) {
    inv(rotYawPitchLeft(hipYawPitch)).apply(xAnkle);
  }
  else {
    inv(rotYawPitchRight(hipYawPitch)).apply(xAnkle);
  }
  //  for (int i = 0; i < 3; i++) printf("xAnkle[%d] = %g\n", i, xAnkle[i]);

  double hipRoll = atan2(xAnkle[1], -xAnkle[2]);
  double pitch1 = asin(tibiaLength*sin(kneePitch)/lLeg);
  double hipPitch = asin(-xAnkle[0]/lLeg) - pitch1;

  qLeg[0] = hipYawPitch;
  qLeg[1] = hipRoll;
  qLeg[2] = hipPitch;
  qLeg[3] = kneePitch;
  qLeg[4] = anklePitch;
  qLeg[5] = ankleRoll;
  return qLeg;
}