RosMsgConverter.cpp

Go to the documentation of this file.

#include <robodyn_utilities/RosMsgConverter.h>

#include "nasa_common_logging/Logger.h"
#include <tf_conversions/tf_kdl.h>
#include <boost/tuple/tuple.hpp>

namespace RosMsgConverter
{
    void JointStateToJntArray(const sensor_msgs::JointState& jointState,
                              const std::vector<std::string> jointNames,
                              KDL::JntArray& jointArray)
    {
        if (jointArray.rows() != jointNames.size())
        {
            jointArray.resize(jointNames.size());
        }

        if (jointState.name.size() != jointState.position.size())
        {
            // length mismatch
            std::stringstream err;
            err << "JointStateToJntArray() - jointState name length doesn't match positions length: " << jointState.name.size() << " vs " << jointState.position.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArray", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        // if M = jointState.name.size() & N = jointNames.size(),
        // a simple repetitive search would be worst case O(M*N);
        // let's sort instead to get worst case O((M+N)*log(M))
        std::map<std::string, double> jsMap;

        for (unsigned int i = 0; i < jointState.name.size(); ++i)
        {
            jsMap[jointState.name[i]] = jointState.position[i];
        }

        std::string jointName;

        for (unsigned int nameIndex = 0; nameIndex < jointNames.size(); ++nameIndex)
        {
            jointName = jointNames[nameIndex];

            std::map<std::string, double>::iterator jsMapIt = jsMap.find(jointName);

            if (jsMapIt == jsMap.end())
            {
                // not found
                std::stringstream err;
                err << "JointStateToJntArray() - jointState doesn't contain joint: " << jointName;
                NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArray", log4cpp::Priority::ERROR, err.str());
                throw std::logic_error(err.str());
                return;
            }
            else
            {
                jointArray(nameIndex) = jsMapIt->second;
            }
        }
    }

    void JointStateToJntArrayVel(const sensor_msgs::JointState& jointState,
                                 const std::vector<std::string> jointNames,
                                 KDL::JntArrayVel& jointArray)
    {
        if (jointArray.q.rows() != jointNames.size())
        {
            jointArray.resize(jointNames.size());
        }

        if (jointState.name.size() != jointState.position.size())
        {
            // length mismatch
            std::stringstream err;
            err << "JointStateToJntArray() - jointState name length doesn't match positions length: " << jointState.name.size() << " vs " << jointState.position.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArray", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        bool noVels = false;

        if (jointState.name.size() != jointState.velocity.size())
        {
            if (jointState.velocity.size() == 0)
            {
                noVels = true;
            }
            else
            {
                // length mismatch
                std::stringstream err;
                err << "JointStateToJntArray() - jointState name length doesn't match velocity length: " << jointState.name.size() << " vs " << jointState.velocity.size();
                NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArray", log4cpp::Priority::ERROR, err.str());
                throw std::logic_error(err.str());
                return;
            }
        }

        // if M = jointState.name.size() & N = jointNames.size(),
        // a simple repetitive search would be worst case O(M*N);
        // let's sort instead to get worst case O((M+N)*log(M))
        std::map<std::string, std::pair<double, double> > jsMap;

        for (unsigned int i = 0; i < jointState.name.size(); ++i)
        {
            if (noVels)
            {
                jsMap[jointState.name[i]] = std::make_pair(jointState.position[i], 0.);
            }
            else
            {
                jsMap[jointState.name[i]] = std::make_pair(jointState.position[i], jointState.velocity[i]);
            }
        }

        std::string jointName;

        for (unsigned int nameIndex = 0; nameIndex < jointNames.size(); ++nameIndex)
        {
            jointName = jointNames[nameIndex];

            std::map<std::string, std::pair<double, double> >::iterator jsMapIt = jsMap.find(jointName);

            if (jsMapIt == jsMap.end())
            {
                // not found
                std::stringstream err;
                err << "JointStateToJntArrayVel() - jointState doesn't contain joint: " << jointName;
                NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArrayVel", log4cpp::Priority::ERROR, err.str());
                throw std::logic_error(err.str());
                return;
            }
            else
            {
                jointArray.q(nameIndex)    = jsMapIt->second.first;
                jointArray.qdot(nameIndex) = jsMapIt->second.second;
            }
        }
    }

    void JointStateToJntArrayAcc(const sensor_msgs::JointState& jointState,
                                 const std::vector<std::string> jointNames,
                                 KDL::JntArrayAcc& jointArray)
    {
        if (jointArray.q.rows() != jointNames.size())
        {
            jointArray.resize(jointNames.size());
        }

        if (jointState.name.size() != jointState.position.size())
        {
            // length mismatch
            std::stringstream err;
            err << "JointStateToJntArray() - jointState name length doesn't match positions length: " << jointState.name.size() << " vs " << jointState.position.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArray", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointState.name.size() != jointState.velocity.size())
        {
            // length mismatch
            std::stringstream err;
            err << "JointStateToJntArray() - jointState name length doesn't match velocities length: " << jointState.name.size() << " vs " << jointState.velocity.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArray", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointState.name.size() != jointState.effort.size())
        {
            // length mismatch
            std::stringstream err;
            err << "JointStateToJntArray() - jointState name length doesn't match positions length: " << jointState.name.size() << " vs " << jointState.effort.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArray", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        // if M = jointState.name.size() & N = jointNames.size(),
        // a simple repetitive search would be worst case O(M*N);
        // let's sort instead to get worst case O((M+N)*log(M))
        std::map<std::string, boost::tuples::tuple<double, double, double> > jsMap;

        for (unsigned int i = 0; i < jointState.name.size(); ++i)
        {
            jsMap[jointState.name[i]] = boost::tuples::make_tuple(jointState.position[i], jointState.velocity[i], jointState.effort[i]);
        }

        for (unsigned int nameIndex = 0; nameIndex < jointNames.size(); ++nameIndex)
        {
            std::map<std::string, boost::tuples::tuple<double, double, double> >::iterator jsMapIt = jsMap.find(jointNames[nameIndex]);

            if (jsMapIt == jsMap.end())
            {
                // not found
                std::stringstream err;
                err << "JointStateToJntArrayVel() - jointState doesn't contain joint: " << jointNames[nameIndex];
                NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntArrayVel", log4cpp::Priority::ERROR, err.str());
                throw std::logic_error(err.str());
                return;
            }
            else
            {
                jointArray.q(nameIndex)       = jsMapIt->second.get<0>();
                jointArray.qdot(nameIndex)    = jsMapIt->second.get<1>();
                jointArray.qdotdot(nameIndex) = jsMapIt->second.get<2>();
            }
        }
    }

    void PoseStateToFrame(const r2_msgs::PoseState& poseState, const std::string& baseName,
                          const std::string& tipName, KDL::Frame& frame)
    {
        if (poseState.name.size() != poseState.positions.size())
        {
            // length mismatch
            std::stringstream err;
            err << "PoseStateToFrame() - poseState name length doesn't match pose length";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.PoseStateToFrame", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        // find the needed poses
        bool       foundBase = false;
        bool       foundTip  = false;
        KDL::Frame baseFrame;
        KDL::Frame tipFrame;

        // check poseState frame
        if (poseState.header.frame_id == baseName)
        {
            foundBase = true;
        }

        if (poseState.header.frame_id == tipName)
        {
            foundTip = true;
        }

        // check poses
        for (unsigned int i = 0; i < poseState.name.size(); ++i)
        {
            if (!foundBase && poseState.name[i] == baseName)
            {
                foundBase = true;
                tf::poseMsgToKDL(poseState.positions.at(i), baseFrame);

                if (foundTip)
                {
                    break;
                }
            }

            if (!foundTip && poseState.name[i] == tipName)
            {
                foundTip = true;
                tf::poseMsgToKDL(poseState.positions.at(i), tipFrame);

                if (foundBase)
                {
                    break;
                }
            }
        }

        if (!foundBase || !foundTip)
        {
            // not found
            std::stringstream err;
            err << "PoseStateToFrame() - poseState doesn't contain the necessary transform information for " << baseName << " to " << tipName;
            NasaCommonLogging::Logger::log("gov.nasa.controllers.PoseStateToFrame", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        // transform
        frame = baseFrame.Inverse() * tipFrame;
    }

    void PoseStateToFrameVel(const r2_msgs::PoseState& poseState, const std::string& baseName,
                             const std::string& tipName, KDL::FrameVel& frame)
    {
        if (poseState.name.size() != poseState.positions.size() || poseState.name.size() != poseState.velocities.size())
        {
            // length mismatch
            std::stringstream err;
            err << "PoseStateToFrameVel() - poseState name and value lengths don't match";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.PoseStateToFrameVel", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        // find the needed poses
        bool          foundBase = false;
        bool          foundTip  = false;
        KDL::FrameVel baseFrame;
        KDL::FrameVel tipFrame;

        // check poseState frame
        if (poseState.header.frame_id == baseName)
        {
            foundBase = true;
        }

        if (poseState.header.frame_id == tipName)
        {
            foundTip = true;
        }

        // check poses
        for (unsigned int i = 0; i < poseState.name.size(); ++i)
        {
            if (!foundBase && poseState.name[i] == baseName)
            {
                foundBase = true;

                // position
                baseFrame.p.p.x(poseState.positions[i].position.x);
                baseFrame.p.p.y(poseState.positions[i].position.y);
                baseFrame.p.p.z(poseState.positions[i].position.z);
                baseFrame.M.R = KDL::Rotation::Quaternion(poseState.positions[i].orientation.x, poseState.positions[i].orientation.y,
                                poseState.positions[i].orientation.z, poseState.positions[i].orientation.w);

                //velocity
                baseFrame.p.v.x(poseState.velocities[i].linear.x);
                baseFrame.p.v.y(poseState.velocities[i].linear.y);
                baseFrame.p.v.z(poseState.velocities[i].linear.z);
                baseFrame.M.w.x(poseState.velocities[i].angular.x);
                baseFrame.M.w.y(poseState.velocities[i].angular.y);
                baseFrame.M.w.z(poseState.velocities[i].angular.z);

                if (foundTip)
                {
                    break;
                }
            }

            if (!foundTip && poseState.name[i] == tipName)
            {
                foundTip = true;

                // position
                tipFrame.p.p.x(poseState.positions[i].position.x);
                tipFrame.p.p.y(poseState.positions[i].position.y);
                tipFrame.p.p.z(poseState.positions[i].position.z);
                tipFrame.M.R = KDL::Rotation::Quaternion(poseState.positions[i].orientation.x, poseState.positions[i].orientation.y,
                               poseState.positions[i].orientation.z, poseState.positions[i].orientation.w);

                //velocity
                tipFrame.p.v.x(poseState.velocities[i].linear.x);
                tipFrame.p.v.y(poseState.velocities[i].linear.y);
                tipFrame.p.v.z(poseState.velocities[i].linear.z);
                tipFrame.M.w.x(poseState.velocities[i].angular.x);
                tipFrame.M.w.y(poseState.velocities[i].angular.y);
                tipFrame.M.w.z(poseState.velocities[i].angular.z);

                if (foundBase)
                {
                    break;
                }
            }
        }

        if (!foundBase || !foundTip)
        {
            // not found
            std::stringstream err;
            err << "PoseStateToFrameVel() - poseState doesn't contain the necessary transform information for " << baseName << " to " << tipName;
            NasaCommonLogging::Logger::log("gov.nasa.controllers.PoseStateToFrame", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        // transform
        frame = baseFrame.Inverse() * tipFrame;
    }

    void PoseStateToFrameAcc(const r2_msgs::PoseState& poseState, const std::string& baseName,
                             const std::string& tipName, KDL::FrameAcc& frame)
    {
        if (poseState.name.size() != poseState.positions.size() || poseState.name.size() != poseState.velocities.size()
                || poseState.name.size() != poseState.accelerations.size())
        {
            // length mismatch
            std::stringstream err;
            err << "PoseStateToFrameAcc() - poseState name and value lengths don't match";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.PoseStateToFrameAcc", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        // find the needed poses.at(i)
        bool          foundBase = false;
        bool          foundTip  = false;
        KDL::FrameAcc baseFrame;
        KDL::FrameAcc tipFrame;

        // check poseState frame
        if (poseState.header.frame_id == baseName)
        {
            foundBase = true;
        }

        if (poseState.header.frame_id == tipName)
        {
            foundTip = true;
        }

        // check poses
        for (unsigned int i = 0; i < poseState.name.size(); ++i)
        {
            if (!foundBase && poseState.name[i] == baseName)
            {
                foundBase = true;

                // position
                baseFrame.p.p.x(poseState.positions[i].position.x);
                baseFrame.p.p.y(poseState.positions[i].position.y);
                baseFrame.p.p.z(poseState.positions[i].position.z);
                baseFrame.M.R = KDL::Rotation::Quaternion(poseState.positions[i].orientation.x, poseState.positions[i].orientation.y,
                                poseState.positions[i].orientation.z, poseState.positions[i].orientation.w);

                //velocity
                baseFrame.p.v.x(poseState.velocities[i].linear.x);
                baseFrame.p.v.y(poseState.velocities[i].linear.y);
                baseFrame.p.v.z(poseState.velocities[i].linear.z);
                baseFrame.M.w.x(poseState.velocities[i].angular.x);
                baseFrame.M.w.y(poseState.velocities[i].angular.y);
                baseFrame.M.w.z(poseState.velocities[i].angular.z);

                //acceleration
                baseFrame.p.dv.x(poseState.accelerations[i].linear.x);
                baseFrame.p.dv.y(poseState.accelerations[i].linear.y);
                baseFrame.p.dv.z(poseState.accelerations[i].linear.z);
                baseFrame.M.dw.x(poseState.accelerations[i].angular.x);
                baseFrame.M.dw.y(poseState.accelerations[i].angular.y);
                baseFrame.M.dw.z(poseState.accelerations[i].angular.z);

                if (foundTip)
                {
                    break;
                }
            }

            if (!foundTip && poseState.name[i] == tipName)
            {
                foundTip = true;

                // position
                tipFrame.p.p.x(poseState.positions[i].position.x);
                tipFrame.p.p.y(poseState.positions[i].position.y);
                tipFrame.p.p.z(poseState.positions[i].position.z);
                tipFrame.M.R = KDL::Rotation::Quaternion(poseState.positions[i].orientation.x, poseState.positions[i].orientation.y,
                               poseState.positions[i].orientation.z, poseState.positions[i].orientation.w);

                //velocity
                tipFrame.p.v.x(poseState.velocities[i].linear.x);
                tipFrame.p.v.y(poseState.velocities[i].linear.y);
                tipFrame.p.v.z(poseState.velocities[i].linear.z);
                tipFrame.M.w.x(poseState.velocities[i].angular.x);
                tipFrame.M.w.y(poseState.velocities[i].angular.y);
                tipFrame.M.w.z(poseState.velocities[i].angular.z);

                //acceleration
                tipFrame.p.dv.x(poseState.accelerations[i].linear.x);
                tipFrame.p.dv.y(poseState.accelerations[i].linear.y);
                tipFrame.p.dv.z(poseState.accelerations[i].linear.z);
                tipFrame.M.dw.x(poseState.accelerations[i].angular.x);
                tipFrame.M.dw.y(poseState.accelerations[i].angular.y);
                tipFrame.M.dw.z(poseState.accelerations[i].angular.z);

                if (foundBase)
                {
                    break;
                }
            }
        }

        if (!foundBase || !foundTip)
        {
            // not found
            std::stringstream err;
            err << "PoseStateToFrameAcc() - poseState doesn't contain the necessary transform information for " << baseName << " to " << tipName;
            NasaCommonLogging::Logger::log("gov.nasa.controllers.PoseStateToFrame", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        // transform
        frame = baseFrame.Inverse() * tipFrame;
    }

    void PoseStateToFrameMap(const r2_msgs::PoseState& poseState, const std::string& base, std::map<std::string, KDL::Frame>& frames)
    {
        KDL::Frame  frame;
        std::string tipName;

        if (frames.size() != poseState.name.size())
        {
            NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.PoseStateToFrameMap") << log4cpp::Priority::DEBUG << "resizing frames";
            frames.clear();

            for (unsigned int i = 0; i < poseState.name.size(); ++i)
            {
                frames.insert(std::make_pair(poseState.name[i], KDL::Frame()));
            }
        }

        for (unsigned int i = 0; i < poseState.name.size(); ++i)
        {
            tipName = poseState.name[i];

            try
            {
                PoseStateToFrame(poseState, base, tipName, frame );
                frames[tipName] = frame;
            }
            catch (std::exception& e)
            {
                NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.PoseStateToFrameMap") << log4cpp::Priority::WARN << "Squelched error " << e.what() << ".. continuing..";
            }
        }
    }

    void PoseStateToFrameVelMap(const r2_msgs::PoseState& poseState, const std::string& base, std::map<std::string, KDL::FrameVel>& framevels)
    {
        KDL::FrameVel frameVel;
        std::string   tipName;

        if (framevels.size() != poseState.name.size())
        {
            NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.PoseStateToFrameVelMap") << log4cpp::Priority::DEBUG << "resizing framevels";
            framevels.clear();

            for (unsigned int i = 0; i < poseState.name.size(); ++i)
            {
                framevels.insert(std::make_pair(poseState.name[i], KDL::FrameVel()));
            }
        }

        for (unsigned int i = 0; i < poseState.name.size(); ++i)
        {
            tipName = poseState.name[i];

            try
            {
                PoseStateToFrameVel(poseState, base, tipName, frameVel );
                framevels[tipName] = frameVel;
            }
            catch (std::exception& e)
            {
                NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.PoseStateToFrameVelMap") << log4cpp::Priority::WARN << "Squelched error " << e.what() << ".. continuing..";
            }
        }
    }

    void PoseStateToFrameAccMap(const r2_msgs::PoseState& poseState, const std::string& base, std::map<std::string, KDL::FrameAcc>& frameaccs)
    {
        KDL::FrameAcc frameAcc;
        std::string   tipName;

        if (frameaccs.size() != poseState.name.size())
        {
            NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.PoseStateToFrameAccMap") << log4cpp::Priority::DEBUG << "resizing frameaccs";
            frameaccs.clear();

            for (unsigned int i = 0; i < poseState.name.size(); ++i)
            {
                frameaccs.insert(std::make_pair(poseState.name[i], KDL::FrameAcc()));
            }
        }

        for (unsigned int i = 0; i < poseState.name.size(); ++i)
        {
            tipName = poseState.name[i];

            try
            {
                RosMsgConverter::PoseStateToFrameAcc(poseState, base, tipName, frameAcc );
                frameaccs[tipName] = frameAcc;
            }
            catch (std::exception& e)
            {
                NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.PoseStateToFrameAccMap") << log4cpp::Priority::WARN << "Squelched error " << e.what() << ".. continuing..";
            }
        }
    }

    void JointStateToJntMap(const sensor_msgs::JointState& jointState, std::map<std::string, double>& jointMap)
    {
        if (jointState.name.size() != jointState.position.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointState name length doesn't match positions length: " << jointState.name.size() << " vs " << jointState.position.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJntMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointMap.empty())
        {
            for (unsigned int i = 0; i < jointState.name.size(); ++i)
            {
                jointMap[jointState.name[i]] = jointState.position[i];
            }
        }
        else
        {
            std::map<std::string, double>::iterator jmIt;

            for (unsigned int i = 0; i < jointState.name.size(); ++i)
            {
                jmIt = jointMap.find(jointState.name[i]);

                if (jmIt != jointMap.end())
                {
                    jmIt->second = jointState.position[i];
                }
            }
        }
    }

    void JointStateToJointVelMap(const sensor_msgs::JointState& jointState, std::map<std::string, JointVel>& jointMap)
    {
        if (jointState.name.size() != jointState.velocity.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointState name length doesn't match velocity length: " << jointState.name.size() << " vs " << jointState.velocity.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJointVelMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointState.name.size() != jointState.position.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointState name length doesn't match positions length: " << jointState.name.size() << " vs " << jointState.position.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJointVelMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointMap.empty())
        {
            for (unsigned int i = 0; i < jointState.name.size(); ++i)
            {
                jointMap[jointState.name[i]] = (JointVel)
                {
                    jointState.position[i], jointState.velocity[i]
                };
            }
        }
        else
        {
            std::map<std::string, JointVel>::iterator jmIt;

            for (unsigned int i = 0; i < jointState.name.size(); ++i)
            {
                jmIt = jointMap.find(jointState.name[i]);

                if (jmIt != jointMap.end())
                {
                    jmIt->second = (JointVel)
                    {
                        jointState.position[i], jointState.velocity[i]
                    };
                }
            }
        }
    }

    void JointStateToJointAccMap(const sensor_msgs::JointState& jointState, std::map<std::string, JointAcc>& jointMap)
    {
        if (jointState.name.size() != jointState.effort.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointState name length doesn't match effort length: " << jointState.name.size() << " vs " << jointState.effort.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJointAccMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointState.name.size() != jointState.velocity.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointState name length doesn't match velocity length: " << jointState.name.size() << " vs " << jointState.velocity.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJointAccMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointState.name.size() != jointState.position.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointState name length doesn't match positions length: " << jointState.name.size() << " vs " << jointState.position.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointStateToJointAccMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointMap.empty())
        {
            for (unsigned int i = 0; i < jointState.name.size(); ++i)
            {
                jointMap[jointState.name[i]] = (JointAcc)
                {
                    jointState.position[i], jointState.velocity[i], jointState.effort[i]
                };
            }
        }
        else
        {
            std::map<std::string, JointAcc>::iterator jmIt;

            for (unsigned int i = 0; i < jointState.name.size(); ++i)
            {
                jmIt = jointMap.find(jointState.name[i]);

                if (jmIt != jointMap.end())
                {
                    jmIt->second = (JointAcc)
                    {
                        jointState.position[i], jointState.velocity[i], jointState.effort[i]
                    };
                }
            }
        }
    }

    void JointControlDataToCoeffStateMap(const r2_msgs::JointControlDataArray& jointData,
                                         std::map<std::string, r2_msgs::JointControlCoeffState>& jointMap)
    {
        if (jointData.joint.size() != jointData.data.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointData name length doesn't match data length: " << jointData.joint.size() << " vs " << jointData.data.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointControlDataToCoeffLoadedMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointMap.empty())
        {
            for (unsigned int i = 0; i < jointData.joint.size(); ++i)
            {
                jointMap[jointData.joint[i]] = jointData.data[i].coeffState;
            }
        }
        else
        {
            std::map<std::string, r2_msgs::JointControlCoeffState>::iterator jmIt;

            for (unsigned int i = 0; i < jointData.joint.size(); ++i)
            {
                jmIt = jointMap.find(jointData.joint[i]);

                if (jmIt != jointMap.end())
                {
                    jmIt->second = jointData.data[i].coeffState;
                }
            }
        }
    }

    KDL::Wrench wrenchMsgToWrench(const geometry_msgs::Wrench& wrenchMsg)
    {
        return (KDL::Wrench(KDL::Vector(wrenchMsg.force.x, wrenchMsg.force.y, wrenchMsg.force.z),
                            KDL::Vector(wrenchMsg.torque.x, wrenchMsg.torque.y, wrenchMsg.torque.z)));
    }

    geometry_msgs::Wrench wrenchToWrenchMsg(const KDL::Wrench& wrench)
    {
        geometry_msgs::Wrench wrenchOut;
        wrenchOut.force.x  = wrench.force.x();
        wrenchOut.force.y  = wrench.force.y();
        wrenchOut.force.z  = wrench.force.z();
        wrenchOut.torque.x = wrench.torque.x();
        wrenchOut.torque.y = wrench.torque.y();
        wrenchOut.torque.z = wrench.torque.z();
        return wrenchOut;
    }

    void JointCommandToJointCommandMap(const r2_msgs::JointCommand& jointCommand, std::map<std::string, JointCommand>& jointCommandMap)
    {
        if (jointCommand.name.size() != jointCommand.desiredPosition.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match desiredPosition length: " << jointCommand.name.size() << " vs " << jointCommand.desiredPosition.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommand.name.size() != jointCommand.desiredPositionVelocityLimit.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match desiredPositionVelocityLimit length: " << jointCommand.name.size() << " vs " << jointCommand.desiredPositionVelocityLimit.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommand.name.size() != jointCommand.feedForwardTorque.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match feedForwardTorque length: " << jointCommand.name.size() << " vs " << jointCommand.feedForwardTorque.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommand.name.size() != jointCommand.proportionalGain.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match proportionalGain length: " << jointCommand.name.size() << " vs " << jointCommand.proportionalGain.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommand.name.size() != jointCommand.derivativeGain.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match derivativeGain length: " << jointCommand.name.size() << " vs " << jointCommand.derivativeGain.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommand.name.size() != jointCommand.integralGain.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match integralGain length: " << jointCommand.name.size() << " vs " << jointCommand.integralGain.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommand.name.size() != jointCommand.positionLoopTorqueLimit.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match positionLoopTorqueLimit length: " << jointCommand.name.size() << " vs " << jointCommand.positionLoopTorqueLimit.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommand.name.size() != jointCommand.positionLoopWindupLimit.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match positionLoopWindupLimit length: " << jointCommand.name.size() << " vs " << jointCommand.positionLoopWindupLimit.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommand.name.size() != jointCommand.torqueLoopVelocityLimit.size())
        {
            // length mismatch
            std::stringstream err;
            err << "jointCommand name length doesn't match torqueLoopVelocityLimit length: " << jointCommand.name.size() << " vs " << jointCommand.torqueLoopVelocityLimit.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        if (jointCommandMap.empty())
        {
            for (unsigned int i = 0; i < jointCommand.name.size(); ++i)
            {
                jointCommandMap[jointCommand.name[i]] = (JointCommand)
                {
                    jointCommand.desiredPosition[i],
                                                 jointCommand.desiredPositionVelocityLimit[i],
                                                 jointCommand.feedForwardTorque[i],
                                                 jointCommand.proportionalGain[i],
                                                 jointCommand.derivativeGain[i],
                                                 jointCommand.integralGain[i],
                                                 jointCommand.positionLoopTorqueLimit[i],
                                                 jointCommand.positionLoopWindupLimit[i],
                                                 jointCommand.torqueLoopVelocityLimit[i]
                };
            }
        }
        else
        {
            for (unsigned int i = 0; i < jointCommand.name.size(); ++i)
            {
                try
                {
                    jointCommandMap.at(jointCommand.name[i]) = (JointCommand)
                    {
                        jointCommand.desiredPosition[i],
                                                     jointCommand.desiredPositionVelocityLimit[i],
                                                     jointCommand.feedForwardTorque[i],
                                                     jointCommand.proportionalGain[i],
                                                     jointCommand.derivativeGain[i],
                                                     jointCommand.integralGain[i],
                                                     jointCommand.positionLoopTorqueLimit[i],
                                                     jointCommand.positionLoopWindupLimit[i],
                                                     jointCommand.torqueLoopVelocityLimit[i]
                    };
                }
                catch (std::exception& e)
                {
                    // not found
                    std::stringstream err;
                    err << "jointCommandMap doesn't contain joint: " << jointCommand.name[i] << ".. skipping..";
                    NasaCommonLogging::Logger::log("gov.nasa.controllers.JointCommandToJointCommandMap", log4cpp::Priority::DEBUG, err.str());
                    err.str("");
                }
            }
        }
    }

    void WrenchStateToWrenchMap(const r2_msgs::WrenchState& wrenchState, std::map<std::string, KDL::Wrench>& wrenchMap)
    {
        if (wrenchState.name.size() != wrenchState.wrench.size())
        {
            std::stringstream err;
            err << "wrenchstate name length doesn't match wrench length: " << wrenchState.name.size() << " vs " << wrenchState.wrench.size();
            NasaCommonLogging::Logger::log("gov.nasa.controllers.WrenchStateToWrenchMap", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        for (unsigned int i = 0; i < wrenchState.name.size(); ++i)
        {
            wrenchMap[wrenchState.name.at(i)] = wrenchMsgToWrench(wrenchState.wrench.at(i));
        }

    }
    void ToolFrameToSegment(const r2_msgs::ToolFrame& toolFrame, std::vector<ToolFrame>& segmentList)
    {
        if (toolFrame.name.size() != toolFrame.refFrame.size() || toolFrame.name.size() != toolFrame.location.size())
        {
            // length mismatch
            std::stringstream err;
            err << "ToolFrameToSegment() - toolFrame name and refFrame and location lengths don't match";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.ToolFrameToSegment", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        for (unsigned int i = 0; i < toolFrame.name.size(); ++i)
        {
            std::string _name = toolFrame.name[i];
            geometry_msgs::Pose loc = toolFrame.location[i];
            KDL::Rotation rotation = KDL::Rotation::Quaternion(loc.orientation.x, loc.orientation.y, loc.orientation.z, loc.orientation.w);
            KDL::Vector position = KDL::Vector(loc.position.x, loc.position.y, loc.position.z);
            KDL::Frame _f_tip(rotation, position);
            double mass = toolFrame.toolMass[i];
            KDL::Vector cog(toolFrame.toolCog[i].x, toolFrame.toolCog[i].y, toolFrame.toolCog[i].z);
            KDL::RotationalInertia inertia(toolFrame.toolInertia[i].ixx, toolFrame.toolInertia[i].iyy, toolFrame.toolInertia[i].izz, toolFrame.toolInertia[i].ixy, toolFrame.toolInertia[i].ixz, toolFrame.toolInertia[i].iyz);
            KDL::Segment segment(_name, KDL::Joint(KDL::Joint::None), _f_tip, KDL::RigidBodyInertia(mass, cog, inertia));
            segmentList.push_back((ToolFrame) {toolFrame.name[i], toolFrame.refFrame[i], segment});
        }
    }

    void ToolFrameToSegmentAdd(const r2_msgs::ToolFrame& toolFrame, std::vector<ToolFrame>& segmentList, std::vector<ToolFrame>& addSegmentList)
    {
        if (toolFrame.name.size() != toolFrame.refFrame.size() || toolFrame.name.size() != toolFrame.location.size())
        {
            // length mismatch
            std::stringstream err;
            err << "ToolFrameToSegmentAdd() - toolFrame name and refFrame and location lengths don't match";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.ToolFrameToSegmentAdd", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        for (unsigned int i = 0; i < toolFrame.name.size(); ++i)
        {
            std::string _name = toolFrame.name[i];
            geometry_msgs::Pose loc = toolFrame.location[i];
            KDL::Rotation rotation = KDL::Rotation::Quaternion(loc.orientation.x, loc.orientation.y, loc.orientation.z, loc.orientation.w);
            KDL::Vector position = KDL::Vector(loc.position.x, loc.position.y, loc.position.z);
            KDL::Frame _f_tip(rotation, position);
            double mass = toolFrame.toolMass[i];
            KDL::Vector cog(toolFrame.toolCog[i].x, toolFrame.toolCog[i].y, toolFrame.toolCog[i].z);
            KDL::RotationalInertia inertia(toolFrame.toolInertia[i].ixx, toolFrame.toolInertia[i].iyy, toolFrame.toolInertia[i].izz, toolFrame.toolInertia[i].ixy, toolFrame.toolInertia[i].ixz, toolFrame.toolInertia[i].iyz);
            KDL::Segment segment(_name, KDL::Joint(KDL::Joint::None), _f_tip, KDL::RigidBodyInertia(mass, cog, inertia));
            segmentList.push_back((ToolFrame) {toolFrame.name[i], toolFrame.refFrame[i], segment});
            addSegmentList.push_back((ToolFrame) {toolFrame.name[i], toolFrame.refFrame[i], segment});
        }
    }

    void ToolFrameToSegmentRemove(const r2_msgs::ToolFrame& toolFrame, std::vector<ToolFrame>& removeSegmentList)
    {
        if (toolFrame.name.size() != toolFrame.refFrame.size() || toolFrame.name.size() != toolFrame.location.size())
        {
            // length mismatch
            std::stringstream err;
            err << "ToolFrameToSegmentRemove() - toolFrame name and refFrame and location lengths don't match";
            NasaCommonLogging::Logger::log("gov.nasa.controllers.ToolFrameToSegmentAdd", log4cpp::Priority::ERROR, err.str());
            throw std::logic_error(err.str());
            return;
        }

        for (unsigned int i = 0; i < toolFrame.name.size(); ++i)
        {
            std::string _name = toolFrame.name[i];
            geometry_msgs::Pose loc = toolFrame.location[i];
            KDL::Rotation rotation = KDL::Rotation::Quaternion(loc.orientation.x, loc.orientation.y, loc.orientation.z, loc.orientation.w);
            KDL::Vector position = KDL::Vector(loc.position.x, loc.position.y, loc.position.z);
            KDL::Frame _f_tip(rotation, position);
            double mass = toolFrame.toolMass[i];
            KDL::Vector cog(toolFrame.toolCog[i].x, toolFrame.toolCog[i].y, toolFrame.toolCog[i].z);
            KDL::RotationalInertia inertia(toolFrame.toolInertia[i].ixx, toolFrame.toolInertia[i].iyy, toolFrame.toolInertia[i].izz, toolFrame.toolInertia[i].ixy, toolFrame.toolInertia[i].ixz, toolFrame.toolInertia[i].iyz);
            KDL::Segment segment(_name, KDL::Joint(KDL::Joint::None), _f_tip, KDL::RigidBodyInertia(mass, cog, inertia));
            removeSegmentList.push_back((ToolFrame) {toolFrame.name[i], toolFrame.refFrame[i], segment});
        }
    }

}