WristMechanism.cpp

Go to the documentation of this file.

#include "robodyn_mechanisms/WristMechanism.h"
#include "nasa_common_logging/Logger.h"
#include <Eigen/LU>

using namespace Eigen;
using namespace std;
using namespace log4cpp;

WristMechanism::WristMechanism()
    : logCategory("gov.nasa.robodyn.WristMechanism")
{
    maxIt        = 20;
    eps          = 0.001;
    isCalibrated = false;
}

WristMechanism::~WristMechanism()
{
}

void WristMechanism::setSliderOffsetGain(Vector2d sliderOffset, Vector2d sliderGain)
{
    this->sliderOffset = sliderOffset;
    this->sliderGain = sliderGain;
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Slider Offset: " << sliderOffset;
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Slider Gain: " << sliderGain;
}

void WristMechanism::setAngleOffsetGain(Vector2d angleOffset, Vector2d angleGain)
{
    this->angleOffset = angleOffset;
    this->angleGain  = angleGain;
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG
            << "Angle Offset: " << angleOffset << "\r\n"
            << "Angle Gain: " << angleGain;
}

void WristMechanism::loadDesignParams(Vector3d A, Vector3d A0, Vector3d B, Vector3d C, Vector3d D, Vector3d D0, Vector3d Pitch, Vector3d Yaw, Vector3d M, Vector3d N, double linkLength)
{
    this->linkLength = linkLength;

    //shift origin to pitch center
    alpha = A0  - Pitch;
    beta  = B   - Yaw;
    gamma = C   - Yaw;
    delta = D0  - Pitch;
    p     = Yaw - Pitch;

    // find unit vector for 1st slider axis
    Vector3d uMag = A - A0;
    double uNorm   = uMag.norm();

    if (uNorm != 0)
    {
        u = uMag / uNorm;
    }
    else
    {
        std::stringstream err;
        err << "in loadDesignParams A and A0 cannot be the same or produce a normal of 0";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    // find unit vector for 2nd slider axis
    Vector3d vMag = D - D0;
    double vNorm   = vMag.norm();

    if (vNorm != 0)
    {
        v = vMag / vNorm;
    }
    else
    {
        std::stringstream err;
        err << "in loadDesignParams D and D0 cannot be the same or produce a normal of 0";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    // find length^2
    lengthSq = linkLength * linkLength;

    m = M;
    n = N;

    R1m = AA2Rot(m, ONE);
    R1n = AA2Rot(n, ONE);
    Rsm = AA2Rot(m, SIN);
    Rsn = AA2Rot(n, SIN);
    Rcm = AA2Rot(m, COS);
    Rcn = AA2Rot(n, COS);

    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG
            << "a: (" << alpha(0) << "," << alpha(1) << "," << alpha(2) << ")\r\n"
            << "b: (" << beta(0) << "," << beta(1) << "," << beta(2) << ")\r\n"
            << "c: (" << gamma(0) << "," << gamma(1) << "," << gamma(2) << ")\r\n"
            << "d: (" << delta(0) << "," << delta(1) << "," << delta(2) << ")\r\n"
            << "m: (" << m(0) << "," << m(1) << "," << m(2) << ")\r\n"
            << "n: (" << n(0) << "," << n(1) << "," << n(2) << ")\r\n"
            << "u: (" << u(0) << "," << u(1) << "," << u(2) << ")\r\n"
            << "v: (" << v(0) << "," << v(1) << "," << v(2) << ")\r\n"
            << "p: (" << p(0) << "," << p(1) << "," << p(2) << ")\r\n"
            << "link length: " << linkLength;

}

void WristMechanism::setLimits(const double& upperPitchMin,
                               const double& upperPitchMax,
                               const double& lowerPitchMin,
                               const double& lowerPitchMax,
                               const double& upperYawMin,
                               const double& upperYawMax,
                               const double& lowerYawMin,
                               const double& lowerYawMax)
{
    this->upperPitchMin = upperPitchMin;
    this->upperPitchMax = upperPitchMax;
    this->lowerPitchMin = lowerPitchMin;
    this->lowerPitchMax = lowerPitchMax;
    this->upperYawMin = upperYawMin;
    this->upperYawMax = upperYawMax;
    this->lowerYawMin = lowerYawMin;
    this->lowerYawMax = lowerYawMax;

    getLineFromPoints(upperYawMax, upperYawMin, upperPitchMin, upperPitchMax, mUpperPitchUpperYaw, bUpperPitchUpperYaw);
    getLineFromPoints(upperYawMax, upperYawMin, lowerPitchMax, lowerPitchMin, mLowerPitchUpperYaw, bLowerPitchUpperYaw);
    getLineFromPoints(lowerYawMax, lowerYawMin, lowerPitchMin, lowerPitchMax, mLowerPitchLowerYaw, bLowerPitchLowerYaw);
    getLineFromPoints(lowerYawMin, lowerYawMax, upperPitchMin, upperPitchMax, mUpperPitchLowerYaw, bUpperPitchLowerYaw);

    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG
            << "Upper Pitch Max: " << upperPitchMax << "\r\n"
            << "Upper Pitch Min: " << upperPitchMin << "\r\n"
            << "Lower Pitch Max: " << lowerPitchMax << "\r\n"
            << "Lower Pitch Min: " << lowerPitchMin << "\r\n"
            << "  Upper Yaw Max: " << upperYawMax << "\r\n"
            << "  Upper Yaw Min: " << upperYawMin << "\r\n"
            << "  Lower Yaw Max: " << lowerYawMax << "\r\n"
            << "  Lower Yaw Min: " << lowerYawMin << "\r\n"
            << "UpperPitchUpperYaw equation: y=" << mUpperPitchUpperYaw << "x + " << bUpperPitchUpperYaw << "\r\n"
            << "UpperPitchLowerYaw equation: y=" << mUpperPitchLowerYaw << "x + " << bUpperPitchLowerYaw << "\r\n"
            << "LowerPitchUpperYaw equation: y=" << mLowerPitchUpperYaw << "x + " << bLowerPitchUpperYaw << "\r\n"
            << "LowerPitchLowerYaw equation: y=" << mLowerPitchLowerYaw << "x + " << bLowerPitchLowerYaw;
}

void WristMechanism::getLimits(double pitch, double yaw, double& upperPitch, double& lowerPitch, double& upperYaw, double& lowerYaw)
{
    upperPitch = upperPitchMax;
    lowerPitch = lowerPitchMin;
    upperYaw = upperYawMax;
    lowerYaw = lowerYawMin;

    if (yaw > upperYawMax)
    {
        yaw = upperYawMax;
    }
    else if (yaw < lowerYawMin)
    {
        yaw = lowerYawMin;
    }

    if (pitch > upperPitchMax)
    {
        pitch = upperPitchMax;
    }
    else if (pitch < lowerPitchMin)
    {
        pitch = lowerPitchMin;
    }

    if (pitch > upperPitchMin && yaw > upperYawMin)
    {
        getNearest(mUpperPitchUpperYaw, bUpperPitchUpperYaw, yaw, pitch, upperYaw, upperPitch);
        upperYaw = std::max(upperYawMin, upperYaw);
        upperPitch = std::max(upperPitchMin, upperPitch);
    }
    else if (pitch > upperPitchMin && yaw < lowerYawMax)
    {
        getNearest(mUpperPitchLowerYaw, bUpperPitchLowerYaw, yaw, pitch, lowerYaw, upperPitch);
        lowerYaw = std::min(lowerYawMax, lowerYaw);
        upperPitch = std::max(upperPitchMin, upperPitch);
    }
    else if (pitch < lowerPitchMax && yaw > upperYawMin)
    {
        getNearest(mLowerPitchUpperYaw, bLowerPitchUpperYaw, yaw, pitch, upperYaw, lowerPitch);
        upperYaw = std::max(upperYawMin, upperYaw);
        lowerPitch = std::min(lowerPitchMax, lowerPitch);
    }
    else if (pitch < lowerPitchMax && yaw < lowerYawMax)
    {
        getNearest(mLowerPitchLowerYaw, bLowerPitchLowerYaw, yaw, pitch, lowerYaw, lowerPitch);
        lowerYaw = std::min(lowerYawMax, lowerYaw);
        lowerPitch = std::min(lowerPitchMax, lowerPitch);
    }
}

void WristMechanism::applyLimits(double& pitch, double& yaw)
{
    double upperPitch, lowerPitch, upperYaw, lowerYaw;
    getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);

    if (pitch > upperPitch)
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "limiting pitch from: " << pitch << " to: " << upperPitch;
        pitch = upperPitch;
    }
    else if (pitch < lowerPitch)
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "limiting pitch from: " << pitch << " to: " << lowerPitch;
        pitch = lowerPitch;
    }

    if (yaw > upperYaw)
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "limiting yaw from: " << yaw << " to: " << upperYaw;
        yaw = upperYaw;
    }
    else if (yaw < lowerYaw)
    {
        NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "limiting yaw from: " << yaw << " to: " << lowerYaw;
        yaw = lowerYaw;
    }
}

void WristMechanism::getNearest(double m, double b, double x_in, double y_in, double& x_out, double& y_out)
{
    x_out = (m * y_in + x_in - m * b) / (m * m + 1);
    y_out = (m * m * y_in + m * x_in + b) / (m * m + 1);
}

void WristMechanism::getLineFromPoints(double x1, double x2, double y1, double y2, double& m, double& b)
{
    if (fabs(x2 - x1) < eps)
    {
        throw std::runtime_error("in WristMechanism::getLineFromPoints denominator is zero!");
    }

    m = (y2 - y1) / (x2 - x1);
    b = y1 - m * x1;
}

int WristMechanism::doFwdKin(Vector2d& ang, Vector2d pos, Vector2d lastang)
{
    if (angleGain(0) == 0.0)
    {
        stringstream err;
        err << "angleGain(0) must be non-zero. Provided value of [" << angleGain(0) << "] is invalid.";
        NasaCommonLogging::Logger::log("gov.nasa.robodyn.WristMechanism", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    if (angleGain(1) == 0.0)
    {
        stringstream err;
        err << "angleGain(0) must be non-zero. Provided value of [" << angleGain(1) << "] is invalid.";
        NasaCommonLogging::Logger::log("gov.nasa.robodyn.WristMechanism", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    lastang(0) = (lastang(0) - angleOffset(0)) / angleGain(0);
    lastang(1) = (lastang(1) - angleOffset(1)) / angleGain(1);

    ang = Vector2d::Zero();

    Matrix3d AB = findFwdKinMatrix(alpha, beta, this->u, pos(0));
    Matrix3d DC = findFwdKinMatrix(delta, gamma, this->v, pos(1));

    MatrixXd Cfk = MatrixXd::Zero(8, 8);
    MatrixXd Dfk = MatrixXd::Zero(8, 8);
    MatrixXd Efk = MatrixXd::Zero(8, 8);
    MatrixXd Ffk = MatrixXd::Zero(8, 8);


    Cfk <<  AB(0, 0), AB(0, 1), AB(0, 2), 0,       AB(1, 0), AB(1, 1), AB(2, 0), AB(2, 1),
        0,       0,       0,       0,       AB(0, 0), AB(0, 1), AB(1, 0), AB(1, 1),
        0,       AB(0, 0), AB(0, 1), AB(0, 2), 0,       AB(1, 0), 0,       AB(2, 0),
        0,       0,       0,       0,       0,       AB(0, 0), 0,       AB(1, 0),
        DC(0, 0), DC(0, 1), DC(0, 2), 0,       DC(1, 0), DC(1, 1), DC(2, 0), DC(2, 1),
        0,       0,       0,       0,       DC(0, 0), DC(0, 1), DC(1, 0), DC(1, 1),
        0,       DC(0, 0), DC(0, 1), DC(0, 2), 0,       DC(1, 0), 0,       DC(2, 0),
        0,       0,       0,       0,       0,       DC(0, 0), 0,       DC(1, 0);



    Dfk <<  AB(1, 2), 0,       AB(2, 2), 0,       0,       0,       0,       0,
        AB(0, 2), 0,       AB(1, 2), 0,       AB(2, 0), AB(2, 1), AB(2, 2), 0,
        AB(1, 1), AB(1, 2), AB(2, 1), AB(2, 2), 0,       0,       0,       0,
        AB(0, 1), AB(0, 2), AB(1, 1), AB(1, 2), 0,       AB(2, 0), AB(2, 1), AB(2, 2),
        DC(1, 2), 0,       DC(2, 2), 0,       0,       0,       0,       0,
        DC(0, 2), 0,       DC(1, 2), 0,       DC(2, 0), DC(2, 1), DC(2, 2), 0,
        DC(1, 1), DC(1, 2), DC(2, 1), DC(2, 2), 0,       0,       0,       0,
        DC(0, 1), DC(0, 2), DC(1, 1), DC(1, 2), 0,       DC(2, 0), DC(2, 1), DC(2, 2);


    Efk <<  0, 0, 0, 0, 1, 0, 0, 0,
        0, 0, 0, 0, 0, 1, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 1, 0,
        0, 0, 0, 0, 0, 0, 0, 1,
        0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0;

    Ffk <<  0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0,
        1, 0, 0, 0, 0, 0, 0, 0,
        0, 1, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 1, 0, 0, 0,
        0, 0, 0, 0, 0, 1, 0, 0;


    MatrixXd Dfki = Dfk.lu().inverse();

    MatrixXd Mfk = (Efk - (Ffk * Dfki * Cfk));

    EigenSolver<MatrixXd> es(Mfk);

    // the eigenvalues are solutions for x
    VectorXcd xtheta = es.eigenvalues();

    // the eigenvectors give y as y= m2'' / m1''
    MatrixXcd ephi = es.eigenvectors();

    VectorXcd yphi = VectorXcd::Zero(8);

    if (ephi.rows() != 8)
    {
        std::stringstream err;
        err << "in doFwdKin there should be 8 rows in ephi";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    if (ephi.cols() != 8)
    {
        std::stringstream err;
        err << "in doFwdKin there should be 8 cols in ephi";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    if (yphi.rows() != 8)
    {
        std::stringstream err;
        err << "in doFwdKin there should be 8 rows in yphi";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    std::complex<double> zero(0, 0);

    for (unsigned int i = 0; i < ephi.rows(); i++)
    {
        if (ephi(0, i) != zero)
        {
            yphi(i) = ephi(1, i) / ephi(0, i);
        }
        else
        {
            std::stringstream err;
            err << "in doFwdKin -- divide by zero error in calculating yphi for " << ephi(i, 1) << ", M1'' is zero " << ephi(i, 0);
            NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
            throw std::runtime_error(err.str());
        }

    }

    std::vector<double> ptheta;
    std::vector<double> pphi;
    double             tantheta, tanphi, sintheta, costheta, theta, sinphi, cosphi, phi, dist;

    ptheta.clear();
    pphi.clear();

    // remove complex solutions
    for (unsigned int i = 0; i < xtheta.rows(); i++)
    {
        if (xtheta(i).imag() == 0 && yphi(i).imag() == 0)
        {
            //solve for theta and phi
            tantheta = xtheta(i).real();
            tanphi   = yphi(i).real();

            sintheta = (2 * tantheta) / (1 + tantheta * tantheta);
            costheta = (1 - tantheta * tantheta) / (1 + tantheta * tantheta);
            theta    = atan2(sintheta, costheta);

            sinphi = 2 * tanphi / (1 + tanphi * tanphi);
            cosphi = (1 - tanphi * tanphi) / (1 + tanphi * tanphi);
            phi    = atan2(sinphi, cosphi);

            //remove solutions outside of joint space for manipulator (within 5 degrees of limits to account for accumulated error)
            if (theta > lowerPitchMin - .1 && theta < upperPitchMax + .1 &&
                    phi > lowerYawMin - .1 && phi < upperYawMax + .1)
            {
                //std::cout<<"added (" <<theta <<","<<phi<<")"<<std::endl;
                ptheta.push_back(theta);
                pphi.push_back(phi);
            }


        }
    }

    if (ptheta.size() != pphi.size())
    {
        std::stringstream err;
        err << "in doFwdKin vectors ptheta and pphi should be the same size";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
        return -1;
    }


    // choose solution nearest to previous solution
    if (ptheta.size() > 1 && pphi.size() > 1)
    {
        unsigned int i = 0;

        theta = ptheta.at(i);
        phi   = pphi.at(i);

        dist = sqrt(pow(theta - lastang(0), 2.0) + pow(phi - lastang(1), 2.0));

        ang(0) = theta;
        ang(1) = phi;

        i++;

        for (; i < ptheta.size(); i++)
        {
            theta = ptheta.at(i);
            phi   = pphi.at(i);

            if (dist > sqrt(pow((theta - lastang(0)), 2.0) + pow((phi - lastang(1)), 2.0)))
            {
                dist = sqrt(pow((theta - lastang(0)), 2.0) + pow((phi - lastang(1)), 2.0));

                ang(0) = theta;
                ang(1) = phi;
            }
        }
    }
    else if (ptheta.size() == 1 && pphi.size() == 1)
    {

        ang(0) = ptheta.at(0);
        ang(1) = pphi.at(0);
    }
    else
    {
        return -1;
    }

//    //do newton's method
//    Vector2d dang = NewtonsMethod(ang, pos);
//    int maxit = maxIt;
//    while(maxit > 0 && (fabs(dang(0)) > eps || fabs(dang(1)) > eps))
//    {
//        ang = ang-dang;
//        dang = NewtonsMethod(ang, pos);
//        maxit--;

//        //std::cout<<"iterating"<<std::endl;
//    }

//    std::cout<<"iterations: "<<maxIt-maxit<<std::endl;

//    if(abs(dang(0))> eps || abs(dang(1)) > eps)
//    {
//        std::stringstream err;
//        err << "Fwd Kinematics solution not good"<<std::endl;
//        err << "ang: "<<std::endl<<ang<<std::endl;
//        err << "dang: "<<std::endl<<dang<<std::endl;
//        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::WARN, err.str());
//        return -1;
//    }

    return 0;

}

Matrix3d WristMechanism::findFwdKinMatrix(Vector3d a, Vector3d b, Vector3d u, double q)
{
    Matrix3d aOut;

    Vector3d    ahat  = a + q * u;
    RowVector3d pT    = p.transpose();
    RowVector3d ahatT = ahat.transpose();

    double a1, b1, c1, d1, e1, f1, g1, h1, k1;

    a1        = p.dot(p) + b.dot(b) + ahat.dot(ahat) - lengthSq + 2 * pT * R1n * b - 2 * ahatT * R1m * (p + R1n * b);
    double b11 = -2 * ahatT * R1m * Rsn * b;
    b1        = b11 + 2 * pT * Rsn * b ;
    double c11 = -2 * ahatT * R1m * Rcn * b;
    c1        = c11 + 2 * pT * Rcn * b ;
    d1        = -2 * ahatT * Rsm * (p + R1n * b);
    e1        = -2 * ahatT * Rsm * Rsn * b;
    f1        = -2 * ahatT * Rsm * Rcn * b;
    g1        = -2 * ahatT * Rcm * (p + R1n * b);
    h1        = -2 * ahatT * Rcm * Rsn * b;
    k1        = -2 * ahatT * Rcm * Rcn * b;


    aOut(0, 0) = a1 + c1 + g1 + k1;
    aOut(1, 0) = 2 * d1 + 2 * f1;
    aOut(2, 0) = a1 + c1 - g1 - k1;

    aOut(0, 1) = 2 * b1 + 2 * h1;
    aOut(1, 1) = 4 * e1;
    aOut(2, 1) = 2 * b1 - 2 * h1;

    aOut(0, 2) = a1 - c1 + g1 - k1;
    aOut(1, 2) = 2 * d1 - 2 * f1;
    aOut(2, 2) = a1 - c1 - g1 + k1;

    return aOut;
}

int WristMechanism::doInvKin(Vector2d& pos, Vector2d ang)
{
    int   success = 0;
    double pitch   = ang(0);
    double yaw     = ang(1);

    //applyLimits(pitch, yaw);

    double    bravo, charlie, david;
    Vector3d r1, r2;

    // find rotation matrices around the pitch axis
    Matrix3d Rpitch = AA2Rot(m, pitch);
    // find rotation matrices around the yaw axis
    Matrix3d Ryaw = AA2Rot(n, yaw);


    r1 = Ryaw * beta;
    r2 = Rpitch * (p + r1);

    bravo   = -2 * u.dot(r2) + 2 * alpha.dot(u);
    charlie = p.dot(p) + 2 * p.dot(r1) + beta.dot(beta) - 2 * alpha.dot(r2) + alpha.dot(alpha) - lengthSq;
    david   = bravo * bravo - 4 * charlie;

    if (david >= 0)
    {
        pos(0) = -0.5 * bravo - 0.5 * sqrt(david);
    }
    else
    {
        success = -1;
    }

    r1 = Ryaw * gamma;
    r2 = Rpitch * (p + r1);

    bravo   = -2 * v.dot(r2) + 2 * delta.dot(v);
    charlie = p.dot(p) + 2 * p.dot(r1) + gamma.dot(gamma) - 2 * delta.dot(r2) + delta.dot(delta) - lengthSq;
    david   = bravo * bravo - 4 * charlie;

    if (david >= 0)
    {
        pos(1) = -0.5 * bravo - 0.5 * sqrt(david);
    }
    else
    {
        success = -1;
    }

    return success;
}


Vector2d WristMechanism::getAngleFromSlider(Vector2d slider)
{
    Vector2d ang;

    if (doFwdKin(ang, slider, lastAng) != 0)
    {
        std::stringstream err;
        err << "Could not find fwd kinematic solution";
        throw std::runtime_error(err.str());
    }

    //std::cout<<"angle out of fwd kin: "<<std::endl<<ang<<std::endl;
    ang(0) = ang(0) * angleGain(0) + angleOffset(0);
    ang(1) = ang(1) * angleGain(1) + angleOffset(1);
    lastAng = ang;

    return ang;
}

Vector2d WristMechanism::getSliderFromWristEncoder(Vector2d encoder)
{
    if (!isCalibrated)
    {
        throw std::runtime_error("wrist has not been calibrated");
    }

    Vector2d encoderTared = encoder - encoderTarePos;

    Vector2d taredSliderEncoderPos = encoderTared + calSliderPos;

    Vector2d sliderPos;

    sliderPos(0) = taredSliderEncoderPos(0) * sliderGain(0) + sliderOffset(0);
    sliderPos(1) = taredSliderEncoderPos(1) * sliderGain(1) + sliderOffset(1);

    return sliderPos;
}

Vector2d WristMechanism::getWristEncoderFromSlider(Vector2d sliderPos)
{
    if (!isCalibrated)
    {
        throw std::runtime_error("wrist has not been calibrated");
    }

    Vector2d encoder, sliderEncoderPos, encoderTared;

    if (sliderGain(0) == 0.0)
    {
        stringstream err;
        err << "sliderGain(0) must be non-zero. Provided value of [" << sliderGain(0) << "] is invalid.";
        NasaCommonLogging::Logger::log("gov.nasa.robodyn.WristMechanism", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    if (sliderGain(1) == 0.0)
    {
        stringstream err;
        err << "sliderGain(1) must be non-zero. Provided value of [" << sliderGain(1) << "] is invalid.";
        NasaCommonLogging::Logger::log("gov.nasa.robodyn.WristMechanism", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    sliderEncoderPos(0) = (sliderPos(0) - sliderOffset(0)) / sliderGain(0);
    sliderEncoderPos(1) = (sliderPos(1) - sliderOffset(1)) / sliderGain(1);

    encoderTared = sliderEncoderPos - calSliderPos;
    encoder      = encoderTared + encoderTarePos;

    return encoder;
}

Vector2d WristMechanism::getSliderFromAngle(Vector2d ang)
{
    Vector2d angle;

    if (angleGain(0) == 0.0)
    {
        stringstream err;
        err << "angleGain(0) must be non-zero. Provided value of [" << angleGain(0) << "] is invalid.";
        NasaCommonLogging::Logger::log("gov.nasa.robodyn.WristMechanism", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    if (angleGain(1) == 0.0)
    {
        stringstream err;
        err << "angleGain(1) must be non-zero. Provided value of [" << angleGain(1) << "] is invalid.";
        NasaCommonLogging::Logger::log("gov.nasa.robodyn.WristMechanism", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    angle(0) = (ang(0) - angleOffset(0)) / angleGain(0);
    angle(1) = (ang(1) - angleOffset(1)) / angleGain(1);

    Vector2d sliderPos;

    if (doInvKin(sliderPos, angle) != 0)
    {
        std::stringstream err;
        err << "Could not find inv kinematic solution: " << angle(0) << ", " << angle(1);
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::DEBUG, err.str());
        throw std::runtime_error(err.str());
    }

    return sliderPos;
}


void WristMechanism::tareWristEncoders(Vector2d encoder)
{
    this->encoderTarePos = encoder;
    return;
}

void WristMechanism::calWrist(Vector2d encoder, Vector2d sliderPos, Vector2d angle)
{
    tareWristEncoders(encoder);

    if (sliderGain(0) == 0.0)
    {
        stringstream err;
        err << "sliderGain(0) must be non-zero. Provided value of [" << sliderGain(0) << "] is invalid.";
        NasaCommonLogging::Logger::log("gov.nasa.robodyn.WristMechanism", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    if (sliderGain(1) == 0.0)
    {
        stringstream err;
        err << "sliderGain(1) must be non-zero. Provided value of [" << sliderGain(1) << "] is invalid.";
        NasaCommonLogging::Logger::log("gov.nasa.robodyn.WristMechanism", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    this->calSliderPos(0) = (sliderPos(0) - sliderOffset(0)) / sliderGain(0);
    this->calSliderPos(1) = (sliderPos(1) - sliderOffset(1)) / sliderGain(1);
    //std::cout<<"calSliderPos: "<<std::endl<<calSliderPos<<std::endl;
    isCalibrated = true;
    lastAng = angle;

    return;
}

Matrix3d WristMechanism::AA2Rot(Vector3d axis, double theta)
{
    Matrix3d rot = Matrix3d::Zero();
    //convert axis to unit vector
    double norm = axis.norm();

    if (norm == 0)
    {
        std::stringstream err;
        err << "in AA2Rot(axis, theta) could not find find unit vector for axis (axis.norm() = 0)";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    Vector3d unitAxis = 1 / norm * axis;

    double c  = cos(theta);
    double s  = sin(theta);
    double v  = 1 - c;
    double kx = unitAxis(0);
    double ky = unitAxis(1);
    double kz = unitAxis(2);

    rot(0, 0) = (kx * kx * v) + c;
    rot(1, 0) = (kx * ky * v) + (kz * s);
    rot(2, 0) = (kx * kz * v) - (ky * s);

    rot(0, 1) = (kx * ky * v) - (kz * s);
    rot(1, 1) = (ky * ky * v) + c;
    rot(2, 1) = (ky * kz * v) + (kx * s);

    rot(0, 2) = (kx * kz * v) + (ky * s);
    rot(1, 2) = (ky * kz * v) - (kx * s);
    rot(2, 2) = (kz * kz * v) + c;


    return rot;
}


Matrix3d WristMechanism::AA2Rot(Vector3d axis, RotationType index)
{
    Matrix3d rot = Matrix3d::Zero();
    //convert axis to unit vector
    double norm = axis.norm();

    if (norm == 0)
    {
        std::stringstream err;
        err << "in AA2Rot(axis, index) could not find find unit vector for axis (axis.norm() = 0)";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }

    Vector3d unitAxis = 1 / norm * axis;

    double c, s, v;

    switch (index)
    {
        case ONE:
            c = 0;
            s = 0;
            v = 1;
            break;

        case SIN:
            c = 0;
            s = 1;
            v = 0;
            break;

        case COS:
            c = 1;
            s = 0;
            v = -1;
            break;

        default:
            std::stringstream err;
            err << "AA2Rot Rotation type not defined ";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.WristMechanism", log4cpp::Priority::ERROR, err.str());
            throw std::invalid_argument(err.str());
    }

    double kx = unitAxis(0);
    double ky = unitAxis(1);
    double kz = unitAxis(2);

    rot(0, 0) = (kx * kx * v) + c;
    rot(1, 0) = (kx * ky * v) + (kz * s);
    rot(2, 0) = (kx * kz * v) - (ky * s);

    rot(0, 1) = (kx * ky * v) - (kz * s);
    rot(1, 1) = (ky * ky * v) + c;
    rot(2, 1) = (ky * kz * v) + (kx * s);

    rot(0, 2) = (kx * kz * v) + (ky * s);
    rot(1, 2) = (ky * kz * v) - (kx * s);
    rot(2, 2) = (kz * kz * v) + c;

    return rot;
}

Vector2d WristMechanism::NewtonsMethod(Vector2d ang, Vector2d pos)
{
    WristMechanism::Partials Pabpo = findPartialDerivatives(alpha, beta, u, pos(0), ang);
    WristMechanism::Partials Pdcpo = findPartialDerivatives(delta, gamma, v, pos(1), ang);

    Matrix2d Fy;
    Fy <<   Pabpo.ftheta, Pabpo.fphi,
    Pdcpo.ftheta, Pdcpo.fphi;

    Vector2d Fxy;
    Fxy <<  Pabpo.f - lengthSq,
        Pdcpo.f - lengthSq;

    Vector2d dy = Fy.inverse() * Fxy;

    return dy;
}

WristMechanism::Partials WristMechanism::findPartialDerivatives(Vector3d a, Vector3d b, Vector3d w, double q, Vector2d ang)
{
    WristMechanism::Partials wrist;

    Matrix3d Rmtheta = AA2Rot(m, ang(0));
    Matrix3d Rnphi = AA2Rot(n, ang(1));

    Vector3d r1  = Rnphi * b;
    Vector3d r2  = Rmtheta * (p + r1);
    Vector3d r   = r2 - a - q * w;
    Vector3d r0  = r2 - a;
    wrist.f0     = r0.dot(r0);
    wrist.f      = r.dot(r);
    wrist.fq     = -2 * r.dot(w);
    wrist.ftheta = 2 * r.dot(m.cross(r2));
    wrist.fphi   = 2 * r.dot(Rmtheta * (n.cross(r1)));

    return wrist;
}