Task.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */

#ifndef THREED_ODOMETRY_TASK_TASK_HPP
#define THREED_ODOMETRY_TASK_TASK_HPP

#include "threed_odometry/TaskBase.hpp"

#include <boost/shared_ptr.hpp> 
#include <Eigen/Core>
#include <Eigen/SVD> 
#include <Eigen/Dense> 
#include <Eigen/StdVector> 
#include <threed_odometry/KinematicKDL.hpp> 
#include <threed_odometry/MotionModel.hpp> 
#include <threed_odometry/IIR.hpp> 
namespace threed_odometry {

    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> WeightingMatrix;

    template<class scalar> inline scalar tolerance();

    template<> inline float  tolerance<float >() { return 1e-5f; }
    template<> inline double tolerance<double>() { return 1e-11; }

    static const unsigned int NORDER_BESSEL_FILTER = 8; 
    class Task : public TaskBase
    {
        friend class TaskBase;
    protected:

        virtual void joints_samplesTransformerCallback(const base::Time &ts, const ::base::samples::Joints &joints_samples_sample);

        virtual void orientation_samplesTransformerCallback(const base::Time &ts, const ::base::samples::RigidBodyState &orientation_samples_sample);

    protected:

        /**************************/
        /*** Property Variables ***/
        /**************************/

        std::string urdfFile;

        std::vector<std::string> contact_point_segments;

        std::vector<std::string> contact_angle_segments;

        std::vector<std::string> all_joint_names;

        std::vector<std::string> slip_joint_names;

        std::vector<std::string> contact_joint_names;

        ModelType kinematic_model_type;

        IIRCoefficients iirConfig;

        /******************************************/
        /*** General Internal Storage Variables ***/
        /******************************************/

        int number_robot_joints;

        std::vector<std::string> motion_model_joint_names;

        Eigen::Matrix< double, Eigen::Dynamic, 1  > joint_positions;

        Eigen::Matrix< double, Eigen::Dynamic, 1  > joint_velocities;

        Eigen::Matrix< double, 6, 1  > cartesian_velocities;

        std::vector< Eigen::Matrix <double, 6, 1> , Eigen::aligned_allocator < Eigen::Matrix <double, 6, 1> > > vector_cartesian_velocities;

        boost::shared_ptr< threed_odometry::KinematicKDL > robotKinematics;

        boost::shared_ptr< threed_odometry::MotionModel<double> > motionModel;

        std::vector<Eigen::Affine3d> fkRobotTrans;

        std::vector<base::Matrix6d> fkRobotCov;

        Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > modelVelCov;

        Eigen::Matrix< double, 6, 6  > cartesianVelCov;

        boost::shared_ptr< threed_odometry::IIR<NORDER_BESSEL_FILTER, 3> > bessel;

        WeightingMatrix weight_matrix;

        ::base::samples::RigidBodyState delta_pose;

        /***************************/
        /***************************/

        ::base::samples::Joints joints_samples;

        ::base::samples::RigidBodyState orientation_samples;


        /***************************/
        /***************************/

        Eigen::Affine3d pose;
        Eigen::Matrix<double, 6, 6> poseCov;


    public:
        Task(std::string const& name = "exoter_odometry::Task");

        Task(std::string const& name, RTT::ExecutionEngine* engine);

        ~Task();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();

        void motionVelocities();

        void deadReckoning(const double &delta_t);

        void joints_samplesUnpack(const ::base::samples::Joints &original_joints,
                                const std::vector<std::string> &order_names,
                                Eigen::Matrix< double, Eigen::Dynamic, 1  > &joint_positions,
                                Eigen::Matrix< double, Eigen::Dynamic, 1  > &joint_velocities);

        bool joints_samplesMotionModel(std::vector<std::string> &order_names,
                                const std::vector<std::string> &joint_names,
                                const std::vector<std::string> &slip_names,
                                const std::vector<std::string> &contact_names);


        void outputPortPose(const Eigen::Matrix< double, 6, 1  > &cartesian_velocities);

        void outputPortContactPoints();

    public:

        template <typename _MatrixType>
        static _MatrixType guaranteeSPD (const _MatrixType &A)
        {
            _MatrixType spdA;
            Eigen::VectorXd s;
            s.resize(A.rows(), 1);

            Eigen::JacobiSVD <Eigen::MatrixXd > svdOfA (A, Eigen::ComputeThinU | Eigen::ComputeThinV);

            s = svdOfA.singularValues(); 

            #ifdef DEBUG_PRINTS
            std::cout<<"[SPD-SVD] s: \n"<<s<<"\n";
            std::cout<<"[SPD-SVD] svdOfA.matrixU():\n"<<svdOfA.matrixU()<<"\n";
            std::cout<<"[SPD-SVD] svdOfA.matrixV():\n"<<svdOfA.matrixV()<<"\n";

            Eigen::EigenSolver<_MatrixType> eig(A);
            std::cout << "[SPD-SVD] BEFORE: eigen values: " << eig.eigenvalues().transpose() << std::endl;
            #endif

            for (register int i=0; i<s.size(); ++i)
            {
                #ifdef DEBUG_PRINTS
                std::cout<<"[SPD-SVD] i["<<i<<"]\n";
                #endif

                if (s(i) < 0.00)
                    s(i) = 0.00;
            }
            spdA = svdOfA.matrixU() * s.matrix().asDiagonal() * svdOfA.matrixV().transpose();

            #ifdef DEBUG_PRINTS
            Eigen::EigenSolver<_MatrixType> eigSPD(spdA);
            if (eig.eigenvalues() == eigSPD.eigenvalues())
            std::cout<<"[SPD-SVD] EQUAL!!\n";

            std::cout << "[SPD-SVD] AFTER: eigen values: " << eigSPD.eigenvalues().transpose() << std::endl;
            #endif

            return spdA;
        };

        Eigen::Vector3d boxminus(const double &w, const Eigen::Vector3d& vec, const double &scale, bool plus_minus_periodicity)
        {
            Eigen::Vector3d result;

            double nv = vec.norm();
            if(nv < threed_odometry::tolerance<double>())
            {
                if(!plus_minus_periodicity)
                {
                    // find the maximal entry:
                    int i;
                    vec.maxCoeff(&i);
                    result = scale * std::atan2(0, w) * Eigen::Vector3d::Unit(i);
                    return result;
                }
                nv = threed_odometry::tolerance<double>();
            }
            double s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );

            result = s * vec;

            return result;
        };

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW


    };
}

#endif