00001 #include "robodyn_mechanisms/JointCommandRigid.h"
00002
00003 using namespace std;
00004 using namespace log4cpp;
00005 namespace SU = StringUtilities;
00006
00007 JointCommandRigid::JointCommandRigid(const std::string& mechanism, IoFunctions ioFunctions)
00008 : JointCommandInterface(mechanism, ioFunctions),
00009 completeMessageSize(2)
00010 {
00011 if (mechanism == "")
00012 {
00013 stringstream err;
00014 err << "Constructor requires mechanism be non-empty.";
00015 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::FATAL, err.str());
00016 throw invalid_argument(err.str());
00017 }
00018
00019 if (io.getFloat.empty() or
00020 io.setFloat.empty() or
00021 io.getUInt32.empty() or
00022 io.getMotorCoeff.empty() or
00023 io.hasBrainstemCoeff.empty() or
00024 io.getBrainstemCoeff.empty() or
00025 io.getCommandFile.empty())
00026 {
00027 stringstream err;
00028 err << "Constructor requires 'io.getFloat', 'io.setFloat', 'io.getUInt32', 'io.getMotorCoeff', 'io.getBrainstemCoeff', and 'io.getCommandFile' be non-empty.";
00029 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::FATAL, err.str());
00030 throw invalid_argument(err.str());
00031 }
00032
00033 roboDynJoints.push_back(mechanism);
00034 encoderName = SU::getRoboDynEverythingButJoint(mechanism) + SU::TOKEN_DELIMITER + string("encoder") + SU::getNumber(SU::getRoboDynJoint(mechanism));
00035
00036 loadCoeffs();
00037 }
00038
00039 JointCommandRigid::~JointCommandRigid()
00040 {}
00041
00042 void JointCommandRigid::loadCoeffs()
00043 {
00044 std::string parameterFile = io.getCommandFile(mechanism);
00045
00047 simpleMeasuredStateMsg.name.resize(1);
00048 simpleMeasuredStateMsg.position.resize(1);
00049 simpleMeasuredStateMsg.velocity.resize(1);
00050 simpleMeasuredStateMsg.effort.resize(1);
00051
00052 simpleMeasuredStateMsg.name[0] = roboDynJoints[0];
00053
00054 completeMeasuredStateMsg.name.resize(completeMessageSize);
00055 completeMeasuredStateMsg.position.resize(completeMessageSize);
00056 completeMeasuredStateMsg.velocity.resize(completeMessageSize);
00057 completeMeasuredStateMsg.effort.resize(completeMessageSize);
00058
00059 completeMeasuredStateMsg.name[0] = roboDynJoints[0];
00060 completeMeasuredStateMsg.name[1] = encoderName;
00061
00062 commandedStateMsg.name.resize(1);
00063 commandedStateMsg.desiredPosition.resize(1);
00064 commandedStateMsg.desiredPositionVelocityLimit.resize(1);
00065 commandedStateMsg.feedForwardTorque.resize(1);
00066 commandedStateMsg.proportionalGain.resize(1);
00067 commandedStateMsg.derivativeGain.resize(1);
00068 commandedStateMsg.integralGain.resize(1);
00069 commandedStateMsg.positionLoopTorqueLimit.resize(1);
00070 commandedStateMsg.positionLoopWindupLimit.resize(1);
00071 commandedStateMsg.torqueLoopVelocityLimit.resize(1);
00072
00073 commandedStateMsg.name[0] = roboDynJoints[0];
00074
00075 jointCapabilityMsg.name.resize(1);
00076 jointCapabilityMsg.positionLimitMax.resize(1);
00077 jointCapabilityMsg.positionLimitMin.resize(1);
00078 jointCapabilityMsg.velocityLimit.resize(1);
00079 jointCapabilityMsg.torqueLimit.resize(1);
00080
00081 jointCapabilityMsg.name[0] = roboDynJoints[0];
00082
00083 positionVals.resize(completeMessageSize, 0.);
00084 velocityVals.resize(completeMessageSize, 0.);
00085 effortVals.resize(completeMessageSize, 0.);
00086
00088 TiXmlDocument file(parameterFile.c_str());
00089 bool loadOkay = file.LoadFile();
00090
00091 if (!loadOkay)
00092 {
00093 stringstream err;
00094 err << "Failed to load file [" << parameterFile << "]";
00095 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::FATAL, err.str());
00096 throw runtime_error(err.str());
00097 }
00098
00099 TiXmlHandle doc(&file);
00100 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::INFO, "CommandFile [" + parameterFile + "] successfully loaded.");
00101
00102
00103 TiXmlHandle parametersElement(doc.FirstChildElement("ApiMap"));
00104
00105 if (parametersElement.ToElement())
00106 {
00108 jointPositionStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointPositionStatus"));
00109 encoderPositionStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderPositionStatus"));
00110 jointVelocityStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointVelocityStatus"));
00111 encoderVelocityStatusElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "EncoderVelocityStatus"));
00113 desiredPositionCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "DesiredPositionCommand"));
00114 desiredPositionVelocityLimitCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "DesiredPositionVelocityLimitCommand"));
00116 jointKinematicOffset = io.getBrainstemCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "APSKinematicOffset")));
00117 jointCapabilityMsg.positionLimitMax[0] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLimitMax")))
00118 + jointKinematicOffset;
00119 jointCapabilityMsg.positionLimitMin[0] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "PositionLimitMin")))
00120 + jointKinematicOffset;
00121 jointGearRatio = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointGearRatio")));
00122
00123 if (jointGearRatio <= 0.0)
00124 {
00125 stringstream err;
00126 err << "[" << StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "JointGearRatio")) << "] must be positive. Provided value of [" << jointGearRatio << "] is invalid.";
00127 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::ERROR, err.str());
00128 throw runtime_error(err.str());
00129 }
00130
00131 jointCapabilityMsg.velocityLimit[0] = io.getMotorCoeff(StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotorVelLimit"))) / jointGearRatio ;
00132
00134 brakePwmCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "BrakePWM"));
00135 motComLimitCommandElement = StringUtilities::makeFullyQualifiedRoboDynElement( roboDynJoints[0], ApiMap::getXmlElementValue(parametersElement, "MotComLimit"));
00136
00137 if (not(io.hasBrainstemCoeff(brakePwmCommandElement)))
00138 {
00139 stringstream err;
00140 err << "The BrainstemCoeff " << brakePwmCommandElement << " does not exist";
00141 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00142 throw runtime_error(err.str());
00143 }
00144
00145 if (not(io.hasBrainstemCoeff(motComLimitCommandElement)))
00146 {
00147 stringstream err;
00148 err << "The BrainstemCoeff " << motComLimitCommandElement << " does not exist";
00149 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandSeriesElastic", Priority::ERROR, err.str());
00150 throw runtime_error(err.str());
00151 }
00152
00153 io.setUInt16(brakePwmCommandElement, io.getBrainstemCoeff(brakePwmCommandElement));
00154 io.setUInt16(motComLimitCommandElement, io.getBrainstemCoeff(motComLimitCommandElement));
00155 }
00156 else
00157 {
00158 stringstream err;
00159 err << "The file " << parameterFile << " has no element named [ApiMap]";
00160 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::ERROR, err.str());
00161 throw runtime_error(err.str());
00162 }
00163 }
00164
00165 void JointCommandRigid::updateMeasuredState(r2_msgs::JointControlData msg)
00166 {
00167
00168 if (msg.coeffState.state == r2_msgs::JointControlCoeffState::LOADED)
00169 {
00171 positionVals[0] = io.getFloat(jointPositionStatusElement) + jointKinematicOffset;
00172 velocityVals[0] = io.getFloat(jointVelocityStatusElement);
00173 effortVals[0] = 0;
00174
00176 positionVals[1] = io.getFloat(encoderPositionStatusElement);
00177 velocityVals[1] = io.getFloat(encoderVelocityStatusElement);
00178 effortVals[1] = 0;
00179 }
00180 else
00181 {
00182 positionVals.assign(completeMessageSize, 0.0);
00183 velocityVals.assign(completeMessageSize, 0.0);
00184 effortVals.assign(completeMessageSize, 0.0);
00185 }
00186 }
00187
00188 sensor_msgs::JointState JointCommandRigid::getSimpleMeasuredState()
00189 {
00190 simpleMeasuredStateMsg.header.stamp = ros::Time::now();
00191
00192 simpleMeasuredStateMsg.position[0] = positionVals[0];
00193 simpleMeasuredStateMsg.velocity[0] = velocityVals[0];
00194 simpleMeasuredStateMsg.effort[0] = effortVals[0];
00195
00196 return simpleMeasuredStateMsg;
00197 }
00198
00199 sensor_msgs::JointState JointCommandRigid::getCompleteMeasuredState()
00200 {
00201 completeMeasuredStateMsg.header.stamp = ros::Time::now();
00202
00203 completeMeasuredStateMsg.position = positionVals;
00204 completeMeasuredStateMsg.velocity = velocityVals;
00205 completeMeasuredStateMsg.effort = effortVals;
00206
00207 return completeMeasuredStateMsg;
00208 }
00209
00210 void JointCommandRigid::setFaultState()
00211 {
00212 }
00213
00214 r2_msgs::JointCommand JointCommandRigid::getCommandedState()
00215 {
00216 commandedStateMsg.header.stamp = ros::Time::now();
00217
00218 commandedStateMsg.desiredPosition[0] = io.getFloat(desiredPositionCommandElement) + jointKinematicOffset;
00219 commandedStateMsg.desiredPositionVelocityLimit[0] = io.getFloat(desiredPositionVelocityLimitCommandElement);
00220
00221 return commandedStateMsg;
00222 }
00223
00224 void JointCommandRigid::setCommand(r2_msgs::JointCommand msg, r2_msgs::JointControlData)
00225 {
00226 if (msg.name.size() != 1)
00227 {
00228 stringstream err;
00229 err << "setCommand() requires one and only one entry in JointCommand.name when setCommand() is called.";
00230 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::ERROR, err.str());
00231 throw runtime_error(err.str());
00232 }
00233
00234 if ((msg.desiredPosition.size() > 1) ||
00235 (msg.desiredPositionVelocityLimit.size() > 1) ||
00236 (msg.feedForwardTorque.size() > 1) ||
00237 (msg.proportionalGain.size() > 1) ||
00238 (msg.derivativeGain.size() > 1) ||
00239 (msg.integralGain.size() > 1) ||
00240 (msg.positionLoopTorqueLimit.size() > 1) ||
00241 (msg.positionLoopWindupLimit.size() > 1) ||
00242 (msg.torqueLoopVelocityLimit.size() > 1))
00243 {
00244 stringstream err;
00245 err << "setCommand() requires at most one entry in JointCommand.* when setCommand() is called.";
00246 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::ERROR, err.str());
00247 throw runtime_error(err.str());
00248 }
00249
00250 if (msg.name[0] != roboDynJoints[0])
00251 {
00252 stringstream err;
00253 err << "setCommand() received JointCommand message for joint [" << msg.name[0] << "]. It expected [" << roboDynJoints[0] << "].";
00254 NasaCommonLogging::Logger::log("gov.nasa.robonet.JointCommandRigid", Priority::ERROR, err.str());
00255 throw runtime_error(err.str());
00256 }
00257
00258 if (not msg.desiredPosition.empty())
00259 {
00260 io.setFloat(desiredPositionCommandElement, msg.desiredPosition[0] - jointKinematicOffset);
00261 }
00262
00263 if (not msg.desiredPositionVelocityLimit.empty())
00264 {
00265 io.setFloat(desiredPositionVelocityLimitCommandElement, msg.desiredPositionVelocityLimit[0]);
00266 }
00267 }
00268
00269 r2_msgs::JointCapability JointCommandRigid::getCapability()
00270 {
00271 return jointCapabilityMsg;
00272 }