GainCalculator.cpp

Go to the documentation of this file.

#include "robodyn_controllers/GainCalculator.h"

namespace GainCalculator
{
    /***************************************************************************/
    void CalculateGains(JointGainsData& jg)
    {
        // Find stiffness and limit
        jg.stiffness = jg.naturalFreq * jg.naturalFreq * jg.mass;

        if (jg.stiffness > jg.maxK)
        {
            jg.stiffness = jg.maxK;
        }

        // Find damping and limit
        GainCalculator::CalculateDamping(jg);
    }

    /***************************************************************************/
    void CalculateDamping(JointGainsData& jg)
    {
        // Find damping and limit
        jg.damping = 2.0 * jg.dampingRatio * sqrt( jg.stiffness * jg.mass ) ;

        if (jg.damping > jg.maxD)
        {
            jg.damping = jg.maxD;
            // recalc stiffness if necessary to limit damping
            jg.stiffness = pow((jg.damping / (2.0 * jg.dampingRatio)), 2.0) / jg.mass;
        }

        if (jg.damping < jg.minD)
        {
            jg.damping = jg.minD;

            if (jg.dampingRatio != 0. && jg.mass != 0.)
            {
                jg.stiffness = pow((jg.damping / (2.0 * jg.dampingRatio)), 2.0) / jg.mass;
            }
            else
            {
                jg.stiffness = 0.0;
            }

            if (jg.stiffness > jg.maxK)
            {
                jg.stiffness = jg.maxK;
            }
        }
    }
}