ForceEstimationMomentumBased

[matteoparigi]

Hi @alaurenzi, I was launching the force_estimation_node with ForceEstimationMomentumBased and I had to add the following 2 lines in ForceEstimation.cpp to prevent the node from crashing:

ForceEstimationMomentumBased::ForceEstimationMomentumBased(XBot::ModelInterface::ConstPtr model,
                                                           double rate,
                                                           double svd_threshold,
                                                           double obs_bw):
    ForceEstimation(model, svd_threshold),
    _k_obs(2.0 * M_PI * obs_bw),
    _rate(rate) // ADD 1
{
    init_momentum_obs();
}

bool ForceEstimationMomentumBased::getResiduals(Eigen::VectorXd& res) const
{
    res = _y;
    return true;
}

void ForceEstimationMomentumBased::compute_residual(Eigen::VectorXd& res)
{
    _model->getJointVelocity(_qdot);
    _model->getJointEffort(_tau);
    _model->computeGravityCompensation(_g);

    /* Observer */
    _model->getInertiaMatrix(_M);
    _p1 = _M * _qdot;

    _Mdot = (_M - _M_old) * _rate;
    _M_old = _M;
    _model->computeNonlinearTerm(_h);
    _model->computeGravityCompensation(_g);
    _coriolis = _h - _g;
    _p2 += (_tau + (_Mdot * _qdot - _coriolis) - _g + _y) / _rate;

    _y = _k_obs*(_p1 - _p2 - _p0);
      
    getResiduals(res); // ADD 2
    
}

[alaurenzi]

Thanks for the fix, can you push it on a separate branch?

[matteoparigi]

Done! I've pushed the fix in branch force_estimation_momentum

[alaurenzi]

Merged to 2.0-devel!