JointControlCommandSeriesElastic.cpp

Go to the documentation of this file.

#include "robodyn_mechanisms/JointControlCommandSeriesElastic.h"
/***************************************************************************/
JointControlCommandSeriesElastic::JointControlCommandSeriesElastic(const std::string& mechanism, IoFunctions ioFunctions, NodeRegisterManagerPtr nodeRegisterManager)
    : JointControlCommandInterface(mechanism, ioFunctions), nodeRegisterManager(nodeRegisterManager), logCategory("gov.nasa.JointControlCommandSeriesElastic")
{
    setParameters();
    bootLoader();
    motCom();
    disableCalibrationMode();
    disableClearFaultMode();
}

JointControlCommandSeriesElastic::~JointControlCommandSeriesElastic()
{
    // Nothing
}
void JointControlCommandSeriesElastic::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())
    {
        // Control bit names
        BootEnableControlName      = ApiMap::getXmlElementValue(parametersElement, "BootEnableControl");
        BridgeEnableControlName    = ApiMap::getXmlElementValue(parametersElement, "BridgeEnableControl");
        BrakeReleaseControlName    = ApiMap::getXmlElementValue(parametersElement, "BrakeReleaseControl");
        MotorEnableControlName     = ApiMap::getXmlElementValue(parametersElement, "MotorEnableControl");
        MotComSourceControlName    = ApiMap::getXmlElementValue(parametersElement, "MotComSourceControl");
        ControlModeControlName     = ApiMap::getXmlElementValue(parametersElement, "ControlModeControl");
        PosComVelocityControlName  = ApiMap::getXmlElementValue(parametersElement, "PosComVelocityControl");
        CalibrationModeControlName = ApiMap::getXmlElementValue(parametersElement, "CalibrationModeControl");
        ClearFaultControlName      = ApiMap::getXmlElementValue(parametersElement, "ClearFaultControl");
        InitAPS2toAPS1ControlName  = ApiMap::getXmlElementValue(parametersElement, "InitAPS2toAPS1Control");
        HeadLightControlName       = ApiMap::getXmlElementValue(parametersElement, "HeadLightControl");
        LaserEnableControlName     = ApiMap::getXmlElementValue(parametersElement, "LaserEnableControl");
        AwareControlName           = ApiMap::getXmlElementValue(parametersElement, "AwareControl");
    }
    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());
    }

    HeadLightName   = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, HeadLightControlName);
    LaserEnableName = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, LaserEnableControlName);

    if (nodeRegisterManager->hasControlProperty(mechanism, HeadLightControlName))
    {
        headLight = nodeRegisterManager->getControlValue(mechanism, HeadLightControlName);
        io.setIntParam(HeadLightName, headLight);
    }

    if (nodeRegisterManager->hasControlProperty(mechanism, LaserEnableControlName))
    {
        laserEnable = nodeRegisterManager->getControlValue(mechanism, LaserEnableControlName);
        io.setIntParam(LaserEnableName, laserEnable);
    }

    if (nodeRegisterManager->hasControlProperty(mechanism, InitAPS2toAPS1ControlName))
    {
        nodeRegisterManager->setControlValue(mechanism, InitAPS2toAPS1ControlName, 0); // We think this is used for neck
    }

}

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

void JointControlCommandSeriesElastic::updateParameters()
{
    if (nodeRegisterManager->hasControlProperty(mechanism, HeadLightControlName))
    {
        io.getIntParam(HeadLightName, headLight);
        nodeRegisterManager->setControlValue(mechanism, HeadLightControlName, headLight);
    }

    if (nodeRegisterManager->hasControlProperty(mechanism, LaserEnableControlName))
    {
        io.getIntParam(LaserEnableName, laserEnable);
        nodeRegisterManager->setControlValue(mechanism, LaserEnableControlName, laserEnable);
    }
}

// controlMode functions
/***************************************************************************/
void JointControlCommandSeriesElastic::bootLoader(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::BOOTLOADER successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, BootEnableControlName,   0);
    nodeRegisterManager->setControlValue(mechanism, BridgeEnableControlName, 1);
    nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 0);
    nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  0);
    states.controlMode.state = r2_msgs::JointControlMode::BOOTLOADER;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::off(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::OFF successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, BootEnableControlName,   1);
    nodeRegisterManager->setControlValue(mechanism, BridgeEnableControlName, 1); //setting bridge enabled to 1 in off as per harrison's suggestion
    nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 0);
    nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  0);
    states.controlMode.state = r2_msgs::JointControlMode::OFF;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::park(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::PARK successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, BridgeEnableControlName, 1);
    nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 0);
    nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  1);
    states.controlMode.state = r2_msgs::JointControlMode::PARK;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::neutral(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::NEUTRAL successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, BridgeEnableControlName, 1);
    nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 1);
    nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  0);
    states.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::drive(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::DRIVE successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, BrakeReleaseControlName, 1);
    nodeRegisterManager->setControlValue(mechanism, MotorEnableControlName,  1);
    states.controlMode.state = r2_msgs::JointControlMode::DRIVE;
}

// commandMode functions
/***************************************************************************/
void JointControlCommandSeriesElastic::motCom(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::MOTCOM successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, MotComSourceControlName, 0); // Brainstem mot com
    nodeRegisterManager->setControlValue(mechanism, ControlModeControlName,  0); // MotCom control mode
    states.commandMode.state = r2_msgs::JointControlCommandMode::MOTCOM;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::stallMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::STALLMODE successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, MotComSourceControlName, 1); // Embedded mot com
    nodeRegisterManager->setControlValue(mechanism, ControlModeControlName,  1); // StallMode control mode

    states.commandMode.state = r2_msgs::JointControlCommandMode::STALLMODE;
    return;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::multiLoopStep(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::MULTILOOPSTEP successful on joint: " << mechanism;

    nodeRegisterManager->setControlValue(mechanism, MotComSourceControlName,   1); // Embedded mot com
    nodeRegisterManager->setControlValue(mechanism, ControlModeControlName,    3); // MultiLoop control mode
    nodeRegisterManager->setControlValue(mechanism, PosComVelocityControlName, 0); // Step

    states.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
    return;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::multiLoopSmooth(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::MULTILOOPSMOOTH successful on joint: " << mechanism;

    nodeRegisterManager->setControlValue(mechanism, MotComSourceControlName,   1); // Embedded mot com
    nodeRegisterManager->setControlValue(mechanism, ControlModeControlName,    3); // MultiLoop control mode
    nodeRegisterManager->setControlValue(mechanism, PosComVelocityControlName, 1); // Smooth

    states.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH;
    return;
}

void JointControlCommandSeriesElastic::actuator(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "ACTUATOR is not a valid JointControlCommandMode for " << mechanism;
}

// calibrationMode functions
/***************************************************************************/
void JointControlCommandSeriesElastic::disableCalibrationMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::DISABLE successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, CalibrationModeControlName, 0);
    states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::enableCalibrationMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::ENABLE successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, CalibrationModeControlName, 1);
    states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::resetCalibrationMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::ENABLE successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, CalibrationModeControlName, 0);
    states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
}


// clearFaultMode functions
/***************************************************************************/
void JointControlCommandSeriesElastic::disableClearFaultMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlClearFaultMode::DISABLE successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, ClearFaultControlName, 0);
    states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::DISABLE;
}
/***************************************************************************/
void JointControlCommandSeriesElastic::enableClearFaultMode(void)
{
    NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlClearFaultMode::ENABLE successful on joint: " << mechanism;
    nodeRegisterManager->setControlValue(mechanism, ClearFaultControlName, 1);
    states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::ENABLE;
}