robodyn_mechanisms: JointControlManagerWrist.cpp Source File

Go to the documentation of this file.
00001 
00005 #include "robodyn_mechanisms/JointControlManagerWrist.h"
00006 
00007 JointControlManagerWrist::JointControlManagerWrist(const std::string& mechanism, IoFunctions ioFunctions, double timeLimit, const std::string& type)
00008     : JointControlManagerInterface(mechanism, ioFunctions, timeLimit, type), logCategory("gov.nasa.JointControlManagerWrist")
00009 {
00010     if (io.getLiveCoeff.empty())
00011     {
00012         std::stringstream err;
00013         err << "Constructor requires 'io.getLiveCoeff' be non-empty.";
00014         NasaCommonLogging::Logger::log(logCategory, log4cpp::Priority::FATAL, err.str());
00015         throw std::invalid_argument(err.str());
00016     }
00017 
00018     setParameters();
00019 
00020     // Initialize variables
00021     lastControlModeMatchTime = lastCommandModeMatchTime = lastCalibrationModeMatchTime = lastClearFaultModeMatchTime = ros::Time::now();
00022 
00023 
00024     // Instantiate state machines
00025     jointControlActual  = boost::make_shared<JointControlActualWrist> (mechanism, ioFunctions);
00026     jointControlCommand = boost::make_shared<JointControlCommandWrist>(mechanism, ioFunctions);
00027 }
00028 
00029 JointControlManagerWrist::~JointControlManagerWrist()
00030 {
00031     // Nothing
00032 }
00033 
00034 void JointControlManagerWrist::setParameters()
00035 {
00037     defaultStates.controlMode.state     = r2_msgs::JointControlMode::OFF;
00038     defaultStates.commandMode.state     = r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH;
00039     defaultStates.calibrationMode.state = r2_msgs::JointControlCalibrationMode::UNCALIBRATED;
00040     defaultStates.clearFaultMode.state  = r2_msgs::JointControlClearFaultMode::DISABLE;
00041     defaultStates.coeffState.state      = r2_msgs::JointControlCoeffState::NOTLOADED;
00042 
00044     prevActualStates = defaultStates;
00045     actualStates = defaultStates;
00046     commandStates = defaultStates;
00047 }
00048 
00049 r2_msgs::JointControlData JointControlManagerWrist::getActualStates(void)
00050 {
00051     prevActualStates = actualStates;
00052     jointControlActual->getStates(actualStates);
00053 
00055     if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00056     {
00057         actualStates = defaultStates;
00058     }
00060     else
00061     {
00063         switch (actualStates.controlMode.state)
00064         {
00065             case r2_msgs::JointControlMode::OFF:
00066                 if (actualStates.calibrationMode.state == r2_msgs::JointControlCalibrationMode::DISABLE)
00067                 {
00068                     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::NOTICE << "JointControlCalibrationMode::DISABLE while in JointControlMode::OFF -- automatic transition to JointControlMode::PARK on joint: " << mechanism;
00069                     jointControlCommand->park();
00070                 }
00071 
00072                 break;
00073 
00074             case r2_msgs::JointControlMode::DRIVE:
00075                 if (actualStates.calibrationMode.state != r2_msgs::JointControlCalibrationMode::DISABLE)
00076                 {
00077                     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Cannot calibrate in DRIVE on " << mechanism << "; setting to park()";
00078                     jointControlCommand->park();
00079                 }
00080 
00081                 break;
00082 
00083             case r2_msgs::JointControlMode::PARK:
00084             case r2_msgs::JointControlMode::NEUTRAL:
00085             case r2_msgs::JointControlMode::BOOTLOADER:
00086             case r2_msgs::JointControlMode::INVALID:
00087             case r2_msgs::JointControlMode::IGNORE:
00088             default:
00089                 break;
00090         }
00091 
00092         switch (actualStates.calibrationMode.state)
00093         {
00094             case r2_msgs::JointControlCalibrationMode::DISABLE:
00095                 if (commandStates.calibrationMode.state != r2_msgs::JointControlCalibrationMode::DISABLE)
00096                 {
00097                     jointControlCommand->disableCalibrationMode();
00098                 }
00099 
00100                 break;
00101 
00102             case r2_msgs::JointControlCalibrationMode::ENABLE:
00103             case r2_msgs::JointControlCalibrationMode::IGNORE:
00104             default:
00105                 break;
00106         }
00107     }
00108 
00109     return actualStates;
00110 }
00111 
00112 r2_msgs::JointControlData JointControlManagerWrist::getCommandStates(void)
00113 {
00114     jointControlCommand->getStates(commandStates);
00115     return commandStates;
00116 }
00117 
00118 void JointControlManagerWrist::setCommandStates(const r2_msgs::JointControlData& command)
00119 {
00120     switch (command.controlMode.state)
00121     {
00122         case r2_msgs::JointControlMode::BOOTLOADER:
00123 
00125             if ( (powerState.data == r2_msgs::PowerState::MOTOR48_POWER) ||
00126                     (powerState.data == r2_msgs::PowerState::MOTOR96_POWER) ||
00127                     (powerState.data == r2_msgs::PowerState::MOTOR_POWER))
00128             {
00129                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00130                 break;
00131             }
00132 
00135             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED    ||
00136                      actualStates.controlMode.state == r2_msgs::JointControlMode::BOOTLOADER ||
00137                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF))
00138             {
00139                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00140                 break;
00141             }
00142 
00143             jointControlCommand->bootLoader();
00144             break;
00145 
00146         case r2_msgs::JointControlMode::OFF:
00147 
00149             if ((powerState.data == r2_msgs::PowerState::MOTOR48_POWER) ||
00150                     (powerState.data == r2_msgs::PowerState::MOTOR96_POWER) ||
00151                     (powerState.data == r2_msgs::PowerState::MOTOR_POWER))
00152             {
00153                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00154                 break;
00155             }
00156 
00158             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED    ||
00159                      actualStates.controlMode.state == r2_msgs::JointControlMode::BOOTLOADER ||
00160                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF))
00161             {
00162                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00163                 break;
00164             }
00165 
00166             jointControlCommand->off();
00167             break;
00168 
00169         case r2_msgs::JointControlMode::PARK:
00170 
00172             if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00173             {
00174                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00175                 break;
00176             }
00177 
00179             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED ||
00180                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF     ||
00181                      actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00182                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL ||
00183                      actualStates.controlMode.state == r2_msgs::JointControlMode::DRIVE))
00184             {
00185                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00186                 break;
00187             }
00188 
00189             if (actualStates.controlMode.state == r2_msgs::JointControlMode::OFF)
00190             {
00192                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Enabling calibration on transition to PARK from OFF on " <<  mechanism;
00193                 jointControlCommand->enableCalibrationMode();
00194             }
00195 
00196             jointControlCommand->park();
00197             break;
00198 
00199         case r2_msgs::JointControlMode::NEUTRAL:
00200 
00202             if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00203             {
00204                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00205                 break;
00206             }
00207 
00209             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED ||
00210                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF     ||
00211                      actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00212                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL))
00213             {
00214                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00215                 break;
00216             }
00217 
00218             if (actualStates.controlMode.state == r2_msgs::JointControlMode::OFF)
00219             {
00221                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Enabling calibration on transition to NEUTRAL from OFF on " <<  mechanism;
00222                 jointControlCommand->enableCalibrationMode();
00223             }
00224 
00225             jointControlCommand->neutral();
00226             break;
00227 
00228         case r2_msgs::JointControlMode::DRIVE:
00229 
00231             if (powerState.faulted ||
00232                     powerState.data == r2_msgs::PowerState::LOGIC_POWER ||
00233                     powerState.data == r2_msgs::PowerState::POWER_OFF ||
00234                     powerState.data == r2_msgs::PowerState::TRANSITIONING ||
00235                     powerState.data == r2_msgs::PowerState::UNKNOWN)
00236             {
00237                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current power state: " << RosMsgPrinter::powerStateToString(powerState);
00238                 break;
00239             }
00240 
00242             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00243                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL ||
00244                      actualStates.controlMode.state == r2_msgs::JointControlMode::DRIVE))
00245             {
00246                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::" << RosMsgPrinter::jointControlModeToString(command.controlMode) << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00247                 break;
00248             }
00249 
00251             if (actualStates.calibrationMode.state != r2_msgs::JointControlCalibrationMode::DISABLE)
00252             {
00253                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlMode::DRIVE on mechanism: " << mechanism << " can not occur until it has been calibrated";
00254                 break;
00255             }
00256 
00257             jointControlCommand->drive();
00258             break;
00259 
00260         default:
00261             // nothing
00262             break;
00263     }
00264 
00266     switch (command.commandMode.state)
00267     {
00268         case r2_msgs::JointControlCommandMode::MOTCOM:
00269             if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00270             {
00271                 jointControlCommand->motCom();
00272             }
00273             else
00274             {
00275                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlCommandMode::MOTCOM not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00276             }
00277 
00278             break;
00279 
00280         case r2_msgs::JointControlCommandMode::STALLMODE:
00281             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Command transition to JointControlCommandMode::STALLMODE not allowed on joint: " << mechanism;
00282             break;
00283 
00284         case r2_msgs::JointControlCommandMode::MULTILOOPSTEP:
00285             if ((actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH) ||
00286                     (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP))
00287             {
00288                 jointControlCommand->multiLoopStep();
00289             }
00290             else if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00291             {
00292                 jointControlCommand->multiLoopStep();
00293             }
00294             else
00295             {
00296                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00297                         << "current control mode:" << RosMsgPrinter::jointControlModeToString(actualStates.controlMode)
00298                         << "current command mode: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00299             }
00300 
00301             break;
00302 
00303         case r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH:
00304             if ((actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH) ||
00305                     (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP))
00306             {
00307                 jointControlCommand->multiLoopSmooth();
00308             }
00309             else if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00310             {
00311                 jointControlCommand->multiLoopSmooth();
00312             }
00313             else
00314             {
00315                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00316                         << "current control mode:" << RosMsgPrinter::jointControlModeToString(actualStates.controlMode)
00317                         << "current command mode: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00318             }
00319 
00320             break;
00321 
00322         case r2_msgs::JointControlCommandMode::ACTUATOR:
00323             if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00324             {
00325                 jointControlCommand->actuator();
00326             }
00327             else
00328             {
00329                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlCommandMode::ACTUATOR not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00330             }
00331 
00332             break;
00333 
00334         default:
00335             // nothing
00336             break;
00337     }
00338 
00339     switch (command.calibrationMode.state)
00340     {
00341         case r2_msgs::JointControlCalibrationMode::DISABLE:
00342             jointControlCommand->disableCalibrationMode();
00343             break;
00344 
00345         case r2_msgs::JointControlCalibrationMode::ENABLE:
00346 
00348             if (actualStates.controlMode.state == r2_msgs::JointControlMode::DRIVE)
00349             {
00350                 jointControlCommand->park();
00351                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::NOTICE << "Cannot calibrate while in DRIVE, Automatic transition to PARK on joint: " << mechanism;
00352             }
00353 
00354             jointControlCommand->enableCalibrationMode();
00355             break;
00356 
00357         default:
00358             // nothing
00359             break;
00360     }
00361 
00362     switch (command.clearFaultMode.state)
00363     {
00364         case r2_msgs::JointControlClearFaultMode::DISABLE:
00365             jointControlCommand->disableClearFaultMode();
00366             break;
00367 
00368         case r2_msgs::JointControlClearFaultMode::ENABLE:
00369             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Can not enable ClearFaultMode on " << mechanism << ", ignoring";
00370             break;
00371 
00372         default:
00373             // nothing
00374             break;
00375     }
00376 }
00377 
00378 bool JointControlManagerWrist::verifyStates(void)
00379 {
00380     return verifyControlModeState() && verifyCommandModeState() && verifyCalibrationModeState() && verifyClearFaultModeState();
00381 }
00382 
00383 bool JointControlManagerWrist::verifyControlModeState(void)
00384 {
00385     if (actualStates.controlMode.state == commandStates.controlMode.state)
00386     {
00387         lastControlModeMatchTime = ros::Time::now();
00388         return true;
00389     }
00390 
00391     // Handle faults
00392     if (actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED)
00393     {
00394         if (prevActualStates.controlMode.state != r2_msgs::JointControlMode::FAULTED)
00395         {
00396             NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Joint faulted: " << mechanism;
00397         }
00398 
00399         return false;
00400     }
00401 
00402     // Handle other state mismatches
00403     ros::Duration timeSinceLastMatch = ros::Time::now() - lastControlModeMatchTime;
00404 
00405     if (timeSinceLastMatch.toSec() > timeLimit)
00406     {
00407         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "Timed out waiting for " << RosMsgPrinter::jointControlModeToString(commandStates.controlMode) << " received " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode) << " on mechanism: " << mechanism;
00408         return false;
00409     }
00410 
00411     return true;
00412 }
00413 
00414 bool JointControlManagerWrist::verifyCommandModeState(void)
00415 {
00416     if (actualStates.commandMode.state == commandStates.commandMode.state)
00417     {
00418         lastCommandModeMatchTime = ros::Time::now();
00419         return true;
00420     }
00421 
00422     // Handle state mismatches
00423     ros::Duration timeSinceLastMatch = ros::Time::now() - lastCommandModeMatchTime;
00424 
00425     if (timeSinceLastMatch.toSec() > timeLimit)
00426     {
00427         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "verifyCommandModeState: Timed out waiting for " << RosMsgPrinter::jointControlCommandModeToString(commandStates.commandMode) << " received " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode) << " on mechanism: " << mechanism;
00428         return false;
00429     }
00430 
00431     return true;
00432 }
00433 
00434 bool JointControlManagerWrist::verifyCalibrationModeState(void)
00435 {
00436     if (actualStates.calibrationMode.state == commandStates.calibrationMode.state)
00437     {
00438         lastCalibrationModeMatchTime = ros::Time::now();
00439         return true;
00440     }
00441 
00442     // Handle state mismatches
00443     ros::Duration timeSinceLastMatch = ros::Time::now() - lastCalibrationModeMatchTime;
00444 
00445     if (timeSinceLastMatch.toSec() > timeLimit)
00446     {
00447         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "verifyCalibrationModeState: Timed out waiting for " << RosMsgPrinter::jointControlCalibrationModeToString(commandStates.calibrationMode) << " received " << RosMsgPrinter::jointControlCalibrationModeToString(actualStates.calibrationMode) << " on mechanism: " << mechanism;
00448         return false;
00449     }
00450 
00451     return true;
00452 }
00453 
00454 bool JointControlManagerWrist::verifyClearFaultModeState(void)
00455 {
00456     if (actualStates.clearFaultMode.state == commandStates.clearFaultMode.state)
00457     {
00458         lastClearFaultModeMatchTime = ros::Time::now();
00459         return true;
00460     }
00461 
00462     // Handle state mismatches
00463     ros::Duration timeSinceLastMatch = ros::Time::now() - lastClearFaultModeMatchTime;
00464 
00465     if (timeSinceLastMatch.toSec() > timeLimit)
00466     {
00467         NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::INFO << "verifyClearFaultModeState: Timed out waiting for " << RosMsgPrinter::jointControlClearFaultModeToString(commandStates.clearFaultMode) << " received " << RosMsgPrinter::jointControlClearFaultModeToString(actualStates.clearFaultMode) << " on mechanism: " << mechanism;
00468         return false;
00469     }
00470 
00471     return true;
00472 }
00473 
00474 /***************************************************************************/
00479 diagnostic_msgs::DiagnosticStatus JointControlManagerWrist::getFaults()
00480 {
00481     diagnostic_msgs::DiagnosticStatus faultStatus;
00482     jointControlActual->getFaults(faultStatus);
00483     return faultStatus;
00484 }