RosMsgGainCalculator.cpp

Go to the documentation of this file.

#include "robodyn_controllers/RosMsgGainCalculator.h"

RosMsgGainCalculator::RosMsgGainCalculator() : initialized(false)
{
}

RosMsgGainCalculator::~RosMsgGainCalculator()
{
}

void RosMsgGainCalculator::initialize()
{
    std::string jntName;

    // Grab tree segment map and iterator
    KDL::SegmentMap segments = tree.getSegments();
    KDL::SegmentMap::const_iterator it = segments.begin();

    // Iterate through map
    while (it != segments.end())
    {
        // Find joints, and add to map
        if (it->second.segment.getJoint().getType() != KDL::Joint::None)
        {
            jntName = it->second.segment.getJoint().getName();
            jointGainsData[jntName] = JointGainsData();
        }

        // Next element, please
        ++it;
    }

    initialized = true;
}

void RosMsgGainCalculator::storeGainInformation(const r2_msgs::Gains& desiredDyn)
{
    if (desiredDyn.joint_names.size() != desiredDyn.naturalFreq.size() || desiredDyn.joint_names.size() != desiredDyn.dampingRatio.size())
    {
        throw std::runtime_error("StoreGainInformation input desired dynamics invalid");
        return;
    }

    // Store gain info in maps
    for (unsigned int i = 0; i < desiredDyn.joint_names.size(); i++)
    {
        jointGainsIt = jointGainsData.find(desiredDyn.joint_names[i]);

        if (jointGainsIt != jointGainsData.end())
        {
            if (desiredDyn.naturalFreq[i] < 0 || desiredDyn.dampingRatio[i] <= 0 || desiredDyn.windupLimit[i] < 0)
            {
                throw std::runtime_error("StoreGainInformation input desired dynamics invalid");
                return;
            }

            if (desiredDyn.naturalFreq[i]  > jointGainsIt->second.maxWn)
            {
                jointGainsIt->second.naturalFreq  = jointGainsIt->second.maxWn;
            }
            else
            {
                jointGainsIt->second.naturalFreq  = desiredDyn.naturalFreq[i];
            }

            if (desiredDyn.dampingRatio[i]  > jointGainsIt->second.maxZeta)
            {
                jointGainsIt->second.dampingRatio  = jointGainsIt->second.maxZeta;
            }
            else
            {
                jointGainsIt->second.dampingRatio  = desiredDyn.dampingRatio[i];
            }

            jointGainsIt->second.windupLim    = desiredDyn.windupLimit[i];
        }
    }
}

void RosMsgGainCalculator::setDefaultGains(const double& wn, const double& zeta, const double& windup)
{
    // Store gain info in maps
    for (jointGainsIt = jointGainsData.begin(); jointGainsIt != jointGainsData.end(); ++jointGainsIt)
    {
        jointGainsIt->second.naturalFreq  = wn;

        if (jointGainsIt->second.naturalFreq  > jointGainsIt->second.maxWn)
        {
            jointGainsIt->second.naturalFreq  = jointGainsIt->second.maxWn;
        }

        jointGainsIt->second.dampingRatio = zeta;

        if (jointGainsIt->second.dampingRatio  > jointGainsIt->second.maxZeta)
        {
            jointGainsIt->second.dampingRatio  = jointGainsIt->second.maxZeta;
        }

        jointGainsIt->second.windupLim    = windup;
    }
}

void RosMsgGainCalculator::storeGainProperties()
{
    double temp;

    for (jointGainsIt = jointGainsData.begin(); jointGainsIt != jointGainsData.end(); ++jointGainsIt)
    {
        if (!(getCommandFile(jointGainsIt->first).empty()) && JointCommandFactory::Private::getJointTypeFromParameterFile(getCommandFile(jointGainsIt->first)) == "SeriesElastic")
        {
            if ((temp = getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( jointGainsIt->first, "MaxStiffness"))) >= 0)
            {
                jointGainsIt->second.maxK = temp;
            }
            else
            {
                throw std::runtime_error("Negative max stiffness found!");
            }

            if ((temp = getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( jointGainsIt->first, "MaxDamping"))) >= 0)
            {
                jointGainsIt->second.maxD = temp;
            }
            else
            {
                throw std::runtime_error("Negative max damping found!");
            }

            if ((temp = getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( jointGainsIt->first, "MinDamping"))) >= 0)
            {
                jointGainsIt->second.minD = temp;
            }
            else
            {
                throw std::runtime_error("Negative min damping found!");
            }

            if ((temp = getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( jointGainsIt->first, "MaxNaturalFreq"))) >= 0)
            {
                jointGainsIt->second.maxWn = temp;
            }
            else
            {
                throw std::runtime_error("Negative max natural frequency found!");
            }

            if ((temp = getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( jointGainsIt->first, "MaxDampingRatio"))) >= 0)
            {
                jointGainsIt->second.maxZeta = temp;
            }
            else
            {
                throw std::runtime_error("Negative max damping ratio found!");
            }

            jointGainsIt->second.scaleI = getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( jointGainsIt->first, "IntegralScalar"));
        }
        else
        {
            jointGainsIt->second.maxK    = 0.0;
            jointGainsIt->second.maxD    = 0.0;
            jointGainsIt->second.minD    = 0.0;
            jointGainsIt->second.maxWn   = 0.0;
            jointGainsIt->second.maxZeta = 0.0;
            jointGainsIt->second.scaleI  = 0.0;

        }
    }
}

void RosMsgGainCalculator::findDesiredDynamics(const sensor_msgs::JointState& inertia,
        r2_msgs::Gains& desiredGains)
{
    // Return if des dyn maps not yet initialized
    if (!initialized)
    {
        throw std::runtime_error("findDesiredDynamics input not initialized");
        return;
    }

    // Return if inertia is not well formed
    if (inertia.name.size() != inertia.position.size())
    {
        throw std::runtime_error("findDesiredDynamics inertia message malformed");
        return;
    }

    // init message
    desiredGains.joint_names.resize(inertia.name.size());
    desiredGains.naturalFreq.resize(inertia.name.size());
    desiredGains.dampingRatio.resize(inertia.name.size());
    desiredGains.K.resize(inertia.name.size());
    desiredGains.D.resize(inertia.name.size());
    desiredGains.I.resize(inertia.name.size());
    desiredGains.windupLimit.resize(inertia.name.size());

    double temp;

    for (unsigned int i = 0; i < inertia.name.size(); ++i)
    {
        jointGainsIt = jointGainsData.find(inertia.name[i]);

        if (jointGainsIt == jointGainsData.end())
        {
            throw std::runtime_error("findDesiredDynamics inertia message includes unknown joints");
            return;
        }

        if (inertia.position[i] < 0)
        {
            throw std::runtime_error("findDesiredDynamics negative inertia value found");
            return;
        }

        jointGainsIt->second.mass = inertia.position[i];
        temp                      = jointGainsIt->second.stiffness;
        GainCalculator::CalculateGains(jointGainsIt->second);

        // Check yank limits
        temp = yl.getLimitedValue(jointGainsIt->second.stiffness, temp);

        if (temp != jointGainsIt->second.stiffness)
        {
            jointGainsIt->second.stiffness = temp;
            GainCalculator::CalculateDamping(jointGainsIt->second);
        }

        jointGainsIt->second.prevWindup = ylWindup.getLimitedValue(jointGainsIt->second.windupLim, jointGainsIt->second.prevWindup);

        // put in message
        desiredGains.joint_names[i]  = inertia.name[i];
        desiredGains.naturalFreq[i]  = jointGainsIt->second.naturalFreq;
        desiredGains.dampingRatio[i] = jointGainsIt->second.dampingRatio;
        desiredGains.K[i]            = jointGainsIt->second.stiffness;
        desiredGains.D[i]            = jointGainsIt->second.damping;
        desiredGains.I[i]            = jointGainsIt->second.scaleI * jointGainsIt->second.stiffness;
        desiredGains.windupLimit[i]  = jointGainsIt->second.prevWindup;


    }

    return;
}

bool RosMsgGainCalculator::getCompletionMessage(r2_msgs::StringArray& completionMsg)
{

    if (yl.getCompletionCondition() == 1)
    {
        completionMsg.header.stamp = ros::Time::now();
        completionMsg.data.clear();
        completionMsg.data.push_back("gains");

        return true;
    }

    return false;
}