PoseEstimation.hpp

Go to the documentation of this file.

#ifndef SPIDAR_POSE_ESTIMATION_HPP
#define SPIDAR_POSE_ESTIMATION_HPP

#include <cstddef>
#include <cstdint>
#include <cmath>

#include "../math/Vector.hpp"
#include "../math/Matrix.hpp"
#include "../math/Cholesky.hpp"

namespace Spidar
{
namespace Core
{
template <class T>
class PoseEstimation
{
public:
    typedef T TraitsType;   

    static const size_t MOTOR_COUNT = TraitsType::MOTOR_COUNT;          

    typedef typename TraitsType::SpecType           SpecType;           

    typedef typename TraitsType::ValueType          ValueType;          
    typedef typename TraitsType::VectorType         VectorType;         
    typedef typename TraitsType::QuaternionType     QuaternionType;     

    typedef typename TraitsType::PoseType           PoseType;           
    typedef typename TraitsType::WireType           WireType;           

    typedef typename TraitsType::ValueFilterType    ValueFilterType;    
    typedef typename TraitsType::VectorFilterType   VectorFilterType;   

    PoseEstimation(void) : initialized_(false), repeatCount_(64), sigma_(ValueType(0.001)) {};
    
    ~PoseEstimation(void) {};

    ValueType   gripRadius(void) const { return gripRadius_; }

    const PoseType&     pose(void) const { return pose_; }

    const WireType&     wireVector(void) const { return wireVector_; }

    bool    initialize(const SpecType& spec, uint32_t frequency);
    void    calibrate(const int32_t(&count)[MOTOR_COUNT]);
    void    estimate(const int32_t (&count)[MOTOR_COUNT]);

private:
    void    makeWireVector(void);
    void    convertWireLength(const int32_t (&count)[MOTOR_COUNT]);

    bool        initialized_;           
    size_t      repeatCount_;           
    ValueType   sigma_;                 

    ValueType   controlFrequency_;      

    ValueType   gripRadius_;            

    PoseType    pose_;                  

    WireType    wireVector_;            

    ValueType   lengthPerCount_[MOTOR_COUNT];

    long    wireOffsetCount_[MOTOR_COUNT];

    VectorType    motorPosition_[MOTOR_COUNT];

    VectorType    gripPosition_[MOTOR_COUNT];

    ValueFilterType     deltaLengthFilter_[MOTOR_COUNT];    

    VectorFilterType    velocityFilter_;

    VectorFilterType    angularVelocityFilter_;

}; // end of class PoseEstimation.

template <class T>
bool PoseEstimation<T>::initialize(const SpecType& spec, uint32_t frequency)
{
    // デバイス情報
    const double PI = 3.14159265358979323846;

    for (size_t i=0; i<MOTOR_COUNT; ++i)
    {
        motorPosition_[i] = spec.motorPosition[i];
        gripPosition_[i] = spec.gripPosition[i];

        ValueType lengthPerCount = static_cast<ValueType>(spec.pulleyRadius[i] * 2.0 * PI / spec.encoderResolution[i]);

        lengthPerCount_[i] = lengthPerCount * spec.encoderDirection[i];
        wireOffsetCount_[i] = static_cast<long>((motorPosition_[i] - gripPosition_[i]).norm() / lengthPerCount_[i]);
    }

    gripRadius_ = spec.gripRadius;

    // ローパスフィルタ
    double timeConstatnt = 0.008;

    if (spec.majorLoopTimeConstant > 0)   timeConstatnt = spec.majorLoopTimeConstant;

    double deltaTime = 1.0 / static_cast<ValueType>(frequency);

    velocityFilter_.initialize(deltaTime, spec.majorLoopTimeConstant);
    angularVelocityFilter_.initialize(deltaTime, spec.majorLoopTimeConstant);

    for (size_t i=0; i<MOTOR_COUNT; ++i)
    {
        deltaLengthFilter_[i].initialize(deltaTime, spec.majorLoopTimeConstant);
    }
    return true;
}

template <class T>
void PoseEstimation<T>::calibrate(const int32_t (&count)[MOTOR_COUNT])
{
    pose_.clear();

    velocityFilter_.clear();
    angularVelocityFilter_.clear();
    
    for (size_t i=0; i<MOTOR_COUNT; ++i)
    {
        deltaLengthFilter_[i].clear();
    }

    makeWireVector();

    convertWireLength(count);
    
    estimate(count);
}

template <class T>
void PoseEstimation<T>::estimate(const int32_t (&count)[MOTOR_COUNT])
{
    // エンコーダ値からワイヤ長への変換
    convertWireLength(count);

    PoseType prevPose = pose_;

    Spidar::Math::Vector<ValueType, 6> v;
    Spidar::Math::Matrix<ValueType, 6, 8> a;
    Spidar::Math::Matrix<ValueType, 6, 6> ata;

    // ワイヤ長の変化量 = 計測ワイヤ長 - 算出ワイヤ長
    ValueType deltaLength[8];

    // repeatCount_回，繰り返しグリップの姿勢を計算
    for (size_t step=0; step<repeatCount_; ++step)
    {
        // 行列の初期化
        for (int i=0; i<8; ++i) 
        { 
            a[0][i] = wireVector_.direction[i].x();
            a[1][i] = wireVector_.direction[i].y();
            a[2][i] = wireVector_.direction[i].z();
            a[3][i] = wireVector_.moment[i].x();
            a[4][i] = wireVector_.moment[i].y();
            a[5][i] = wireVector_.moment[i].z();

            deltaLength[i] = wireVector_.estimate[i] - wireVector_.measurement[i] - deltaLengthFilter_[i].value();
        }

        // 対称行列の計算
        ata = a * a.trans();

        // ベクトルv（求めた姿勢の変化量 = a * deltaLength）の初期化
        for(int i=0; i<6; ++i) 
        {
            v[i] = 0;
            for(int k=0; k<8; ++k)
            {
                v[i] += a[i][k] * deltaLength[k];
            }

            // 対角成分にsigma_を加える．
            ata[i][i] += sigma_;
        }

        // コレスキー法で解く
        Math::Cholesky::solve(ata, v);

        // 出力
        VectorType deltaPosition, deltaRotation;

        deltaPosition[0] = v[0];
        deltaPosition[1] = v[1];
        deltaPosition[2] = v[2];

        deltaRotation[0] = v[3];
        deltaRotation[1] = v[4];
        deltaRotation[2] = v[5];

        // 値の有効性の確認
        if (deltaPosition.norm() > FLT_EPSILON)  
        {
            pose_.position += deltaPosition;
        }

        if (deltaRotation.norm() > FLT_EPSILON)
        {
            pose_.rotation = (pose_.rotation.derivative(deltaRotation) + pose_.rotation).unit();
        }

        // ワイヤベクトルの再計算
        makeWireVector();
    }

    // ローパスフィルタを通し，速度を[m/ms]から[m/s]へ，角速度を[rad/ms]から[rad/s]へ変換
    pose_.velocity = velocityFilter_.filter(pose_.position - prevPose.position) * controlFrequency_;
    pose_.angularVelocity = angularVelocityFilter_.filter(pose_.rotation.angularVelocity(pose_.rotation - prevPose.rotation)) * controlFrequency_;

    // ローパスフィルタ
    for (size_t i=0; i<8; ++i)
    {
        deltaLengthFilter_[i].filter(wireVector_.estimate[i] - wireVector_.measurement[i]);
    }
}

template <class T>
void PoseEstimation<T>::makeWireVector(void)
{
    for (size_t i=0; i<MOTOR_COUNT; ++i)
    {
        // 現在の姿勢でのグリップ上のワイヤの接続位置を回転させて計算
        VectorType rotateGripPos = pose_.rotation.rotate(gripPosition_[i]);

        // ワイヤベクトル（ワイヤの接続位置->モータ位置のベクトル）を計算
        VectorType wireVector = motorPosition_[i] - (rotateGripPos + pose_.position);

        // 推定したワイヤ長はワイヤベクトルのノルム
        wireVector_.estimate[i] = wireVector.norm();

        // ノルムで正規化しワイヤ方向を計算
        wireVector_.direction[i] = wireVector / wireVector_.estimate[i];

        // 回転モーメントは外積から計算
        wireVector_.moment[i] = rotateGripPos.cross(wireVector_.direction[i]);
    }
}

template <class T>
void PoseEstimation<T>::convertWireLength(const int32_t (&count)[MOTOR_COUNT])
{
    for(size_t i=0; i<MOTOR_COUNT; ++i)
    {
        wireVector_.measurement[i] = lengthPerCount_[i] * (wireOffsetCount_[i] + count[i]);
    }   
}

} // end of namespace Core.
} // end of namespace Spidar.

#endif // SPIDAR_POSE_ESTIMATION_HPP

// end of file.