kinecalc.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*  Kinematics calculator implementation based on the analytical method by
 *  Gan et al. See:
 *  J.Q. Gan, E. Oyama, E.M. Rosales, and H. Hu, "A complete analytical
 *  solution to the inverse kinematics of the Pioneer 2 robotic arm,"
 *  Robotica, vol.23, no.1, pp.123-129, 2005.
 */

#include <libplayercore/playercommon.h>
#include "kinecalc.h"

#include <math.h>

#include <stdio.h>

KineCalc::KineCalc (void)
{
  link1 = 0.06875f;
  link2 = 0.16f;
  link3 = 0.0f;
  link4 = 0.13775f;
  link5 = 0.11321f;

  endEffector.p.x = 0.0f; endEffector.p.y = 0.0f; endEffector.p.z = 0.0f;
  endEffector.n.x = 0.0f; endEffector.n.y = 0.0f; endEffector.n.z = 0.0f;
  endEffector.o.x = 0.0f; endEffector.o.y = -1.0f; endEffector.o.z = 1.0f;
  endEffector.a.x = 1.0f; endEffector.a.y = 0.0f; endEffector.a.z = 0.0f;

  for (int ii = 0; ii < 5; ii++)
  {
    joints[ii] = 0.0f;
    jointOffsets[ii] = 0.0f;
    jointMin[ii] = 0.0f;
    jointMax[ii] = 0.0f;
  }
}


///////////////////////////////////////////////////////////////////////////////
//  Accessor functions
///////////////////////////////////////////////////////////////////////////////

void KineCalc::SetP (double newPX, double newPY, double newPZ)
{
  endEffector.p.x = newPX;
  endEffector.p.y = newPY;
  endEffector.p.z = newPZ;
}

void KineCalc::SetN (double newNX, double newNY, double newNZ)
{
  endEffector.n.x = newNX;
  endEffector.n.y = newNY;
  endEffector.n.z = newNZ;
}

void KineCalc::SetO (double newOX, double newOY, double newOZ)
{
  endEffector.o.x = newOX;
  endEffector.o.y = newOY;
  endEffector.o.z = newOZ;
}

void KineCalc::SetA (double newAX, double newAY, double newAZ)
{
  endEffector.a.x = newAX;
  endEffector.a.y = newAY;
  endEffector.a.z = newAZ;
}

double KineCalc::GetTheta (unsigned int index)
{
  return joints[index];
}

void KineCalc::SetTheta (unsigned int index, double newVal)
{
  joints[index] = newVal;
}

void KineCalc::SetLinkLengths (double newLink1, double newLink2, double newLink3, double newLink4, double newLink5)
{
  link1 = newLink1;
  link2 = newLink2;
  link3 = newLink3;
  link4 = newLink4;
  link5 = newLink5;
}

void KineCalc::SetOffset (unsigned int joint, double newOffset)
{
  jointOffsets[joint] = newOffset;
}

void KineCalc::SetJointRange (unsigned int joint, double min, double max)
{
  jointMin[joint] = MIN (min, max);   // So that if min > max we reverse them
  jointMax[joint] = MAX (min, max);
}



///////////////////////////////////////////////////////////////////////////////
//  Utility helper functions
///////////////////////////////////////////////////////////////////////////////

KineVector KineCalc::Normalise (const KineVector &vector)
{
  KineVector result;
  double length = sqrt (vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
  result.x = vector.x / length;
  result.y = vector.y / length;
  result.z = vector.z / length;
  return result;
}

KineVector KineCalc::CalculateN (const EndEffector &pose)
{
  KineVector result;
  result.x = pose.o.y * pose.a.z - pose.a.y * pose.o.z;
  result.y = pose.o.z * pose.a.x - pose.a.z * pose.o.x;
  result.z = pose.o.x * pose.a.y - pose.a.x * pose.o.y;
  return Normalise (result);
}

void KineCalc::PrintEndEffector (const EndEffector &endEffector)
{
  printf ("P: (%f, %f, %f)\tA: (%f, %f, %f)\tO: (%f, %f, %f)\tN: (%f, %f, %f)\n",
          endEffector.p.x, endEffector.p.y, endEffector.p.z,
          endEffector.a.x, endEffector.a.y, endEffector.a.z,
          endEffector.o.x, endEffector.o.y, endEffector.o.z,
          endEffector.n.x, endEffector.n.y, endEffector.n.z);
}


///////////////////////////////////////////////////////////////////////////////
//  The important functions
///////////////////////////////////////////////////////////////////////////////

//  Calculate the forward kinematics
//  The result is stored in endEffector
//  fromJoints[]:   An array of 5 joint values
void KineCalc::CalculateFK (const double fromJoints[])
{
  double adjustedJoints[5];

  adjustedJoints[0] = (fromJoints[0] - jointOffsets[0]) * -1;
  adjustedJoints[1] = fromJoints[1] - jointOffsets[1];
  adjustedJoints[2] = fromJoints[2] - jointOffsets[2];
  adjustedJoints[3] = (fromJoints[3] - jointOffsets[3]) * -1;;
  adjustedJoints[4] = (fromJoints[4] - jointOffsets[4]) * -1;;

  endEffector = CalcFKForJoints (adjustedJoints);
//  printf ("Result of FK:\n");
//  PrintEndEffector (endEffector);
}

//  Calculate the inverse kinematics
//  The result is stored in joints
//  fromPosition:   An EndEffector structure describing the pose
//                  of the end effector
bool KineCalc::CalculateIK (const EndEffector &fromPosition)
{
  // Some references to make the code neater
  const KineVector &p = fromPosition.p;
  const KineVector &a = fromPosition.a;
  // These are the four possible solutions to the IK
  // solution1 = {1a, 2a, 3a, 4a, 5a}
  // solution2 = {1a, 2b, 3b, 4b, 5b}
  // solution3 = {1b, 2c, 3c, 4c, 5c}
  // solution4 = {1b, 2d, 3d, 4d, 5d}
  double solutions[4][5];
  double temp = 0.0f;

  // First calculate the two possible values for theta1, theta1a and theta1b
  temp = atan2 (p.y - link5 * a.y, p.x - link5 * a.x);
  solutions[0][0] = solutions[1][0] = temp;
  temp = atan2 (link5 * a.y - p.y, link5 * a.x - p.x);
  solutions[2][0] = solutions[3][0] = temp;

  // Next, using theta1_a, calculate thetas 2 and 3 (a and b)
  // First up is calculating r and rz
  double r = 0.0f, rz = 0.0f;
  if (sin (solutions[0][0]) < 0.1f || sin(solutions[0][0]) > -0.1f)
  {
    r = ((p.x - (link5 * a.x)) / cos (solutions[0][0])) - link1;
  }
  else
  {
    r = ((p.y - (link5 * a.y)) / sin (solutions[0][0])) - link1;
  }
  rz = p.z - (link5 * a.z);
  // Then calculate theta2a and 3a
  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);
  int m1 = -1;
  do
  {
    if (m1 > 1)
    {
      printf ("m1 > 1!\n");
      break;
    }
    solutions[0][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[0][1] < -(M_PI) || solutions[0][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[0][2] = M_PI - acos (temp);
  // Followed by theta2b and 3b
  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;
    }