kinecalc.cc

Go to the documentation of this file.

/*
 *  P2OS for ROS
 *  Copyright (C) 2015
 *     Hunter Allen, David Feil-Seifer, 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
 *
 */
#include <stdio.h>
#include <math.h>

#include <p2os_driver/kinecalc.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] = fmin(min, max);   // So that if min > max we reverse them
        jointMax[joint] = fmax(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);
        if (length != 0)
        {
                result.x = vector.x / length;
                result.y = vector.y / length;
                result.z = vector.z / length;
        }
        else
        {
                printf ("P2OS: Tried to normalise a vector of zero length.\n");
                result.x = 0;
                result.y = 0;
                result.z = 0;
        }
        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;
        if (result.x == 0 && result.y == 0 && result.z == 0)
        {
                printf ("P2OS: Approach and orientation cannot be the same vector - their cross product cannot be zero.\n");
                // Ensures that a different orientation vector is created
                KineVector orient;
                if (pose.a.y == 0 && pose.a.z == 0)
                {
                        orient.x = 0;
                        orient.y = 1;
                        orient.z = 0;
                }
                else
                {
                        orient.x = 1;
                        orient.y = 0;
                        orient.z = 0;
                }
                result.x = orient.y * pose.a.z - pose.a.y * orient.z;
                result.y = orient.z * pose.a.x - pose.a.z * orient.x;
                result.z = orient.x * pose.a.y - pose.a.x * orient.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 = fmin (fmax (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 = fmin (fmax (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 = fmin (fmax (temp, -1.0f), 1.0f);
        temp = atan2 (rz, r) + acos (temp);
        m1 = -1;
        do
        {
                if (m1 > 1)
                {
                        break;
                }
                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 = fmin (fmax (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 = fmin (fmax (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 = fmin (fmax (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 = fmin (fmax (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 = fmin (fmax (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 = fmin (errors[0], fmin (errors[1], fmin(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] || isnan(angles[ii]))
                        return false;
        }

        return true;
}