robodyn_mechanisms: JointControlManagerFinger.cpp Source File

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