Robot.cpp

Go to the documentation of this file.

/***************************************************************************/
#include "robodyn_controllers/Robot.h"

using namespace KDL;
using namespace std;

/***************************************************************************/
Robot::Robot()
{
}

/***************************************************************************/
Robot::~Robot()
{
    JointDataMap::iterator ittr;

    for (ittr = jointDataMap.begin(); ittr != jointDataMap.end(); ittr++)
    {
        delete ittr->second;
    }

    jointDataMap.clear();

    JointTraitsMap::iterator ittrt;

    for (ittrt = jointTraitsMap.begin(); ittrt != jointTraitsMap.end(); ittrt++)
    {
        delete ittrt->second;
    }

    jointTraitsMap.clear();
}

/***************************************************************************/
KDL::Chain Robot::getChain(const std::string& fromSegment,
                           const std::string& toSegment, KDL::Chain& chain)
{
    if (!robotTree.getChain(fromSegment, toSegment, chain))
    {
        stringstream err;
        err << "Robot::getChain unable to find chain from " << fromSegment << " to " << toSegment;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    return chain;
}

/***************************************************************************/
KDL::Chain Robot::getChainToRoot(const std::string& fromSegment, KDL::Chain& chain)
{
    return (getChain(fromSegment, robotTree.getRootSegment()->first, chain));
}

/***************************************************************************/
KDL::Chain Robot::getChainFromRoot(const std::string& toSegment, KDL::Chain& chain)
{
    return (getChain(robotTree.getRootSegment()->first, toSegment, chain));
}

/***************************************************************************/
KDL::Chain Robot::getChainInReverse(const KDL::Chain& inChain, KDL::Chain& retChain)
{
    Joint currentJoint;

    for (int i = inChain.getNrOfSegments() - 1; i >= 0; i--)
    {
        if ((unsigned)i == inChain.getNrOfSegments() - 1)
        {
            currentJoint = Joint(Joint::None);
        }
        else
        {
            currentJoint = inChain.getSegment(i + 1).getJoint();
        }

        retChain.addSegment(Segment(inChain.getSegment(i).getName(),
                                    currentJoint, inChain.getSegment(i).getFrameToTip().Inverse()));
    }

    return retChain;
}

/***************************************************************************/
double Robot::getMass()
{
    double m = 0.0;
    SegmentMap segmentMap = robotTree.getSegments();

    SegmentMap::const_iterator end = segmentMap.end();

    for (SegmentMap::const_iterator it = segmentMap.begin(); it != end; ++it)
    {
        m = m + (it->second).segment.getInertia().getMass();
    }

    return m;
}

/***************************************************************************/
Vector Robot::getCenterOfMass()
{
    Vector v;

    Eigen::Matrix<double, 3, Eigen::Dynamic> locationMatrix(3, robotTree.getSegments().size() - 1);
    Eigen::VectorXd                          weightVector(robotTree.getSegments().size() - 1);
    Eigen::Vector3d                          temp;

    unsigned i = 0;
    Frame      parentFrame;
    Vector     locationInBase;
    SegmentMap segmentMap = robotTree.getSegments();

    std::string rootSegName = robotTree.getRootSegment()->first;

    SegmentMap::const_iterator end = segmentMap.end();

    for (SegmentMap::const_iterator it = segmentMap.begin(); it != end; ++it)
    {
        if (it->second.segment.getName().compare(rootSegName) != 0)
        {
            Frame parentFrame = getChainTransformTipToBase(it->second.parent->second.segment.getName()) *
                                Frame(it->second.segment.getFrameToTip().M.RotZ(getJointPosition(it->second.segment.getName())));

            locationInBase = parentFrame * it->second.segment.getInertia().getCOG();

            locationMatrix(0, i) = locationInBase.x();
            locationMatrix(1, i) = locationInBase.y();
            locationMatrix(2, i) = locationInBase.z();
            weightVector(i)      = it->second.segment.getInertia().getMass();
            i++;
        }
    }

    temp = (locationMatrix * weightVector) * (1 / getMass());

    v.x(temp(0));
    v.y(temp(1));
    v.z(temp(2));

    return v;
}

/***************************************************************************/
Robot::JointTraits* Robot::addJoint(const std::string& jointName)
{
    JointData* data   = new JointData();
    data->curPosition = 0.0;
    data->curVelocity = 0.0;
    data->curTorque   = 0.0;

    jointDataMap.insert(std::pair<std::string, JointData*>(jointName, data));

    JointTraits* traits    = new JointTraits();
    traits->name           = jointName;
    traits->maxPosition    = 0.0;
    traits->minPosition    = 0.0;
    traits->maxVelocity    = 0.0;
    traits->maxTorque      = 0.0;
    traits->springConstant = 0.0;

    jointTraitsMap.insert(std::pair<std::string, JointTraits*>(jointName, traits));

    return traits;
}

/***************************************************************************/
Robot::JointData* Robot::getJointData(const std::string& jointName)
{
    JointDataMap::iterator it = jointDataMap.find(jointName);

    if (it == jointDataMap.end())
    {
        stringstream err;
        err << "Robot::getJointData unknown joint " << jointName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    return (it->second);
}

/***************************************************************************/
Robot::JointTraits* Robot::getJointTraits(const std::string& segmentName)
{
    JointTraitsMap::iterator it = jointTraitsMap.find(segmentName);

    if (it == jointTraitsMap.end())
    {
        stringstream err;
        err << "Robot::getJointTraits unknown joint " << segmentName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    return (it->second);
}

/***************************************************************************/
Robot::JointTraitsVector Robot::getJointTraits()
{
    JointTraitsVector traits;
    traits.clear();

    JointTraitsMap::iterator it = jointTraitsMap.begin();

    for (it = jointTraitsMap.begin(); it != jointTraitsMap.end(); it++)
    {
        traits.push_back(*(it->second));
    }

    return (traits);
}

/***************************************************************************/
void Robot::zeroJoints()
{
    JointDataMap::iterator ittr;

    for (ittr = jointDataMap.begin(); ittr != jointDataMap.end(); ittr++)
    {
        ittr->second->curPosition = 0.0;
        ittr->second->curVelocity = 0.0;
        ittr->second->curTorque   = 0.0;
    }
}

/***************************************************************************/
void Robot::setJointLimits(const std::string& jointName, double min, double max)
{
    JointTraitsMap::iterator it = jointTraitsMap.find(jointName);

    if (it == jointTraitsMap.end())
    {
        stringstream err;
        err << "Robot::setJointLimits unknown joint " << jointName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    it->second->minPosition = min;
    it->second->maxPosition = max;
}

/***************************************************************************/
void Robot::setJointPosition(const std::string& jointName, double position)
{
    JointDataMap::iterator it = jointDataMap.find(jointName);

    if (it == jointDataMap.end())
    {
        stringstream err;
        err << "Robot::setJointPosition unknown joint " << jointName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    it->second->curPosition = position;
}

/***************************************************************************/
double Robot::getJointPosition(const std::string& jointName)
{
    JointDataMap::iterator it = jointDataMap.find(jointName);

    if (it == jointDataMap.end())
    {
        stringstream err;
        err << "Robot::getJointPosition unknown joint " << jointName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    return it->second->curPosition;
}


/***************************************************************************/
void Robot::setJointVelocity(const std::string& jointName, double velocity)
{
    JointDataMap::iterator it = jointDataMap.find(jointName);

    if (it == jointDataMap.end())
    {
        stringstream err;
        err << "Robot::setJointVelocity unknown joint " << jointName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    it->second->curVelocity = velocity;
}

/***************************************************************************/
double Robot::getJointVelocity(const std::string& jointName)
{
    JointDataMap::iterator it = jointDataMap.find(jointName);

    if (it == jointDataMap.end())
    {
        stringstream err;
        err << "Robot::getJointVelocity unknown joint " << jointName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    return it->second->curVelocity;
}

/***************************************************************************/
void Robot::setJointTorque(const std::string& jointName, double torque)
{
    JointDataMap::iterator it = jointDataMap.find(jointName);

    if (it == jointDataMap.end())
    {
        stringstream err;
        err << "Robot::setJointTorque unknown joint " << jointName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    it->second->curTorque = torque;
}

/***************************************************************************/
double Robot::getJointTorque(const std::string& jointName)
{
    JointDataMap::iterator it = jointDataMap.find(jointName);

    if (it == jointDataMap.end())
    {
        stringstream err;
        err << "Robot::getJointTorque unknown joint " << jointName;
        NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    return it->second->curTorque;
}

/***************************************************************************/
JntArray Robot::getChainJointPositions(const KDL::Chain& chain)
{
    JntArray jointArray;
    unsigned i = 0;
    unsigned j = 0;

    jointArray.resize(chain.getNrOfJoints());

    for (i = 0; i < chain.getNrOfSegments(); i++)
    {
        if (chain.getSegment(i).getJoint().getType() != Joint::None)
        {
            jointArray(j++)
                = jointDataMap[chain.getSegment(i).getName()]->curPosition;
        }
    }

    return jointArray;
}

/***************************************************************************/
JntArray Robot::getChainJointVelocities(const KDL::Chain& chain)
{
    JntArray jointArray;
    unsigned i = 0;
    unsigned j = 0;

    jointArray.resize(chain.getNrOfJoints());

    for (i = 0; i < chain.getNrOfSegments(); i++)
    {
        if (chain.getSegment(i).getJoint().getType() != Joint::None)
        {
            jointArray(j++)
                = jointDataMap[chain.getSegment(i).getName()]->curVelocity;
        }
    }

    return jointArray;
}

/***************************************************************************/
void Robot::getChainJacobian(const KDL::Chain& chain,
                             const KDL::JntArray& positions, KDL::Jacobian& jacobian)
{
    if (chain.getNrOfJoints() > 0)
    {
        jacobian.resize(chain.getNrOfJoints());

        ChainJntToJacSolver jacSolver(chain);
        jacSolver.JntToJac(positions, jacobian);
    }
}

/***************************************************************************/
KDL::Frame Robot::getChainTransform(const KDL::Chain& chain)
{
    return getChainTransform(chain, getChainJointPositions(chain));
}

/***************************************************************************/
Frame Robot::getChainTransformBaseToTip(const std::string& segmentName)
{
    KDL::Chain chain;

    getChainFromRoot(segmentName, chain);
    return getChainTransform(chain);
}

/***************************************************************************/
Frame Robot::getChainTransformTipToBase(const std::string& segmentName)
{
    KDL::Chain chain;

    getChainToRoot(segmentName, chain);
    return getChainTransform(chain);
}

/***************************************************************************/
KDL::Frame Robot::getChainTransform(const KDL::Chain& chain, const KDL::JntArray& pos)
{
    KDL::Frame retVal;

    KDL::ChainFkSolverPos_recursive fkPosSolver(chain);
    fkPosSolver.JntToCart(pos, retVal, -1);  // -1 means end of chain

    return retVal;
}

/***************************************************************************/
Frame Robot::getStaticTransform(const std::string& transformName)
{
    return staticTransforms[transformName];
}

/***************************************************************************/
void Robot::setStaticTransform(const std::string& transformName, const Frame& transform)
{
    staticTransforms[transformName] = transform;
}

/***************************************************************************/
//double Robot::getHeightOfRoot(const KDL::Chain &chain)
//{
//    double height = 0.0;
//
//    if (chain.getNrOfSegments() > 0)
//    {
//        Segment endSegment = chain.getSegment(chain.getNrOfSegments()-1);
//        Frame rootFrameInEndSegment = getChainTransformBaseToTip(endSegment.getName());
//        Vector rootFrameInEndSegmentZup = getRootToWorldTransform().M * rootFrameInEndSegment.Inverse().M * rootFrameInEndSegment.p;
//        height = rootFrameInEndSegmentZup.z();
//    }
//    else
//    {
//        cout << "Invalid chain, no segments" << endl;
//    }
//
//    return height;
//}
//

/***************************************************************************/
void Robot::printChain(const Chain& chain)
{
    printf("%12s %12s %12s \n", "joint", "type", "link");
    printf("============ ============ ============\n");

    for (size_t i = 0; i < chain.getNrOfSegments(); i++)
    {
        printf("%s", Robot::segmentToString(chain.segments[i]).c_str());
    }

    printf("--------------------------------------\n\n");
}

/***************************************************************************/
std::string Robot::segmentToString(const Segment& segment)
{
    std::stringstream ss;

    if (segment.getJoint().getType() != Joint::None)
    {
        ss.width(12);
        ss << segment.getJoint().getName();
        ss.width(13);
        ss << segment.getJoint().getTypeName().c_str();
        ss.width(14);
        ss << "";
        ss << frameToString(segment.getFrameToTip()) << std::endl;
    }
    else
    {
        ss.width(39);
        ss << "";
        ss << frameToString(segment.getFrameToTip()) << std::endl;

    }

    ss.width(26);
    ss << " ";
    ss.width(12);
    ss << segment.getName() << " " << rigidBodyInertiaToString(segment.getInertia()) << std::endl;

    return ss.str();
}

/***************************************************************************/
std::string Robot::rigidBodyInertiaToString(const KDL::RigidBodyInertia& rbi)
{
    std::stringstream ss;

    ss.setf(ios::fixed, ios::floatfield);
    ss.precision(3);

    ss << "m=";
    ss.width(8);
    ss << rbi.getMass();
    ss << "  cog=" << vectorToString(rbi.getCOG());

    return ss.str();
}

/***************************************************************************/
std::string Robot::frameToString(const Frame& frame)
{
    std::stringstream ss;

    ss << "rpy=" << rotationToRpyString(frame.M) << "  xyz=" << vectorToString(frame.p);

    return ss.str();
}

/***************************************************************************/
std::string Robot::rotationToRpyString(const Rotation& rotation)
{
    std::stringstream ss;

    ss.setf(ios::fixed, ios::floatfield);
    ss.precision(3);

    double r, p, y;
    rotation.GetRPY(r, p, y);

    ss << "[ ";
    ss.width(8);
    ss << r << ", ";
    ss.width(8);
    ss <<  p << ", ";
    ss.width(8);
    ss <<  y << " ] ";

    return ss.str();
}

/***************************************************************************/
std::string Robot::rotationToString(const Rotation& rotation)
{
    std::stringstream ss;

    ss.setf(ios::fixed, ios::floatfield);
    ss.precision(3);

    ss << "[ ";
    ss.width(8);
    ss << rotation.data[0] << ", ";
    ss.width(8);
    ss <<  rotation.data[1] << ", ";
    ss.width(8);
    ss <<  rotation.data[2] << " ] ";

    ss << "[ ";
    ss.width(8);
    ss << rotation.data[3] << ", ";
    ss.width(8);
    ss <<  rotation.data[4] << ", ";
    ss.width(8);
    ss <<  rotation.data[5] << " ] ";

    ss << "[ ";
    ss.width(8);
    ss << rotation.data[6] << ", ";
    ss.width(8);
    ss <<  rotation.data[7] << ", ";
    ss.width(8);
    ss <<  rotation.data[8] << " ] ";

    return ss.str();
}

/***************************************************************************/
std::string Robot::vectorToString(const Vector& vector)
{
    std::stringstream ss;

    ss.setf(ios::fixed, ios::floatfield);
    ss.precision(3);

    ss << "[ ";
    ss.width(8);
    ss << vector.data[0] << ", ";
    ss.width(8);
    ss <<  vector.data[1] << ", ";
    ss.width(8);
    ss <<  vector.data[2] << " ]";

    return ss.str();
}

/***************************************************************************/
std::string Robot::jacobianToString(KDL::Jacobian& jacobian)
{
    std::stringstream ss;

    ss.setf(ios::fixed, ios::floatfield);
    ss.precision(3);

    unsigned int rows = jacobian.rows();
    unsigned int cols = jacobian.columns();

    ss << "[\n";

    for (unsigned int i = 0 ; i < rows; i++)
    {
        ss << "   [";

        for (unsigned int j = 0; j < cols - 1; j++)
        {
            ss.width(8);
            ss << jacobian(i, j) << ", ";
        }

        ss.width(8);
        ss << jacobian(i, cols - 1) << " ]\n";
    }

    ss << "]";

    return ss.str();
}