Go to the documentation of this file.00001
00005 #include "robodyn_mechanisms/JointControlCommandFinger.h"
00006
00011 JointControlCommandFinger::JointControlCommandFinger(const std::string& mechanism, IoFunctions ioFunctions)
00012 : JointControlCommandInterface(mechanism, ioFunctions), logCategory("gov.nasa.JointControlCommandFinger")
00013 {
00014 setParameters();
00015
00016 bootLoader();
00017 multiLoopSmooth();
00018 resetCalibrationMode();
00019 disableClearFaultMode();
00020 }
00021
00022 JointControlCommandFinger::~JointControlCommandFinger()
00023 {
00024
00025 }
00026
00027 void JointControlCommandFinger::setParameters()
00028 {
00029 std::string parameterFile = io.getControlFile(mechanism);
00030
00032 TiXmlDocument file(parameterFile.c_str());
00033
00034 if (!file.LoadFile())
00035 {
00036 std::stringstream err;
00037 err << "Failed to load file [" << parameterFile << "]";
00038 NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::FATAL, err.str());
00039 throw std::runtime_error(err.str());
00040 }
00041
00042 TiXmlHandle doc(&file);
00043 NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::INFO, "File [" + parameterFile + "] successfully loaded.");
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));
00054
00055 if (parametersElement.ToElement())
00056 {
00057
00058 ControlModeLiveCoeffName = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "ControlModeLiveCoeff"));
00059 CommandModeLiveCoeffName = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "CommandModeLiveCoeff"));
00060 CalibrationModeLiveCoeffName = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "CalibrationModeLiveCoeff"));
00061 ClearFaultStateLiveCoeffName = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, ApiMap::getXmlElementValue(parametersElement, "ClearFaultStateLiveCoeff"));
00062 }
00063 else
00064 {
00065 std::stringstream err;
00066 err << "The file " << parameterFile << " has no element named [ApiMap]";
00067 NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::ERROR, err.str());
00068 throw std::runtime_error(err.str());
00069 }
00070 }
00071
00072
00078 void JointControlCommandFinger::getStates(r2_msgs::JointControlData& commandStates)
00079 {
00080 commandStates = states;
00081 }
00082
00083
00084
00089 void JointControlCommandFinger::bootLoader(void)
00090 {
00091 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::BOOTLOADER successful on mechanism: " << mechanism;
00092 states.controlMode.state = r2_msgs::JointControlMode::BOOTLOADER;
00093 io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::BOOTLOADER);
00095 resetCalibrationMode();
00096 }
00097
00102 void JointControlCommandFinger::off(void)
00103 {
00104 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::OFF successful on mechanism: " << mechanism;
00105 states.controlMode.state = r2_msgs::JointControlMode::OFF;
00106 io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::OFF);
00107 }
00108
00113 void JointControlCommandFinger::park(void)
00114 {
00115 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::PARK successful on mechanism: " << mechanism;
00116 states.controlMode.state = r2_msgs::JointControlMode::PARK;
00117 io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::PARK);
00118 }
00119
00124 void JointControlCommandFinger::neutral(void)
00125 {
00126 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::NEUTRAL successful on mechanism: " << mechanism;
00127 states.controlMode.state = r2_msgs::JointControlMode::NEUTRAL;
00128 io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::NEUTRAL);
00129 }
00130
00135 void JointControlCommandFinger::drive(void)
00136 {
00137 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlMode::DRIVE successful on mechanism: " << mechanism;
00138 states.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00139 io.setLiveCoeff(ControlModeLiveCoeffName, r2_msgs::JointControlMode::DRIVE);
00140 }
00141
00142
00143
00148 void JointControlCommandFinger::motCom(void)
00149 {
00150 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCommandMode::MOTCOM successful on mechanism: " << mechanism;
00151 states.commandMode.state = r2_msgs::JointControlCommandMode::MOTCOM;
00152 io.setLiveCoeff(CommandModeLiveCoeffName, r2_msgs::JointControlCommandMode::MOTCOM);
00153 }
00154
00159 void JointControlCommandFinger::stallMode(void)
00160 {
00161 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Command transition to JointControlCommandMode::STALLMODE not allowed on mechanism: " << mechanism << ", Transitioning to MULTILOOPSMOOTH";
00162 multiLoopSmooth();
00163 }
00164
00169 void JointControlCommandFinger::multiLoopStep(void)
00170 {
00171 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCommandMode::MULTILOOPSTEP successful on mechanism: " << mechanism;
00172 states.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSTEP;
00173 io.setLiveCoeff(CommandModeLiveCoeffName, r2_msgs::JointControlCommandMode::MULTILOOPSTEP);
00174 }
00175
00180 void JointControlCommandFinger::multiLoopSmooth(void)
00181 {
00182 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCommandMode::MULTILOOPSMOOTH successful on mechanism: " << mechanism;
00183 states.commandMode.state = r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH;
00184 io.setLiveCoeff(CommandModeLiveCoeffName, r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH);
00185 }
00186
00187 void JointControlCommandFinger::actuator(void)
00188 {
00189 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCommandMode::ACTUATOR successful on mechanism: " << mechanism;
00190 states.commandMode.state = r2_msgs::JointControlCommandMode::ACTUATOR;
00191 io.setLiveCoeff(CommandModeLiveCoeffName, r2_msgs::JointControlCommandMode::ACTUATOR);
00192 }
00193
00194
00195
00200 void JointControlCommandFinger::disableCalibrationMode(void)
00201 {
00202 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::DISABLE successful on mechanism: " << mechanism;
00203 states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
00204 }
00205
00210 void JointControlCommandFinger::enableCalibrationMode(void)
00211 {
00212 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::ENABLE successful on mechanism: " << mechanism;
00213 io.setLiveCoeff(CalibrationModeLiveCoeffName, r2_msgs::JointControlCalibrationMode::ENABLE);
00214 states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00215 }
00216
00221 void JointControlCommandFinger::resetCalibrationMode(void)
00222 {
00223 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlCalibrationMode::ENABLE successful on mechanism: " << mechanism;
00224 io.setLiveCoeff(CalibrationModeLiveCoeffName, r2_msgs::JointControlCalibrationMode::UNCALIBRATED);
00225 states.calibrationMode.state = r2_msgs::JointControlCalibrationMode::UNCALIBRATED;
00226 }
00227
00228
00229
00234 void JointControlCommandFinger::disableClearFaultMode(void)
00235 {
00236 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlClearFaultMode::DISABLE successful on mechanism: " << mechanism;
00237 io.setLiveCoeff(ClearFaultStateLiveCoeffName, r2_msgs::JointControlClearFaultMode::DISABLE);
00238 states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::DISABLE;
00239 }
00240
00245 void JointControlCommandFinger::enableClearFaultMode(void)
00246 {
00247 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::DEBUG << "Command transition to JointControlClearFaultMode::ENABLE successful on mechanism: " << mechanism;
00248 io.setLiveCoeff(ClearFaultStateLiveCoeffName, r2_msgs::JointControlClearFaultMode::ENABLE);
00249 states.clearFaultMode.state = r2_msgs::JointControlClearFaultMode::ENABLE;
00250 }