RightWristMechanism_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 RightWristMechanismTest : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        rightWrist = boost::make_shared<WristMechanism>();

        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;
        linkLength = 40.8;

        rightWrist->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.75;
        upperYawMin = 0.6;
        lowerYawMax = 0.05;
        lowerYawMin = -0.15;

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

        SliderGain(0) = .003906;
        SliderGain(1) = .003906;
        rightWrist->setSliderOffsetGain(Vector2d::Zero(), SliderGain);
        rightWrist->setAngleOffsetGain(Vector2d::Zero(), Vector2d(1, 1));
        rightWrist->eps = .0001;
        rightWrist->maxIt = 1000;

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

        angZeroKnown = Vector2d::Zero();
    }

    virtual void TearDown()
    {
    }

    boost::shared_ptr<WristMechanism> rightWrist;

    Vector3d A;
    Vector3d A0;
    Vector3d B;
    Vector3d C;
    Vector3d D;
    Vector3d D0;
    Vector3d Pitch;
    Vector3d Yaw;
    Vector3d PalmCenter;
    Vector3d M;
    Vector3d N;
    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(RightWristMechanismTest, setBadParamsTest)
{

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

    //D and D0 cannot be the same
    EXPECT_THROW(rightWrist->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(rightWrist->loadDesignParams(A, A0, B, C, D, D0, Pitch, Yaw, Zero, N, linkLength), std::runtime_error);

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

}

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

    rightWrist->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;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.75, upperYaw);
    EXPECT_FLOAT_EQ(-0.15, lowerYaw);

    rightWrist->applyLimits(pitch, yaw);

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

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

    rightWrist->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;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.75, upperYaw);
    EXPECT_FLOAT_EQ(-0.15, lowerYaw);

    rightWrist->applyLimits(pitch, yaw);

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

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

    rightWrist->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 = 1;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.7280, upperPitch);
    EXPECT_FLOAT_EQ(0.6540, upperYaw);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(-0.15, lowerYaw);

    // set upper pitch and upper yaw between min and max
    pitch = 0.7;
    yaw = 0.7;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.6880, upperPitch);
    EXPECT_FLOAT_EQ(0.6840, upperYaw);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(-0.15, lowerYaw);

    // set upper pitch and lower yaw outside limits
    pitch = 1;
    yaw = -0.25;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.7, upperPitch);
    EXPECT_FLOAT_EQ(-0.05, lowerYaw);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.75, upperYaw);

    // set upper pitch and lower yaw between min and max
    pitch = 0.7;
    yaw = -0.1;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(0.675, upperPitch);
    EXPECT_FLOAT_EQ(-0.075, lowerYaw);
    EXPECT_FLOAT_EQ(-0.8, lowerPitch);
    EXPECT_FLOAT_EQ(0.75, upperYaw);

    // set lower pitch and upper yaw outside limits
    pitch = -1;
    yaw = 1;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(-0.728, lowerPitch);
    EXPECT_FLOAT_EQ(0.654, upperYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.15, lowerYaw);

    // set lower pitch and upper yaw bewteen min and max
    pitch = -0.7;
    yaw = 0.7;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(-0.688, lowerPitch);
    EXPECT_FLOAT_EQ(0.684, upperYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(-0.15, lowerYaw);

    // set lower pitch and lower yaw outside limits
    pitch = -1;
    yaw = -0.25;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(-0.7, lowerPitch);
    EXPECT_FLOAT_EQ(-.05, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(0.75, upperYaw);

    // set lower pitch and lower yaw bewteen min and max
    pitch = -0.7;
    yaw = -0.1;
    rightWrist->getLimits(pitch, yaw, upperPitch, lowerPitch, upperYaw, lowerYaw);
    EXPECT_FLOAT_EQ(-0.675, lowerPitch);
    EXPECT_FLOAT_EQ(-.075, lowerYaw);
    EXPECT_FLOAT_EQ(0.8, upperPitch);
    EXPECT_FLOAT_EQ(0.75, upperYaw);

}

TEST_F(RightWristMechanismTest, testRedoZero)
{
    rightWrist->calWrist(encoder, sliderZeroKnown, Vector2d::Zero());

    std::cout << "angleCalKnown: (0,0)" << std::endl;
    Vector2d sliderCalPos = rightWrist->getSliderFromAngle(Vector2d::Zero());
    std::cout << "slider pos:" << std::endl << sliderCalPos << std::endl;

    Vector2d angOut = rightWrist->getAngleFromSlider(sliderCalKnown);
    std::cout << "angle at cal pos: " << std::endl << angOut << std::endl;
}

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

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

    //check cal position
    std::cout << "angleCalKnown: (" << angleCalKnown(0) << "," << angleCalKnown(1) << ")" << std::endl;
    Vector2d sliderCalPos = rightWrist->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 = rightWrist->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 = rightWrist->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 = rightWrist->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 decrease thumbside and increase littleside
    ang(1) = 0.0872664626; //5 degreess
    slider = rightWrist->getSliderFromAngle(ang);
    std::cout << "slider pos:" << std::endl << slider << std::endl;

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

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


}


TEST_F(RightWristMechanismTest, sliderWristConversionTest)
{
    //initialize
    Vector2d ang     = Vector2d::Zero();
    Vector2d slider  = rightWrist->getSliderFromAngle(ang);
    Vector2d encoder = Vector2d::Zero();
    rightWrist->calWrist(encoder, slider, ang);

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

    Vector2d encoder2 = rightWrist->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  = rightWrist->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  = rightWrist->getSliderFromWristEncoder(encoderDec);
    EXPECT_LT(sliderDec(0), slider(0));
    EXPECT_LT(sliderDec(1), slider(1));

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

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

}

TEST_F(RightWristMechanismTest, NewtonsMethodTest)
{
    Vector2d angKnown = Vector2d(0.0070703288, 0.00459021593);
    Vector2d ang      = Vector2d::Zero();

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

    for (int i = 0; i < 10; ++i)
    {
        correction = rightWrist->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 = rightWrist->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 = rightWrist->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 = rightWrist->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 = rightWrist->NewtonsMethod(ang, slider);
        ang = ang - correction;
    }

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

}

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

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

    Vector2d angOut;
    //check calibration point
    EXPECT_NO_THROW(angOut = rightWrist->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 = rightWrist->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 = rightWrist->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 = rightWrist->getAngleFromSlider(slider));
    std::cout << "angle:" << std::endl << angOut << std::endl;
    EXPECT_GT(angZeroKnown(0), angOut(0));

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

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

}

TEST_F(RightWristMechanismTest, FindDesignSpaceTest)
{
    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;
                rightWrist->applyLimits(pitch, yaw);
                sliderOut = rightWrist->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      = rightWrist->getAngleFromSlider(sliderOut);
                double pitch = i;
                double yaw   = j;
                rightWrist->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.0005);
                EXPECT_NEAR(yaw, angOut(1), 0.0005);

                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(RightWristMechanismTest, calibration)
{
    rightWrist->calWrist(encoder,  sliderCalKnown, angleCalKnown);

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

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

    Vector2d sliderOut = rightWrist->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();
}