JointTorqueLimiter.cpp

Go to the documentation of this file.

#include "robodyn_controllers/JointTorqueLimiter.h"
/***************************************************************************/
JointTorqueLimiter::JointTorqueLimiter(const std::string& urdfFile)
{
    loadFromFile(urdfFile);
}

JointTorqueLimiter::~JointTorqueLimiter()
{
}
/***************************************************************************/
void JointTorqueLimiter::setYankLimit(const double lim)
{
    yl.setRateLimit(lim);
}
/***************************************************************************/
void JointTorqueLimiter::initializeMaps()
{
    // Initialize zeros in the right ways
    double zero = 0.0;

    // 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 set data and torque to zero
        if (it->second.segment.getJoint().getType() != KDL::Joint::None)
        {
            jointTorqueLimMap[it->second.segment.getJoint().getName()] = zero;
        }

        // Next element, please
        it++;
    }

    return;

}
/***************************************************************************/
int JointTorqueLimiter::populateTorqueMsg(sensor_msgs::JointState& tauLim)
{

    std::map<std::string, double>::const_iterator it = jointTorqueLimMap.begin();

    // Fill out header info
    tauLim.header.stamp = ros::Time::now();

    // Resize message appropriately (should not usually change)
    tauLim.name.resize(jointTorqueLimMap.size());
    tauLim.effort.resize(jointTorqueLimMap.size());

    // Fill in message
    unsigned int i = 0;

    while (it != jointTorqueLimMap.end())
    {
        tauLim.name[i]   = it->first;
        tauLim.effort[i] = it->second;
        i++;
        it++;
    }

    return 0;

}
/***************************************************************************/
int JointTorqueLimiter::storeJointTorques(const double& tau)
{
    std::map<std::string, double>::iterator it = jointTorqueLimMap.begin();

    // Iterate through joints
    while (it != jointTorqueLimMap.end())
    {
        // Set torque value for each joint
        it->second = yl.getLimitedValue(fabs(tau), it->second);
        it++;

    }

    return 0;
}
/***************************************************************************/
int JointTorqueLimiter::storeJointTorques(const sensor_msgs::JointState& desiredTauLim)
{
    // Match chain properties with jntArray properties
    unsigned int ns = desiredTauLim.name.size();
    std::map<std::string, double>::iterator it;

    // Iterate through joints
    for (unsigned int i = 0; i < ns; i++)
    {
        it = jointTorqueLimMap.find(desiredTauLim.name[i]);

        if (it != jointTorqueLimMap.end())
        {
            it->second =
                yl.getLimitedValue(fabs(desiredTauLim.effort[i]), jointTorqueLimMap[desiredTauLim.name[i]]);
        }
    }

    return 0;
}


bool JointTorqueLimiter::getCompletionMessage(r2_msgs::StringArray& completionMsg)
{
    if (yl.getCompletionCondition() == 1)
    {
        completionMsg.header.stamp = ros::Time::now();
        completionMsg.data.clear();
        completionMsg.data.push_back("torque_limits");

        return true;
    }

    return false;

}