bool parseInertial(Inertial &i, TiXmlElement *config)
{
i.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parsePose(i.origin, o))
return false;
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
logError("Inertial element must have a mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
logError("Inertial: mass element must have value attribute");
return false;
}
if( !stringToDouble(mass_xml->Attribute("value"),i.mass) )
{
std::stringstream stm;
stm << "Inertial: mass [" << mass_xml->Attribute("value")
<< "] is not a float";
logError(stm.str().c_str());
return false;
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
logError("Inertial element must have inertia element");
return false;
}
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
if( !stringToDouble(inertia_xml->Attribute("ixx"),i.ixx)
|| !stringToDouble(inertia_xml->Attribute("ixy"),i.ixy)
|| !stringToDouble(inertia_xml->Attribute("ixz"),i.ixz)
|| !stringToDouble(inertia_xml->Attribute("iyy"),i.iyy)
|| !stringToDouble(inertia_xml->Attribute("iyz"),i.iyz)
|| !stringToDouble(inertia_xml->Attribute("izz"),i.izz) )
{
std::stringstream stm;
stm << "Inertial: one of the inertia elements is not a valid double:"
<< " ixx [" << inertia_xml->Attribute("ixx") << "]"
<< " ixy [" << inertia_xml->Attribute("ixy") << "]"
<< " ixz [" << inertia_xml->Attribute("ixz") << "]"
<< " iyy [" << inertia_xml->Attribute("iyy") << "]"
<< " iyz [" << inertia_xml->Attribute("iyz") << "]"
<< " izz [" << inertia_xml->Attribute("izz") << "]";
logError(stm.str().c_str());
return false;
}
return true;
}
bool parseInertial(Inertial &i, TiXmlElement *config)
{
i.clear();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parsePose(i.origin, o))
return false;
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
CONSOLE_BRIDGE_logError("Inertial element must have a mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
CONSOLE_BRIDGE_logError("Inertial: mass element must have value attribute");
return false;
}
try
{
i.mass = boost::lexical_cast<double>(mass_xml->Attribute("value"));
}
catch (boost::bad_lexical_cast &/*e*/)
{
std::stringstream stm;
stm << "Inertial: mass [" << mass_xml->Attribute("value")
<< "] is not a float";
CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
CONSOLE_BRIDGE_logError("Inertial element must have inertia element");
return false;
}
if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
CONSOLE_BRIDGE_logError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
try
{
i.ixx = boost::lexical_cast<double>(inertia_xml->Attribute("ixx"));
i.ixy = boost::lexical_cast<double>(inertia_xml->Attribute("ixy"));
i.ixz = boost::lexical_cast<double>(inertia_xml->Attribute("ixz"));
i.iyy = boost::lexical_cast<double>(inertia_xml->Attribute("iyy"));
i.iyz = boost::lexical_cast<double>(inertia_xml->Attribute("iyz"));
i.izz = boost::lexical_cast<double>(inertia_xml->Attribute("izz"));
}
catch (boost::bad_lexical_cast &/*e*/)
{
std::stringstream stm;
stm << "Inertial: one of the inertia elements is not a valid double:"
<< " ixx [" << inertia_xml->Attribute("ixx") << "]"
<< " ixy [" << inertia_xml->Attribute("ixy") << "]"
<< " ixz [" << inertia_xml->Attribute("ixz") << "]"
<< " iyy [" << inertia_xml->Attribute("iyy") << "]"
<< " iyz [" << inertia_xml->Attribute("iyz") << "]"
<< " izz [" << inertia_xml->Attribute("izz") << "]";
CONSOLE_BRIDGE_logError(stm.str().c_str());
return false;
}
return true;
}
