robodyn_mechanisms: JointControlManagerSeriesElastic.cpp Source File

Go to the documentation of this file.
00001 
00005 #include "robodyn_mechanisms/JointControlManagerSeriesElastic.h"
00006 
00007 JointControlManagerSeriesElastic::JointControlManagerSeriesElastic(const std::string& mechanism, IoFunctions ioFunctions, double timeLimit, const std::string& type, NodeRegisterManagerPtr nodeRegisterManager)
00008     : JointControlManagerInterface(mechanism, ioFunctions, timeLimit, type), nodeRegisterManager(nodeRegisterManager), logCategory("gov.nasa.JointControlManagerSeriesElastic")
00009 {
00010     setParameters();
00011 
00012     // Initialize variables
00013     lastControlModeMatchTime = lastCommandModeMatchTime = lastCalibrationModeMatchTime = lastClearFaultModeMatchTime = ros::Time::now();
00014 
00015     // Instantiate state machines
00016     jointControlActual  = boost::make_shared<JointControlActualSeriesElastic> (mechanism, ioFunctions, nodeRegisterManager);
00017     jointControlCommand = boost::make_shared<JointControlCommandSeriesElastic>(mechanism, ioFunctions, nodeRegisterManager);
00018 
00020     enable96VHeadLedsName = "Enable96VHeadLeds";
00021     // Set constant control register bits for this type of joint
00022     //    nodeRegisterManager->setControlValue(mechanism, InitAPS2toAPS1ControlName, 0); // We think this is used for neck
00023 }
00024 
00025 JointControlManagerSeriesElastic::~JointControlManagerSeriesElastic()
00026 {
00027     // Nothing
00028 }
00029 
00030 void JointControlManagerSeriesElastic::setParameters()
00031 {
00033     defaultStates.controlMode.state     = r2_msgs::JointControlMode::OFF;
00034     defaultStates.commandMode.state     = r2_msgs::JointControlCommandMode::MOTCOM;
00035     defaultStates.calibrationMode.state = r2_msgs::JointControlCalibrationMode::DISABLE;
00036     defaultStates.clearFaultMode.state  = r2_msgs::JointControlClearFaultMode::DISABLE;
00037     defaultStates.coeffState.state      = r2_msgs::JointControlCoeffState::NOTLOADED;
00038 
00040     prevActualStates = defaultStates;
00041     actualStates = defaultStates;
00042     commandStates = defaultStates;
00043 }
00044 
00045 r2_msgs::JointControlData JointControlManagerSeriesElastic::getActualStates(void)
00046 {
00047     prevActualStates = actualStates;
00048     jointControlActual->getStates(actualStates);
00049 
00050     if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00051     {
00052         actualStates = defaultStates;
00053     }
00054 
00056     if (powerState.data == r2_msgs::PowerState::MOTOR48_POWER ||
00057             powerState.data == r2_msgs::PowerState::MOTOR96_POWER ||
00058             powerState.data == r2_msgs::PowerState::MOTOR_POWER)
00059     {
00060         if (nodeRegisterManager->hasControlProperty(mechanism, enable96VHeadLedsName))
00061         {
00062             nodeRegisterManager->setControlValue(mechanism, enable96VHeadLedsName, 1);
00063         }
00064     }
00065     else
00066     {
00067         if (nodeRegisterManager->hasControlProperty(mechanism, enable96VHeadLedsName))
00068         {
00069             nodeRegisterManager->setControlValue(mechanism, enable96VHeadLedsName, 0);
00070         }
00071     }
00072 
00074     if (actualStates.clearFaultMode.state == r2_msgs::JointControlClearFaultMode::ENABLE &&
00075             commandStates.clearFaultMode.state == r2_msgs::JointControlClearFaultMode::ENABLE)
00076     {
00077         jointControlCommand->disableClearFaultMode();
00078     }
00079 
00080     return actualStates;
00081 }
00082 
00083 r2_msgs::JointControlData JointControlManagerSeriesElastic::getCommandStates(void)
00084 {
00085     jointControlCommand->getStates(commandStates);
00086     return commandStates;
00087 }
00088 
00089 void JointControlManagerSeriesElastic::setCommandStates(const r2_msgs::JointControlData& command)
00090 {
00091     switch (command.controlMode.state)
00092     {
00093         case r2_msgs::JointControlMode::BOOTLOADER:
00094 
00096             if ( (powerState.data == r2_msgs::PowerState::MOTOR48_POWER) ||
00097                     (powerState.data == r2_msgs::PowerState::MOTOR96_POWER) ||
00098                     (powerState.data == r2_msgs::PowerState::MOTOR_POWER))
00099             {
00100                 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);
00101                 break;
00102             }
00103 
00106             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED    ||
00107                      actualStates.controlMode.state == r2_msgs::JointControlMode::BOOTLOADER ||
00108                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF))
00109             {
00110                 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);
00111                 break;
00112             }
00113 
00114             jointControlCommand->bootLoader();
00115             break;
00116 
00117         case r2_msgs::JointControlMode::OFF:
00118 
00120             if ((powerState.data == r2_msgs::PowerState::MOTOR48_POWER) ||
00121                     (powerState.data == r2_msgs::PowerState::MOTOR96_POWER) ||
00122                     (powerState.data == r2_msgs::PowerState::MOTOR_POWER))
00123             {
00124                 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);
00125                 break;
00126             }
00127 
00129             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED    ||
00130                      actualStates.controlMode.state == r2_msgs::JointControlMode::BOOTLOADER ||
00131                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF))
00132             {
00133                 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);
00134                 break;
00135             }
00136 
00137             jointControlCommand->off();
00138             break;
00139 
00140         case r2_msgs::JointControlMode::PARK:
00141 
00143             if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00144             {
00145                 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);
00146                 break;
00147             }
00148 
00150             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED ||
00151                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF     ||
00152                      actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00153                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL ||
00154                      actualStates.controlMode.state == r2_msgs::JointControlMode::DRIVE))
00155             {
00156                 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);
00157                 break;
00158             }
00159 
00160             jointControlCommand->park();
00161             break;
00162 
00163         case r2_msgs::JointControlMode::NEUTRAL:
00164 
00166             if (powerState.data == r2_msgs::PowerState::POWER_OFF)
00167             {
00168                 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);
00169                 break;
00170             }
00171 
00173             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::FAULTED ||
00174                      actualStates.controlMode.state == r2_msgs::JointControlMode::OFF     ||
00175                      actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00176                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL))
00177             {
00178                 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);
00179                 break;
00180             }
00181 
00182             jointControlCommand->neutral();
00183             break;
00184 
00185         case r2_msgs::JointControlMode::DRIVE:
00186 
00188             if (powerState.faulted ||
00189                     powerState.data == r2_msgs::PowerState::LOGIC_POWER ||
00190                     powerState.data == r2_msgs::PowerState::POWER_OFF ||
00191                     powerState.data == r2_msgs::PowerState::TRANSITIONING ||
00192                     powerState.data == r2_msgs::PowerState::UNKNOWN)
00193             {
00194                 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);
00195                 break;
00196             }
00197 
00199             if ( not(actualStates.controlMode.state == r2_msgs::JointControlMode::PARK    ||
00200                      actualStates.controlMode.state == r2_msgs::JointControlMode::NEUTRAL ||
00201                      actualStates.controlMode.state == r2_msgs::JointControlMode::DRIVE))
00202             {
00203                 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);
00204                 break;
00205             }
00206 
00207             jointControlCommand->drive();
00208             break;
00209 
00210         default:
00211             // nothing
00212             break;
00213     }
00214 
00215     switch (command.commandMode.state)
00216     {
00217         case r2_msgs::JointControlCommandMode::MOTCOM:
00218             if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00219             {
00220                 jointControlCommand->motCom();
00221             }
00222             else
00223             {
00224                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode)
00225                         << " not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode);
00226             }
00227 
00228             break;
00229 
00230         case r2_msgs::JointControlCommandMode::STALLMODE:
00231             if ((actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE) &&
00232                     (actualStates.coeffState.state  == r2_msgs::JointControlCoeffState::LOADED))
00233             {
00234                 jointControlCommand->stallMode();
00235             }
00236             else
00237             {
00238                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00239                         << ", current control mode: " << RosMsgPrinter::jointControlModeToString(actualStates.controlMode)
00240                         << ", current coeffs state: " << RosMsgPrinter::jointControlCoeffsLoadedStateToString(actualStates.coeffState);
00241             }
00242 
00243             break;
00244 
00245         case r2_msgs::JointControlCommandMode::MULTILOOPSTEP:
00246             if (actualStates.coeffState.state  == r2_msgs::JointControlCoeffState::LOADED)
00247             {
00248                 if ((actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH) ||
00249                         (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP))
00250                 {
00251                     jointControlCommand->multiLoopStep();
00252                 }
00253                 else if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00254                 {
00255                     jointControlCommand->multiLoopStep();
00256                 }
00257                 else
00258                 {
00259                     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00260                             << "current control mode:" << RosMsgPrinter::jointControlModeToString(actualStates.controlMode)
00261                             << "current command mode: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00262                 }
00263             }
00264             else
00265             {
00266                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00267                         << ", current coeffs state: " << RosMsgPrinter::jointControlCoeffsLoadedStateToString(actualStates.coeffState);
00268             }
00269 
00270             break;
00271 
00272         case r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH:
00273             if (actualStates.coeffState.state  == r2_msgs::JointControlCoeffState::LOADED)
00274             {
00275                 if ((actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSMOOTH) ||
00276                         (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MULTILOOPSTEP))
00277                 {
00278                     jointControlCommand->multiLoopSmooth();
00279                 }
00280                 else if (actualStates.controlMode.state != r2_msgs::JointControlMode::DRIVE)
00281                 {
00282                     jointControlCommand->multiLoopSmooth();
00283                 }
00284                 else
00285                 {
00286                     NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00287                             << "current control mode:" << RosMsgPrinter::jointControlModeToString(actualStates.controlMode)
00288                             << "current command mode: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00289                 }
00290             }
00291             else
00292             {
00293                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to " << RosMsgPrinter::jointControlCommandModeToString(command.commandMode) << " not allowed on joint: " << mechanism
00294                         << ", current coeffs state: " << RosMsgPrinter::jointControlCoeffsLoadedStateToString(actualStates.coeffState);
00295             }
00296 
00297             break;
00298 
00299         default:
00300             // nothing
00301             break;
00302     }
00303 
00304     switch (command.calibrationMode.state)
00305     {
00306         case r2_msgs::JointControlCalibrationMode::DISABLE:
00307             if (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MOTCOM)
00308             {
00309                 jointControlCommand->disableCalibrationMode();
00310             }
00311             else
00312             {
00313                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlCalibrationMode::DISABLE not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00314             }
00315 
00316             break;
00317 
00318         case r2_msgs::JointControlCalibrationMode::ENABLE:
00319             if (actualStates.commandMode.state == r2_msgs::JointControlCommandMode::MOTCOM)
00320             {
00321                 jointControlCommand->enableCalibrationMode();
00322             }
00323             else
00324             {
00325                 NasaCommonLogging::Logger::getCategory(logCategory) << log4cpp::Priority::WARN << "Command transition to JointControlCalibrationMode::ENABLE not allowed on joint: " << mechanism << ", current actual state: " << RosMsgPrinter::jointControlCommandModeToString(actualStates.commandMode);
00326             }
00327 
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             jointControlCommand->enableClearFaultMode();
00343             break;
00344 
00345         default:
00346             // nothing
00347             break;
00348     }
00349 }
00350 
00351 bool JointControlManagerSeriesElastic::verifyStates(void)
00352 {
00353     return verifyControlModeState() && verifyCommandModeState() && verifyCalibrationModeState() && verifyClearFaultModeState();
00354 }
00355 
00356 bool JointControlManagerSeriesElastic::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 JointControlManagerSeriesElastic::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 JointControlManagerSeriesElastic::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 JointControlManagerSeriesElastic::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 
00442         if (actualStates.clearFaultMode.state == r2_msgs::JointControlClearFaultMode::DISABLE)
00443         {
00444             jointControlCommand->disableClearFaultMode();
00445         }
00446 
00447         return false;
00448     }
00449 
00450     return true;
00451 }
00452 
00453 diagnostic_msgs::DiagnosticStatus JointControlManagerSeriesElastic::getFaults()
00454 {
00455     diagnostic_msgs::DiagnosticStatus faultStatus;
00456     jointControlActual->getFaults(faultStatus);
00457     return faultStatus;
00458 }