00001
00002 #include "robodyn_mechanisms/JointCommandSeriesElastic.h"
00003
00004 using namespace std;
00005 using namespace log4cpp;
00006 namespace SU = StringUtilities;
00007
00008 JointCommandSeriesElastic::JointCommandSeriesElastic(const std::string& mechanism, IoFunctions ioFunctions)
00009 : JointCommandInterface(mechanism, ioFunctions),
00010 springConstant(0.0),
00011 gearStiffness(0.0),
00012 combinedStiffness(0.0),
00013 jointKinematicOffset(0.0),
00014 jointKinematicDirection(0.0),
00015 jointGearRatio(0.0),
00016 backEmfConstant(0.0),
00017 incEncNow(0),
00018 incEncPrev(0),
00019 incEncRef(0),
00020 encPosRef(0.0),
00021 hallEncNow(0),
00022 hallEncPrev(0),
00023 hallEncRef(0),
00024 hallPosRef(0.0),
00025 timeNow(0.0),
00026 timePrev(0.0),
00027 deltaTime(0.0),
00028 deltaTimeSec(0.0),
00029 deltaEncPos(0.0),
00030 commandRate(0.0),
00031 isInitialized(false),
00032 completeMessageSize(7)
00033 {
00034 if (mechanism == "")
00035 {
00036 stringstream err;
00037 err << "Constructor requires mechanism be non-empty.";
00038 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00039 throw invalid_argument(err.str());
00040 }
00041
00042 if (io.getFloat.empty())
00043 {
00044 stringstream err;
00045 err << "Constructor requires 'io.getFloat' be non-empty.";
00046 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00047 throw invalid_argument(err.str());
00048 }
00049
00050 if (io.setFloat.empty())
00051 {
00052 stringstream err;
00053 err << "Constructor requires 'io.setFloat' be non-empty.";
00054 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00055 throw invalid_argument(err.str());
00056 }
00057
00058 if (io.getInt32.empty())
00059 {
00060 stringstream err;
00061 err << "Constructor requires 'io.getInt32' be non-empty.";
00062 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00063 throw invalid_argument(err.str());
00064 }
00065
00066 if (io.setUInt16.empty())
00067 {
00068 stringstream err;
00069 err << "Constructor requires 'io.setUInt16' be non-empty.";
00070 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00071 throw invalid_argument(err.str());
00072 }
00073
00074 if (io.getMotorCoeff.empty())
00075 {
00076 stringstream err;
00077 err << "Constructor requires 'io.getMotorCoeff' be non-empty.";
00078 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00079 throw invalid_argument(err.str());
00080 }
00081
00082 if (io.hasBrainstemCoeff.empty())
00083 {
00084 stringstream err;
00085 err << "Constructor requires 'io.hasBrainstemCoeff' be non-empty.";
00086 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00087 throw invalid_argument(err.str());
00088 }
00089
00090 if (io.getBrainstemCoeff.empty())
00091 {
00092 stringstream err;
00093 err << "Constructor requires 'io.getBrainstemCoeff' be non-empty.";
00094 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00095 throw invalid_argument(err.str());
00096 }
00097
00098 if (io.getCommandFile.empty())
00099 {
00100 stringstream err;
00101 err << "Constructor requires 'io.getCommandFile' be non-empty.";
00102 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00103 throw invalid_argument(err.str());
00104 }
00105
00106 roboDynJoints.push_back(mechanism);
00107 motorName = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("motor") + SU::getNumber(SU::getRoboDynJoint(mechanism));
00108 encoderName = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("encoder") + SU::getNumber(SU::getRoboDynJoint(mechanism));
00109 jointCalculatedName = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("jointCalculated") + SU::getNumber(SU::getRoboDynJoint(mechanism));
00110 encoderCalculatedName = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("encoderCalculated") + SU::getNumber(SU::getRoboDynJoint(mechanism));
00111 hallsCalculatedName = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("hallsCalculated") + SU::getNumber(SU::getRoboDynJoint(mechanism));
00112 embeddedCommandName = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("embeddedCommand") + SU::getNumber(SU::getRoboDynJoint(mechanism));
00113
00114 loadCoeffs();
00115 }
00116
00117 JointCommandSeriesElastic::~JointCommandSeriesElastic()
00118 {}
00119
00120 void JointCommandSeriesElastic::loadCoeffs()
00121 {
00122 springConstant = 0.0;
00123 gearStiffness = 0.0;
00124 combinedStiffness = 0.0;
00125 jointKinematicOffset = 0.0;
00126 jointKinematicDirection = 0.0;
00127 jointGearRatio = 0.0;
00128 backEmfConstant = 0.0;
00129 incEncNow = 0;
00130 incEncPrev = 0;
00131 incEncRef = 0;
00132 encPosRef = 0.0;
00133 hallEncNow = 0;
00134 hallEncPrev = 0;
00135 hallEncRef = 0;
00136 hallPosRef = 0.0;
00137 timeNow = ros::Time(0.0);
00138 timePrev = ros::Time(0.0);
00139 deltaTime = ros::Duration(0.0);
00140 deltaTimeSec = 0.0;
00141 deltaEncPos = 0.0;
00142 commandRate = 0.0;
00143 isInitialized = false;
00144
00145 std::string parameterFile = io.getCommandFile(mechanism);
00146
00148 simpleMeasuredStateMsg.name.resize(1);
00149 simpleMeasuredStateMsg.position.resize(1);
00150 simpleMeasuredStateMsg.velocity.resize(1);
00151 simpleMeasuredStateMsg.effort.resize(1);
00152
00153 simpleMeasuredStateMsg.name[0] = roboDynJoints[0];
00154
00155 completeMeasuredStateMsg.name.resize(completeMessageSize);
00156 completeMeasuredStateMsg.position.resize(completeMessageSize);
00157 completeMeasuredStateMsg.velocity.resize(completeMessageSize);
00158 completeMeasuredStateMsg.effort.resize(completeMessageSize);
00159
00160 completeMeasuredStateMsg.name[0] = roboDynJoints[0];
00161 completeMeasuredStateMsg.name[1] = motorName;
00162 completeMeasuredStateMsg.name[2] = encoderName;
00163 completeMeasuredStateMsg.name[3] = jointCalculatedName;
00164 completeMeasuredStateMsg.name[4] = encoderCalculatedName;
00165 completeMeasuredStateMsg.name[5] = hallsCalculatedName;
00166 completeMeasuredStateMsg.name[6] = embeddedCommandName;
00167
00168 commandedStateMsg.name.resize(1);
00169 commandedStateMsg.desiredPosition.resize(1);
00170 commandedStateMsg.desiredPositionVelocityLimit.resize(1);
00171 commandedStateMsg.feedForwardTorque.resize(1);
00172 commandedStateMsg.proportionalGain.resize(1);
00173 commandedStateMsg.derivativeGain.resize(1);
00174 commandedStateMsg.integralGain.resize(1);
00175 commandedStateMsg.positionLoopTorqueLimit.resize(1);
00176 commandedStateMsg.positionLoopWindupLimit.resize(1);
00177 commandedStateMsg.torqueLoopVelocityLimit.resize(1);
00178
00179 commandedStateMsg.name[0] = roboDynJoints[0];
00180
00181 jointCapabilityMsg.name.resize(1);
00182 jointCapabilityMsg.positionLimitMax.resize(1);
00183 jointCapabilityMsg.positionLimitMin.resize(1);
00184 jointCapabilityMsg.velocityLimit.resize(1);
00185 jointCapabilityMsg.torqueLimit.resize(1);
00186
00187 jointCapabilityMsg.name[0] = roboDynJoints[0];
00188
00189 positionVals.resize(completeMessageSize, 0.0);
00190 velocityVals.resize(completeMessageSize, 0.0);
00191 effortVals.resize(completeMessageSize, 0.0);
00192
00194 TiXmlDocument file(parameterFile.c_str());
00195 bool loadOkay = file.LoadFile();
00196
00197 if (!loadOkay)
00198 {
00199 stringstream err;
00200 err << "Failed to load file [" << parameterFile << "]";
00201 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::FATAL, err.str());
00202 throw runtime_error(err.str());
00203 }
00204
00205 TiXmlHandle doc(&file);
00206 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::INFO, "CommandFile [" + parameterFile + "] successfully loaded.");
00207
00208
00209 TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));
00210
00211 if (parametersElement.ToElement())
00212 {
00215 jointPositionStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointPositionStatus"));
00216 jointVelocityStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointVelocityStatus"));
00217 jointCalculatedEffortElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointCalculatedEffort"));
00219 motorPositionStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotorPositionStatus"));
00220 motorVelocityStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotorVelocityStatus"));
00221 motorCurrentElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotorCurrent"));
00223 encoderPositionStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderPositionStatus"));
00224 encoderVelocityStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderVelocityStatus"));
00226 jointPositionFilteredStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointPositionFilteredStatus"));
00227 motorPositionFilteredStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotorPositionFilteredStatus"));
00228 springConstant = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "SpringConstant")));
00229
00230 if (springConstant <= 0.0)
00231 {
00232 stringstream err;
00233 err << "[" << StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "SpringConstant")) << "] must be positive. Provided value of [" << springConstant << "] is invalid.";
00234 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00235 throw runtime_error(err.str());
00236 }
00237
00238 gearStiffness = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "GearStiffness")));
00239
00240 if (gearStiffness <= 0.0)
00241 {
00242 stringstream err;
00243 err << "[" << StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "GearStiffness")) << "] must be positive. Provided value of [" << gearStiffness << "] is invalid.";
00244 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00245 throw runtime_error(err.str());
00246 }
00247
00249 encoderRawPositionStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderRawPositionStatus"));
00250 encoderTimeElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderTime"));
00251 encoderTimeoutElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderTimeout"));
00253 hallPositionStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "HallPositionStatus"));
00254 hallRawPositionStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "HallRawPositionStatus"));
00255 hallTimeElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "HallTime"));
00256 hallTimeoutElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "HallTimeout"));
00258 embeddedCommandPositionElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EmbeddedCommandPosition"));
00259 embeddedCommandVelocityElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EmbeddedCommandVelocity"));
00260 embeddedCommandEffortElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EmbeddedCommandEffort"));
00262 desiredPositionCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "DesiredPositionCommand"));
00263 desiredPositionVelocityLimitCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "DesiredPositionVelocityLimitCommand"));
00264 feedForwardTorqueCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "FeedForwardTorqueCommand"));
00265 proportionalGainCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "ProportionalGainCommand"));
00266 derivativeGainCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "DerivativeGainCommand"));
00267 integralGainCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "IntegralGainCommand"));
00268 positionLoopTorqueLimitCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLoopTorqueLimitCommand"));
00269 positionLoopWindupLimitCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLoopWindupLimitCommand"));
00270 torqueLoopVelocityLimitCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "TorqueLoopVelocityLimitCommand"));
00272 jointKinematicOffset = io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "APSKinematicOffset")));
00273 jointKinematicDirection = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointKinematicDir")));
00274 jointCapabilityMsg.positionLimitMax[0] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLimitMax")))
00275 + jointKinematicOffset;
00276 jointCapabilityMsg.positionLimitMin[0] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLimitMin")))
00277 + jointKinematicOffset;
00278 jointCapabilityMsg.torqueLimit[0] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "TorqueLimit")));
00279 jointGearRatio = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointGearRatio")));
00280
00281 if (jointGearRatio <= 0.0)
00282 {
00283 stringstream err;
00284 err << "[" << StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointGearRatio")) << "] must be positive. Provided value of [" << jointGearRatio << "] is invalid.";
00285 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00286 throw runtime_error(err.str());
00287 }
00288
00289 jointCapabilityMsg.velocityLimit[0] = io.getMotorCoeff(torqueLoopVelocityLimitCommandElement) / jointGearRatio;
00290 backEmfConstant = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "BackEMFConstant")));
00291
00293 brakePwmCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "BrakePWM"));
00294 motComLimitCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotComLimit"));
00295
00296 if (not(io.hasBrainstemCoeff(brakePwmCommandElement)))
00297 {
00298 stringstream err;
00299 err << "The BrainstemCoeff " << brakePwmCommandElement << " does not exist";
00300 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00301 throw runtime_error(err.str());
00302 }
00303
00304 if (not(io.hasBrainstemCoeff(motComLimitCommandElement)))
00305 {
00306 stringstream err;
00307 err << "The BrainstemCoeff " << motComLimitCommandElement << " does not exist";
00308 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00309 throw runtime_error(err.str());
00310 }
00311
00312 io.setUInt16(brakePwmCommandElement, io.getBrainstemCoeff(brakePwmCommandElement));
00313 io.setUInt16(motComLimitCommandElement, io.getBrainstemCoeff(motComLimitCommandElement));
00314 }
00315 else
00316 {
00317 stringstream err;
00318 err << "The file " << parameterFile << " has no element named [ApiMap]";
00319 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00320 throw runtime_error(err.str());
00321 }
00322
00323
00324 incEncPrev = io.getInt32(encoderRawPositionStatusElement);
00325 timePrev = ros::Time::now();
00326
00328 motorEncoderStateCalculator = boost::make_shared<MotorEncoderStateCalculator>(MotorEncoderStateCalculator(jointGearRatio,
00329 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PwmFrequency"))),
00330 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PwmDeadTime"))),
00331 io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "BusVoltage"))),
00332 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotorCurrentLimit"))),
00333 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PhaseResistance"))),
00334 backEmfConstant,
00335 jointKinematicDirection,
00336 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderMountingDir"))),
00337 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderCountsPerRevolution"))),
00338 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderGlitchScalar"))),
00339 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderVelocityBlendStop"))),
00340 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderVelocityBlendStart")))));
00341 hallsEncoderStateCalculator = boost::make_shared<HallsEncoderStateCalculator>(HallsEncoderStateCalculator(jointGearRatio,
00342 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PwmFrequency"))),
00343 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PwmDeadTime"))),
00344 io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "BusVoltage"))),
00345 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotorCurrentLimit"))),
00346 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PhaseResistance"))),
00347 backEmfConstant,
00348 jointKinematicDirection,
00349 1,
00350 io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotorPoleCount"))),
00351 io.getMotorCoeff( StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderGlitchScalar"))),
00352 io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "HallVelocityBlendStop"))),
00353 io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "HallVelocityBlendStart")))));
00354
00355 combinedStiffness = 1.0 / ((1.0 / springConstant) + (1.0 / gearStiffness));
00356 }
00357
00358 void JointCommandSeriesElastic::updateMeasuredState(r2_msgs::JointControlData msg)
00359 {
00360
00361 if (msg.coeffState.state == r2_msgs::JointControlCoeffState::LOADED)
00362 {
00364 timeNow = ros::Time::now();
00365 incEncNow = io.getInt32(encoderRawPositionStatusElement);
00366 hallEncNow = io.getInt32(hallRawPositionStatusElement);
00367
00368 if (not isInitialized)
00369 {
00370 isInitialized = true;
00371 incEncRef = incEncNow;
00372 encPosRef = io.getFloat(motorPositionStatusElement) - ((io.getFloat(jointCalculatedEffortElement) * -1.0) / gearStiffness);
00373 hallEncRef = hallEncNow;
00374 hallPosRef = io.getFloat(motorPositionStatusElement) - ((io.getFloat(jointCalculatedEffortElement) * -1.0) / gearStiffness);
00375 timePrev = ros::Time::now();
00376 }
00377
00378 commandRate = 1.0 / ((timeNow - timePrev).toSec());
00379 timePrev = timeNow;
00380
00382 positionVals[0] = io.getFloat(jointPositionStatusElement) + jointKinematicOffset;
00383 velocityVals[0] = io.getFloat(jointVelocityStatusElement);
00384 effortVals[0] = io.getFloat(jointCalculatedEffortElement);
00385
00387 positionVals[1] = io.getFloat(motorPositionStatusElement) + jointKinematicOffset;
00388 velocityVals[1] = io.getFloat(motorVelocityStatusElement);
00389 effortVals[1] = io.getFloat(motorCurrentElement) * backEmfConstant * jointGearRatio;
00390
00392 positionVals[2] = io.getFloat(encoderPositionStatusElement) + jointKinematicOffset;
00393 velocityVals[2] = io.getFloat(encoderVelocityStatusElement) / jointGearRatio;
00394
00396 effortVals[3] = (io.getFloat(motorPositionFilteredStatusElement) - io.getFloat(jointPositionFilteredStatusElement)) * springConstant;
00397
00400 positionVals[4] = motorEncoderStateCalculator->getPosition(encPosRef, incEncRef, incEncNow) + jointKinematicOffset;
00401 velocityVals[4] = motorEncoderStateCalculator->getVelocity(io.getInt32(encoderTimeElement), io.getInt32(encoderTimeoutElement), incEncNow, commandRate) / jointGearRatio;
00402 effortVals[4] = (io.getFloat(encoderPositionStatusElement) - io.getFloat(jointPositionFilteredStatusElement)) * combinedStiffness;
00403
00405 positionVals[5] = hallsEncoderStateCalculator->getPosition(hallPosRef, hallEncRef, hallEncNow) + jointKinematicOffset;
00406 velocityVals[5] = hallsEncoderStateCalculator->getVelocity(io.getInt32(hallTimeElement), io.getInt32(hallTimeoutElement), hallEncNow, commandRate) / jointGearRatio;
00407
00409 positionVals[6] = io.getFloat(embeddedCommandPositionElement) + jointKinematicOffset;
00410 velocityVals[6] = io.getFloat(embeddedCommandVelocityElement);
00411 effortVals[6] = io.getFloat(embeddedCommandEffortElement);
00412 }
00413 else
00414 {
00415 isInitialized = false;
00416 positionVals.assign(completeMessageSize, 0.0);
00417 velocityVals.assign(completeMessageSize, 0.0);
00418 effortVals.assign(completeMessageSize, 0.0);
00419 }
00420 }
00421
00422 sensor_msgs::JointState JointCommandSeriesElastic::getSimpleMeasuredState()
00423 {
00424 simpleMeasuredStateMsg.header.stamp = ros::Time::now();
00425
00426 simpleMeasuredStateMsg.position[0] = positionVals[0];
00427 simpleMeasuredStateMsg.velocity[0] = velocityVals[0];
00428 simpleMeasuredStateMsg.effort[0] = effortVals[0];
00429
00430 return simpleMeasuredStateMsg;
00431 }
00432
00433 sensor_msgs::JointState JointCommandSeriesElastic::getCompleteMeasuredState()
00434 {
00435 completeMeasuredStateMsg.header.stamp = ros::Time::now();
00436
00437 completeMeasuredStateMsg.position = positionVals;
00438 completeMeasuredStateMsg.velocity = velocityVals;
00439 completeMeasuredStateMsg.effort = effortVals;
00440
00441 return completeMeasuredStateMsg;
00442 }
00443
00444 void JointCommandSeriesElastic::setFaultState()
00445 {
00446 }
00447
00448 r2_msgs::JointCommand JointCommandSeriesElastic::getCommandedState()
00449 {
00450 commandedStateMsg.header.stamp = ros::Time::now();
00451
00452 commandedStateMsg.desiredPosition[0] = io.getFloat(desiredPositionCommandElement) + jointKinematicOffset;
00453 commandedStateMsg.desiredPositionVelocityLimit[0] = io.getFloat(desiredPositionVelocityLimitCommandElement);
00454 commandedStateMsg.feedForwardTorque[0] = io.getFloat(feedForwardTorqueCommandElement);
00455 commandedStateMsg.proportionalGain[0] = io.getFloat(proportionalGainCommandElement);
00456 commandedStateMsg.derivativeGain[0] = io.getFloat(derivativeGainCommandElement);
00457 commandedStateMsg.integralGain[0] = io.getFloat(integralGainCommandElement);
00458 commandedStateMsg.positionLoopTorqueLimit[0] = io.getFloat(positionLoopTorqueLimitCommandElement);
00459 commandedStateMsg.positionLoopWindupLimit[0] = io.getFloat(positionLoopWindupLimitCommandElement);
00460
00461 return commandedStateMsg;
00462 }
00463
00464 void JointCommandSeriesElastic::setCommand(r2_msgs::JointCommand msg, r2_msgs::JointControlData)
00465 {
00466 if (msg.name.size() != 1)
00467 {
00468 stringstream err;
00469 err << "setCommand() requires one and only one entry in JointCommand.name when setCommand() is called.";
00470 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00471 throw runtime_error(err.str());
00472 }
00473
00474 if ((msg.desiredPosition.size() > 1) ||
00475 (msg.desiredPositionVelocityLimit.size() > 1) ||
00476 (msg.feedForwardTorque.size() > 1) ||
00477 (msg.proportionalGain.size() > 1) ||
00478 (msg.derivativeGain.size() > 1) ||
00479 (msg.integralGain.size() > 1) ||
00480 (msg.positionLoopTorqueLimit.size() > 1) ||
00481 (msg.positionLoopWindupLimit.size() > 1) ||
00482 (msg.torqueLoopVelocityLimit.size() > 1))
00483 {
00484 stringstream err;
00485 err << "setCommand() requires at most one entry in JointCommand.* when setCommand() is called.";
00486 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00487 throw runtime_error(err.str());
00488 }
00489
00490 if (msg.name[0] != roboDynJoints[0])
00491 {
00492 stringstream err;
00493 err << "setCommand() received JointCommand message for joint [" << msg.name[0] << "]. It expected [" << roboDynJoints[0] << "].";
00494 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00495 throw runtime_error(err.str());
00496 }
00497
00498 if (not msg.desiredPosition.empty())
00499 {
00500 io.setFloat(desiredPositionCommandElement, msg.desiredPosition[0] - jointKinematicOffset);
00501 }
00502
00503 if (not msg.desiredPositionVelocityLimit.empty())
00504 {
00505 io.setFloat(desiredPositionVelocityLimitCommandElement, msg.desiredPositionVelocityLimit[0]);
00506 }
00507
00508 if (not msg.feedForwardTorque.empty())
00509 {
00510 io.setFloat(feedForwardTorqueCommandElement, msg.feedForwardTorque[0]);
00511 }
00512
00513 if (not msg.proportionalGain.empty())
00514 {
00515 io.setFloat(proportionalGainCommandElement, msg.proportionalGain[0]);
00516 }
00517
00518 if (not msg.derivativeGain.empty())
00519 {
00520 io.setFloat(derivativeGainCommandElement, msg.derivativeGain[0]);
00521 }
00522
00523 if (not msg.integralGain.empty())
00524 {
00525 io.setFloat(integralGainCommandElement, msg.integralGain[0]);
00526 }
00527
00528 if (not msg.positionLoopTorqueLimit.empty())
00529 {
00530 io.setFloat(positionLoopTorqueLimitCommandElement, msg.positionLoopTorqueLimit[0]);
00531 }
00532
00533 if (not msg.positionLoopWindupLimit.empty())
00534 {
00535 io.setFloat(positionLoopWindupLimitCommandElement, msg.positionLoopWindupLimit[0]);
00536 }
00537 }
00538
00539 r2_msgs::JointCapability JointCommandSeriesElastic::getCapability()
00540 {
00541 return jointCapabilityMsg;
00542 }