Odometry.hpp

Go to the documentation of this file.

#ifndef __ODOMETRY_ODOMETRY_HPP__
#define __ODOMETRY_ODOMETRY_HPP__

#include <base/Time.hpp>
#include <base/Pose.hpp>

#include <odometry/Gaussian.hpp>
#include <odometry/Configuration.hpp>
#include <odometry/BodyState.hpp>
#include <odometry/State.hpp>
#include <odometry/Gaussian3D.hpp>
#include <odometry/Sampling3D.hpp>
#include <odometry/Sampling2D.hpp>
#include <base/samples/Joints.hpp>

namespace odometry
{
    class SkidOdometry
        : public Gaussian3D,
          public Sampling3D,
          public Sampling2D
    {
    public:
        typedef base::Matrix6d Matrix6d;
        typedef base::Vector6d Vector6d;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        SkidOdometry(const Configuration& config, double wheelRadiusAvg, double trackWidth, double wheelBase,
                     const std::vector<std::string> &leftWheelNames, const std::vector<std::string> &rightWheelNames);
        
        SkidOdometry(const Configuration& config, double wheelRadiusAvg, double trackWidth, double wheelBase,
                     const std::vector<std::string> &leftWheelNames, const std::vector<std::string> &rightWheelNames,
                     const std::vector<std::string> &leftSteeringNames, const std::vector<std::string> &rightSteeringNames);

        void update( Eigen::Vector2d diff, const Eigen::Quaterniond& orientation );
        
        void update( double d, const Eigen::Quaterniond& orientation );

        void update(const base::samples::Joints &js, const Eigen::Quaterniond& orientation);
        
        void update(const base::samples::Joints &jsw,const base::samples::Joints &jss, const Eigen::Quaterniond& orientation);

        base::Pose getPoseDelta();
        Eigen::Matrix3d getPositionError();
        Eigen::Matrix3d getOrientationError();

        Matrix6d getPoseError();

    public:
        base::Pose getPoseDeltaSample();

        base::Pose2D getPoseDeltaSample2D();

    protected:
        Configuration config;

        double wheelRadiusAvg;
        double trackWidth;
        double wheelBase;

        GaussianSamplingPose3D sampling;

        Eigen::Quaterniond orientation, prevOrientation;
        base::Pose pose;
        
        State<base::samples::Joints> jointState;
        State<base::samples::Joints> steeringJointState;
            
        std::vector<std::string> leftWheelNames;
        std::vector<std::string> rightWheelNames;
        std::vector<std::string> leftSteeringNames;
        std::vector<std::string> rightSteeringNames;

        double getTranslation(const std::vector< std::string >& actuatorNames);
        
        double getRotation(const std::vector< std::string >& actuatorNames);
    };

    class Skid4Odometry
        : public SkidOdometry 
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        Skid4Odometry(const Configuration& config, double wheelRadiusAvg, double trackWidth, double wheelBase);
        void update(const BodyState &bs, const Eigen::Quaterniond& orientation);

        Eigen::Matrix3d getPositionError();
        Eigen::Matrix3d getOrientationError();

        Matrix6d getPoseError();

        // DEPRECATED, PLEASE REMOVE.
        // only use the interface defined in the base class.
        // if interface not sufficient, discuss.
        Eigen::Vector3d getTranslation();
        Eigen::Vector3d getVelocity();
        Eigen::Vector3d getAngularVelocity();
        Eigen::Matrix3d getVelocityError();
        double getDeltaYaw();

    public:
        State<BodyState> state;
    };

}

#endif