MotionModel.hpp

Go to the documentation of this file.

#ifndef ODOMETRY_MOTION_MODEL_HPP
#define ODOMETRY_MOTION_MODEL_HPP

#include <iostream> 
#include <vector> 
#include <boost/shared_ptr.hpp> 
#include <base-logging/Logging.hpp> 
#include <Eigen/Geometry> 
#include <Eigen/Core> 
#include <Eigen/Dense> 
#include <Eigen/Cholesky> 
#include "KinematicModel.hpp" 
//#define DEBUG_PRINTS_ODOMETRY_MOTION_MODEL 1 //TO-DO: Remove this. Only for testing (master branch) purpose

namespace odometry
{
    struct TreeContactPoint
    {
        unsigned int number;

        int contactId;

        TreeContactPoint()
            : number(1), contactId(0) {}
    };

    template <typename _Scalar, int _RobotTrees, int _RobotJointDoF, int _SlipDoF, int _ContactDoF>
    class MotionModel
    {
        public:
            static const unsigned int MAX_CHAIN_DOF = _RobotJointDoF+_SlipDoF+_ContactDoF;

            static const unsigned int MODEL_DOF = _RobotJointDoF+_RobotTrees*(_SlipDoF+_ContactDoF);

            static const int NO_CONTACT = -1;

        public:
            typedef boost::shared_ptr< KinematicModel <_Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF> > kinematics_ptr;

            enum methodContactPoint
            {
                LOWEST_POINT  = 0,
                COMBINATORICS  = 1
            };

            methodContactPoint contactSelection; 
            kinematics_ptr robotModel; 
            std::vector<Eigen::Affine3d> fkRobot; 
            std::vector<base::Matrix6d> fkCov; 
            std::vector<TreeContactPoint> contactPoints; 
        protected:

            /* This function is deprecated
            bool contactPointComparison(Eigen::Affine3d fkRobot1, Eigen::Affine3d fkRobot2)
            {
                return fkRobot1.translation()[2] < fkRobot2.translation()[2];
            }*/

            void lowestPointInContact()
            {
                register int j = 0;

                for (std::vector<TreeContactPoint>::iterator it = contactPoints.begin() ; it != contactPoints.end(); it++)
                {
                    if ((*it).number > 1)
                    {
                        double zdistance = 0.00;
                        for (register unsigned int i=0; i<(*it).number; ++i)
                        {
                            if (zdistance > fkRobot[j+i].translation()[2])
                            {
                                zdistance = fkRobot[j+i].translation()[2];
                                (*it).contactId = i;
                            }
                        }
                    }
                    else if ((*it).number == 1) 
                    {
                        contactPoints[j].contactId = 0;
                    }
                    else 
                    {
                        contactPoints[j].contactId = NO_CONTACT;
                    }

                    j = j+(*it).number;
                }

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"[MOTION_MODEL] Selected Points in Contact:";
                for (std::vector<TreeContactPoint>::iterator it = contactPoints.begin() ; it != contactPoints.end(); it++)
                {
                    std::cout<<" "<<(*it).contactId;
                }
                std::cout<<"\n";
                #endif


                return;
            }

            void combinatoricsPointInContact()
            {
                return;
            }

            void selectPointsInContact(MotionModel::methodContactPoint method)
            {

                if (method == LOWEST_POINT)
                {
                    #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                    std::cout<< "[MOTION_MODEL SelectPoints] Lowest Points method\n";
                    #endif
                    this->lowestPointInContact();
                }
                else if (method == COMBINATORICS)
                {
                    #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                    std::cout<< "[MOTION_MODEL SelectPoints] Combinatorics Points method\n";
                    #endif

                    this->combinatoricsPointInContact();
                }

               return;
            }

            inline void navEquations(const Eigen::Matrix <_Scalar, 6, 1> &cartesianVelocities,
                                    const Eigen::Matrix <_Scalar, MODEL_DOF, 1> &modelVelocities,
                                    const Eigen::Matrix <_Scalar, 6*_RobotTrees, MODEL_DOF> &J,
                                    const Eigen::Matrix <_Scalar, 6, 6> &cartesianVelCov,
                                    const Eigen::Matrix <_Scalar, MODEL_DOF, MODEL_DOF> &modelVelCov,
                                    Eigen::Matrix <_Scalar, 6*_RobotTrees, 3+_RobotTrees+(_RobotTrees*_ContactDoF)> &unknownA,
                                    Eigen::Matrix <_Scalar, 3+_RobotTrees+(_RobotTrees*_ContactDoF), 1> &unknownx,
                                    Eigen::Matrix <_Scalar, 6*_RobotTrees, 3+_RobotJointDoF> &knownB,
                                    Eigen::Matrix <_Scalar, 3+_RobotJointDoF, 1> &knowny,
                                    Eigen::Matrix <_Scalar, 6*_RobotTrees, 6*_RobotTrees> &Weight)
            {
                Eigen::Matrix<_Scalar, 6*_RobotTrees, 6> spareI;

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"[MOTION_MODEL] navEquations\n";
                #endif

                for (register int i=0; i<_RobotTrees; ++i)
                    spareI.template block<6, 6>(i*6, 0) = Eigen::Matrix <_Scalar, 6, 6>::Identity();

                unknownA.template block<6*_RobotTrees, 3> (0,0) = spareI.template block<6*_RobotTrees, 3> (0,0);//Linear Velocities

                unknownx.template block<3, 1> (0,0) = cartesianVelocities.template block<3, 1> (0,0);

                for (register int i=0; i<_RobotTrees; ++i)
                {
                    unknownA.col(3+i) = -J.col(_RobotJointDoF+(_SlipDoF*(i+1)-1));
                    unknownx[3+i] = modelVelocities[_RobotJointDoF+(_SlipDoF*(i+1)-1)];
                }

                for (register int i=0; i<(_RobotTrees*_ContactDoF); ++i)
                {
                    unknownA.col(3+_RobotTrees+i) = -J.col(_RobotJointDoF+(_RobotTrees*_SlipDoF)+i);
                    unknownx[3+_RobotTrees+i] = modelVelocities[_RobotJointDoF+(_RobotTrees*_SlipDoF)+i];
                }

                knownB.template block< (6* _RobotTrees), 3> (0,0) = -spareI.template block<6*_RobotTrees, 3> (0,3); //Angular velocities
                knownB.template block< (6* _RobotTrees), _RobotJointDoF> (0,3) = J.template block<6*_RobotTrees, _RobotJointDoF> (0,0);

                knowny.template block<3, 1> (0,0) = cartesianVelocities.template block<3, 1> (3,0);
                knowny.template block<_RobotJointDoF, 1> (3,0) = modelVelocities.template block<_RobotJointDoF, 1> (0,0);

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<< "[MOTION_MODEL] spareI is of size "<<spareI.rows()<<"x"<<spareI.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The spareI matrix:\n" << spareI << std::endl;
                std::cout<< "[MOTION_MODEL] unknownA is of size "<<unknownA.rows()<<"x"<<unknownA.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The unknownA matrix:\n" << unknownA << std::endl;
                std::cout<< "[MOTION_MODEL] knownB is of size "<<knownB.rows()<<"x"<<knownB.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The knownB matrix:\n" << knownB << std::endl;
                std::cout<< "[MOTION_MODEL] Weight is of size "<<Weight.rows()<<"x"<<Weight.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The Weight matrix:\n" << Weight << std::endl;
                #endif

                return;
            }

            inline void navEquationsNoAngVelo(const Eigen::Matrix <_Scalar, 6, 1> &cartesianVelocities,
                                    const Eigen::Matrix <_Scalar, MODEL_DOF, 1> &modelVelocities,
                                    const Eigen::Matrix <_Scalar, 6*_RobotTrees, MODEL_DOF> &J,
                                    const Eigen::Matrix <_Scalar, 6, 6> &cartesianVelCov,
                                    const Eigen::Matrix <_Scalar, MODEL_DOF, MODEL_DOF> &modelVelCov,
                                    Eigen::Matrix <_Scalar, 6*_RobotTrees, 6+_RobotTrees+(_RobotTrees*_ContactDoF)> &unknownA,
                                    Eigen::Matrix <_Scalar, 6+_RobotTrees+(_RobotTrees*_ContactDoF), 1> &unknownx,
                                    Eigen::Matrix <_Scalar, 6*_RobotTrees, _RobotJointDoF> &knownB,
                                    Eigen::Matrix <_Scalar, _RobotJointDoF, 1> &knowny,
                                    Eigen::Matrix <_Scalar, 6*_RobotTrees, 6*_RobotTrees> &Weight)
            {
                Eigen::Matrix<_Scalar, 6*_RobotTrees, 6> spareI;

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"[MOTION_MODEL] navEquations\n";
                #endif

                for (register int i=0; i<_RobotTrees; ++i)
                    spareI.template block<6, 6>(i*6, 0) = Eigen::Matrix <_Scalar, 6, 6>::Identity();

                unknownA.template block<6*_RobotTrees, 6> (0,0) = spareI;//Linear and Angular Velocities

                unknownx.template block<6, 1> (0,0) = cartesianVelocities;

                for (register int i=0; i<_RobotTrees; ++i)
                {
                    unknownA.col(6+i) = -J.col(_RobotJointDoF+(_SlipDoF*(i+1)-1));
                    unknownx[6+i] = modelVelocities[_RobotJointDoF+(_SlipDoF*(i+1)-1)];
                }

                for (register int i=0; i<(_RobotTrees*_ContactDoF); ++i)
                {
                    unknownA.col(6+_RobotTrees+i) = -J.col(_RobotJointDoF+(_RobotTrees*_SlipDoF)+i);
                    unknownx[6+_RobotTrees+i] = modelVelocities[_RobotJointDoF+(_RobotTrees*_SlipDoF)+i];
                }

                knownB.template block< (6*_RobotTrees), _RobotJointDoF> (0,0) = J.template block<6*_RobotTrees, _RobotJointDoF> (0,0);

                knowny.template block<_RobotJointDoF, 1> (0,0) = modelVelocities.template block<_RobotJointDoF, 1> (0,0);

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<< "[MOTION_MODEL] spareI is of size "<<spareI.rows()<<"x"<<spareI.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The spareI matrix:\n" << spareI << std::endl;
                std::cout<< "[MOTION_MODEL] unknownA is of size "<<unknownA.rows()<<"x"<<unknownA.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The unknownA matrix:\n" << unknownA << std::endl;
                std::cout<< "[MOTION_MODEL] knownB is of size "<<knownB.rows()<<"x"<<knownB.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The knownB matrix:\n" << knownB << std::endl;
                std::cout<< "[MOTION_MODEL] Weight is of size "<<Weight.rows()<<"x"<<Weight.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The Weight matrix:\n" << Weight << std::endl;
                #endif

                return;
            }
        public:

            MotionModel()
            {
            }

            MotionModel(bool &status, MotionModel::methodContactPoint method,  kinematics_ptr robotModel)
            {
                std::vector<unsigned int> numberContactPoints (_RobotTrees, 0);

                contactPoints.resize (_RobotTrees);

                this->robotModel = robotModel;
                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"[MOTION_MODEL] Constructor\n";
                #endif

                robotModel->contactPointsPerTree(numberContactPoints);

                this->contactSelection = method;

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"[MOTION_MODEL] Get number of trees "<<robotModel->getNumberOfTrees()<<"\n";
                #endif

                if ((robotModel->getNumberOfTrees() == static_cast<int>(numberContactPoints.size())) &&(numberContactPoints.size() == _RobotTrees))
                {
                    for (register unsigned int i=0; i<numberContactPoints.size(); ++i)
                    {
                        contactPoints[i].number = numberContactPoints[i]; 
                        contactPoints[i].contactId = 0; 
                    }

                    LOG_INFO("[MOTION_MODEL] Created Motion Model using %s Robot Kinematics implementation\n", this->robotModel->name().c_str());
                    LOG_INFO("[MOTION_MODEL] %s has %d independent kinematic trees.\n", this->robotModel->name().c_str(), this->robotModel->getNumberOfTrees());
                    status = true;
                }
                else
                {
                    LOG_ERROR("[MOTION_MODEL] Malfunction of the MotionModel. WRONG number of template parameters\n");
                    status = false;
                }

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<<"[MOTION_MODEL] contactPoints contains: ";
                for (std::vector<TreeContactPoint>::iterator it = contactPoints.begin() ; it != contactPoints.end(); it++)
                {
                    std::cout<<" number: "<<(*it).number<<" contactId: "<<(*it).contactId;
                }

                std::cout<<"\n[MOTION_MODEL] **** END ****\n";
                #endif

                return;
            }

            ~MotionModel()
            {
            }

            void updateKinematics (const Eigen::Matrix <_Scalar, MODEL_DOF, 1> &modelPositions)
            {
                register int i=0;
                std::vector<_Scalar> vectorPositions(MODEL_DOF, 0);
                std::vector<int> contactId (_RobotTrees, 0);

                Eigen::Map <Eigen::Matrix <_Scalar, MODEL_DOF, 1> > (&(vectorPositions[0]), MODEL_DOF) = modelPositions;

                robotModel->fkSolver(vectorPositions, fkRobot, fkCov);

                this->selectPointsInContact (this->contactSelection);

                for (std::vector<TreeContactPoint>::iterator it = contactPoints.begin() ; it != contactPoints.end(); it++)
                {
                    contactId[i] = (*it).contactId;
                    i++;
                }
                robotModel->setPointsInContact(contactId);

                return;
            }


            inline virtual void getKinematics (std::vector<Eigen::Affine3d> &currentFkRobot, std::vector<base::Matrix6d> &currentFkCov)
            {
                currentFkRobot = this->fkRobot;
                currentFkCov = this->fkCov;

                return;
            }

            virtual std::vector< int > getPointsInContact ()
            {
                register int i=0;
                std::vector<int> contactId (_RobotTrees, 0);

                for (std::vector<TreeContactPoint>::iterator it = contactPoints.begin() ; it != contactPoints.end(); it++)
                {
                    contactId[i] = (*it).contactId;
                    i++;
                }

                return contactId;
            }


            virtual double navSolver(const Eigen::Matrix <_Scalar, MODEL_DOF, 1> &modelPositions,
                                            Eigen::Matrix <_Scalar, 6, 1> &cartesianVelocities,
                                            Eigen::Matrix <_Scalar, MODEL_DOF, 1> &modelVelocities,
                                            Eigen::Matrix <_Scalar, 6, 6> &cartesianVelCov,
                                            Eigen::Matrix <_Scalar, MODEL_DOF, MODEL_DOF> &modelVelCov,
                                            Eigen::Matrix <_Scalar, 6*_RobotTrees, 6*_RobotTrees> Weight)
            {
                double normalizedError = std::numeric_limits<double>::quiet_NaN(); //solution error of the Least-Squares
                std::vector<_Scalar> vectorPositions(MODEL_DOF, 0); // model positions in std_vector form
                Eigen::Matrix <_Scalar, 6*_RobotTrees, MODEL_DOF> J; // Robot Jacobian
                Eigen::Matrix <_Scalar, 6*_RobotTrees, 3+_RobotTrees+(_RobotTrees*_ContactDoF)> unknownA; // Nav non-sensed values matrix
                Eigen::Matrix <_Scalar, 3+_RobotTrees+(_RobotTrees*_ContactDoF), 1> unknownx; // Nav non-sensed values vector
                Eigen::Matrix <_Scalar, 6*_RobotTrees, 3+_RobotJointDoF> knownB; // Nav sensed values matrix
                Eigen::Matrix <_Scalar, 3+_RobotJointDoF, 1> knowny; // Nav sensed values vector

                unknownA.setZero(); unknownx.setZero();
                knownB.setZero(); knowny.setZero();

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout << "[MOTION_MODEL] cartesianVelocities is of size "<<cartesianVelocities.rows()<<"x"<<cartesianVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] cartesianVelocities is \n" << cartesianVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] modelVelocities is of size "<<modelVelocities.rows()<<"x"<<modelVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] modelVelocities is \n" << modelVelocities<< std::endl;
                #endif

                Eigen::Map <Eigen::Matrix <_Scalar, MODEL_DOF, 1> > (&(vectorPositions[0]), MODEL_DOF) = modelPositions;

                J = robotModel->jacobianSolver (vectorPositions);

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<< "[MOTION_MODEL] J is of size "<<J.rows()<<"x"<<J.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The J matrix \n" << J << std::endl;
                #endif

                this->navEquations (cartesianVelocities, modelVelocities, J,
                        cartesianVelCov, modelVelCov, unknownA, unknownx, knownB, knowny, Weight);

                //Eigen::Matrix <_Scalar,3+_RobotTrees+(_RobotTrees*_ContactDoF), 3+_RobotTrees+(_RobotTrees*_ContactDoF)> pseudoInvUnknownA;
                Eigen::Matrix <_Scalar, 6*_RobotTrees, 1> knownb = knownB*knowny;

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL

                typedef Eigen::Matrix <_Scalar, 6*_RobotTrees, 3+_RobotTrees+(_RobotTrees*_ContactDoF)> matrixAType;
                typedef Eigen::Matrix <_Scalar, 6*_RobotTrees, 3+_RobotJointDoF> matrixBType;
                typedef Eigen::Matrix <_Scalar, 6*_RobotTrees, 3+_RobotTrees+(_RobotTrees*_ContactDoF)+1> matrixConjType; // columns are columns of A + 1

                matrixConjType Conj;

                Eigen::FullPivLU<matrixAType> lu_decompA(unknownA);
                std::cout << "[MOTION_MODEL] The rank of A is " << lu_decompA.rank() << std::endl;

                Eigen::FullPivLU<matrixBType> lu_decompB(knownB);
                std::cout << "[MOTION_MODEL] The rank of B is " << lu_decompB.rank() << std::endl;

                Conj.template block<6*_RobotTrees,3+_RobotTrees+(_RobotTrees*_ContactDoF)>(0,0) = unknownA;
                Conj.template block<6*_RobotTrees, 1>(0,3+_RobotTrees+(_RobotTrees*_ContactDoF)) = knownb;
                Eigen::FullPivLU<matrixConjType> lu_decompConj(Conj);
                std::cout << "[MOTION_MODEL] The rank of A|B*y is " << lu_decompConj.rank() << std::endl;
                std::cout << "[MOTION_MODEL] Pseudoinverse of A\n" << (unknownA.transpose() * Weight * unknownA).inverse() << std::endl;
                /*******************/
                #endif

                unknownx = (unknownA.transpose() * Weight * unknownA).ldlt().solve(unknownA.transpose() * Weight * knownb);

                Eigen::Matrix<double, 1,1> squaredError = (((unknownA*unknownx - knownb).transpose() * Weight * (unknownA*unknownx - knownb)));
                if (knownb.norm() != 0.00)
                    normalizedError = sqrt(squaredError[0]) / knownb.norm();


                Eigen::Matrix <_Scalar, 6*_RobotTrees, 6*_RobotTrees> errorCov = (unknownA*unknownx - knownb).asDiagonal(); errorCov *= errorCov;// L-S error covariance
                Eigen::Matrix <_Scalar, 3+_RobotTrees+(_RobotTrees*_ContactDoF), 3+_RobotTrees+(_RobotTrees*_ContactDoF)> uncertaintyCov; // noise cov
                uncertaintyCov = (unknownA.transpose() * errorCov.inverse() * unknownA).inverse(); // Observer
                uncertaintyCov = 0.5*(uncertaintyCov + uncertaintyCov.transpose());// Guarantee symmetry

                cartesianVelocities.template block<3, 1>(0,0) = unknownx.template block<3, 1>(0,0); // Linear velocities
                cartesianVelCov.template block<3, 3> (0,0) = uncertaintyCov.template block<3,3>(0,0);//Linear Velocities noise

                if (cartesianVelCov.template block<3, 3> (3,3) == Eigen::Matrix3d::Zero())
                {
                    for (register size_t i=0; i<_RobotTrees; ++i)
                    {
                        cartesianVelCov.template block<3, 3> (3,3) += Weight.template block<3,3> (3+(6*i), 3+(6*i)) * errorCov.template block<3,3> (3+(6*i),3+(6*i));//Angular Velocities noise
                    }
                }

                for (register int i=0; i<_RobotTrees; ++i)
                {
                    modelVelocities[_RobotJointDoF+(_SlipDoF*(i+1)-1)] = unknownx[3+i];
                    modelVelCov.col(_RobotJointDoF+(_SlipDoF*(i+1)-1))[_RobotJointDoF+(_SlipDoF*(i+1)-1)] = uncertaintyCov.col(3+i)[3+i];//pseudoInvUnknownA.col(3+i)[3+i]; For the time being set the error to the error in the estimation
                }

                for (register int i=0; i<(_RobotTrees*_ContactDoF); ++i)
                {
                    modelVelocities[_RobotJointDoF+(_RobotTrees*_SlipDoF)+i] = unknownx[3+_RobotTrees+i];
                    modelVelCov.col(_RobotJointDoF+(_RobotTrees*_SlipDoF)+i)[_RobotJointDoF+(_RobotTrees*_SlipDoF)+i] = uncertaintyCov.col(3+_RobotTrees+i)[3+_RobotTrees+i];//pseudoInvUnknownA.col(3+_RobotTrees+i)[3+_RobotTrees+i];
                }

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout << "[MOTION_MODEL] L-S solution:\n"<<unknownx<<std::endl;

                std::cout << "[MOTION_MODEL] RESULT errorCov is of size "<<errorCov.rows()<<"x"<<errorCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT errorCov is \n" << errorCov << std::endl;

                std::cout << "[MOTION_MODEL] RESULT uncertaintyCov is of size "<<uncertaintyCov.rows()<<"x"<<uncertaintyCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT uncertaintyCov is \n" << uncertaintyCov << std::endl;

                std::cout << "[MOTION_MODEL] RESULT cartesianVelocities is of size "<<cartesianVelocities.rows()<<"x"<<cartesianVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT cartesianVelocities is \n" << cartesianVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] RESULT cartesianVelCov is of size "<<cartesianVelCov.rows()<<"x"<<cartesianVelCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT cartesianVelCov is \n" << cartesianVelCov<< std::endl;

                std::cout << "[MOTION_MODEL] RESULT modelVelocities is of size "<<modelVelocities.rows()<<"x"<<modelVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT modelVelocities is \n" << modelVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] RESULT modelVelCov is of size "<<modelVelCov.rows()<<"x"<<modelVelCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT modelVelCov is \n" << modelVelCov<< std::endl;

                std::cout << "[MOTION_MODEL] RESULT The absolute least squared error is:\n" << squaredError << std::endl;
                std::cout << "[MOTION_MODEL] RESULT The relative error is:\n" << normalizedError << std::endl;
                std::cout << "[MOTION_MODEL] RESULT The error vector is \n"<<(unknownA*unknownx - knownb)<<"\n";
                std::cout << "[MOTION_MODEL] RESULT The error variance is \n"<<errorCov<<"\n";
                std::cout << "[MOTION_MODEL] RESULT The error variance.inverse() is \n"<<errorCov.inverse()<<"\n";
                std::cout << "[MOTION_MODEL] RESULT The solution covariance is \n"<<cartesianVelCov<<"\n";
                #endif


                return normalizedError;
            }

            virtual double navSolverNoAngVelo(const Eigen::Matrix <_Scalar, MODEL_DOF, 1> &modelPositions,
                                            Eigen::Matrix <_Scalar, 6, 1> &cartesianVelocities,
                                            Eigen::Matrix <_Scalar, MODEL_DOF, 1> &modelVelocities,
                                            Eigen::Matrix <_Scalar, 6, 6> &cartesianVelCov,
                                            Eigen::Matrix <_Scalar, MODEL_DOF, MODEL_DOF> &modelVelCov,
                                            Eigen::Matrix <_Scalar, 6*_RobotTrees, 6*_RobotTrees> Weight)
            {
                double normalizedError = std::numeric_limits<double>::quiet_NaN(); //solution error of the Least-Squares
                std::vector<_Scalar> vectorPositions(MODEL_DOF, 0); // model positions in std_vector form
                Eigen::Matrix <_Scalar, 6*_RobotTrees, MODEL_DOF> J; // Robot Jacobian
                Eigen::Matrix <_Scalar, 6*_RobotTrees, 6+_RobotTrees+(_RobotTrees*_ContactDoF)> unknownA; // Nav non-sensed values matrix
                Eigen::Matrix <_Scalar, 6+_RobotTrees+(_RobotTrees*_ContactDoF), 1> unknownx; // Nav non-sensed values vector
                Eigen::Matrix <_Scalar, 6*_RobotTrees, _RobotJointDoF> knownB; // Nav sensed values matrix
                Eigen::Matrix <_Scalar, _RobotJointDoF, 1> knowny; // Nav sensed values vector

                unknownA.setZero(); unknownx.setZero();
                knownB.setZero(); knowny.setZero();

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout << "[MOTION_MODEL] cartesianVelocities is of size "<<cartesianVelocities.rows()<<"x"<<cartesianVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] cartesianVelocities is \n" << cartesianVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] modelVelocities is of size "<<modelVelocities.rows()<<"x"<<modelVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] modelVelocities is \n" << modelVelocities<< std::endl;
                #endif

                Eigen::Map <Eigen::Matrix <_Scalar, MODEL_DOF, 1> > (&(vectorPositions[0]), MODEL_DOF) = modelPositions;

                J = robotModel->jacobianSolver (vectorPositions);

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout<< "[MOTION_MODEL] J is of size "<<J.rows()<<"x"<<J.cols()<<"\n";
                std::cout<< "[MOTION_MODEL] The J matrix \n" << J << std::endl;
                #endif

                this->navEquationsNoAngVelo (cartesianVelocities, modelVelocities, J,
                        cartesianVelCov, modelVelCov, unknownA, unknownx, knownB, knowny, Weight);

                //Eigen::Matrix <_Scalar,6+_RobotTrees+(_RobotTrees*_ContactDoF), 6+_RobotTrees+(_RobotTrees*_ContactDoF)> pseudoInvUnknownA;
                Eigen::Matrix <_Scalar, 6*_RobotTrees, 1> knownb = knownB*knowny;

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL

                typedef Eigen::Matrix <_Scalar, 6*_RobotTrees, 6+_RobotTrees+(_RobotTrees*_ContactDoF)> matrixAType;
                typedef Eigen::Matrix <_Scalar, 6*_RobotTrees, _RobotJointDoF> matrixBType;
                typedef Eigen::Matrix <_Scalar, 6*_RobotTrees, 6+_RobotTrees+(_RobotTrees*_ContactDoF)+1> matrixConjType; // columns are columns of A + 1

                matrixConjType Conj;

                Eigen::FullPivLU<matrixAType> lu_decompA(unknownA);
                std::cout << "[MOTION_MODEL] The rank of A is " << lu_decompA.rank() << std::endl;

                Eigen::FullPivLU<matrixBType> lu_decompB(knownB);
                std::cout << "[MOTION_MODEL] The rank of B is " << lu_decompB.rank() << std::endl;

                Conj.template block<6*_RobotTrees,6+_RobotTrees+(_RobotTrees*_ContactDoF)>(0,0) = unknownA;
                Conj.template block<6*_RobotTrees, 1>(0,_RobotTrees+(_RobotTrees*_ContactDoF)) = knownb;
                Eigen::FullPivLU<matrixConjType> lu_decompConj(Conj);
                std::cout << "[MOTION_MODEL] The rank of A|B*y is " << lu_decompConj.rank() << std::endl;
                std::cout << "[MOTION_MODEL] Pseudoinverse of A\n" << (unknownA.transpose() * Weight * unknownA).inverse() << std::endl;
                /*******************/
                #endif

                unknownx = (unknownA.transpose() * Weight * unknownA).ldlt().solve(unknownA.transpose() * Weight * knownb);

                Eigen::Matrix<double, 1,1> squaredError = (((unknownA*unknownx - knownb).transpose() * Weight * (unknownA*unknownx - knownb)));
                if (knownb.norm() != 0.00)
                    normalizedError = sqrt(squaredError[0]) / knownb.norm();

                Eigen::Matrix <_Scalar, 6*_RobotTrees, 6*_RobotTrees> errorCov; // L-S error covariance
                Eigen::Matrix <_Scalar, 6+_RobotTrees+(_RobotTrees*_ContactDoF), 6+_RobotTrees+(_RobotTrees*_ContactDoF)> uncertaintyCov; // noise cov
                errorCov = 0.6827 * squaredError[0] * Eigen::Matrix <_Scalar, 6*_RobotTrees, 6*_RobotTrees>::Identity();
                uncertaintyCov = (unknownA.transpose() * errorCov.inverse() * unknownA).inverse();

                cartesianVelocities.template block<3, 1>(0,0) = unknownx.template block<3, 1>(0,0); // Linear velocities
                cartesianVelocities.template block<3, 1>(3,0) = unknownx.template block<3, 1>(3,0); // Angular velocities
                cartesianVelCov = uncertaintyCov.template block<6,6>(0,0);

                for (register int i=0; i<_RobotTrees; ++i)
                {
                    modelVelocities[_RobotJointDoF+(_SlipDoF*(i+1)-1)] = unknownx[6+i];
                    modelVelCov.col(_RobotJointDoF+(_SlipDoF*(i+1)-1))[_RobotJointDoF+(_SlipDoF*(i+1)-1)] = uncertaintyCov.col(6+i)[6+i];
                }

                for (register int i=0; i<(_RobotTrees*_ContactDoF); ++i)
                {
                    modelVelocities[_RobotJointDoF+(_RobotTrees*_SlipDoF)+i] = unknownx[6+_RobotTrees+i];
                    modelVelCov.col(_RobotJointDoF+(_RobotTrees*_SlipDoF)+i)[_RobotJointDoF+(_RobotTrees*_SlipDoF)+i] = uncertaintyCov.col(6+_RobotTrees+i)[6+_RobotTrees+i];
                }

                #ifdef DEBUG_PRINTS_ODOMETRY_MOTION_MODEL
                std::cout << "[MOTION_MODEL] L-S solution:\n"<<unknownx<<std::endl;

                std::cout << "[MOTION_MODEL] cartesianVelocities is of size "<<cartesianVelocities.rows()<<"x"<<cartesianVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] cartesianVelocities is \n" << cartesianVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] cartesianVelCov is of size "<<cartesianVelCov.rows()<<"x"<<cartesianVelCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] cartesianVelCov is \n" << cartesianVelCov<< std::endl;

                std::cout << "[MOTION_MODEL] modelVelocities is of size "<<modelVelocities.rows()<<"x"<<modelVelocities.cols()<<"\n";
                std::cout << "[MOTION_MODEL] modelVelocities is \n" << modelVelocities<< std::endl;

                std::cout << "[MOTION_MODEL] modelVelCov is of size "<<modelVelCov.rows()<<"x"<<modelVelCov.cols()<<"\n";
                std::cout << "[MOTION_MODEL] modelVelCov is \n" << modelVelCov<< std::endl;

                std::cout << "[MOTION_MODEL] The absolute least squared error is:\n" << squaredError << std::endl;
                std::cout << "[MOTION_MODEL] The relative error is:\n" << normalizedError << std::endl;
                #endif


                return normalizedError;
            }
            virtual base::Vector6d slipSolver(void)
            {
                base::Vector6d s;

                return s;
            }
    };

    template<typename _Derived>
    static inline bool isnotnan(const Eigen::MatrixBase<_Derived>& x)
    {
        return ((x.array() == x.array())).all();
    };

}

#endif //ODOMETRY_MOTION_MODEL_HPP