LeftWristMechanism_Test.cpp

Go to the documentation of this file.

#include <iostream>
#include <gtest/gtest.h>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "robodyn_mechanisms/WristMechanism.h"
#include <ros/package.h>
#include <stdexcept>

using namespace Eigen;

class LeftWristMechanismTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        leftWrist = boost::make_shared<WristMechanism>();

//        //! values for cal = 0
//        A  << -26.40, 30, 191;
//        A0 << -26.93, 30, 174.85;
//        B  << -27.47,31.77,231;
//        C  << -27.47,-26.43,246;
//        D  << -25.75,-30, 210;
//        D0 << -25.93, -30, 204.72;
//        sliderZeroKnown = Vector2d(15.41, 0.667);
//        sliderCalKnown = Vector2d(0, 0);


//        //! values for cal = 30
//        A  << -26.40, 30, 191;
//        A0 << -27.91, 30, 144.86;
//        B  << -27.47,31.77,231;
//        C  << -27.47,-26.43,246;
//        D  << -25.92, -30, 205;
//        D0 << -26.93, -30, 174.73;
//        sliderZeroKnown = Vector2d(45.41, 30.67);
//        sliderCalKnown = Vector2d(30, 30);

//        //! values for cal = 46
//        //! @note 46 is the only value that makes fwd and inv kinematics agree in the range test
//        A  << -26.40, 30, 191;
//        A0 << -28.43, 30, 128.87;
//        B  << -27.47,31.77,231;
//        C  << -27.47,-26.43,246;
//        D  << -25.92, -30, 205;
//        D0 << -27.46, -30, 158.74;
//        sliderZeroKnown = Vector2d(61.4, 46.7);
//        sliderCalKnown = Vector2d(46, 46);

        A  << -26.35, 30, 192.4;
        A0 << -28.39, 30, 130.27;
        B  << -27.47, 31.77, 231;
        C  << -27.47, -26.43, 246;
        D  << -25.75, -30, 210.2;
        D0 << -27.29, -30, 163.94;
        sliderZeroKnown = Vector2d(60, 41.5);
        sliderCalKnown = Vector2d(46, 46);

        Pitch << 0, 0, 222;
        Yaw   << 0, 0, 228;
        PalmCenter << 0, 0, 281;
        M  << 0, 1, 0;
        N  << 1, 0, 0;
        maxTravel  = 100;
        linkLength = 40.8;

        leftWrist->loadDesignParams(A, A0, B, C, D, D0, Pitch, Yaw, M, N, linkLength);

        upperPitchMax = 0.8;
        upperPitchMin = 0.6;
        lowerPitchMax = -0.6;
        lowerPitchMin = -0.8;
        upperYawMax   = 0.15;
        upperYawMin   = -0.05;
        lowerYawMax   = -0.6;
        lowerYawMin   = -0.75;

        leftWrist->setLimits(upperPitchMin, upperPitchMax, lowerPitchMin, lowerPitchMax, upperYawMin, upperYawMax, lowerYawMin, lowerYawMax);

        SliderGain(0) = .003906;
        SliderGain(1) = .003906;
        leftWrist->setSliderOffsetGain(Vector2d::Zero(), SliderGain);
        leftWrist->setAngleOffsetGain(Vector2d::Zero(), Vector2d(1, 1));

        //angleCalKnown = Vector2d(-0.27, -0.252);
        angleCalKnown = Vector2d(-0.1473, -0.3136);
        encoder       = Vector2d::Zero();

        angZeroKnown = Vector2d::Zero();

    }

    virtual void TearDown()
    {
    }

    boost::shared_ptr<WristMechanism> leftWrist;

    Vector3d A;
    Vector3d A0;
    Vector3d B;
    Vector3d C;
    Vector3d D;
    Vector3d D0;
    Vector3d Pitch;
    Vector3d Yaw;
    Vector3d PalmCenter;
    Vector3d M;
    Vector3d N;
    double maxTravel;
    double linkLength;

    double upperPitchMax;
    double upperPitchMin;
    double lowerPitchMax;
    double lowerPitchMin;
    double upperYawMax;
    double upperYawMin;
    double lowerYawMax;
    double lowerYawMin;

    Vector2d SliderGain;
    Vector2d sliderCalKnown;
    Vector2d angleCalKnown;
    Vector2d encoder;
    Vector2d sliderZeroKnown;
    Vector2d angZeroKnown;

};

TEST_F(LeftWristMechanismTest, setBadParamsTest)
{
    //A and A0 cannot be the same
    EXPECT_THROW(leftWrist->loadDesignParams(A, A, B, C, D, D0, Pitch, Yaw, M, N, linkLength), std::runtime_error);

    //D and D0 cannot be the same
    EXPECT_THROW(leftWrist->loadDesignParams(A, A0, B, C, D, D, Pitch, Yaw, M, N, linkLength), std::runtime_error);

    Vector3d Zero = Vector3d::Zero();;

    //M cannot be a zero vector
    EXPECT_THROW(leftWrist->loadDesignParams(A, A0, B, C, D, D0, Pitch, Yaw, Zero, N, linkLength), std::runtime_error);

    //N cannot be a zero vector
    EXPECT_THROW(leftWrist->loadDesignParams(A, A0, B, C, D, D0, Pitch, Yaw, M, Zero, linkLength), std::runtime_error);

}

TEST_F(LeftWristMechanismTest, limitsTest)
{
    // set value within limits
    double pitch = 0;
    double yaw = -0.3;
    double upperPitch, upperYaw, lowerPitch, lowerYaw;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    leftWrist->applyLimits(pitch, yaw);

    EXPECT_FLOAT_EQ(0.0, pitch);
    EXPECT_FLOAT_EQ(-0.3, yaw);

    // set pitch outside of limits
    pitch = 1;
    yaw = -0.3;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    leftWrist->applyLimits(pitch, yaw);

    EXPECT_FLOAT_EQ(0.8, pitch);
    EXPECT_FLOAT_EQ(-0.3, yaw);

    pitch = -1;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    leftWrist->applyLimits(pitch, yaw);

    EXPECT_FLOAT_EQ(-0.8, pitch);
    EXPECT_FLOAT_EQ(-0.3, yaw);


    // set yaw outside of limits
    pitch = 0;
    yaw = 0.25;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    leftWrist->applyLimits(pitch, yaw);

    EXPECT_FLOAT_EQ(0, pitch);
    EXPECT_FLOAT_EQ(0.15, yaw);

    yaw = -1;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    leftWrist->applyLimits(pitch, yaw);

    EXPECT_FLOAT_EQ(0, pitch);
    EXPECT_FLOAT_EQ(-0.75, yaw);

    // set upper pitch and upper yaw outside limits
    pitch = 1;
    yaw = 0.25;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_GE(0.8, upperPitch);
    EXPECT_LE(0.6, upperPitch);
    EXPECT_LE(-0.05, upperYaw);
    EXPECT_GE(0.15, upperYaw);

    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    // set upper pitch and upper yaw between min and max
    pitch = 0.7;
    yaw = 0.1;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_GT(0.8, upperPitch);
    EXPECT_LT(0.6, upperPitch);
    EXPECT_LT(-0.05, upperYaw);
    EXPECT_GT(0.15, upperYaw);

    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    // set upper pitch and lower yaw outside limits
    pitch = 1;
    yaw = -1;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_GE(0.8, upperPitch);
    EXPECT_LE(0.6, upperPitch);
    EXPECT_LE(-0.75, lowerYaw);
    EXPECT_GE(-0.6, lowerYaw);

    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);


    // set upper pitch and lower yaw between min and max
    pitch = 0.7;
    yaw = -0.7;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_GT(0.8, upperPitch);
    EXPECT_LT(0.6, upperPitch);
    EXPECT_LT(-0.75, lowerYaw);
    EXPECT_GT(-0.6, lowerYaw);

    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);

    // set lower pitch and upper yaw outside limits
    pitch = -1;
    yaw = 0.25;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_GE(-0.6, lowerPitch);
    EXPECT_LE(-0.8, lowerPitch);
    EXPECT_LE(-0.05, upperYaw);
    EXPECT_GE(0.15, upperYaw);

    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    // set lower pitch and upper yaw bewteen min and max
    pitch = -0.7;
    yaw = 0.1;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_GT(-0.6, lowerPitch);
    EXPECT_LT(-0.8, lowerPitch);
    EXPECT_LT(-0.05, upperYaw);
    EXPECT_GT(0.15, upperYaw);

    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.75, lowerYaw);

    // set lower pitch and lower yaw outside limits
    pitch = -1;
    yaw = -1;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_GE(-0.6, lowerPitch);
    EXPECT_LE(-0.8, lowerPitch);
    EXPECT_LE(-0.75, lowerYaw);
    EXPECT_GE(-0.6, lowerYaw);

    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);

    // set lower pitch and upper yaw bewteen min and max
    pitch = -0.7;
    yaw = -0.7;
    leftWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_GT(-0.6, lowerPitch);
    EXPECT_LT(-0.8, lowerPitch);
    EXPECT_LT(-0.75, lowerYaw);
    EXPECT_GT(-0.6, lowerYaw);

    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(0.15, upperYaw);

}


TEST_F(LeftWristMechanismTest, getSliderFromAngleTest)
{
    //checks inverse kinematics

    leftWrist->calWrist(encoder,  sliderCalKnown, angleCalKnown);

    //check cal position
    std::cout << "angleCalKnown: (" << angleCalKnown(0) << "," << angleCalKnown(1) << ")" << std::endl;
    Vector2d sliderCalPos = leftWrist->getSliderFromAngle(angleCalKnown);
    std::cout << "slider pos:" << std::endl << sliderCalPos << std::endl;
    EXPECT_NEAR(sliderCalKnown(0), sliderCalPos(0),  1);
    EXPECT_NEAR(sliderCalKnown(1), sliderCalPos(1),  1);

    //check zero position
    Vector2d sliderZeroPos = leftWrist->getSliderFromAngle(Vector2d::Zero());

    std::cout << "slider pos:" << std::endl << sliderZeroPos << std::endl;
    EXPECT_NEAR(sliderZeroKnown(0), sliderZeroPos(0), 1);
    EXPECT_NEAR(sliderZeroKnown(1), sliderZeroPos(1), 1);

    Vector2d ang = Vector2d::Zero();
    //change pitch -- increase of pitch angle should increase both sliders
    ang(0) = 0.0872664626; //5 degrees
    Vector2d slider = leftWrist->getSliderFromAngle(ang);
    std::cout << "slider pos:" << std::endl << slider << std::endl;
    EXPECT_LT(sliderZeroKnown(0), slider(0));
    EXPECT_LT(sliderZeroKnown(1), slider(1));


    ang = Vector2d::Zero();
    //change pitch -- decrease of pitch angle should decrease both sliders
    ang(0) = -0.0872664626; //-5 degrees
    slider = leftWrist->getSliderFromAngle(ang);
    std::cout << "slider pos:" << std::endl << slider << std::endl;
    EXPECT_GT(sliderZeroKnown(0), slider(0));
    EXPECT_GT(sliderZeroKnown(1), slider(1));

    ang = Vector2d::Zero();
    //change yaw -- increase of yaw should increase thumbside and decrease littleside
    ang(1) = 0.0872664626; //5 degreess
    slider = leftWrist->getSliderFromAngle(ang);
    std::cout << "slider pos:" << std::endl << slider << std::endl;

    EXPECT_LT(sliderZeroKnown(0), slider(0));
    EXPECT_GT(sliderZeroKnown(1), slider(1));

    ang = Vector2d::Zero();
    //change yaw -- decrease of yaw should decrease thumside and increase littleside
    ang(1) = -0.0872664626; // -5 degrees
    slider = leftWrist->getSliderFromAngle(ang);
    std::cout << "slider pos:" << std::endl << slider << std::endl;
    EXPECT_GT(sliderZeroKnown(0), slider(0));
    EXPECT_LT(sliderZeroKnown(1), slider(1));


}

TEST_F(LeftWristMechanismTest, sliderWristConversionTest)
{

    //initialize
    Vector2d ang     = Vector2d::Zero();
    Vector2d slider  = leftWrist->getSliderFromAngle(ang);
    Vector2d encoder = Vector2d::Zero();
    leftWrist->calWrist(encoder, slider, ang);

    //test forward and back give same result
    Vector2d slider2 = leftWrist->getSliderFromWristEncoder(encoder);
    EXPECT_FLOAT_EQ(slider(0), slider2(0));
    EXPECT_FLOAT_EQ(slider(1), slider2(1));

    Vector2d encoder2 = leftWrist->getWristEncoderFromSlider(slider);
    EXPECT_FLOAT_EQ(encoder(0), encoder2(0));
    EXPECT_FLOAT_EQ(encoder(1), encoder2(1));

    //test increased encoder is increased sliderpos
    Vector2d constant   = Vector2d::Constant(10);
    Vector2d encoderInc = encoder + constant;
    Vector2d sliderInc  = leftWrist->getSliderFromWristEncoder(encoderInc);
    EXPECT_GT(sliderInc(0), slider(0));
    EXPECT_GT(sliderInc(1), slider(1));

    //test decreased encoder decreases sliderpos
    Vector2d encoderDec = encoder - constant;
    Vector2d sliderDec  = leftWrist->getSliderFromWristEncoder(encoderDec);
    EXPECT_LT(sliderDec(0), slider(0));
    EXPECT_LT(sliderDec(1), slider(1));

    //test increasing sliderpos increases encoder
    sliderInc  = slider + constant;
    encoderInc = leftWrist->getWristEncoderFromSlider(sliderInc);
    EXPECT_GT(encoderInc(0), encoder(0));
    EXPECT_GT(encoderInc(1), encoder(1));

    //test decreasing sliderpos decreases encoder
    sliderDec  = slider - constant;
    encoderDec = leftWrist->getWristEncoderFromSlider(sliderDec);
    EXPECT_LT(encoderDec(0), encoder(0));
    EXPECT_LT(encoderDec(1), encoder(1));

}

TEST_F(LeftWristMechanismTest, newtonsMethodTest)
{
    Vector2d angKnown = Vector2d(0.0070703288, 0.00459021593);
    Vector2d ang      = Vector2d::Zero();

    leftWrist->eps  = 0.0001;
    Vector2d slider = leftWrist->getSliderFromAngle(angKnown);
    Vector2d correction;

    for (int i = 0; i < 10; ++i)
    {
        correction = leftWrist->NewtonsMethod(ang, slider);
        ang        = ang - correction;
    }

    EXPECT_NEAR(angKnown(0), ang(0), .01);
    EXPECT_NEAR(angKnown(1), ang(1), .01);


    ang = Vector2d::Zero();
    //change pitch (increase)
    ang(0) = 0.0872664626; //5 degrees

    for (int i = 0; i < 1000; ++i)
    {
        correction = leftWrist->NewtonsMethod(ang, slider);
        ang = ang - correction;
    }

    EXPECT_NEAR(angKnown(0), ang(0), .01);
    EXPECT_NEAR(angKnown(1), ang(1), .01);

    //change pitch(decrease)
    ang(0) = -0.0872664626; //-5 degrees

    for (int i = 0; i < 1000; ++i)
    {
        correction = leftWrist->NewtonsMethod(ang, slider);
        ang = ang - correction;
    }

    EXPECT_NEAR(angKnown(0), ang(0), .01);
    EXPECT_NEAR(angKnown(1), ang(1), .01);

    ang = Vector2d::Zero();

    //change yaw (increase)
    ang(1) = 0.0872664626; //5 degrees

    for (int i = 0; i < 1000; ++i)
    {
        correction = leftWrist->NewtonsMethod(ang, slider);
        ang = ang - correction;
    }

    EXPECT_NEAR(angKnown(0), ang(0), .01);
    EXPECT_NEAR(angKnown(1), ang(1), .01);

    //change yaw (decrease)
    ang(1) = -0.0872664626; //-5 degrees

    for (int i = 0; i < 1000; ++i)
    {
        correction = leftWrist->NewtonsMethod(ang, slider);
        ang = ang - correction;
    }

    EXPECT_NEAR(angKnown(0), ang(0), .01);
    EXPECT_NEAR(angKnown(1), ang(1), .01);

}

TEST_F(LeftWristMechanismTest, getAngleFromSliderTest)
{
    //checks fwd kinematics

    EXPECT_NO_THROW(leftWrist->calWrist(encoder,  sliderCalKnown, angleCalKnown));
    leftWrist->maxIt = 150;
    leftWrist->eps   = .0001;

    Vector2d angOut;
    //check calibration point
    EXPECT_NO_THROW(angOut = leftWrist->getAngleFromSlider(sliderCalKnown));
    std::cout << "angle:" << std::endl << angOut << std::endl;
    EXPECT_NEAR(angleCalKnown(0), angOut(0), 0.005);
    EXPECT_NEAR(angleCalKnown(1), angOut(1), 0.005);

    //check zero pos
    EXPECT_NO_THROW(angOut = leftWrist->getAngleFromSlider(sliderZeroKnown));
    std::cout << "angle:" << std::endl << angOut << std::endl;
    EXPECT_NEAR(angZeroKnown(0), angOut(0), 0.005);
    EXPECT_NEAR(angZeroKnown(1), angOut(1), 0.005);

    //increasing both sliders should increase pitch
    Vector2d slider = sliderZeroKnown + Vector2d::Constant(10);
    EXPECT_NO_THROW(angOut = leftWrist->getAngleFromSlider(slider));
    std::cout << "angle:" << std::endl << angOut << std::endl;
    EXPECT_LT(angZeroKnown(0), angOut(0));

    //decreasing both sliders should decrease pitch
    slider = sliderZeroKnown - Vector2d::Constant(10);
    EXPECT_NO_THROW(angOut = leftWrist->getAngleFromSlider(slider));
    std::cout << "angle:" << std::endl << angOut << std::endl;
    EXPECT_GT(angZeroKnown(0), angOut(0));

    //decreasing thumbside, and increasing littleside should decrease yaw
    slider = sliderZeroKnown + Vector2d(-5, 5);
    EXPECT_NO_THROW(angOut = leftWrist->getAngleFromSlider(slider));
    std::cout << "angle:" << std::endl << angOut << std::endl;
    EXPECT_GT(angZeroKnown(1), angOut(1));

    //increasing thumbside, and decreasing littleside should increase yaw
    slider = sliderZeroKnown + Vector2d(5, -5);
    EXPECT_NO_THROW(angOut = leftWrist->getAngleFromSlider(slider));
    std::cout << "angle:" << std::endl << angOut << std::endl;
    EXPECT_LT(angZeroKnown(1), angOut(1));

}

TEST_F(LeftWristMechanismTest, designSpaceTest)
{
    Vector2d maxSlider1 = Vector2d::Zero();
    Vector2d minSlider1 = Vector2d::Zero();
    Vector2d maxSlider2 = Vector2d::Zero();
    Vector2d minSlider2 = Vector2d::Zero();
    Vector2d maxPitch   = Vector2d::Zero();
    Vector2d minPitch   = Vector2d::Zero();
    Vector2d maxYaw     = Vector2d::Zero();
    Vector2d minYaw     = Vector2d::Zero();
    Vector2d sliderOut  = Vector2d::Zero();
    Vector2d angOut     = Vector2d::Zero();

    for (double i = lowerPitchMin; i < upperPitchMax; i += .01)
    {
        for (double j = lowerYawMin; j < upperYawMax; j += .01)
        {
            try
            {
                double pitch = i;
                double yaw = j;
                leftWrist->applyLimits(pitch, yaw);
                sliderOut = leftWrist->getSliderFromAngle(Vector2d(pitch, yaw));

//                if(abs(sliderOut(0)-sliderOut(1))>30 || sliderOut(0)<0 || sliderOut(0)>80 || sliderOut(1)<0 || sliderOut(1)>80)
//                {
//                    //std::cout<<"angle ("<<i<<","<<j<<")"<<" violates slider limits: ("<<sliderOut(0)<<","<<sliderOut(1)<<")"<<std::endl;
//                    continue;
//                }
                if (maxSlider1(0) < sliderOut(0))
                {
                    maxSlider1 = sliderOut;
                }

                if (minSlider1(0) > sliderOut(0))
                {
                    minSlider1 = sliderOut;
                }

                if (maxSlider2(1) < sliderOut(1))
                {
                    maxSlider2 = sliderOut;
                }

                if (minSlider2(1) > sliderOut(1))
                {
                    minSlider2 = sliderOut;
                }
            }
            catch (std::runtime_error)
            {
                std::cout << "could not find inv kinematic solution (" << i << "," << j << ")" << std::endl;
                std::cin.ignore();
            }

            try
            {
                angOut      = leftWrist->getAngleFromSlider(sliderOut);
                double pitch = i;
                double yaw   = j;
                leftWrist->applyLimits(pitch, yaw);
//                if(fabs(pitch-angOut(0)) > 0.005)
//                {
//                    std::cout<<"angle ("<<pitch<<","<<yaw<<")"<<" and ("<<angOut(0)<<","<<angOut(1)<<") are more than 0.005 apart: ("<<sliderOut(0)<<","<<sliderOut(1)<<")"<<std::endl;
//                }
//                if(fabs(yaw-angOut(1))>0.005)
//                {
//                    std::cout<<"angle ("<<pitch<<","<<yaw<<")"<<" and ("<<angOut(0)<<","<<angOut(1)<<") are more than 0.005 apart: ("<<sliderOut(0)<<","<<sliderOut(1)<<")"<<std::endl;
//                }
                EXPECT_NEAR(pitch, angOut(0), 0.005);
                EXPECT_NEAR(yaw, angOut(1), 0.005);

                if (maxPitch(0) < angOut(0))
                {
                    maxPitch = angOut;
                }

                if (minPitch(0) > angOut(0))
                {
                    minPitch = angOut;
                }

                if (maxYaw(1) < angOut(1))
                {
                    maxYaw = angOut;
                }

                if (minYaw(1) > angOut(1))
                {
                    minYaw = angOut;
                }
            }
            catch (std::runtime_error)
            {
                std::cout << "Fwd Kinematics not valid for (" << i << "," << j << "), slider pos: (" << sliderOut(0) << "," << sliderOut(1) << ")" << std::endl;
                std::cin.ignore();
            }


        }
    }

    std::cout << "Max Slider1: " << maxSlider1 << std::endl;
    std::cout << "Min Slider1: " << minSlider1 << std::endl;
    std::cout << "Max Slider2: " << maxSlider2 << std::endl;
    std::cout << "Min Slider2: " << minSlider2 << std::endl;
    std::cout << "Max Pitch: " << maxPitch << std::endl;
    std::cout << "Min Pitch: " << minPitch << std::endl;
    std::cout << "Max Yaw: " << maxYaw << std::endl;
    std::cout << "Min Yaw: " << minYaw << std::endl;
}

TEST_F(LeftWristMechanismTest, calibration)
{

    leftWrist->calWrist(encoder,  sliderCalKnown, angleCalKnown);

    Vector2d sliderTest = leftWrist->getSliderFromAngle(angleCalKnown);
    EXPECT_NEAR(sliderCalKnown(0), sliderTest(0), 1);
    EXPECT_NEAR(sliderCalKnown(1), sliderTest(1), 1);

    Vector2d angOut = leftWrist->getAngleFromSlider(sliderCalKnown);
    EXPECT_NEAR(angleCalKnown(0), angOut(0), 0.005 );
    EXPECT_NEAR(angleCalKnown(1), angOut(1), 0.005 );

    Vector2d sliderOut = leftWrist->getSliderFromWristEncoder(encoder);
    EXPECT_NEAR(sliderCalKnown(0), sliderOut(0), 1);
    EXPECT_NEAR(sliderCalKnown(1), sliderOut(1), 1);

}


int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}