JointCommandGripper.cpp

Go to the documentation of this file.

#include "robodyn_mechanisms/JointCommandGripper.h"

using namespace std;
using namespace log4cpp;
namespace SU = StringUtilities;

JointCommandGripper::JointCommandGripper(const std::string& mechanism, IoFunctions ioFunctions)
    : JointCommandInterface(mechanism, ioFunctions),
      completeMessageSize(3)
{
    if (mechanism == "")
    {
        stringstream err;
        err << "Constructor requires mechanism be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::FATAL, err.str());
        throw invalid_argument(err.str());
    }

    if (io.getFloat.empty())
    {
        stringstream err;
        err << "Constructor requires 'io.getFloat' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::FATAL, err.str());
        throw invalid_argument(err.str());
    }

    if (io.setFloat.empty())
    {
        stringstream err;
        err << "Constructor requires 'io.setFloat' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::FATAL, err.str());
        throw invalid_argument(err.str());
    }

    if (io.getMotorCoeff.empty())
    {
        stringstream err;
        err << "Constructor requires 'io.getMotorCoeff' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::FATAL, err.str());
        throw invalid_argument(err.str());
    }

    if (io.hasBrainstemCoeff.empty())
    {
        stringstream err;
        err << "Constructor requires 'io.hasBrainstemCoeff' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::FATAL, err.str());
        throw invalid_argument(err.str());
    }

    if (io.getBrainstemCoeff.empty())
    {
        stringstream err;
        err << "Constructor requires 'io.getBrainstemCoeff' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::FATAL, err.str());
        throw invalid_argument(err.str());
    }

    if (io.getCommandFile.empty())
    {
        stringstream err;
        err << "Constructor requires 'io.getCommandFile' be non-empty.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::FATAL, err.str());
        throw invalid_argument(err.str());
    }

    roboDynJoints.push_back(mechanism);
    jawLeftName  = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("jawLeft");
    jawRightName = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("jawRight");

    loadCoeffs();
}

JointCommandGripper::~JointCommandGripper()
{}

void JointCommandGripper::loadCoeffs()
{
    std::string parameterFile = io.getCommandFile(mechanism);

    simpleMeasuredStateMsg.name.resize(completeMessageSize);
    simpleMeasuredStateMsg.position.resize(completeMessageSize);
    simpleMeasuredStateMsg.velocity.resize(completeMessageSize);
    simpleMeasuredStateMsg.effort.resize(completeMessageSize);

    simpleMeasuredStateMsg.name[0] = roboDynJoints[0];
    simpleMeasuredStateMsg.name[1] = jawLeftName;
    simpleMeasuredStateMsg.name[2] = jawRightName;

    completeMeasuredStateMsg.name.resize(completeMessageSize);
    completeMeasuredStateMsg.position.resize(completeMessageSize);
    completeMeasuredStateMsg.velocity.resize(completeMessageSize);
    completeMeasuredStateMsg.effort.resize(completeMessageSize);

    completeMeasuredStateMsg.name[0] = roboDynJoints[0];
    completeMeasuredStateMsg.name[1] = jawLeftName;
    completeMeasuredStateMsg.name[2] = jawRightName;

    // just send an empty commanded message to bypass robodyn

    jointCapabilityMsg.name.resize(1);
    jointCapabilityMsg.positionLimitMax.resize(1);
    jointCapabilityMsg.positionLimitMin.resize(1);
    jointCapabilityMsg.velocityLimit.resize(1);
    jointCapabilityMsg.torqueLimit.resize(1);

    jointCapabilityMsg.name[0] = roboDynJoints[0];

    positionVals.resize(completeMessageSize, 0.);
    velocityVals.resize(completeMessageSize, 0.);
    effortVals.resize(completeMessageSize, 0.);

    TiXmlDocument file(parameterFile.c_str());
    bool loadOkay = file.LoadFile();

    if (!loadOkay)
    {
        stringstream err;
        err << "Failed to load file [" << parameterFile << "]";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::FATAL, err.str());
        throw runtime_error(err.str());
    }

    TiXmlHandle doc(&file);
    NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::INFO, "CommandFile [" + parameterFile + "] successfully loaded.");

    // Check for ApiMap
    TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));

    if (parametersElement.ToElement())
    {
        jointPositionStatusElement                 = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointPositionStatus"));
        jawLeftPositionStatusElement               = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JawLeftPositionStatus"));
        jawRightPositionStatusElement              = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JawRightPositionStatus"));
        jointVelocityStatusElement                 = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointVelocityStatus"));
        jawLeftTorqueStatusElement                 = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JawLeftTorqueStatus"));
        jawRightTorqueStatusElement                = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JawRightTorqueStatus"));
        desiredPositionCommandElement              = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "DesiredPositionCommand"));
        desiredPositionVelocityLimitCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "DesiredPositionVelocityLimitCommand"));
        feedForwardTorqueCommandElement            = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "FeedForwardTorqueCommand"));
        proportionalGainCommandElement             = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "ProportionalGainCommand"));
        derivativeGainCommandElement               = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "DerivativeGainCommand"));
        integralGainCommandElement                 = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "IntegralGainCommand"));
        positionLoopTorqueLimitCommandElement      = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLoopTorqueLimitCommand"));
        positionLoopWindupLimitCommandElement      = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLoopWindupLimitCommand"));
        jointCapabilityMsg.positionLimitMax[0]     = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLimitMax")));
        jointCapabilityMsg.positionLimitMin[0]     = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLimitMin")));
        jointCapabilityMsg.velocityLimit[0]        = io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "VelLimit")));
        jointCapabilityMsg.torqueLimit[0]          = 0.;

        velLimitCommandElement =                     StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "VelLimit"));
        curLimitCommandElement =                     StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "CurLimit"));
        posKpCommandElement =                        StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PosKp"));
        pLKiCommandElement =                         StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PLKi"));
        posKdCommandElement =                        StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PosKd"));
        pLWindupLimCommandElement =                  StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PLWindupLim"));
        pLKiMinCommandElement =                      StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PLKiMin"));

        if (not(io.hasBrainstemCoeff(velLimitCommandElement)))
        {
            stringstream err;
            err << "The BrainstemCoeff " << velLimitCommandElement << " does not exist";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }

        if (not(io.hasBrainstemCoeff(curLimitCommandElement)))
        {
            stringstream err;
            err << "The BrainstemCoeff " << curLimitCommandElement << " does not exist";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }

        if (not(io.hasBrainstemCoeff(posKpCommandElement)))
        {
            stringstream err;
            err << "The BrainstemCoeff " << posKpCommandElement << " does not exist";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }

        if (not(io.hasBrainstemCoeff(pLKiCommandElement)))
        {
            stringstream err;
            err << "The BrainstemCoeff " << pLKiCommandElement << " does not exist";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }

        if (not(io.hasBrainstemCoeff(posKdCommandElement)))
        {
            stringstream err;
            err << "The BrainstemCoeff " << posKdCommandElement << " does not exist";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }

        if (not(io.hasBrainstemCoeff(pLWindupLimCommandElement)))
        {
            stringstream err;
            err << "The BrainstemCoeff " << pLWindupLimCommandElement << " does not exist";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }

        if (not(io.hasBrainstemCoeff(pLKiMinCommandElement)))
        {
            stringstream err;
            err << "The BrainstemCoeff " << pLKiMinCommandElement << " does not exist";
            NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
            throw runtime_error(err.str());
        }

        io.setFloat(velLimitCommandElement,    io.getBrainstemCoeff(velLimitCommandElement));
        io.setFloat(curLimitCommandElement,    io.getBrainstemCoeff(curLimitCommandElement));
        io.setFloat(posKpCommandElement,       io.getBrainstemCoeff(posKpCommandElement));
        io.setFloat(pLKiCommandElement,        io.getBrainstemCoeff(pLKiCommandElement));
        io.setFloat(posKdCommandElement,       io.getBrainstemCoeff(posKdCommandElement));
        io.setFloat(pLWindupLimCommandElement, io.getBrainstemCoeff(pLWindupLimCommandElement));
        io.setFloat(pLKiMinCommandElement,     io.getBrainstemCoeff(pLKiMinCommandElement));
    }
    else
    {
        stringstream err;
        err << "The file " << parameterFile << " has no element named [ApiMap]";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }
}

void JointCommandGripper::updateMeasuredState(r2_msgs::JointControlData msg)
{
    // If coeffs are not loaded, override with zeros RDEV-1075
    if (msg.coeffState.state == r2_msgs::JointControlCoeffState::LOADED)
    {
        positionVals[0] = io.getFloat(jointPositionStatusElement);
        velocityVals[0] = io.getFloat(jointVelocityStatusElement);
        effortVals[0]   = 0;

        positionVals[1] = io.getFloat(jawLeftPositionStatusElement);
        velocityVals[1] = 0;
        effortVals[1]   = io.getFloat(jawLeftTorqueStatusElement);

        positionVals[2] = io.getFloat(jawRightPositionStatusElement);
        velocityVals[2] = 0;
        effortVals[2]   = io.getFloat(jawRightTorqueStatusElement);
    }
    else
    {
        positionVals.assign(completeMessageSize, 0.0);
        velocityVals.assign(completeMessageSize, 0.0);
        effortVals.assign(completeMessageSize,   0.0);
    }
}

sensor_msgs::JointState JointCommandGripper::getSimpleMeasuredState()
{
    simpleMeasuredStateMsg.header.stamp = ros::Time::now();

    simpleMeasuredStateMsg.position = positionVals;
    simpleMeasuredStateMsg.velocity = velocityVals;
    simpleMeasuredStateMsg.effort   = effortVals;

    return simpleMeasuredStateMsg;
}

sensor_msgs::JointState JointCommandGripper::getCompleteMeasuredState()
{
    completeMeasuredStateMsg.header.stamp = ros::Time::now();

    completeMeasuredStateMsg.position = positionVals;
    completeMeasuredStateMsg.velocity = velocityVals;
    completeMeasuredStateMsg.effort   = effortVals;

    return completeMeasuredStateMsg;
}

void JointCommandGripper::setFaultState()
{
}

r2_msgs::JointCommand JointCommandGripper::getCommandedState()
{
    // just send an empty message to bypass robodyn

    return commandedStateMsg;
}

void JointCommandGripper::setCommand(r2_msgs::JointCommand msg, r2_msgs::JointControlData)
{
    if (msg.name.size() != 1)
    {
        stringstream err;
        err << "setCommand() requires one and only one entry in JointCommand.name when setCommand() is called.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    if ((msg.desiredPosition.size() > 1) ||
            (msg.desiredPositionVelocityLimit.size() > 1) ||
            (msg.feedForwardTorque.size() > 1) ||
            (msg.proportionalGain.size() > 1) ||
            (msg.derivativeGain.size() > 1) ||
            (msg.integralGain.size() > 1) ||
            (msg.positionLoopTorqueLimit.size() > 1) ||
            (msg.positionLoopWindupLimit.size() > 1) ||
            (msg.torqueLoopVelocityLimit.size() > 1))
    {
        stringstream err;
        err << "setCommand() requires at most one entry in JointCommand.* when setCommand() is called.";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    if (msg.name[0] != roboDynJoints[0])
    {
        stringstream err;
        err << "setCommand() received JointCommand message for joint [" << msg.name[0] << "]. It expected [" << roboDynJoints[0] << "].";
        NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandGripper", Priority::ERROR, err.str());
        throw runtime_error(err.str());
    }

    if (not msg.desiredPosition.empty())
    {
        io.setFloat(desiredPositionCommandElement, msg.desiredPosition[0]);
    }

    if (not msg.desiredPositionVelocityLimit.empty())
    {
        io.setFloat(desiredPositionVelocityLimitCommandElement, msg.desiredPositionVelocityLimit[0]);
    }

    if (not msg.feedForwardTorque.empty())
    {
        io.setFloat(feedForwardTorqueCommandElement, msg.feedForwardTorque[0]);
    }

    if (not msg.proportionalGain.empty())
    {
        io.setFloat(proportionalGainCommandElement, msg.proportionalGain[0]);
    }

    if (not msg.derivativeGain.empty())
    {
        io.setFloat(derivativeGainCommandElement, msg.derivativeGain[0]);
    }

    if (not msg.integralGain.empty())
    {
        io.setFloat(integralGainCommandElement, msg.integralGain[0]);
    }

    if (not msg.positionLoopTorqueLimit.empty())
    {
        io.setFloat(positionLoopTorqueLimitCommandElement, msg.positionLoopTorqueLimit[0]);
    }

    if (not msg.positionLoopWindupLimit.empty())
    {
        io.setFloat(positionLoopWindupLimitCommandElement, msg.positionLoopWindupLimit[0]);
    }
}

r2_msgs::JointCapability JointCommandGripper::getCapability()
{
    return jointCapabilityMsg;
}