robotAbstract.hpp

Go to the documentation of this file.

#ifndef __RobotAbstract_H__
#define __RobotAbstract_H__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include "kernel/jafarDebug.hpp"
#include "kernel/IdFactory.hpp"
#include "kernel/dataLog.hpp"
#include "kernel/timingTools.hpp"
#include "jmath/jblas.hpp"
#include "jmath/indirectArray.hpp"

#include "rtslam/rtSlam.hpp"
#include "rtslam/gaussian.hpp"
#include "rtslam/mapObject.hpp"
#include "rtslam/perturbation.hpp"
#include "rtslam/quatTools.hpp"
// include parents
#include "rtslam/mapAbstract.hpp"
#include "rtslam/mapObject.hpp"
#include "rtslam/parents.hpp"

#include "rtslam/hardwareSensorAbstract.hpp"

namespace jafar {
  namespace rtslam {


    //  Forward declarations of children
    class SensorAbstract;

    class RobotQuantity
    {
      public:
        enum Quantity { qPos, qOriQuat, qAbsVel, qAngVel, qAbsAcc, qAngAcc, qAbsJerk, qAngJerk, qG, qNormG, qNQuantity };
        static const int QuantityDataSizes[qNQuantity];
        typedef std::vector<Quantity> QuantityList;
      protected:
        size_t state_size;
        int quantities[qNQuantity];
      public:
        RobotQuantity()
          { clearQuantities(); }
        RobotQuantity(QuantityList quants)
          { clearQuantities(); for(QuantityList::iterator it = quants.begin(); it != quants.end(); ++it) registerRobotQuantity(*it); }
        RobotQuantity(RobotQuantity const & robQuant, QuantityList quants)
          { clearQuantities(); for(QuantityList::iterator it = quants.begin(); it != quants.end(); ++it) quantities[*it] = robQuant.quantities[*it]; }

        virtual bool registerRobotQuantity(Quantity quantity)
          { if (quantities[quantity] < 0) { quantities[quantity] = state_size; state_size += QuantityDataSizes[quantity]; return false; } else return true; }
        virtual ublas::range allocateSensorQuantity(size_t size)
          { size_t pos = state_size; state_size += size; return ublas::range(pos, state_size); }
        virtual void clearQuantities()
          { for(int i = 0; i < qNQuantity; ++i) quantities[i] = -1; state_size = 0; }

        inline bool hasQuantity(Quantity quantity)
          { return (quantities[quantity] >= 0); }
        inline ublas::range getQuantity(Quantity quantity)
          { return hasQuantity(quantity) ? ublas::range(quantities[quantity], quantities[quantity]+QuantityDataSizes[quantity]) : ublas::range(0,0); }
        inline jblas::ind_array getQuantities()
          { jblas::ind_array ia; for(int i = 0; i < qNQuantity; ++i) if (hasQuantity((Quantity)i)) ia = ia_union(ia, ia_set(getQuantity((Quantity)i))); return ia; }
    };


    class RobotAbstract: public MapObject,
                         public ChildOf<MapAbstract>,
                         public boost::enable_shared_from_this<RobotAbstract>,
                         public ParentOf<SensorAbstract>,
                         public kernel::DataLoggable,
                         public RobotQuantity {

        friend ostream& operator <<(ostream & s, RobotAbstract const & rob);

      protected:
        bool constructed;
        static IdFactory robotIds;
      public:
        virtual bool registerRobotQuantity(Quantity quantity)
        {
          if (!RobotQuantity::registerRobotQuantity(quantity))
          {
            if (constructed) construct(mapPtr(), state_size, control.size(), perturbation.size());
            return false;
          } else
            return true;
        }
        virtual ublas::range allocateSensorQuantity(size_t size)
        {
          ublas::range r = RobotQuantity::allocateSensorQuantity(size);
          if (constructed) construct(mapPtr(), state_size, control.size(), perturbation.size());
          return r;
        }

        enum type_enum {
          ODOMETRY, CONSTANT_VELOCITY, CONSTANT_MOTION_MODEL, INERTIAL, CENTERED_CONSTANT_VELOCITY
        };
        type_enum type;

        sensor_ptr_t last_updated;

        // define the function linkToParentMap().
        ENABLE_LINK_TO_PARENT(MapAbstract,Map,RobotAbstract)
        ;
        // define the functions mapPtr() and map().
        ENABLE_ACCESS_TO_PARENT(MapAbstract,map)
        ;
        // define the type SensorList, and the function sensorList().
        ENABLE_ACCESS_TO_CHILDREN(SensorAbstract,Sensor,sensor);

        hardware::hardware_sensorprop_ptr_t hardwareEstimatorPtr;
        history_manager_ptr_t historyManager;

        RobotAbstract(const map_ptr_t & _mapPtr);

        void construct(const map_ptr_t & _mapPtr, const size_t _size_state, const size_t _size_control, const size_t _size_pert);

        // Mandatory virtual destructor.
        virtual ~RobotAbstract() {
        }

        void setPoseStd(double x, double y, double z,
          double roll, double pitch, double yaw,
          double xStd, double yStd, double zStd,
          double rollStd, double pitchStd, double yawStd,
          bool degrees = false);
        void setPositionStd(double x, double y, double z,
          double xStd, double yStd, double zStd);
        void setOrientationStd(double roll, double pitch, double yaw,
          double rollStd, double pitchStd, double yawStd,
          bool degrees = false);

        
        virtual std::string categoryName() const {
          return "ROBOT";
        }


        void setId(){id(robotIds.getId());}

        Gaussian pose; 
        vec control; 
        Perturbation perturbation; 

        bool constantPerturbation;
        kernel::Timestamp self_time; 
        double dt_or_dx; 
        jblas::mat controls;

        jblas::mat XNEW_x; 
        jblas::mat XNEW_pert; 
        jblas::sym_mat Q; 
        
        void setRobotPose(jblas::vec6 const & pose_euler, bool degrees = false);
        jblas::vec robot_pose; 

        void setInitialPose(double x, double y, double z,
          double roll, double pitch, double yaw,
          double xStd, double yStd, double zStd,
          double rollStd, double pitchStd, double yawStd,
          bool degrees = false);
        void setOrigin(double x, double y, double z)
        {
          start_pos_abs(0) = x;
          start_pos_abs(1) = y;
          start_pos_abs(2) = z;
          start_pos_abs += start_pos_export;
          is_start_pos_abs_init = true;
        }

        jblas::vec start_pos_export; 
        jblas::vec start_pos_abs; 
        bool is_start_pos_abs_init; 

        void setInitialOrientation(double roll, double pitch, double yaw,
          double rollStd, double pitchStd, double yawStd,
          bool degrees = false);
        void getCurrentPose(double time, jblas::vec & x, jblas::sym_mat & P);
        template<class VEC, class SYM_MAT>
        void slamPoseToRobotPose(VEC & slam_x, SYM_MAT & slam_P, jblas::vec & robot_x, jblas::sym_mat & robot_P) const
        {
          // normalize quat
          jblas::vec4 q = ublas::subrange(slam_x, 3, 7);
          jblas::mat Q_q(4,4), FQ_fq(7,7); FQ_fq.clear();
          ublasExtra::normalizeJac(q, Q_q);
          ublas::subrange(FQ_fq, 0,3, 0,3) = jblas::identity_mat(3);
          ublas::subrange(FQ_fq, 3,7, 3,7) = Q_q;
          slam_P = ublasExtra::prod_JPJt(slam_P, FQ_fq);
          ublasExtra::normalize(q);
          ublas::subrange(slam_x, 3, 7) = q;

          // convert mean
          jblas::vec7 quat_pose = quaternion::composeFrames(slam_x, robot_pose);
          jblas::vec6 euler_pose;
          ublas::subrange(euler_pose, 0,3) = ublas::subrange(quat_pose, 0,3) + start_pos_export;
          mat E_q(3, 4);
          jblas::vec3 euler_ori;
          quaternion::q2e(ublas::subrange(quat_pose, 3, 7), euler_ori, E_q);
          ublas::subrange(euler_pose, 3,6) = euler_ori;

          // convert cov
          mat FE_fq(6,7); FE_fq.clear();
          ublas::subrange(FE_fq,0,3,0,3) = identity_mat(3);
          ublas::subrange(FE_fq, 3,6,3,7) = E_q;

          jblas::mat Q_s(7,7);
          quaternion::composeFrames_by_dglobal(slam_x, robot_pose, Q_s);

          mat FE_s = ublas::prod(FE_fq, Q_s);

          robot_x = euler_pose;
          robot_P = prod_JPJt(slam_P, FE_s);
        }


        boost::mutex mutex_extrapol;
        bool extrapol_up_to_date;
        double self_time_extrapol, self_time_extrapol_init, self_time_extrapol_inc;
        jblas::vec state_extrapol_x, state_extrapol_x_init, state_extrapol_x_inc;
        jblas::sym_mat state_extrapol_P, state_extrapol_P_init, state_extrapol_P_inc;
        vec control_extrapol;
        Perturbation perturbation_extrapol;
        double dt_or_dx_extrapol;
        jblas::mat controls_extrapol;
        jblas::mat XNEW_x_extrapol;
        jblas::mat XNEW_pert_extrapol;
        jblas::sym_mat Q_extrapol;


        virtual size_t mySize() = 0;
        virtual size_t mySize_control() = 0;
        virtual size_t mySize_perturbation() = 0;
        static size_t size_control() {
          return 0;
        }
        static size_t size_perturbation() {
          return 0;
        }
        void set_control(const vec & c) {
          JFR_ASSERT(c.size() == mySize_control(), "RobotAbstract::set_control(vec&, double): Sizes mismatch");
          control = c;
        }
        void set_perturbation(const Perturbation & _pert) {
          perturbation = _pert;
        }

        void setHardwareEstimator(hardware::hardware_sensorprop_ptr_t hardwareEstimatorPtr_)
        {
          hardwareEstimatorPtr = hardwareEstimatorPtr_;
        }


        inline void init(const vec & _u, const vec & _U) {
          JFR_ASSERT(_u.size() >= control.size(), "robotAbstract.hpp: init: wrong control size.");
          control = ublas::subrange(_u, 0, control.size());
          vec x = state.x();
          vec xnew(x.size());

          init_func(x, control, _U, xnew);
          state.x() = xnew;
        }
        
        inline void init(const vec & _u) {
          JFR_ASSERT(_u.size() >= control.size(), "robotAbstract.hpp: init: wrong control size.");
          control = ublas::subrange(_u, 0, control.size());
          vec x = state.x();
          vec xnew(x.size());

          init_func(x, control, xnew);
          state.x() = xnew;
        }

        void move();

        //template<class V>
        inline void move(const vec & _u) {
          JFR_ASSERT(_u.size() >= control.size(), "robotAbstract.hpp: move: wrong control size.");
          control = ublas::subrange(_u, 0, control.size());
          move();
        }

        bool computeControls(double time1, double time2, jblas::mat & controls, bool release) const;

        virtual bool move(double time);

        void move_extrapolate();
        void move_extrapolate(double time);
        void reinit_extrapolate();
        void move_fake(double time);

        void computeStatePerturbation();
        void computeStatePerturbation_extrapol();

        void applyConstraints()
        {
          // TODO should also constrain sensors pose quaternions
          ublas::range iaOriQuat = getQuantity(qOriQuat);
          jblas::sym_mat Q(4,4); Q.clear();
          jblas::mat QN_q(4,4);
          jblas::vec q = ublas::project(state.x(), iaOriQuat);
          ublasExtra::normalizeJac(q, QN_q);
          ublas::project(state.x(), iaOriQuat) = ublasExtra::normalized(q);
          mapPtr()->filterPtr->predict(mapPtr()->ia_used_states(), QN_q, state.ia().compose(iaOriQuat), Q);
        }

        virtual void writeLogHeader(kernel::DataLogger& log) const;
        virtual void writeLogData(kernel::DataLogger& log) const;

        void stopDumping();

        void setHistoryManager(history_manager_ptr_t hm) { historyManager = hm; }

      protected:


        virtual void move_func(const vec & _x, const vec & _u, const vec& _n, const double _dt, vec & _xnew,
                               mat & _XNEW_x, mat & _XNEW_pert, unsigned tempSet = 0) const = 0;
        virtual void init_func(const vec & _x, const vec & _u, const vec & _U, vec & _xnew) {}
        virtual void init_func(const vec & _x, const vec & _u, vec & _xnew) {}


    };

  }
}

#endif // #ifndef __RobotAbstract_H__
/*
 * Local variables:
 * mode: c++
 * indent-tabs-mode: t
 * tab-width: 2
 * c-basic-offset: 2
 * End:
 */