serialchaintest.cpp

Go to the documentation of this file.

#include <kdl/kinfam/serialchain.hpp>
#include <kdl/frames.hpp>
#include <kdl/framevel.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/kinfam/crs450.hpp>
#include <kdl/kinfam/kuka160.hpp>
#include <kdl/kinfam/serialchaincartpos2jnt.hpp>
#include <kdl/kinfam/lineartransmission.hpp>

using namespace KDL;



void CompareFamilies(KinematicFamily* KF1,KinematicFamily* KF2) {
        Jnt2CartPos* jnt2cartpos1 = KF1->createJnt2CartPos();
        Jnt2CartPos* jnt2cartpos2 = KF2->createJnt2CartPos();
        JointVector q(6);
        q[0] = 0*KDL::deg2rad;
        q[1] = 10*KDL::deg2rad;
        q[2] = 20*KDL::deg2rad;
        q[3] = 30*KDL::deg2rad;
        q[4] = 40*KDL::deg2rad;
        q[5] = 50*KDL::deg2rad;
        Frame F1,F2;
        jnt2cartpos1->evaluate(q);
        jnt2cartpos1->getFrame(F1);
        jnt2cartpos2->evaluate(q);
        jnt2cartpos2->getFrame(F2);
        if (!Equal(F1,F2,1E-7)) {
          std::cout << "the two kinematic families do not give the same result." << std::endl;
          std::cout << "Result of first kinematic family " << std::endl;
          std::cout << F1 << std::endl;
          std::cout << "Result of second kinematic family " << std::endl;
          std::cout << F2 << std::endl;
          exit(1);
        }
    delete jnt2cartpos1;
    delete jnt2cartpos2;
}

//
// Test whether Jnt2CartPos and CartPos2Jnt give a consistent result.
//
class TestForwardAndInverse {
        KinematicFamily*      family;
        Jnt2CartPos*          jnt2cartpos;
        Frame                         F_base_ee;
        Frame                         F_base_ee2;
        JointVector           q_solved;
        JointVector           q_initial;
public:
        CartPos2Jnt*          cartpos2jnt;
public:
        static void TestFamily(KinematicFamily* _family) {
                JointVector q_initial(6);
                q_initial[0] = 0*KDL::deg2rad;
                q_initial[1] = 0*KDL::deg2rad;
                q_initial[2] = 90*KDL::deg2rad;
                q_initial[3] = 0*KDL::deg2rad;
                q_initial[4] = 90*KDL::deg2rad;
                q_initial[5] = 0*KDL::deg2rad;
                TestForwardAndInverse testobj(_family,q_initial);
                JointVector q(6);
                q[0] = 0*KDL::deg2rad;
                q[1] = 10*KDL::deg2rad;
                q[2] = 20*KDL::deg2rad;
                q[3] = 30*KDL::deg2rad;
                q[4] = 40*KDL::deg2rad;
                q[5] = 50*KDL::deg2rad;
                testobj.test(q);
                //std::cout << "number of iterations " << ((SerialChainCartPos2Jnt*)testobj.cartpos2jnt)->iter << std::endl;
                q[0] = -10*KDL::deg2rad;
                q[1] = -10*KDL::deg2rad;
                q[2] = 40*KDL::deg2rad;
                q[3] = -30*KDL::deg2rad;
                q[4] = 20*KDL::deg2rad;
                q[5] = 60*KDL::deg2rad;
                testobj.test(q);
                //std::cout << "number of iterations " << ((SerialChainCartPos2Jnt*)testobj.cartpos2jnt)->iter << std::endl;
        }


//
// Test whether Jnt2CartPos and Jnt2Jac give consistent
// results.
//
class TestForwardPosAndJac {
        KinematicFamily* family;
        Jnt2CartPos*     jnt2cartpos;
        Jnt2Jac*         jnt2jac;
    Jacobian<Frame>  FJ_base_ee;
        Frame                    F_base_ee1;
        Frame                    F_base_ee2;
public:
        static void TestFamily(KinematicFamily* _family) {
                TestForwardPosAndJac testobj(_family);
                JointVector q(6);
                q[0] = 0*KDL::deg2rad;
                q[1] = 10*KDL::deg2rad;
                q[2] = 20*KDL::deg2rad;
                q[3] = 30*KDL::deg2rad;
                q[4] = 40*KDL::deg2rad;
                q[5] = 50*KDL::deg2rad;
                testobj.test(q);
                q[0] = -50*KDL::deg2rad;
                q[1] = -10*KDL::deg2rad;
                q[2] = 20*KDL::deg2rad;
                q[3] = -30*KDL::deg2rad;
                q[4] = 20*KDL::deg2rad;
                q[5] = 110*KDL::deg2rad;
                testobj.test(q);
        }

    TestForwardPosAndJac(KinematicFamily* _family) :
                family(_family),
                jnt2cartpos(_family->createJnt2CartPos()),
                jnt2jac(_family->createJnt2Jac()),
        FJ_base_ee(_family->nrOfJoints())
    {
                // the transformations should exist :
                assert( jnt2jac != 0);
                assert( jnt2cartpos != 0);
    }

    int test(JointVector& q) {
        double deltaq = 1E-4;
        double epsJ   = 1E-4;
                if (jnt2jac->evaluate(q)!=0) return 1;
                jnt2jac->getJacobian(FJ_base_ee);
        for (int i=0; i< q.size() ;i++) {
            // test the derivative of J towards qi
            double oldqi = q[i];
            q[i] = oldqi+deltaq;
                        if (jnt2cartpos->evaluate(q)!=0) return 1;
                        jnt2cartpos->getFrame(F_base_ee2);
            q[i] = oldqi-deltaq;
                        if (jnt2cartpos->evaluate(q)!=0) return 1;
                        jnt2cartpos->getFrame(F_base_ee1);
            q[i] = oldqi;
            // check Jacobian :
            Twist Jcol = diff(F_base_ee1,F_base_ee2,2*deltaq);
            if (!Equal(Jcol,FJ_base_ee.deriv(i),epsJ)) {
                std::cout << "Difference between symbolic and numeric calculation of Jacobian for column "
                          << i << std::endl;
                std::cout << "Numeric " << Jcol << std::endl;
                std::cout << "Symbolic " << FJ_base_ee.deriv(i) << std::endl;
                exit(1);
            }
        }
    }

    ~TestForwardPosAndJac() {
                delete jnt2cartpos;
                delete jnt2jac;
    }
};


//
// Test whether Jnt2CartVel and Jnt2Jac give consistent
// results.
//
class TestCartVelAndJac {
        KinematicFamily* family;
        Jnt2CartVel*     jnt2cartvel;
        Jnt2Jac*         jnt2jac;
    Jacobian<Frame>  FJ_base_ee;
        FrameVel                 F_base_ee;
public:
        static void TestFamily(KinematicFamily* _family) {
                TestCartVelAndJac testobj(_family);
                JointVector qdot(6);
        qdot[0] = 0.1;
        qdot[1] = 0.2;
        qdot[2] = -0.3;
        qdot[3] = 0.4;
        qdot[4] = -0.5;
        qdot[5] = 0.6;
                JointVector q(6);
                q[0] = 0*KDL::deg2rad;
                q[1] = 10*KDL::deg2rad;
                q[2] = 20*KDL::deg2rad;
                q[3] = 30*KDL::deg2rad;
                q[4] = 40*KDL::deg2rad;
                q[5] = 50*KDL::deg2rad;
        std::cout << "q[1] = " << q[1] << std::endl;
                testobj.test(q,qdot);
                q[0] = -50*KDL::deg2rad;
                q[1] = -10*KDL::deg2rad;
                q[2] = 20*KDL::deg2rad;
                q[3] = -30*KDL::deg2rad;
                q[4] = 20*KDL::deg2rad;
                q[5] = 110*KDL::deg2rad;
                testobj.test(q,qdot);
        }

    TestCartVelAndJac(KinematicFamily* _family) :
                family(_family),
                jnt2cartvel(_family->createJnt2CartVel()),
                jnt2jac(_family->createJnt2Jac()),
        FJ_base_ee(_family->nrOfJoints())
    {
                // the transformations should exist :
                assert( jnt2jac != 0);
                assert( jnt2cartvel != 0);
    }

    int test(JointVector& q,JointVector& qdot) {
        std::cout << "Testing wether Jnt2CartVel and Jnt2Jac are consistent " << std::endl;
        std::cout << "q[1] = " << q[1] << std::endl;
        double deltaq = 1E-4;
        double epsJ   = 1E-4;
                int result;
        result = jnt2jac->evaluate(q);
        assert(result==0);
                jnt2jac->getJacobian(FJ_base_ee);
        result = jnt2cartvel->evaluate(q,qdot);
        jnt2cartvel->getFrameVel(F_base_ee);
        assert(result==0);
        Twist t = Twist::Zero();
        for (int i=0; i< q.size() ;i++) {
            t += FJ_base_ee.deriv(i)*qdot[i];
        }
        if (!Equal(t,F_base_ee.GetTwist(),1E-6)) {
                std::cout << "Difference between the resuls"<< std::endl;
                std::cout << "via the Jacobian  " << t << std::endl;
                std::cout << "via the jnt2cartvel transformation  " << F_base_ee.GetTwist() << std::endl;
                exit(1);
        }
    }

    ~TestCartVelAndJac() {
                delete jnt2cartvel;
                delete jnt2jac;
    }
};



//
// Test whether Jnt2CartVel and CartVel2Jnt give consistent
// results.
//
class TestCartVelAndInverse {
        KinematicFamily* family;
        Jnt2CartVel*     jnt2cartvel;
        CartVel2Jnt*     cartvel2jnt;
        FrameVel                 F_base_ee;
    JointVector      qdot2;
public:
        static void TestFamily(KinematicFamily* _family) {
                TestCartVelAndInverse testobj(_family);
                JointVector qdot(6);
        qdot[0] = 0.1;
        qdot[1] = 0.2;
        qdot[2] = -0.3;
        qdot[3] = 0.4;
        qdot[4] = -0.5;
        qdot[5] = 0.6;
        JointVector q(6);
                q[0] = 0*KDL::deg2rad;
                q[1] = 10*KDL::deg2rad;
                q[2] = 20*KDL::deg2rad;
                q[3] = 30*KDL::deg2rad;
                q[4] = 40*KDL::deg2rad;
                q[5] = 50*KDL::deg2rad;
        testobj.test(q,qdot);
                q[0] = -50*KDL::deg2rad;
                q[1] = -10*KDL::deg2rad;
                q[2] = 20*KDL::deg2rad;
                q[3] = -30*KDL::deg2rad;
                q[4] = 20*KDL::deg2rad;
                q[5] = 110*KDL::deg2rad;
                testobj.test(q,qdot);
        }

    TestCartVelAndInverse(KinematicFamily* _family) :
                family(_family),
                jnt2cartvel(_family->createJnt2CartVel()),
                cartvel2jnt(_family->createCartVel2Jnt()),
        qdot2(_family->nrOfJoints())
    {
                // the transformations should exist :
                assert( cartvel2jnt != 0);
                assert( jnt2cartvel != 0);
    }

    int test(const JointVector& q,const JointVector& qdot) {
        std::cout << "Testing wether Jnt2CartVel and CartVel2Jnt are consistent " << std::endl;
        double epsJ   = 1E-7;
                int result;

        result = jnt2cartvel->evaluate(q,qdot);
        assert(result==0);
        jnt2cartvel->getFrameVel(F_base_ee);

        cartvel2jnt->setTwist(F_base_ee.GetTwist());
        result = cartvel2jnt->evaluate(q, qdot2);
        assert(result==0);

        for (int i=0;i<qdot.size();++i) {
           if (fabs(qdot[i]-qdot2[i])>epsJ)  {
               std::cout << " original joint velocities and calculated joint velocities do not match" << std::endl;
               //std::cerr << " original joint velocities and calculated joint velocities do not match" << std::endl;
               for (int j=0;j<qdot.size();j++) {
                    std::cout << "qdot["<<j<<"]="<<qdot[j]<<"  and qdot2["<<j<<"]="<<qdot2[j] << std::endl;
                }
                std::cout << "Frame : " << F_base_ee.GetFrame() << std::endl;
                std::cout << "Twist : " << F_base_ee.GetTwist() << std::endl;
               exit(1);
           }
        }
    }

    ~TestCartVelAndInverse() {
                delete jnt2cartvel;
                delete cartvel2jnt;
    }
};








class CRS450_exp: public SerialChain {
        public:
        explicit CRS450_exp(int jointoffset=0) :
                SerialChain("CRS450", 6, jointoffset,new LinearTransmission(6) )
        {
                double L1 = 0.33;
                double L2 = 0.305;
                double L3 = 0.33;
                double L4 = 0.176;
                LinearTransmission* tr = (LinearTransmission*)transmission;
                tr->setTransmission(2,2.0,1.0);
                tr->setTransmission(3,3.0,2.0);
        addJoint(new JointRotZ(Frame::Identity())); // j1
        addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L1))));// j2
        addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L2))));// j3
        addJoint(new JointRotZ(Frame(Rotation::Identity(),Vector(0,0,0)))); // j4
        addJoint(new JointRotY(Frame(Rotation::Identity(),Vector(0,0,L3))));// j5
        addJoint(new JointRotZ(Frame(Rotation::Identity(),Vector(0,0,0)))); // j6
        setLastJointToEE(Frame(Rotation::Identity(),Vector(0,0,L4)));
        };
};


int main(int argc,char* argv[]) {
        KinematicFamily* family1,*family2,*family3,*family4;
        std::cout << std::endl << "Tests on CRS450 " << std::endl;
        family1 = new CRS450();
        TestForwardAndInverse::TestFamily(family1);
        TestForwardPosAndJac::TestFamily(family1);
    TestCartVelAndJac::TestFamily(family1);
    TestCartVelAndInverse::TestFamily(family1);//
        std::cout <<std::endl <<  "Tests on CRS450_exp " << std::endl;
        family2 = new CRS450_exp();
        TestForwardAndInverse::TestFamily(family2);
        TestForwardPosAndJac::TestFamily(family2);
    TestCartVelAndJac::TestFamily(family2);
    TestCartVelAndInverse::TestFamily(family2);//
        std::cout << std::endl << "Tests on CRS450Feath " << std::endl;
        family3 = new CRS450Feath();
        TestForwardAndInverse::TestFamily(family3);
        TestForwardPosAndJac::TestFamily(family3);
    TestCartVelAndJac::TestFamily(family3);
    TestCartVelAndInverse::TestFamily(family3);//
        std::cout << std::endl << "Comparing the kinematic families "<< std::endl;
        CompareFamilies(family1,family3);
        std::cout << std::endl << "Tests on CRS450Feath->createSerialChain " << std::endl;
    family4 = ((ZXXZXZ*)family3)->createSerialChain();
        TestForwardAndInverse::TestFamily(family4);
        TestForwardPosAndJac::TestFamily(family4);
    TestCartVelAndJac::TestFamily(family4);
    TestCartVelAndInverse::TestFamily(family4);//



    // testing the clone functionality (valgrind)
        KinematicFamily* family1b,*family2b,*family3b,*family4b;
    family1b = family1->clone();
    family2b = family2->clone();
    family3b = family3->clone();
    family4b = family4->clone();

    delete family4;
        delete family3;
        delete family2;
        delete family1;

    delete family4b;
        delete family3b;
        delete family2b;
        delete family1b;

        return 0;
}