JointCommandWrist.cpp
Go to the documentation of this file.
00001 #include "robodyn_mechanisms/JointCommandWrist.h"
00002 #include "r2_msgs/JointControlDataArray.h"
00003 
00004 using namespace std;
00005 using namespace log4cpp;
00006 using namespace Eigen;
00007 
00008 JointCommandWrist::JointCommandWrist(const std::string& mechanism, IoFunctions ioFunctions)
00009     : JointCommandInterface(mechanism, ioFunctions)
00010     , desiredPitchVel(0.f)
00011     , desiredYawVel(0.f)
00012     , sliderFilterInitialized(false)
00013     , hallFilterInitialized(false)
00014     , prevPosInitialized(false)
00015     , useHalls(true)
00016     , calFailureReported(false)
00017     , completeMessageSize(12)
00018 {
00019     if (mechanism == "")
00020     {
00021         stringstream err;
00022         err << "Constructor requires mechanism be non-empty.";
00023         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00024         throw invalid_argument(err.str());
00025     }
00026 
00027     if (io.setInt16.empty())
00028     {
00029         stringstream err;
00030         err << "Constructor requires 'io.setInt16' to be non-empty.";
00031         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00032         throw invalid_argument(err.str());
00033     }
00034 
00035     if (io.getInt16.empty())
00036     {
00037         stringstream err;
00038         err << "Constructor requires 'io.getInt16' to be non-empty.";
00039         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00040         throw invalid_argument(err.str());
00041     }
00042 
00043     if (io.getUInt16.empty())
00044     {
00045         stringstream err;
00046         err << "Constructor requires 'io.getUInt16' to be non-empty.";
00047         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00048         throw invalid_argument(err.str());
00049     }
00050 
00051     if (io.getBrainstemCoeff.empty())
00052     {
00053         stringstream err;
00054         err << "Constructor requires 'io.getBrainstemCoeff' to be non-empty.";
00055         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00056         throw invalid_argument(err.str());
00057     }
00058 
00059     if (io.getMotorCoeff.empty())
00060     {
00061         stringstream err;
00062         err << "Constructor requires 'io.getMotorCoeff' to be non-empty.";
00063         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00064         throw invalid_argument(err.str());
00065     }
00066 
00067     if (io.getMotorCoeff.empty())
00068     {
00069         stringstream err;
00070         err << "Constructor requires 'io.getMotorCoeff' to be non-empty.";
00071         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00072         throw invalid_argument(err.str());
00073     }
00074 
00075     if (io.hasLiveCoeff.empty())
00076     {
00077         stringstream err;
00078         err << "Constructor requires 'io.hasLiveCoeff' to be non-empty.";
00079         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00080         throw invalid_argument(err.str());
00081     }
00082 
00083     if (io.getLiveCoeff.empty())
00084     {
00085         stringstream err;
00086         err << "Constructor requires 'io.getLiveCoeff' to be non-empty.";
00087         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00088         throw invalid_argument(err.str());
00089     }
00090 
00091     if (io.setLiveCoeff.empty())
00092     {
00093         stringstream err;
00094         err << "Constructor requires 'io.setLiveCoeff' to be non-empty.";
00095         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00096         throw invalid_argument(err.str());
00097     }
00098 
00099     if (io.getJointNames.empty())
00100     {
00101         stringstream err;
00102         err << "Constructor requires 'io.getJointNames' to be non-empty.";
00103         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00104         throw invalid_argument(err.str());
00105     }
00106 
00107     if (io.getActuatorNames.empty())
00108     {
00109         stringstream err;
00110         err << "Constructor requires 'io.getActuatorNames' to be non-empty.";
00111         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00112         throw invalid_argument(err.str());
00113     }
00114 
00115     if (io.getCommandFile.empty())
00116     {
00117         stringstream err;
00118         err << "Constructor requires 'io.getCommandFile' to be non-empty.";
00119         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00120         throw invalid_argument(err.str());
00121     }
00122 
00123     if (io.hasParam.empty())
00124     {
00125         stringstream err;
00126         err << "Constructor requires 'io.hasParam' to be non-empty.";
00127         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00128         throw invalid_argument(err.str());
00129     }
00130 
00131     if (io.getDoubleParam.empty())
00132     {
00133         stringstream err;
00134         err << "Constructor requires 'io.getDoubleParam' to be non-empty.";
00135         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00136         throw invalid_argument(err.str());
00137     }
00138 
00139     if (io.setDoubleParam.empty())
00140     {
00141         stringstream err;
00142         err << "Constructor requires 'io.setDoubleParam' to be non-empty.";
00143         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00144         throw invalid_argument(err.str());
00145     }
00146 
00147     if (io.getBoolParam.empty())
00148     {
00149         stringstream err;
00150         err << "Constructor requires 'io.getBoolParam' to be non-empty.";
00151         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00152         throw invalid_argument(err.str());
00153     }
00154 
00155     if (io.setBoolParam.empty())
00156     {
00157         stringstream err;
00158         err << "Constructor requires 'io.setBoolParam' to be non-empty.";
00159         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00160         throw invalid_argument(err.str());
00161     }
00162 
00163     roboDynJoints    = io.getJointNames(mechanism);
00164     roboDynActuators = io.getActuatorNames(mechanism);
00165 
00166 
00167     if (roboDynJoints.size() != 2 or
00168             roboDynActuators.size() != 2)
00169     {
00170         stringstream err;
00171         err << "Constructor requires roboDynJoints and roboDynActuators to have 2 values";
00172         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00173         throw invalid_argument(err.str());
00174 
00175     }
00176 
00177     if (roboDynJoints[0].find("pitch", 0) == string::npos)
00178     {
00179         stringstream err;
00180         err << "Expected pitch element, got " << roboDynJoints[0] << "." << std::endl;
00181         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00182         throw invalid_argument(err.str());
00183     }
00184 
00185     if (roboDynJoints[1].find("yaw", 0) == string::npos)
00186     {
00187         stringstream err;
00188         err << "Expected yaw element, got " << roboDynJoints[1] << "." << std::endl;
00189         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00190         throw invalid_argument(err.str());
00191     }
00192 
00193     if (roboDynActuators[0].find("thumbside", 0) == string::npos)
00194     {
00195         stringstream err;
00196         err << "Expected thumbside element, got " << roboDynActuators[0] << "." << std::endl;
00197         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00198         throw invalid_argument(err.str());
00199     }
00200 
00201     if (roboDynActuators[1].find("littleside", 0) == string::npos)
00202     {
00203         stringstream err;
00204         err << "Expected littlside element, got " << roboDynActuators[1] << "." << std::endl;
00205         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00206         throw invalid_argument(err.str());
00207     }
00208 
00209     wrist.reset(new WristMechanism());
00210     wristFault.reset(new WristMechanism());
00211     q1Controller.reset(new MultiLoopController());
00212     q2Controller.reset(new MultiLoopController());
00213 
00214     loadCoeffs();
00215 }
00216 
00217 JointCommandWrist::~JointCommandWrist()
00218 {
00219 
00220 }
00221 
00222 void JointCommandWrist::loadCoeffs()
00223 {
00224     io.setLiveCoeff(nameCoeffState, r2_msgs::JointControlCoeffState::NOTLOADED);
00225 
00226     desiredPitchVel         = 0.f;
00227     desiredYawVel           = 0.f;
00228     sliderFilterInitialized = false;
00229     hallFilterInitialized   = false;
00230     prevPosInitialized      = false;
00231     calFailureReported      = false;
00232 
00233     std::string parameterFile = io.getCommandFile(mechanism);
00234 
00236     namePitchLimit            = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "PitchLimit");
00237     nameYawLimit              = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "YawLimit");
00238     nameLittlesideSliderLimit = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "LittlesideSliderLimit");
00239     nameThumbsideSliderLimit  = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "ThumbsideSliderLimit");
00240     nameSensorError           = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "SensorError");
00241     nameSliderDiffError       = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "SliderDiffError");
00242     nameCalibrationMode       = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "CalibrationMode");
00243     nameCoeffState            = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "CoeffState");
00244 
00245     namePitchMaxValue         = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "PitchMaxValue");
00246     namePitchMinValue         = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "PitchMinValue");
00247     nameYawMaxValue           = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "YawMaxValue");
00248     nameYawMinValue           = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "YawMinValue");
00249     namePitchHallsValue       = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "PitchHallsValue");
00250     namePitchCalcValue        = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "PitchCalcValue");
00251     nameYawHallsValue         = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "YawHallsValue");
00252     nameYawCalcValue          = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "YawCalcValue");
00253 
00254 
00256     simpleMeasuredStateMsg.name.resize(2);
00257     simpleMeasuredStateMsg.position.resize(2, 0.);
00258     simpleMeasuredStateMsg.velocity.resize(2, 0.);
00259     simpleMeasuredStateMsg.effort.resize(2, 0.);
00260 
00261     simpleMeasuredStateMsg.name[0] = roboDynJoints[0];
00262     simpleMeasuredStateMsg.name[1] = roboDynJoints[1];
00263 
00264     completeMeasuredStateMsg.name.resize(completeMessageSize);
00265     completeMeasuredStateMsg.position.resize(completeMessageSize, 0.);
00266     completeMeasuredStateMsg.velocity.resize(completeMessageSize, 0.);
00267     completeMeasuredStateMsg.effort.resize(completeMessageSize, 0.);
00268 
00269     completeMeasuredStateMsg.name[0]  = roboDynJoints[0];
00270     completeMeasuredStateMsg.name[1]  = roboDynJoints[1];
00271     completeMeasuredStateMsg.name[2]  = roboDynActuators[0];
00272     completeMeasuredStateMsg.name[3]  = roboDynActuators[1];
00273     completeMeasuredStateMsg.name[4]  = roboDynJoints.at(0) +  StringUtilities::TOKEN_DELIMITER + string("halls");
00274     completeMeasuredStateMsg.name[5]  = roboDynJoints.at(1) +  StringUtilities::TOKEN_DELIMITER + string("halls");
00275     completeMeasuredStateMsg.name[6]  = roboDynActuators.at(0) + StringUtilities::TOKEN_DELIMITER + string("encoder");
00276     completeMeasuredStateMsg.name[7]  = roboDynActuators.at(1) + StringUtilities::TOKEN_DELIMITER + string("encoder");
00277     completeMeasuredStateMsg.name[8]  = roboDynJoints.at(0) +  StringUtilities::TOKEN_DELIMITER + string("embeddedCommand");
00278     completeMeasuredStateMsg.name[9]  = roboDynJoints.at(1) +  StringUtilities::TOKEN_DELIMITER + string("embeddedCommand");
00279     completeMeasuredStateMsg.name[10] = roboDynActuators.at(0) + StringUtilities::TOKEN_DELIMITER + string("embeddedCommand");
00280     completeMeasuredStateMsg.name[11] = roboDynActuators.at(1) + StringUtilities::TOKEN_DELIMITER + string("embeddedCommand");
00281 
00282 
00283     positionVals.resize(completeMessageSize, 0.);
00284     velocityVals.resize(completeMessageSize, 0.);
00285 
00286     commandedStateMsg.name.resize(2);
00287     commandedStateMsg.desiredPosition.resize(2, 0.);
00288     commandedStateMsg.desiredPositionVelocityLimit.resize(2, 0.);
00289     commandedStateMsg.feedForwardTorque.resize(2, 0.);
00290     commandedStateMsg.proportionalGain.resize(2, 0.);
00291     commandedStateMsg.derivativeGain.resize(2, 0.);
00292     commandedStateMsg.integralGain.resize(2, 0.);
00293     commandedStateMsg.positionLoopTorqueLimit.resize(2, 0.);
00294     commandedStateMsg.positionLoopWindupLimit.resize(2, 0.);
00295     commandedStateMsg.torqueLoopVelocityLimit.resize(2, 0.);
00296 
00297     commandedStateMsg.name[0]            = roboDynJoints[0];
00298     commandedStateMsg.name[1]            = roboDynJoints[1];
00299     commandedStateMsg.desiredPosition[0] = 0;
00300     commandedStateMsg.desiredPosition[1] = 0;
00301 
00302     jointCapabilityMsg.name.resize(2);
00303     jointCapabilityMsg.positionLimitMax.resize(2, 0.);
00304     jointCapabilityMsg.positionLimitMin.resize(2, 0.);
00305     jointCapabilityMsg.velocityLimit.resize(2, 30.);  
00306     jointCapabilityMsg.torqueLimit.resize(2, 0.);
00307 
00308     jointCapabilityMsg.name[0] = roboDynJoints[0];
00309     jointCapabilityMsg.name[1] = roboDynJoints[1];
00310 
00312     TiXmlDocument file(parameterFile.c_str());
00313     bool loadOkay = file.LoadFile();
00314 
00315     if (!loadOkay)
00316     {
00317         stringstream err;
00318         err << "Failed to load file [" << parameterFile << "]";
00319         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::FATAL, err.str());
00320         throw runtime_error(err.str());
00321     }
00322 
00323     TiXmlHandle doc(&file);
00324     NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::INFO, "CommandFile [" + parameterFile + "] successfully loaded.");
00325 
00326     // Check for ApiMap
00327     TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));
00328 
00329     if (parametersElement.ToElement())
00330     {
00331 
00333         encoderThumbPositionStatusElement =        StringUtilities::makeFullyQualifiedRoboDynElement( roboDynActuators[0],    ApiMap::getXmlElementValue(parametersElement, "EncoderThumbSidePositionStatus"));
00334         encoderLittlePositionStatusElement =       StringUtilities::makeFullyQualifiedRoboDynElement( roboDynActuators[1],    ApiMap::getXmlElementValue(parametersElement, "EncoderLittleSidePositionStatus"));
00335         hallsPitchStatusElement =                  StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0],       ApiMap::getXmlElementValue(parametersElement, "HallsPitchStatus"));
00336         hallsYawStatusElement =                    StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[1],       ApiMap::getXmlElementValue(parametersElement, "HallsYawStatus"));
00337 
00339         desiredMotComThumbCommandElement =         StringUtilities::makeFullyQualifiedRoboDynElement( roboDynActuators[0], ApiMap::getXmlElementValue(parametersElement, "DesiredMotComThumbSideCommand"));
00340         desiredMotComLittleCommandElement =        StringUtilities::makeFullyQualifiedRoboDynElement( roboDynActuators[1], ApiMap::getXmlElementValue(parametersElement, "DesiredMotComLittleSideCommand"));
00341 
00343         double bypass =         io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PositionLoopBypass")));
00344         sliderMinPosition =     io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMinPositionCommand")));
00345         sliderMaxPosition =     io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxPositionCommand")));
00346         double pKp =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderKpGainCommand")));
00347 
00348         q1Controller->setPositionLoopParameters(sliderMinPosition, sliderMaxPosition, pKp, bypass <= 0 ? false : true);
00349         q2Controller->setPositionLoopParameters(sliderMinPosition, sliderMaxPosition, pKp, bypass <= 0 ? false : true);
00350 
00351         // torque loop is always bypassed for wrists
00352         q1Controller->setTorqueLoopParameters(0., 0., 0., 0., true);
00353         q2Controller->setTorqueLoopParameters(0., 0., 0., 0., true);
00354 
00355         bypass =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "VelocityLoopBypass")));
00356         double minVelocity =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMinVelocityCommand" )));
00357         double maxVelocity =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxVelocityCommand" )));
00358         double vKp =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderVelocityKpGainCommand" )));
00359         double vKi =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderVelocityKiGainCommand" )));
00360         double windup =         io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderIntegratorWindupLimit" )));
00361 
00362         q1Controller->setVelocityLoopParameters(minVelocity, maxVelocity, vKp, vKi, windup, bypass <= 0 ? false : true);
00363         q2Controller->setVelocityLoopParameters(minVelocity, maxVelocity, vKp, vKi, windup, bypass <= 0 ? false : true);
00364 
00365         bypass =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "CurrentLoopBypass")));
00366         double minCurrent =     io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMinCurrentCommand" )));
00367         double maxCurrent =     io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxCurrentCommand" )));
00368         double maxDutyCycle =   io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "SliderMaxDutyCycleCommand" )));
00369 
00370         q1Controller->setCurrentLoopParameters(minCurrent, maxCurrent, maxDutyCycle, bypass <= 0 ? false : true);
00371         q2Controller->setCurrentLoopParameters(minCurrent, maxCurrent, maxDutyCycle, bypass <= 0 ? false : true);
00372 
00373         double motorViscousDampingComp =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorViscousDampingCompensation" )));
00374         double motorInertia =               io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorInertia" )));
00375         double motorTorqueConst =           io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorTorqueConstant" )));
00376         double motorPhaseRes =              io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorPhaseResistance" )));
00377         double motorBackEMF =               io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorBackEMFConstant" )));
00378         double motorInductance =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotorInductance" )));
00379         double motionRatio =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MotionRatio" )));
00380         double PWMFreq =                    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PWMFrequency" )));
00381         double bridgeDeadTime =             io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "BridgeDeadTime" )));
00382         double bridgeSwitchTime =           io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "BridgeSwitchTime" )));
00383 
00384         q1Controller->setHardwareParameters(motorViscousDampingComp, motorInertia, motorTorqueConst, motorPhaseRes, motorBackEMF, motorInductance, motionRatio, PWMFreq, bridgeDeadTime, bridgeSwitchTime);
00385         q2Controller->setHardwareParameters(motorViscousDampingComp, motorInertia, motorTorqueConst, motorPhaseRes, motorBackEMF, motorInductance, motionRatio, PWMFreq, bridgeDeadTime, bridgeSwitchTime);
00386 
00387         busVoltage    = io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "BusVoltage")));
00388 
00389         // filter
00390         positionAlpha = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PositionAlpha")));
00391         timestep      = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Timestep")));
00392 
00393         // "embedded" smoothing
00394         EmbeddedSmoother::Settings smoothSettings;
00395 
00396         if (timestep <= 0.)
00397         {
00398             throw std::runtime_error("invalid timestep");
00399         }
00400 
00401         smoothSettings.timestep = timestep;
00402         smoothSettings.minVel   = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "VelocityMinPitch")));
00403         smoothSettings.maxAcc   = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "AccelerationMaxPitch")));
00404         smoothSettings.accGain  = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "AccelerationGainPitch")));
00405         smootherPitch.reset(new EmbeddedSmoother(smoothSettings));
00406         smoothSettings.minVel   = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "VelocityMinYaw")));
00407         smoothSettings.maxAcc   = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "AccelerationMaxYaw")));
00408         smoothSettings.accGain  = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "AccelerationGainYaw")));
00409         smootherYaw.reset(new EmbeddedSmoother(smoothSettings));
00410 
00411         // multiloop params
00412         q1Controller->setLoopRate(1 / timestep);
00413         q2Controller->setLoopRate(1 / timestep);
00414 
00416         Vector3d a, a0, b, c, d, d0, pitch, yaw, m, n;
00417 
00418         a(0) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Ax")));
00419         a(1) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Ay")));
00420         a(2) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Az")));
00421 
00422         a0(0) =                 io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "A0x")));
00423         a0(1) =                 io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "A0y")));
00424         a0(2) =                 io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "A0z")));
00425 
00426         b(0) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Bx")));
00427         b(1) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "By")));
00428         b(2) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Bz")));
00429 
00430         c(0) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Cx")));
00431         c(1) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Cy")));
00432         c(2) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Cz")));
00433 
00434         d(0) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Dx")));
00435         d(1) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Dy")));
00436         d(2) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Dz")));
00437 
00438         d0(0) =                 io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "D0x")));
00439         d0(1) =                 io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "D0y")));
00440         d0(2) =                 io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "D0z")));
00441 
00442         pitch(0) =              io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Pitchx")));
00443         pitch(1) =              io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Pitchy")));
00444         pitch(2) =              io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Pitchz")));
00445 
00446         yaw(0) =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Yawx")));
00447         yaw(1) =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Yawy")));
00448         yaw(2) =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Yawz")));
00449 
00450         m(0) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PitchAxisx")));
00451         m(1) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PitchAxisy")));
00452         m(2) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PitchAxisz")));
00453 
00454         n(0) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "YawAxisx")));
00455         n(1) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "YawAxisy")));
00456         n(2) =                  io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "YawAxisz")));
00457 
00458         double linkLength =      io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "LinkLength")));
00459 
00460         wrist->loadDesignParams(a, a0, b, c, d, d0, pitch, yaw, m, n, linkLength);
00461 
00463         double upperPitchMax =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "UpperPitchMax")));
00464         double upperPitchMin =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "UpperPitchMin")));
00465         double lowerPitchMax =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "LowerPitchMax")));
00466         double lowerPitchMin =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "LowerPitchMin")));
00467         double upperYawMax   =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "UpperYawMax")));
00468         double upperYawMin   =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "UpperYawMin")));
00469         double lowerYawMax   =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "LowerYawMax")));
00470         double lowerYawMin   =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "LowerYawMin")));
00471 
00472         wrist->setLimits(upperPitchMin, upperPitchMax, lowerPitchMin, lowerPitchMax, upperYawMin, upperYawMax, lowerYawMin, lowerYawMax);
00473 
00475         Vector2d sliderOffset, sliderGain, angleOffset, angleGain;
00476         sliderOffset(0) =       io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "ThumbSliderOffset")));
00477         sliderOffset(1) =       io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "LittleSliderOffset")));
00478 
00479         sliderGain(0) =         io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "ThumbSliderGain")));
00480         sliderGain(1) =         io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "LittleSliderGain")));
00481 
00482         angleOffset(0) =        io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PitchOffset")));
00483         angleOffset(1) =        io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "YawOffset")));
00484 
00485         angleGain(0) =          io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "PitchGain")));
00486         angleGain(1) =          io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "YawGain")));
00487 
00488         wrist->setSliderOffsetGain(sliderOffset, sliderGain);
00489         wrist->setAngleOffsetGain(angleOffset, angleGain);
00490 
00491         wrist->maxIt =          io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "MaxIter")));
00492         wrist->eps =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,  ApiMap::getXmlElementValue(parametersElement, "Eps")));
00493 
00494         hta.resize(2);
00495         double coeff1, coeff2, coeff3, coeff4, scaleFactor;
00496         coeff1 =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallPitchCoeff1")));
00497         coeff2 =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallPitchCoeff2")));
00498         coeff3 =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallPitchCoeff3")));
00499         coeff4 =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallPitchCoeff4")));
00500         scaleFactor =           io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallScaleFactor")));
00501         hta[0].useCoeffs(coeff1, coeff2, coeff3, coeff4, scaleFactor);
00502         coeff1 =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallYawCoeff1")));
00503         coeff2 =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallYawCoeff2")));
00504         coeff3 =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallYawCoeff3")));
00505         coeff4 =                io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism,   ApiMap::getXmlElementValue(parametersElement, "HallYawCoeff4")));
00506         hta[1].useCoeffs(coeff1, coeff2, coeff3, coeff4, scaleFactor);
00507 
00509         maxSensorErrorFault =    io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "MaxSensorError")));
00510         maxSliderDiffFault =     io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "MaxSliderDiffFault")));
00511         sliderMinPositionFault = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "SliderMinPositionFault")));
00512         sliderMaxPositionFault = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "SliderMaxPositionFault")));
00513         upperPitchMax =          io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "UpperPitchMaxFault")));
00514         upperPitchMin =          io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "UpperPitchMinFault")));
00515         lowerPitchMax =          io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "LowerPitchMaxFault")));
00516         lowerPitchMin =          io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "LowerPitchMinFault")));
00517         upperYawMax =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "UpperYawMaxFault")));
00518         upperYawMin =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "UpperYawMinFault")));
00519         lowerYawMax =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "LowerYawMaxFault")));
00520         lowerYawMin =            io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "LowerYawMinFault")));
00521         wristFault->setLimits(upperPitchMin, upperPitchMax, lowerPitchMin, lowerPitchMax, upperYawMin, upperYawMax, lowerYawMin, lowerYawMax);
00522 
00525         nameUseHalls              = StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, "UseHalls");
00526         nameManualCalThumbSlider  = StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "ManualCalThumbSlider"));
00527         nameManualCalLittleSlider = StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "ManualCalLittleSlider"));
00528         nameManualCalPitch        = StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "ManualCalPitch"));
00529         nameManualCalYaw          = StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "ManualCalYaw"));
00530 
00531         manualCalSliderPos(0) = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "ManualCalThumbSlider")));
00532         manualCalSliderPos(1) = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "ManualCalLittleSlider")));
00533         manualCalAng(0) =       io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "ManualCalPitch")));
00534         manualCalAng(1) =       io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( mechanism, ApiMap::getXmlElementValue(parametersElement, "ManualCalYaw")));
00535 
00537         io.setLiveCoeff(namePitchLimit,            0.);
00538         io.setLiveCoeff(nameYawLimit,              0.);
00539         io.setLiveCoeff(nameLittlesideSliderLimit, 0.);
00540         io.setLiveCoeff(nameThumbsideSliderLimit,  0.);
00541         io.setLiveCoeff(nameSensorError,           0.);
00542         io.setLiveCoeff(nameSliderDiffError,       0.);
00543 
00544         io.setLiveCoeff(namePitchMaxValue, -999);
00545         io.setLiveCoeff(namePitchMinValue, 999);
00546         io.setLiveCoeff(nameYawMaxValue, -999);
00547         io.setLiveCoeff(nameYawMinValue, 999);
00548         io.setLiveCoeff(namePitchHallsValue, 0);
00549         io.setLiveCoeff(namePitchCalcValue, 0);
00550         io.setLiveCoeff(nameYawHallsValue, 0);
00551         io.setLiveCoeff(nameYawCalcValue, 0);
00552 
00553         io.setBoolParam(nameUseHalls, useHalls);
00554         io.setDoubleParam(nameManualCalThumbSlider, manualCalSliderPos(0));
00555         io.setDoubleParam(nameManualCalLittleSlider, manualCalSliderPos(1));
00556         io.setDoubleParam(nameManualCalPitch, manualCalAng(0));
00557         io.setDoubleParam(nameManualCalYaw, manualCalAng(1));
00558 
00560         wrist->isCalibrated    = false;
00561         calCalled              = false;
00562         calibrationState.state = r2_msgs::JointControlCalibrationMode::UNCALIBRATED;
00563         io.setLiveCoeff(nameCoeffState, r2_msgs::JointControlCoeffState::LOADED);
00564 
00565         desiredQ1      = manualCalSliderPos(0);
00566         desiredQ2      = manualCalSliderPos(1);
00567         desiredPitch   = manualCalAng(0);
00568         desiredYaw     = manualCalAng(1);
00569         desiredSlider  = Vector2d::Zero();
00570         desiredAngle   = Vector2d::Zero();
00571         filteredSlider = Vector2d::Zero();
00572     }
00573     else
00574     {
00575         stringstream err;
00576         err << "The file " << parameterFile << " has no element named [ApiMap]";
00577         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::ERROR, err.str());
00578         throw runtime_error(err.str());
00579     }
00580 }
00581 
00582 void JointCommandWrist::updateMeasuredState(r2_msgs::JointControlData msg)
00583 {
00584     // If coeffs are not loaded, override with zeros RDEV-1075
00585     if (msg.coeffState.state != r2_msgs::JointControlCoeffState::LOADED)
00586     {
00587         positionVals.assign(completeMessageSize, 0.0);
00588         velocityVals.assign(completeMessageSize, 0.0);
00589         return;
00590     }
00591 
00592     if (!io.hasLiveCoeff(nameCalibrationMode))
00593     {
00594         stringstream err;
00595         err << "Live coeff [" << nameCalibrationMode << "] does not exist.";
00596         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::ERROR, err.str());
00597         throw runtime_error(err.str());
00598     }
00599 
00600     if (!io.hasParam(nameUseHalls))
00601     {
00602         stringstream err;
00603         err << "param [" << nameUseHalls << "] does not exist.";
00604         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::ERROR, err.str());
00605         throw runtime_error(err.str());
00606     }
00607 
00608     encoder = Vector2d::Zero();
00609     halls   = Vector2d::Zero();
00610     Vector2d slider  = Vector2d::Zero();
00611     ang     = Vector2d::Zero();
00612 
00613     encoder(0) = io.getInt16(encoderThumbPositionStatusElement);
00614     encoder(1) = io.getInt16(encoderLittlePositionStatusElement);
00615     io.getBoolParam(nameUseHalls, useHalls);
00616 
00617     if (useHalls)
00618     {
00619         halls(0) = hta[0].getAngleFromHalls(io.getUInt16(hallsPitchStatusElement));
00620         halls(1) = hta[1].getAngleFromHalls(io.getUInt16(hallsYawStatusElement));
00621 
00622         // filter halls
00623         if (!hallFilterInitialized)
00624         {
00625             filteredHalls = halls;
00626             hallFilterInitialized = true;
00627         }
00628 
00629         filteredHalls = filteredHalls * positionAlpha + halls * (1 - positionAlpha);
00630     }
00631     else
00632     {
00633         halls(0) = io.getUInt16(hallsPitchStatusElement);
00634         halls(1) = io.getUInt16(hallsYawStatusElement);
00635     }
00636 
00637     if (calibrationState.state == r2_msgs::JointControlCalibrationMode::UNCALIBRATED)
00638     {
00639         //don't do anything
00640     }
00641     else if (calibrationState.state == r2_msgs::JointControlCalibrationMode::ENABLE)
00642     {
00643         if (useHalls)
00644         {
00645             //halls cal
00646             Vector2d calSlider;
00647 
00648             try
00649             {
00650                 double upperPitch, lowerPitch, upperYaw, lowerYaw;
00651                 wrist->getLimits(halls(0), halls(1), upperPitch, lowerPitch, upperYaw, lowerYaw);
00652 
00653                 if (halls(0) > upperPitch || halls(0) < lowerPitch
00654                         || halls(1) > upperYaw || halls(1) < lowerYaw)
00655                 {
00656                     stringstream ss;
00657                     ss << "Halls data outside of limits; pitch: " << halls(0) << ", upper limit: " << upperPitch << ", lower limit: " << lowerPitch << "; yaw: " << halls(1) << " upper limit: " << upperYaw << ", lower limit: " << lowerYaw;
00658                     throw std::runtime_error(ss.str());
00659                 }
00660 
00661                 calSlider = wrist->getSliderFromAngle(halls);
00662                 stringstream ss;
00663                 ss << "Halls calibration on " << mechanism << std::endl;
00664                 ss << "slider pos: (" << calSlider(0) << "," << calSlider(1) << "), angle: (" << halls(0) << "," << halls(1) << ")";
00665                 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::NOTICE, ss.str());
00666                 wrist->calWrist(encoder, calSlider, halls);
00668                 wrist->getSliderFromWristEncoder(encoder);
00669 
00670                 if (wrist->isCalibrated)
00671                 {
00672                     io.setLiveCoeff(nameCalibrationMode, r2_msgs::JointControlCalibrationMode::DISABLE);
00673                     calibrationState.state = r2_msgs::JointControlCalibrationMode::DISABLE;
00674                     stringstream ss;
00675                     ss << "Calibrated wrist";
00676                     NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::DEBUG, ss.str());
00677                     calFailureReported = false;
00678                 }
00679                 else
00680                 {
00681                     if (!calFailureReported)
00682                     {
00683                         NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::WARN, "Calibration failed..  remaining uncalibrated");
00684                         calFailureReported = true;
00685                     }
00686 
00687                     return;
00688                 }
00689             }
00690             catch (std::exception& e)
00691             {
00692                 wrist->isCalibrated = false;
00693 
00694                 if (!calFailureReported)
00695                 {
00696                     stringstream ss;
00697                     ss << "Calibration failed (" << e.what() << "): remaining uncalibrated";
00698                     NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::WARN, ss.str());
00699                     calFailureReported = true;
00700                 }
00701 
00702                 return;
00703             }
00704         }
00705         else
00706         {
00707             //manual cal
00708             if (!io.getDoubleParam(nameManualCalThumbSlider, manualCalSliderPos(0))  ||
00709                     !io.getDoubleParam(nameManualCalLittleSlider, manualCalSliderPos(1)) ||
00710                     !io.getDoubleParam(nameManualCalPitch, manualCalAng(0))              ||
00711                     !io.getDoubleParam(nameManualCalYaw, manualCalAng(1)))
00712             {
00713                 stringstream err;
00714                 err << "Invalid params for manual cal.";
00715                 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::ERROR, err.str());
00716                 throw runtime_error(err.str());
00717             }
00718 
00719             stringstream ss;
00720             ss << "Manual calibration on " << mechanism << std::endl;
00721             ss << "slider pos: (" << manualCalSliderPos(0) << "," << manualCalSliderPos(1) << "), angle: (" << manualCalAng(0) << "," << manualCalAng(1) << ")";
00722             NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::NOTICE, ss.str());
00723             wrist->calWrist(encoder,  manualCalSliderPos, manualCalAng);
00724 
00725             if (wrist->isCalibrated)
00726             {
00727                 io.setLiveCoeff(nameCalibrationMode, r2_msgs::JointControlCalibrationMode::DISABLE);
00728                 calibrationState.state = r2_msgs::JointControlCalibrationMode::DISABLE;
00729                 stringstream ss;
00730                 ss << "Calibrated wrist";
00731                 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::DEBUG, ss.str());
00732                 calFailureReported = false;
00733             }
00734             else
00735             {
00736                 if (!calFailureReported)
00737                 {
00738                     stringstream ss;
00739                     ss << "Manual calibration failed.. remaining uncalibrated";
00740                     NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::WARN, ss.str());
00741                     calFailureReported = true;
00742                 }
00743 
00744                 return;
00745             }
00746         }
00747     }
00748     else if (calibrationState.state == r2_msgs::JointControlCalibrationMode::DISABLE)
00749     {
00750         if (wrist->isCalibrated)
00751         {
00752             slider = wrist->getSliderFromWristEncoder(encoder);
00753 
00754             // filter sliders
00755             if (!sliderFilterInitialized)
00756             {
00757                 filteredSlider          = slider;
00758                 sliderFilterInitialized = true;
00759             }
00760 
00761             filteredSlider = filteredSlider * positionAlpha + slider * (1 - positionAlpha);
00762 
00763             try
00764             {
00765                 ang         = wrist->getAngleFromSlider(filteredSlider);
00766                 fwdKinFault = false;
00767             }
00768             catch (std::runtime_error)
00769             {
00771                 if (!fwdKinFault)
00772                 {
00773                     stringstream ss;
00774                     ss << "Unable to find fwd kinematics";
00775                     NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandWrist", Priority::WARN, ss.str());
00776                     fwdKinFault = true;
00777                 }
00778 
00779                 if (useHalls)
00780                 {
00781                     ang = halls;
00782                 }
00783             }
00784         }
00785 
00786         setFaultState();
00787 
00788     }
00789 
00790     positionVals[0]  = ang(0);
00791     positionVals[1]  = ang(1);
00792     positionVals[2]  = filteredSlider(0);
00793     positionVals[3]  = filteredSlider(1);
00794     positionVals[4]  = halls(0);
00795     positionVals[5]  = halls(1);
00796     positionVals[6]  = encoder(0);
00797     positionVals[7]  = encoder(1);
00798     positionVals[8]  = smoothedPitch;
00799     positionVals[9]  = smoothedYaw;
00800     positionVals[10] = desiredQ1;
00801     positionVals[11] = desiredQ2;
00802 
00803     // get velocities
00804     if (!prevPosInitialized)
00805     {
00806         velocityVals.assign(velocityVals.size(), 0.);
00807         prevPosInitialized = true;
00808     }
00809     else
00810     {
00811         for (unsigned int i = 0; i < velocityVals.size(); ++i)
00812         {
00813             velocityVals[i] = (positionVals[i] - prevPositionVals[i]) / timestep;
00814         }
00815     }
00816 
00817     prevPositionVals = positionVals;
00818 
00819     angVel(0)        = (double)velocityVals[0];
00820     angVel(1)        = (double)velocityVals[1];
00821     sliderVel(0)     = (double)velocityVals[2];
00822     sliderVel(1)     = (double)velocityVals[3];
00823     hallsVel(0)      = (double)velocityVals[4];
00824     hallsVel(1)      = (double)velocityVals[5];
00825     encoderVel(0)    = (double)velocityVals[6];
00826     encoderVel(1)    = (double)velocityVals[7];
00827     velocityVals[10] = sliderVelCommand(0);
00828     velocityVals[11] = sliderVelCommand(1);
00829 
00830     return;
00831 }
00832 
00833 sensor_msgs::JointState JointCommandWrist::getSimpleMeasuredState()
00834 {
00835     simpleMeasuredStateMsg.header.stamp = ros::Time::now();
00836 
00837     simpleMeasuredStateMsg.position[0] = positionVals[0];
00838     simpleMeasuredStateMsg.position[1] = positionVals[1];
00839 
00840     simpleMeasuredStateMsg.velocity[0] = velocityVals[0];
00841     simpleMeasuredStateMsg.velocity[1] = velocityVals[1];
00842 
00843     return simpleMeasuredStateMsg;
00844 }
00845 
00846 sensor_msgs::JointState JointCommandWrist::getCompleteMeasuredState()
00847 {
00848     completeMeasuredStateMsg.header.stamp = ros::Time::now();
00849 
00850     completeMeasuredStateMsg.position = positionVals;
00851     completeMeasuredStateMsg.velocity = velocityVals;
00852 
00853     return completeMeasuredStateMsg;
00854 }
00855 
00856 void JointCommandWrist::setFaultState()
00857 {
00859     double up;
00860     double lp;
00861     double uy;
00862     double ly;
00863 
00864     wristFault->getLimits(ang(0), ang(1), up, lp, uy, ly);
00865 
00866     if (ang(0) > up || ang(0) < lp)
00867     {
00868         io.setLiveCoeff(namePitchLimit, 1.);
00869         io.setLiveCoeff(namePitchMaxValue, ang(0));
00870         io.setLiveCoeff(namePitchMinValue, ang(0));
00871     }
00872     else
00873     {
00874         io.setLiveCoeff(namePitchLimit, 0.);
00875         io.setLiveCoeff(namePitchMaxValue, -999);
00876         io.setLiveCoeff(namePitchMinValue, 999);
00877     }
00878 
00879     if (ang(1) > uy || ang(1) < ly)
00880     {
00881         io.setLiveCoeff(nameYawLimit,   1.);
00882         io.setLiveCoeff(nameYawMaxValue, ang(1));
00883         io.setLiveCoeff(nameYawMinValue, ang(1));
00884     }
00885     else
00886     {
00887         io.setLiveCoeff(nameYawLimit,   0.);
00888         io.setLiveCoeff(nameYawMaxValue, -999);
00889         io.setLiveCoeff(nameYawMinValue, 999);
00890     }
00891 
00893     if (filteredSlider(0) > sliderMaxPositionFault || filteredSlider(0) < sliderMinPositionFault)
00894     {
00895         io.setLiveCoeff(nameThumbsideSliderLimit, 1.);
00896     }
00897     else
00898     {
00899         io.setLiveCoeff(nameThumbsideSliderLimit, 0.);
00900     }
00901 
00902     if (filteredSlider(1) > sliderMaxPositionFault || filteredSlider(1) < sliderMinPositionFault)
00903     {
00904         io.setLiveCoeff(nameLittlesideSliderLimit,  1.);
00905     }
00906     else
00907     {
00908         io.setLiveCoeff(nameLittlesideSliderLimit,  0.);
00909     }
00910 
00912     if (useHalls)
00913     {
00914         if (fabs(ang(0) - halls(0)) > maxSensorErrorFault || fabs(ang(1) - halls(1)) > maxSensorErrorFault)
00915         {
00916             io.setLiveCoeff(nameSensorError,     1.);
00917             io.setLiveCoeff(namePitchHallsValue, halls(0));
00918             io.setLiveCoeff(namePitchCalcValue,  ang(0));
00919             io.setLiveCoeff(nameYawHallsValue,   halls(1));
00920             io.setLiveCoeff(nameYawCalcValue,    ang(1));
00921         }
00922         else
00923         {
00924             io.setLiveCoeff(nameSensorError,     0.);
00925             io.setLiveCoeff(namePitchHallsValue, 0.);
00926             io.setLiveCoeff(namePitchCalcValue,  0.);
00927             io.setLiveCoeff(nameYawHallsValue,   0.);
00928             io.setLiveCoeff(nameYawCalcValue,    0.);
00929         }
00930     }
00931     else
00932     {
00933         io.setLiveCoeff(nameSensorError,     0.);
00934         io.setLiveCoeff(namePitchHallsValue, 0.);
00935         io.setLiveCoeff(namePitchCalcValue,  0.);
00936         io.setLiveCoeff(nameYawHallsValue,   0.);
00937         io.setLiveCoeff(nameYawCalcValue,    0.);
00938     }
00939 
00941     if (fabs(filteredSlider(0) - filteredSlider(1)) > maxSliderDiffFault)
00942     {
00943         io.setLiveCoeff(nameSliderDiffError,       1.);
00944     }
00945     else
00946     {
00947         io.setLiveCoeff(nameSliderDiffError,       0.);
00948     }
00949 
00950 }
00951 
00952 r2_msgs::JointCommand JointCommandWrist::getCommandedState()
00953 {
00954     commandedStateMsg.header.stamp = ros::Time::now();
00955     return commandedStateMsg;
00956 }
00957 
00958 void JointCommandWrist::setCommand(r2_msgs::JointCommand msg, r2_msgs::JointControlData control)
00959 {
00960     if (!calCalled && control.calibrationMode.state == r2_msgs::JointControlCalibrationMode::ENABLE)
00961     {
00962         calibrationState.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00963         calCalled = true;
00964     }
00965 
00966     if (control.calibrationMode.state == r2_msgs::JointControlCalibrationMode::DISABLE)
00967     {
00968         calCalled = false;
00969     }
00970 
00972     if (!msg.name.empty() && !msg.desiredPosition.empty())
00973     {
00974         if (msg.desiredPositionVelocityLimit.empty())
00975         {
00976             msg.desiredPositionVelocityLimit.resize(msg.name.size(), 0.);
00977         }
00978 
00980         if (control.commandMode.state == r2_msgs::JointControlCommandMode::MOTCOM)
00981         {
00982             for (unsigned int i = 0; i < msg.name.size(); ++i)
00983             {
00984                 if (msg.name[i] == roboDynActuators[0])
00985                 {
00986                     desiredQ1 = msg.desiredPosition[i];
00987                 }
00988                 else if (msg.name[i] == roboDynActuators[1])
00989                 {
00990                     desiredQ2 = msg.desiredPosition[i];
00991                 }
00992             }
00993         }
00994         else if (control.commandMode.state == r2_msgs::JointControlCommandMode::ACTUATOR)
00995         {
00996             for (unsigned int i = 0; i < msg.name.size(); ++i)
00997             {
00998                 if (msg.name[i] == roboDynActuators[0])
00999                 {
01000                     desiredQ1 = msg.desiredPosition[i];
01001                 }
01002                 else if (msg.name[i] == roboDynActuators[1])
01003                 {
01004                     desiredQ2 = msg.desiredPosition[i];
01005                 }
01006             }
01007 
01008             try
01009             {
01010                 //limitSliderDiff(desiredQ1, desiredQ2);
01011                 desiredAngle = wrist->getAngleFromSlider(Vector2d(desiredQ1, desiredQ2));
01012                 wrist->applyLimits(desiredAngle(0), desiredAngle(1));
01013 
01014                 // smooth if in drive and not in actuator
01015                 smoothedPitch = smootherPitch->reset(desiredAngle(0), 0.);
01016                 smoothedYaw   = smootherYaw->reset(desiredAngle(1), 0.);
01017 
01018                 commandedStateMsg.desiredPosition[0]              = desiredAngle(0);
01019                 commandedStateMsg.desiredPosition[1]              = desiredAngle(1);
01020                 commandedStateMsg.desiredPositionVelocityLimit[0] = 0;
01021                 commandedStateMsg.desiredPositionVelocityLimit[1] = 0;
01022             }
01023             catch (std::runtime_error& e)
01024             {
01026                 NasaCommonLogging::Logger::getCategory("gov.nasa.robonet.JointCommandWrist") << log4cpp::Priority::ERROR << "Kinematics failed with error " << e.what() << " on (" << desiredQ1 << "," << desiredQ2 << " command to actuators " << roboDynActuators[0] << ", " << roboDynActuators[1];
01027                 control.controlMode.state = r2_msgs::JointControlMode::PARK;
01028             }
01029 
01030         }
01031         else
01032         {
01033             if (msg.desiredPositionVelocityLimit.empty())
01034             {
01035                 msg.desiredPositionVelocityLimit.resize(msg.name.size(), 0);
01036             }
01037 
01038             for (unsigned int i = 0; i < msg.name.size(); ++i)
01039             {
01040                 if (msg.name[i] == roboDynJoints[0])
01041                 {
01042                     desiredPitch    = msg.desiredPosition[i];
01043                     desiredPitchVel = msg.desiredPositionVelocityLimit[i];
01044                 }
01045                 else if (msg.name[i] == roboDynJoints[1])
01046                 {
01047                     desiredYaw    = msg.desiredPosition[i];
01048                     desiredYawVel = msg.desiredPositionVelocityLimit[i];
01049                 }
01050             }
01051 
01052             try
01053             {
01054                 if (control.controlMode.state == r2_msgs::JointControlMode::DRIVE)
01055                 {
01056                     // smooth in drive
01057                     smoothedPitch = smootherPitch->update(desiredPitch, desiredPitchVel);
01058                     smoothedYaw   = smootherYaw->update(desiredYaw, desiredYawVel);
01059                 }
01060                 else
01061                 {
01062                     smoothedPitch = smootherPitch->reset(desiredPitch, desiredPitchVel);
01063                     smoothedYaw   = smootherYaw->reset(desiredYaw, desiredYawVel);
01064                 }
01065 
01066                 wrist->applyLimits(smoothedPitch, smoothedYaw);
01067                 desiredSlider = wrist->getSliderFromAngle(Vector2d(smoothedPitch, smoothedYaw));
01068                 //limitSliderDiff(desiredSlider(0), desiredSlider(1));
01069                 desiredQ1                                         = desiredSlider(0);
01070                 desiredQ2                                         = desiredSlider(1);
01071                 commandedStateMsg.desiredPosition[0]              = desiredPitch;
01072                 commandedStateMsg.desiredPosition[1]              = desiredYaw;
01073                 commandedStateMsg.desiredPositionVelocityLimit[0] = desiredPitchVel;
01074                 commandedStateMsg.desiredPositionVelocityLimit[1] = desiredYawVel;
01075             }
01076             catch (std::runtime_error)
01077             {
01079                 NasaCommonLogging::Logger::getCategory("gov.nasa.robonet.JointCommandWrist") << log4cpp::Priority::ERROR << "Kinematics failed on (" << desiredPitch << "," << desiredYaw << ") command to joints " << roboDynJoints[0] << ", " << roboDynJoints[1];
01080                 control.controlMode.state = r2_msgs::JointControlMode::PARK;
01081             }
01082         }
01083     }
01084 
01085     double motcom1 = 0;
01086     double motcom2 = 0;
01087 
01089     if (control.controlMode.state == r2_msgs::JointControlMode::DRIVE)
01090     {
01091         if (control.commandMode.state == r2_msgs::JointControlCommandMode::MOTCOM)
01092         {
01093             motcom1 = desiredQ1;
01094             motcom2 = desiredQ2;
01095         }
01096         else if (wrist->isCalibrated)
01097         {
01098             motcom1 = q1Controller->update(desiredQ1, (double)filteredSlider(0), (double)sliderVel(0), busVoltage, sliderVelCommand(0));
01099             motcom2 = q2Controller->update(desiredQ2, (double)filteredSlider(1), (double)sliderVel(1), busVoltage, sliderVelCommand(1));
01100         }
01101     }
01102 
01103     io.setInt16(desiredMotComThumbCommandElement, motcom1);
01104     io.setInt16(desiredMotComLittleCommandElement, motcom2);
01105 }
01106 
01107 r2_msgs::JointCapability JointCommandWrist::getCapability()
01108 {
01109     double up, uy, lp, ly;
01110     wrist->getLimits(ang(0), ang(1), up, lp, uy, ly);
01111     jointCapabilityMsg.positionLimitMax[0] = up;
01112     jointCapabilityMsg.positionLimitMax[1] = uy;
01113     jointCapabilityMsg.positionLimitMin[0] = lp;
01114     jointCapabilityMsg.positionLimitMin[1] = ly;
01115 
01117 
01118     return jointCapabilityMsg;
01119 }

robodyn_mechanisms
Author(s):
autogenerated on Thu Jun 6 2019 21:22:48