kinecalc.cc

机器人仿真软件

    solutions[1][1] = temp + 2 * m1 * M_PI;
    m1 += 1;  // So that within the 3 iterations we get m1 = -1, 0, 1
  }
  while ((solutions[1][1] < -(M_PI) || solutions[1][1] > M_PI));// && m1 < 1);
  temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
  temp = MIN (MAX (temp, -1.0f), 1.0f);
  solutions[1][2] = -(M_PI) + acos (temp);

  // Using theta2a and 3a, calculate 4a and 5a to complete solution1
  CalcTheta4and5 (solutions[0], fromPosition);
  // Using theta2b and 3b, calculate 4b and 5b to complete solution2
  CalcTheta4and5 (solutions[1], fromPosition);

  // That's two of the possible solutions. To get the other two, repeat with theta1b
  // First up is calculating r and rz
  r = 0.0f;
  rz = 0.0f;
  if (sin (solutions[2][0]) < 0.1f || sin(solutions[2][0]) > -0.1f)
  {
    r = (p.x - link5 * a.x) / cos (solutions[2][0]) - link1;
  }
  else
  {
    r = (p.y - link5 * a.y) / sin (solutions[2][0]) - link1;
  }
  rz = p.z - (link5 * a.z);
  // Then calculate theta2c and 3c
  temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
  temp = MIN (MAX (temp, -1.0f), 1.0f);
  temp = atan2 (rz, r) - acos (temp);
  m1 = -1;
  do
  {
    if (m1 > 1)
    {
      break;
    }
    solutions[2][1] = temp + 2 * m1 * M_PI;
    m1 += 1;  // So that within the 3 iterations we get m1 = -1, 0, 1
  } // Put a catchall here to prevent infinite loops by checking if m1 has gone past 1 (shouldn't happen)
  while ((solutions[2][1] < -(M_PI) || solutions[2][1] > M_PI));// && m1 < 1);
  temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
  temp = MIN (MAX (temp, -1.0f), 1.0f);
  solutions[2][2] = M_PI - acos (temp);
  // Followed by theta2d and 3d
  temp = (r * r + rz * rz + link2 * link2 - link4 * link4) / (2 * link2 * sqrt (r * r + rz * rz));
  temp = MIN (MAX (temp, -1.0f), 1.0f);
  temp = atan2 (rz, r) + acos (temp);
  m1 = -1;
  do
  {
    if (m1 > 1)
    {
      break;
    }
    solutions[3][1] = temp + 2 * m1 * M_PI;
    m1 += 1;  // So that within the 3 iterations we get m1 = -1, 0, 1
  }
  while ((solutions[3][1] < -(M_PI) || solutions[3][1] > M_PI));// && m1 < 1);
  temp = (link2 * link2 + link4 * link4 - r * r - rz * rz) / (2 * link2 * link4);
  temp = MIN (MAX (temp, -1.0f), 1.0f);
  solutions[3][2] = -(M_PI) + acos (temp);

  // Using theta2c and 3c, calculate 4c and 5c to complete solution1
  CalcTheta4and5 (solutions[2], fromPosition);
  // Using theta2d and 3d, calculate 4d and 5d to complete solution2
  CalcTheta4and5 (solutions[3], fromPosition);

  // Choose the best of the four solutions
  int chosenSolution = ChooseSolution (fromPosition, solutions);
  if (chosenSolution == -1)
    // Couldn't find a valid solution
    return false;

  // Offsets and so forth
  joints[0] = (solutions[chosenSolution][0] * -1) + jointOffsets[0];
  joints[1] = solutions[chosenSolution][1] + jointOffsets[1];
  joints[2] = solutions[chosenSolution][2] + jointOffsets[2];
  joints[3] = (solutions[chosenSolution][3] * -1) + jointOffsets[3];
  joints[4] = (solutions[chosenSolution][4] * -1) + jointOffsets[4];

  return true;
}

//  Calculates thetas 4 and 5 based on supplied thetas 1, 2 and 3 and the desired end effector pose
//  angles[]:       A 5-element array, of which elements 0, 1 and 2 should be filled already
//  fromPosition:   The desired end effector pose
void KineCalc::CalcTheta4and5 (double angles[], const EndEffector &fromPosition)
{
  const KineVector &n = fromPosition.n;
  const KineVector &o = fromPosition.o;
  const KineVector &a = fromPosition.a;

  double cos1 = cos (angles[0]);
  double cos23 = cos (angles[1] + angles[2]);
  double sin1 = sin (angles[0]);
  double sin23 = sin (angles[1] + angles[2]);

  if (cos23 != 0.0f)
  {
    if (sin1 < -0.1f || sin1 > 0.1f)
    {
      angles[3] = atan2 (n.z / cos23, -(n.x + ((n.z * cos1 * sin23) / cos23)) / sin1);
    }
    else
    {
      angles[3] = atan2 (n.z / cos23, (n.y + ((n.z * sin1 * sin23) / cos23)) / cos1);
    }

    double cos4 = cos (angles[3]);
    double sin4 = sin (angles[3]);
    if (cos4 != 0 || sin23 != 0)
    {
      angles[4] = atan2 (a.z * cos23 * cos4 - o.z * sin23, o.z * cos23 * cos4 + a.z * sin23);
    }
    else
    {
      angles[4] = atan2 (-(o.x * cos1 + o.y * sin1) / cos23, (o.x * sin1 - o.y * cos1) / sin4);
    }
  }
  else
  {
    angles[4] = atan2 (-o.z / sin23, a.z / sin23);

    double cos5 = cos (angles[4]);
    double sin5 = sin (angles[4]);
    if (cos5 > -0.1f || cos5 < 0.1f)
    {
      angles[3] = atan2 ((a.x * sin1 - a.y * cos1) / sin5, -(n.x * sin1) + n.y * cos1);
    }
    else
    {
      angles[3] = atan2 ((o.x * sin1 - o.y * cos1) / cos5, -(n.x * sin1) + n.y * cos1);
    }
  }
}

//  Choose the best solution from the 4 available based on error and reachability
//  fromPosition:   The desired end effector pose
//  solutions[][]:  The four solutions (each with 5 angles) in an array
int KineCalc::ChooseSolution (const EndEffector &fromPosition, const double solutions[][5])
{
  double errors[4];
  int order[4], jj;

  // We have 4 solutions, calculate the error for each one
  errors[0] = CalcSolutionError (solutions[0], fromPosition);
  errors[1] = CalcSolutionError (solutions[1], fromPosition);
  errors[2] = CalcSolutionError (solutions[2], fromPosition);
  errors[3] = CalcSolutionError (solutions[3], fromPosition);

  for (int ii = 0; ii < 4; ii++)
  {
    double min = MIN (errors[0], MIN (errors[1], MIN (errors[2], errors[3])));
    for (jj = 0; min != errors[jj]; jj++);  // Find the index at which the min is at
    errors[jj] = 999999;
    order[ii] = jj;
  }

  for (int ii = 0; ii < 4; ii++)
  {
    if (SolutionInRange (solutions[order[ii]]))
    {
      return order[ii];
    }
  }

  return -1;
}

//  Calculate the error for a solution from the desired pose
//  solution[]:       An array of 5 angles
//  fromPosition[]:   The end effector pose
double KineCalc::CalcSolutionError (const double solution[], const EndEffector &fromPosition)
{
  EndEffector solutionPos;
  double error = 0.0f;

  // Calculate the position of the end effector this solution gives using FK
  solutionPos = CalcFKForJoints (solution);
  // Calculate the distance from this to the desired position
  double xOffset = solutionPos.p.x - fromPosition.p.x;
  double yOffset = solutionPos.p.y - fromPosition.p.y;
  double zOffset = solutionPos.p.z - fromPosition.p.z;

  error = sqrt (xOffset * xOffset + yOffset * yOffset + zOffset * zOffset);
  if (isnan (error))
    error = 9999;

  return error;
}

//  Calculates the forward kinematics of a set of joint angles
//  angles[]:   The 5 angles to calculate from
EndEffector KineCalc::CalcFKForJoints (const double angles[])
{
  EndEffector result;

  double cos1 = cos (angles[0]);
  double cos2 = cos (angles[1]);
  double cos23 = cos (angles[1] + angles[2]);
  double cos4 = cos (angles[3]);
  double cos5 = cos (angles[4]);
  double sin1 = sin (angles[0]);
  double sin2 = sin (angles[1]);
  double sin23 = sin (angles[1] + angles[2]);
  double sin4 = sin (angles[3]);
  double sin5 = sin (angles[4]);

  result.p.x = link5 * ((cos1 * cos23 * cos5) + (sin1 * sin4 * sin5) - (cos1 * sin23 * cos4 * sin5)) +
      cos1 * ((link4 * cos23) + (link2 * cos2) + link1);
  result.p.y = link5 * ((sin1 * cos23 * cos5) + (cos1 * sin4 * sin5) - (sin1 * sin23 * cos4 * sin5)) +
      sin1 * ((link4 * cos23) + (link2 * cos2) + link1);
  result.p.z = link5 * ((sin23 * cos5) + (cos23 * cos4 * sin5)) + (link4 * sin23) + (link2 * sin2);

  result.n.x = -(sin1 * cos4) - (cos1 * sin23 * sin4);
  result.n.y = (cos1 * cos4) - (sin1 * sin23 * sin4);
  result.n.z = (cos23 * sin4);

  result.o.x = -(cos1 * cos23 * sin5) + (sin1 * sin4 * cos5) - (cos1 * sin23 * cos4 * cos5);
  result.o.y = -(sin1 * cos23 * sin5) - (cos1 * sin4 * cos5) - (sin1 * sin23 * cos4 * cos5);
  result.o.z = -(sin23 * sin5) + (cos23 * cos4 * cos5);

  result.a.x = (cos1 * cos23 * cos5) + (sin1 * sin4 * sin5) - (cos1 * sin23 * cos4 * sin5);
  result.a.y = (sin1 * cos23 * cos5) + (cos1 * sin4 * sin5) - (sin1 * sin23 * cos4 * sin5);
  result.a.z = (sin23 * cos5) + (cos23 * cos4 * sin5);

  return result;
}

//  Checks if the angles for a solution are reachable by the arm
//  angles[]:   The 5 angles to check
bool KineCalc::SolutionInRange (const double angles[])
{
  for (int ii = 0; ii < 5; ii++)
  {
    if (angles[ii] < jointMin[ii] || angles[ii] > jointMax[ii])
      return false;
  }

  return true;
}