JointControlCommandWrist.cpp

Go to the documentation of this file.

#include "robodyn_mechanisms/JointControlCommandWrist.h"
/***************************************************************************/
JointControlCommandWrist::JointControlCommandWrist(const std::string& mechanism, IoFunctions ioFunctions)
    : JointControlCommandInterface(mechanism, ioFunctions), logCategory("gov.nasa.JointControlCommandWrist")
{
    if (io.setLiveCoeff.empty())
    {
        std::stringstream err;
        err << "Constructor requires 'io.setLiveCoeff' be non-empty.";
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::FATAL, err.str());
        throw std::invalid_argument(err.str());
    }

    setParameters();

    bootLoader();
    multiLoopSmooth();
    resetCalibrationMode();
    disableClearFaultMode();
}

JointControlCommandWrist::~JointControlCommandWrist()
{
    // Nothing
}
void JointControlCommandWrist::setParameters()
{
    std::string parameterFile = io.getControlFile(mechanism);

    TiXmlDocument file(parameterFile.c_str());

    if (!file.LoadFile())
    {
        std::stringstream err;
        err << "Failed to load file [" << parameterFile << "]";
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::FATAL, err.str());
        throw std::runtime_error(err.str());
    }

    TiXmlHandle doc(&file);
    NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::INFO, "File [" + parameterFile + "] successfully loaded.");

    // Print the XML file's contents
    //cout << "Successfully loaded file: " << parameterFile << endl;
    //TiXmlPrinter printer;
    //printer.SetIndent("\t");
    //file.Accept(&printer);
    //cout << printer.Str() << endl;

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

    if (parametersElement.ToElement())
    {
        // Live coeffs names
        ControlModeLiveCoeffName           = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "ControlModeLiveCoeff"));
        CommandModeLiveCoeffName           = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "CommandModeLiveCoeff"));
        CalibrationModeLiveCoeffName       = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "CalibrationModeLiveCoeff"));
        ClearFaultStateLiveCoeffName       = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "ClearFaultStateLiveCoeff"));
    }
    else
    {
        std::stringstream err;
        err << "The file " << parameterFile << " has no element named [ApiMap]";
        NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
    }
}

/***************************************************************************/
void JointControlCommandWrist::getStates(r2_msgs::JointControlData& commandStates)
{
    commandStates = states;
}

// controlMode functions
/***************************************************************************/
void JointControlCommandWrist::bootLoader(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::BOOTLOADER successful on mechanism: " << mechanism;
    states.controlMode.state = r2_msgs::JointControlMode::BOOTLOADER;
    io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::BOOTLOADER);
    resetCalibrationMode();
}
/***************************************************************************/
void JointControlCommandWrist::off(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::OFF successful on mechanism: " << mechanism;
    states.controlMode.state = r2_msgs::JointControlMode::OFF;
    io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::OFF);
}
/***************************************************************************/
void JointControlCommandWrist::park(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::PARK successful on mechanism: " << mechanism;
    states.controlMode.state = r2_msgs::JointControlMode::PARK;
    io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::PARK);

}
/***************************************************************************/
void JointControlCommandWrist::neutral(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::NEUTRAL successful on mechanism: " << mechanism;
    states.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
    io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::NEUTRAL);
}
/***************************************************************************/
void JointControlCommandWrist::drive(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::DRIVE successful on mechanism: " << mechanism;
    states.controlMode.state = r2_msgs::JointControlMode::DRIVE;
    io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::DRIVE);
}

// commandMode functions
/***************************************************************************/
void JointControlCommandWrist::motCom(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCommandMode::MOTCOM successful on mechanism: " << mechanism;
    states.commandMode.state = r2_msgs::JointControlCommandMode::MOTCOM;
    io.setLiveCoeff(CommandModeLiveCoeffName, r2_msgs::JointControlCommandMode::MOTCOM);
}
/***************************************************************************/
void JointControlCommandWrist::stallMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Command transition to JointControlCommandMode::STALLMODE not allowed on mechanism: " << mechanism << ", Transitioning to MULTILOOPSMOOTH";
    multiLoopSmooth();
}
/***************************************************************************/
void JointControlCommandWrist::multiLoopStep(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCommandMode::MULTILOOPSTEP successful on mechanism: " << mechanism;
    states.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
    io.setLiveCoeff(CommandModeLiveCoeffName, r2_msgs::JointControlCommandMode::MULTILOOPSTEP);
}
/***************************************************************************/
void JointControlCommandWrist::multiLoopSmooth(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCommandMode::MULTILOOPSMOOTH successful on mechanism: " << mechanism;
    states.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH;
    io.setLiveCoeff(CommandModeLiveCoeffName, r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH);
}

void JointControlCommandWrist::actuator(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCommandMode::ACTUATOR successful on mechanism: " << mechanism;
    states.commandMode.state = r2_msgs::JointControlCommandMode::ACTUATOR;
    io.setLiveCoeff(CommandModeLiveCoeffName, r2_msgs::JointControlCommandMode::ACTUATOR);
}

// calibrationMode functions
/***************************************************************************/
void JointControlCommandWrist::disableCalibrationMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::DISABLE successful on mechanism: " << mechanism;
    states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
}
/***************************************************************************/
void JointControlCommandWrist::enableCalibrationMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::ENABLE successful on mechanism: " << mechanism;
    io.setLiveCoeff(CalibrationModeLiveCoeffName, r2_msgs::JointControlCalibrationMode::ENABLE);
    states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
}

/***************************************************************************/
void JointControlCommandWrist::resetCalibrationMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::ENABLE successful on mechanism: " << mechanism;
    io.setLiveCoeff(CalibrationModeLiveCoeffName, r2_msgs::JointControlCalibrationMode::UNCALIBRATED);
    states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::UNCALIBRATED;
}

// clearFaultMode functions
/***************************************************************************/
void JointControlCommandWrist::disableClearFaultMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlClearFaultMode::DISABLE successful on mechanism: " << mechanism;
    io.setLiveCoeff(ClearFaultStateLiveCoeffName, r2_msgs::JointControlClearFaultMode::DISABLE);
    states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::DISABLE;
}
/***************************************************************************/
void JointControlCommandWrist::enableClearFaultMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlClearFaultMode::ENABLE successful on mechanism: " << mechanism;
    io.setLiveCoeff(ClearFaultStateLiveCoeffName, r2_msgs::JointControlClearFaultMode::ENABLE);
    states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::ENABLE;
}