RosMsgTreeId.cpp

Go to the documentation of this file.

#include "robodyn_controllers/RosMsgTreeId.h"

RosMsgTreeId::RosMsgTreeId()
{
}

RosMsgTreeId::~RosMsgTreeId()
{
}

int RosMsgTreeId::setBaseFrame(const r2_msgs::StringArray& baseMsg, std::string& baseFrame)
{
    // always take first one, except when there are none
    unsigned int n = baseMsg.data.size();

    if (n > 0)
    {
        if (baseFrame == baseMsg.data[0])
        {
            n = -1;
        }
        else
        {
            baseFrame = baseMsg.data[0];
        }
    }

    return n;
}

void RosMsgTreeId::setJointData(const sensor_msgs::JointState& actualState,
                                const sensor_msgs::JointState& desiredStates,
                                const r2_msgs::WrenchState& wrenchState,
                                JointDynamicsData& jd)
{
    JntDynData                                   data;
    std::map<std::string, JntDynData>::iterator  jntIt;
    std::map<std::string, KDL::Wrench>::iterator wrenchIt;

    if (desiredStates.name.empty())
    {
        // Fill with zero vel and acc for pure gravity comp (no traj message)
        jntIt    = jd.jointDataMap.begin();
        data.vel = 0.0;
        data.acc = 0.0;

        while (jntIt != jd.jointDataMap.end())
        {
            jntIt->second = data;
            ++jntIt;
        }
    }
    else
    {
        // Fill with desired vel and acc for inertia comp (full traj message)
        for (unsigned int j = 0; j < desiredStates.name.size(); ++j)
        {
            jntIt = jd.jointDataMap.find(desiredStates.name[j]);

            if (jntIt != jd.jointDataMap.end())
            {
                // Fill in joint data
                data.vel = desiredStates.velocity[j];
                data.acc = desiredStates.effort[j];

                // Set to jointDataMap
                jntIt->second = data;
            }
        }
    }

    // Taking negative here so force is in appropriate sense for algorithm
    for (unsigned int i = 0; i < wrenchState.name.size(); ++i)
    {
        wrenchIt = jd.extForceMap.find(wrenchState.name[i]);

        if (wrenchIt != jd.extForceMap.end())
        {
            wrenchIt->second.force(0)  = -wrenchState.wrench[i].force.x;
            wrenchIt->second.force(1)  = -wrenchState.wrench[i].force.y;
            wrenchIt->second.force(2)  = -wrenchState.wrench[i].force.z;
            wrenchIt->second.torque(0) = -wrenchState.wrench[i].torque.x;
            wrenchIt->second.torque(1) = -wrenchState.wrench[i].torque.y;
            wrenchIt->second.torque(2) = -wrenchState.wrench[i].torque.z;
        }
    }

    // Fill with actual positions
    for (unsigned int i = 0; i < actualState.name.size(); ++i)
    {
        // Check if joint in map already
        jntIt = jd.jointDataMap.find(actualState.name[i]);

        if (jntIt != jd.jointDataMap.end())
        {
            jntIt->second.pos = actualState.position[i];
        }
    }

    return;
}

void RosMsgTreeId::getJointCommand(const JointDynamicsData& jd, sensor_msgs::JointState& tauFF)
{
    std::map<std::string, double>::const_iterator it = jd.jointTorqueCommandMap.begin();

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

    // Resize message appropriately (should usually not change)
    tauFF.name.resize(jd.jointTorqueCommandMap.size());
    tauFF.effort.resize(jd.jointTorqueCommandMap.size());

    // Fill in message
    // Take negative of torque because algorithm computes actual torque
    unsigned int i = 0;

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

    return;
}

void RosMsgTreeId::getSegmentForces(const JointDynamicsData& jd, r2_msgs::WrenchState& segForceMsg)
{
    std::map<std::string, KDL::Wrench>::const_iterator it = jd.segForceMap.begin();

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

    // Resize message appropriately (should usually not change)
    segForceMsg.name.clear();
    segForceMsg.wrench.clear();

    geometry_msgs::Wrench wData;

    while (it != jd.segForceMap.end())
    {
        segForceMsg.name.push_back(it->first);

        wData.force.x  = it->second.force(0);
        wData.force.y  = it->second.force(1);
        wData.force.z  = it->second.force(2);
        wData.torque.x = it->second.torque(0);
        wData.torque.y = it->second.torque(1);
        wData.torque.z = it->second.torque(2);
        segForceMsg.wrench.push_back(wData);

        it++;
    }

    return;
}

void RosMsgTreeId::setEmptyTorqueMsg(const JointDynamicsData& jd, sensor_msgs::JointState& tauFF)
{
    std::map<std::string, double>::const_iterator it = jd.jointTorqueCommandMap.begin();

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

    // Resize message appropriately (should usually not change)
    tauFF.name.resize(jd.jointTorqueCommandMap.size());
    tauFF.effort.resize(jd.jointTorqueCommandMap.size());

    // Fill in message
    // Take negative of torque because algorithm computes actual torque
    unsigned int i = 0;

    while (it != jd.jointTorqueCommandMap.end())
    {
        tauFF.name[i]   = it->first;
        tauFF.effort[i] = 0.0;
        i++;
        it++;
    }

    return;
}

void RosMsgTreeId::getJointInertias(const JointDynamicsData& jd, sensor_msgs::JointState& Hv)
{
    std::map<std::string, double>::const_iterator it = jd.jointInertiaMap.begin();

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

    // Resize message appropriately (should usually not change)
    Hv.name.resize(jd.jointInertiaMap.size());
    Hv.position.resize(jd.jointInertiaMap.size());

    // Fill in message
    unsigned int i = 0;

    while (it != jd.jointInertiaMap.end())
    {
        Hv.name[i]     = it->first;
        Hv.position[i] = it->second;
        i++;
        it++;
    }

    return;
}

bool RosMsgTreeId::GetCompletionMessage(RateLimiter& yl, r2_msgs::StringArray& completionMsg)
{
    if (yl.getCompletionCondition() == 1)
    {
        completionMsg.header.stamp = ros::Time::now();
        completionMsg.data.clear();
        completionMsg.data.push_back("base_frame");

        return true;
    }

    return false;

}