Object3D* SceneParser::parseObject(char token[MAX_PARSER_TOKEN_LENGTH]) {
Object3D *answer = NULL;
if (!strcmp(token, "Group")) {
answer = (Object3D*)parseGroup();
}
else if (!strcmp(token, "Sphere")) {
answer = (Object3D*)parseSphere();
}
else if (!strcmp(token, "Plane")) {
answer = (Object3D*)parsePlane();
}
else if (!strcmp(token, "Triangle")) {
answer = (Object3D*)parseTriangle();
}
else if (!strcmp(token, "TriangleMesh")) {
answer = (Object3D*)parseTriangleMesh();
}
else if (!strcmp(token, "Transform")) {
answer = (Object3D*)parseTransform();
}
else {
printf("Unknown token in parseObject: '%s'\n", token);
exit(0);
}
return answer;
}
bool esvg::Polyline::parseXML(const exml::Element& _element, mat2& _parentTrans, vec2& _sizeMax) {
// line must have a minimum size...
m_paint.strokeWidth = 1;
if (_element.exist() == false) {
return false;
}
parseTransform(_element);
parsePaintAttr(_element);
// add the property of the parrent modifications ...
m_transformMatrix *= _parentTrans;
std::string sss1 = _element.attributes["points"];
if (sss1.size() == 0) {
ESVG_ERROR("(l "<<_element.getPos()<<") polyline: missing points attribute");
return false;
}
_sizeMax.setValue(0,0);
ESVG_VERBOSE("Parse polyline : \"" << sss1 << "\"");
const char* sss = sss1.c_str();
while ('\0' != sss[0]) {
vec2 pos;
int32_t n;
if (sscanf(sss, "%f,%f %n", &pos.m_floats[0], &pos.m_floats[1], &n) == 2) {
m_listPoint.push_back(pos);
_sizeMax.setValue(std::max(_sizeMax.x(), pos.x()),
std::max(_sizeMax.y(), pos.y()));
sss += n;
} else {
break;
}
}
return true;
}
bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config, ErrorLogger* logger)
{
collision.m_linkLocalFrame.setIdentity();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(collision.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(collision.m_geometry,geom,logger))
{
return false;
}
const char *name_char = config->Attribute("name");
if (name_char)
collision.m_name = name_char;
return true;
}
bool esvg::Rectangle::parseXML(const exml::Element& _element, mat2& _parentTrans, vec2& _sizeMax) {
if (_element.exist() == false) {
return false;
}
m_position.setValue(0.0f, 0.0f);
m_size.setValue(0.0f, 0.0f);
m_roundedCorner.setValue(0.0f, 0.0f);
parseTransform(_element);
parsePaintAttr(_element);
// add the property of the parrent modifications ...
m_transformMatrix *= _parentTrans;
parsePosition(_element, m_position, m_size);
std::string content = _element.attributes["rx"];
if (content.size()!=0) {
m_roundedCorner.setX(parseLength(content));
}
content = _element.attributes["ry"];
if (content.size()!=0) {
m_roundedCorner.setY(parseLength(content));
}
_sizeMax.setValue(m_position.x() + m_size.x() + m_paint.strokeWidth,
m_position.y() + m_size.y() + m_paint.strokeWidth);
return true;
}
Matrix3 SvgTransformParser::parse(std::string const &str)
{
Matrix3 result;
Tokenizer tokenizer(str, separator_);
State state = BEFORE_TYPE_STATE;
for (Tokenizer::iterator i = tokenizer.begin(); i != tokenizer.end();
++i)
{
token_ = *i;
if (token_ == "(") {
args_.clear();
state = BEFORE_ARG_STATE;
} else if (token_ == ")") {
// debugPrintTransform();
result *= parseTransform();
state = BEFORE_TYPE_STATE;
} else {
if (state == BEFORE_TYPE_STATE) {
type_ = token_;
state = AFTER_TYPE_STATE;
} else if (state == BEFORE_ARG_STATE ||
state == AFTER_ARG_STATE)
{
args_.push_back(atof(token_.c_str()));
state = AFTER_ARG_STATE;
}
}
}
return result;
}
Frame *XMLDataParser::parseTransformFrame(const dragonBones::XMLElement *frameXML, uint frameRate)
{
TransformFrame *frame = new TransformFrame();
parseFrame(frameXML, frame, frameRate);
frame->visible = uint(frameXML->IntAttribute(ConstValues::A_HIDE.c_str())) != 1;
frame->tweenEasing = Number(frameXML->DoubleAttribute(ConstValues::A_TWEEN_EASING.c_str()));
frame->tweenRotate = int(frameXML->DoubleAttribute(ConstValues::A_TWEEN_ROTATE.c_str()));
frame->displayIndex = int(frameXML->DoubleAttribute(ConstValues::A_DISPLAY_INDEX.c_str()));
//
frame->zOrder = Number(frameXML->DoubleAttribute(ConstValues::A_Z_ORDER.c_str()));
parseTransform(frameXML->FirstChildElement(ConstValues::TRANSFORM.c_str()), &frame->global, &frame->pivot);
frame->transform = frame->global;
const dragonBones::XMLElement * colorTransformXML = frameXML->FirstChildElement(ConstValues::COLOR_TRANSFORM.c_str());
if(colorTransformXML)
{
frame->color = new ColorTransform();
frame->color->alphaOffset = Number(colorTransformXML->DoubleAttribute(ConstValues::A_ALPHA_OFFSET.c_str()));
frame->color->redOffset = Number(colorTransformXML->DoubleAttribute(ConstValues::A_RED_OFFSET.c_str()));
frame->color->greenOffset = Number(colorTransformXML->DoubleAttribute(ConstValues::A_GREEN_OFFSET.c_str()));
frame->color->blueOffset = Number(colorTransformXML->DoubleAttribute(ConstValues::A_BLUE_OFFSET.c_str()));
frame->color->alphaMultiplier = Number(colorTransformXML->DoubleAttribute(ConstValues::A_ALPHA_MULTIPLIER.c_str())) * 0.01f;
frame->color->redMultiplier = Number(colorTransformXML->DoubleAttribute(ConstValues::A_RED_MULTIPLIER.c_str())) * 0.01f;
frame->color->greenMultiplier = Number(colorTransformXML->DoubleAttribute(ConstValues::A_GREEN_MULTIPLIER.c_str())) * 0.01f;
frame->color->blueMultiplier = Number(colorTransformXML->DoubleAttribute(ConstValues::A_BLUE_MULTIPLIER.c_str())) * 0.01f;
}
return frame;
}
bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(visual.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(visual.m_geometry,geom,logger))
{
return false;
}
const char *name_char = config->Attribute("name");
if (name_char)
visual.m_name = name_char;
visual.m_hasLocalMaterial = false;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
//todo(erwincoumans) skip materials in SDF for now (due to complexity)
if (mat && !m_parseSDF)
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
visual.m_hasLocalMaterial = true;
}
}
}
return true;
}
VRPNTrackerAxis::VRPNTrackerAxis(const std::string& axis) :
_transform( Matrix4f::IDENTITY )
{
_transform(2,2) = -1.f; // TODO: remove old default; specify explicitly
if (!axis.empty())
{
if (!parseTransform(axis, _transform))
LBWARN << "VRPN tracker couldn't process axis option: " << axis;
}
}
bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger)
{
inertia.m_linkLocalFrame.setIdentity();
inertia.m_mass = 0.f;
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(inertia.m_linkLocalFrame,o,logger))
{
return false;
}
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
logger->reportError("Inertial element must have a mass element");
return false;
}
if (!mass_xml->Attribute("value"))
{
logger->reportError("Inertial: mass element must have value attribute");
return false;
}
inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
logger->reportError("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")))
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
return true;
}
bool UrdfParser::parseVisual(UrdfVisual& visual, TiXmlElement* config, ErrorLogger* logger)
{
visual.m_linkLocalFrame.setIdentity();
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(visual.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(visual.m_geometry,geom,logger))
{
return false;
}
const char *name_char = config->Attribute("name");
if (name_char)
visual.m_name = name_char;
visual.m_hasLocalMaterial = false;
// Material
TiXmlElement *mat = config->FirstChildElement("material");
if (mat)
{
// get material name
if (!mat->Attribute("name"))
{
logger->reportError("Visual material must contain a name attribute");
return false;
}
visual.m_materialName = mat->Attribute("name");
// try to parse material element in place
TiXmlElement *t = mat->FirstChildElement("texture");
TiXmlElement *c = mat->FirstChildElement("color");
if (t||c)
{
if (parseMaterial(visual.m_localMaterial, mat,logger))
{
visual.m_hasLocalMaterial = true;
}
}
}
return true;
}
bool UrdfParser::parseCollision(UrdfCollision& collision, TiXmlElement* config, ErrorLogger* logger)
{
collision.m_linkLocalFrame.setIdentity();
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(collision.m_linkLocalFrame, pose,logger,m_parseSDF);
}
}
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(collision.m_linkLocalFrame, o,logger))
return false;
}
// Geometry
TiXmlElement *geom = config->FirstChildElement("geometry");
if (!parseGeometry(collision.m_geometry,geom,logger))
{
return false;
}
const char *name_char = config->Attribute("name");
if (name_char)
collision.m_name = name_char;
const char *concave_char = config->Attribute("concave");
if (concave_char)
collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH;
return true;
}
jobjectArray MetaInterface::parseTransformTree(o3d::Transform* root) {
// walk the transform tree until we get to the specified node, then return info on that.
jobjectArray result = NULL;
// first element of mElements should be "geto3d", or maybe "seto3d" one day.
if (root) {
if (mElements.getCount() > 1) {
result = parseTransform(root, 1);
} else {
result = printTransform(root);
}
}
return result;
}
DisplayData *XMLDataParser::parseDisplayData(const dragonBones::XMLElement *displayXML, SkeletonData *data)
{
DisplayData *displayData = new DisplayData();
displayData->name = displayXML->Attribute(ConstValues::A_NAME.c_str());
displayData->type = displayXML->Attribute(ConstValues::A_TYPE.c_str());
displayData->pivot = data->addSubTexturePivot(
0,
0,
displayData->name
);
parseTransform(displayXML->FirstChildElement(ConstValues::TRANSFORM.c_str()), &displayData->transform, &displayData->pivot);
return displayData;
}
jobjectArray MetaInterface::parseTransform(o3d::Transform* root, const int current_index) {
jobjectArray result = NULL;
if (current_index == mElements.getCount() - 1) { // this is the leaf
result = printTransform(root);
} else {
const char* next_child = mElements.get(current_index);
if (isNumber(next_child)) {
int array_index = atoi(next_child);
if (root->GetChildrenRefs().size() > array_index) {
result = parseTransform(root->GetChildrenRefs()[array_index], current_index + 1);
}
}
}
return result;
}
static DataRef<Transform>* parseTransform( InputDeck& deck, token_list_t::iterator& i, bool degree_format = false ){
token_list_t args;
std::string next_token = *i;
if( next_token.find("(") != next_token.npos ){
do{
args.push_back( next_token );
next_token = *(++i);
}
while( next_token.find(")") == next_token.npos );
}
args.push_back( next_token );
return parseTransform( deck, args, degree_format );
}
void SVGGradient::parse(xmlNodePtr node) {
attribs.parseNode(node);
const char *tmp = attribs["gradientUnits"];
if(tmp) {
if(!strcmp(tmp, "userSpaceOnUse")) {
userSpace = true;
} else {
userSpace = false;
}
} else {
userSpace = false;
}
parseGradient();
parseSpreadMethod();
parseTransform();
parseStops(node);
}
BoneData *XMLDataParser::parseBoneData(const dragonBones::XMLElement *boneXML)
{
BoneData *boneData = new BoneData();
boneData->name = boneXML->Attribute(ConstValues::A_NAME.c_str());
const char *parent = boneXML->Attribute(ConstValues::A_PARENT.c_str());
if(parent)
{
boneData->parent = parent;
}
boneData->length = Number(boneXML->DoubleAttribute(ConstValues::A_LENGTH.c_str()));
const char *inheritScale = boneXML->Attribute(ConstValues::A_SCALE_MODE.c_str());
if (inheritScale)
{
boneData->scaleMode = atoi(inheritScale);
}
const char* fixedRotation = boneXML->Attribute(ConstValues::A_FIXED_ROTATION.c_str());
if(fixedRotation)
{
if(strcmp(fixedRotation , "0") == 0
|| strcmp(fixedRotation , "false") == 0
|| strcmp(fixedRotation , "no") == 0
|| strcmp(fixedRotation , "") == 0
)
{
boneData->fixedRotation = false;
}
else
{
boneData->fixedRotation = true;
}
}
else
{
boneData->fixedRotation = false;
}
parseTransform(boneXML->FirstChildElement(ConstValues::TRANSFORM.c_str()), &boneData->global);
boneData->transform = boneData->global;
return boneData;
}
void SceneParser::parseSphere(void)
{
parseToken(Scanner::LeftCurly);
Vector3 center;
parseVector(center);
float radius;
parseNumber(radius);
Sphere* s = new Sphere(center, radius);
raytracer->addObject(s);
advance();
parseMaterial(s);
parseTransform(s);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parseBox(void)
{
parseToken(Scanner::LeftCurly);
Vector3 minCorner;
parseVector(minCorner);
Vector3 maxCorner;
parseVector(maxCorner);
Box* b = new Box(minCorner, maxCorner);
raytracer->addObject(b);
advance();
parseMaterial(b);
parseTransform(b);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parsePlane(void)
{
parseToken(Scanner::LeftCurly);
Vector3 center;
parseVector(center);
Vector3 right;
parseVector(right);
Vector3 up;
parseVector(up);
Plane* p = new Plane(center, right, up);
raytracer->addObject(p);
advance();
parseMaterial(p);
parseTransform(p);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parseCylinder(void)
{
parseToken(Scanner::LeftCurly);
Vector3 base;
parseVector(base);
float height;
parseNumber(height);
float radius;
parseNumber(radius);
Cylinder* c = new Cylinder(base, height, radius);
raytracer->addObject(c);
advance();
parseMaterial(c);
parseTransform(c);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parseTriangle(void)
{
parseToken(Scanner::LeftCurly);
Vector3* p1 = new Vector3();
parseVector(*p1);
Vector3* p2 = new Vector3();
parseVector(*p2);
Vector3* p3 = new Vector3();
parseVector(*p3);
Triangle* t = new Triangle(p1, p2, p3, true);
raytracer->addObject(t);
advance();
parseMaterial(t);
parseTransform(t);
acceptToken(Scanner::RightCurly);
}
void SceneParser::parseMesh(void)
{
parseToken(Scanner::LeftCurly);
parseToken(Scanner::String);
string fileName = scanner.tokenText();
vector<Triangle*>* newMesh = new vector<Triangle*>;
vector<Vector3>* newPoints = new vector<Vector3>;
vector<Vector3>* newNormals = new vector<Vector3>;
if(!parser->loadObj(fileName, *newMesh, *newPoints, *newNormals))
errorFlag = true;
Mesh* m = new Mesh(newMesh, newPoints, newNormals);
raytracer->addObject(m);
advance();
parseMaterial(m);
parseTransform(m);
acceptToken(Scanner::RightCurly);
}
bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger)
{
// Get Joint Name
const char *name = config->Attribute("name");
if (!name)
{
logger->reportError("unnamed joint found");
return false;
}
joint.m_name = name;
joint.m_parentLinkToJointTransform.setIdentity();
// Get transform from Parent Link to Joint Frame
TiXmlElement *origin_xml = config->FirstChildElement("origin");
if (origin_xml)
{
if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml,logger))
{
logger->reportError("Malformed parent origin element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
// Get Parent Link
TiXmlElement *parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
if (m_parseSDF)
{
joint.m_parentLinkName = std::string(parent_xml->GetText());
}
else
{
const char *pname = parent_xml->Attribute("link");
if (!pname)
{
logger->reportError("no parent link name specified for Joint link. this might be the root?");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_parentLinkName = std::string(pname);
}
}
}
// Get Child Link
TiXmlElement *child_xml = config->FirstChildElement("child");
if (child_xml)
{
if (m_parseSDF)
{
joint.m_childLinkName = std::string(child_xml->GetText());
}
else
{
const char *pname = child_xml->Attribute("link");
if (!pname)
{
logger->reportError("no child link name specified for Joint link [%s].");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_childLinkName = std::string(pname);
}
}
}
// Get Joint type
const char* type_char = config->Attribute("type");
if (!type_char)
{
logger->reportError("joint [%s] has no type, check to see if it's a reference.");
logger->reportError(joint.m_name.c_str());
return false;
}
std::string type_str = type_char;
if (type_str == "planar")
joint.m_type = URDFPlanarJoint;
else if (type_str == "floating")
joint.m_type = URDFFloatingJoint;
else if (type_str == "revolute")
joint.m_type = URDFRevoluteJoint;
else if (type_str == "continuous")
joint.m_type = URDFContinuousJoint;
else if (type_str == "prismatic")
joint.m_type = URDFPrismaticJoint;
else if (type_str == "fixed")
joint.m_type = URDFFixedJoint;
else
{
logger->reportError("Joint ");
logger->reportError(joint.m_name.c_str());
logger->reportError("has unknown type:");
logger->reportError(type_str.c_str());
return false;
}
if (m_parseSDF)
{
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
logger->reportWarning(joint.m_name.c_str());
joint.m_localJointAxis.setValue(1,0,0);
}
else{
TiXmlElement *xyz_xml = axis_xml->FirstChildElement("xyz");
if (xyz_xml) {
if (!parseVector3(joint.m_localJointAxis,std::string(xyz_xml->GetText()),logger))
{
logger->reportError("Malformed axis element:");
logger->reportError(joint.m_name.c_str());
logger->reportError(" for joint:");
logger->reportError(xyz_xml->GetText());
return false;
}
}
TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit");
if (limit_xml)
{
if (!parseJointLimits(joint, limit_xml,logger))
{
logger->reportError("Could not parse limit element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
else if (joint.m_type == URDFRevoluteJoint)
{
logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
logger->reportError(joint.m_name.c_str());
return false;
}
else if (joint.m_type == URDFPrismaticJoint)
{
logger->reportError("Joint is of type PRISMATIC without limits");
logger->reportError( joint.m_name.c_str());
return false;
}
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
if (!parseJointDynamics(joint, prop_xml,logger))
{
logger->reportError("Could not parse dynamics element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
}
}
}
else
{
// Get Joint Axis
if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
{
// axis
TiXmlElement *axis_xml = config->FirstChildElement("axis");
if (!axis_xml){
logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
logger->reportWarning(joint.m_name.c_str());
joint.m_localJointAxis.setValue(1,0,0);
}
else{
if (axis_xml->Attribute("xyz"))
{
if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger))
{
logger->reportError("Malformed axis element:");
logger->reportError(joint.m_name.c_str());
logger->reportError(" for joint:");
logger->reportError(axis_xml->Attribute("xyz"));
return false;
}
}
}
}
// Get limit
TiXmlElement *limit_xml = config->FirstChildElement("limit");
if (limit_xml)
{
if (!parseJointLimits(joint, limit_xml,logger))
{
logger->reportError("Could not parse limit element for joint:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
else if (joint.m_type == URDFRevoluteJoint)
{
logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
logger->reportError(joint.m_name.c_str());
return false;
}
else if (joint.m_type == URDFPrismaticJoint)
{
logger->reportError("Joint is of type PRISMATIC without limits");
logger->reportError( joint.m_name.c_str());
return false;
}
joint.m_jointDamping = 0;
joint.m_jointFriction = 0;
// Get Dynamics
TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
if (prop_xml)
{
// Get joint damping
const char* damping_str = prop_xml->Attribute("damping");
if (damping_str)
{
joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
}
// Get joint friction
const char* friction_str = prop_xml->Attribute("friction");
if (friction_str)
{
joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
}
if (damping_str == NULL && friction_str == NULL)
{
logger->reportError("joint dynamics element specified with no damping and no friction");
return false;
}
}
}
return true;
}
bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
{
logger->reportError("Link with no name");
return false;
}
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_linkTransformInWorld.setIdentity();
}
else
{
parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
if (!parseInertia(link.m_inertia, i,logger))
{
logger->reportError("Could not parse inertial element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
} else
{
if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0)
{
link.m_inertia.m_mass = 0.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 0.f;
link.m_inertia.m_iyy = 0.f;
link.m_inertia.m_izz= 0.f;
} else
{
logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
link.m_inertia.m_mass = 1.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 1.f;
link.m_inertia.m_iyy = 1.f;
link.m_inertia.m_izz= 1.f;
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
else
{
logger->reportError("Could not parse visual element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
UrdfCollision col;
if (parseCollision(col, col_xml,logger))
{
link.m_collisionArray.push_back(col);
}
else
{
logger->reportError("Could not parse collision element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
return true;
}
static FillNode parseFillNode( InputDeck& deck, token_list_t::iterator& i, const token_list_t::iterator& end, bool degree_format = false ){
// simple fill. Format is n or n (transform). Transform may be either a TR card number
// or an immediate transform
int n; // the filling universe
DataRef<Transform>* t;
bool has_transform = false;
std::string first_token = *i;
size_t paren_idx = first_token.find("(");
std::string second_token;
if( paren_idx != first_token.npos ){
// first_token has an open paren
std::string n_str(first_token, 0, paren_idx);
n = makeint(n_str);
second_token = first_token.substr(paren_idx,first_token.npos);
has_transform = true;
}
else{
n = makeint(first_token);
if( ++i != end ){
second_token = *i;
if( second_token[0] == '(' ){
has_transform = true;
}
else{
// the next token didn't belong to this fill
i--;
}
}
else{ i--; }
}
if( has_transform ){
token_list_t transform_tokens;
std::string next_token = second_token;
while( next_token.find(")") == next_token.npos ){
transform_tokens.push_back(next_token);
next_token = *(++i);
}
transform_tokens.push_back( next_token );
t = parseTransform( deck, transform_tokens, degree_format );
}
else{
t = new NullRef<Transform>();
}
if( n < 0 ){
n = -n; // TODO: handle negative universe numbers specially
}
return FillNode (n, t );
}
void makeData(){
for( token_list_t::iterator i = data.begin(); i!=data.end(); ++i ){
std::string token = *i;
if( token == "trcl" || token == "*trcl" ){
bool degree_format = (token[0] == '*');
i++;
trcl = parseTransform( parent_deck, i, degree_format );
} // token == {*}trcl
else if( token == "u" ){
universe = makeint(*(++i));
} // token == "u"
else if ( token == "lat" ){
int lat_designator = makeint(*(++i));
assert( lat_designator >= 0 && lat_designator <= 2 );
lat_type = static_cast<lattice_type_t>(lat_designator);
if( OPT_DEBUG ) std::cout << "cell " << ident << " is lattice type " << lat_type << std::endl;
}
else if ( token == "mat" ){
material = makeint(*(++i));
}
else if ( token == "rho" ){
rho = makedouble(*(++i));
}
else if ( token.length() == 5 && token.substr(0,4) == "imp:" ){
double imp = makedouble(*(++i));
importances[token[4]] = imp;
}
else if( token == "fill" || token == "*fill" ){
bool degree_format = (token[0] == '*');
std::string next_token = *(++i);
// an explicit lattice grid exists if
// * the next token contains a colon, or
// * the token after it exists and starts with a colon
bool explicit_grid = next_token.find(":") != next_token.npos;
explicit_grid = explicit_grid || (i+1 != data.end() && (*(i+1)).at(0) == ':' );
if( explicit_grid ){
// convert the grid specifiers (x1:x2, y1:y2, z1:z2) into three spaceless strings for easier parsing
std::string gridspec[3];
for( int dim = 0; dim < 3; ++dim ){
std::string spec;
// add tokens to the spec string until it contains a colon but does not end with one
do{
spec += *i;
i++;
}
while( spec.find(":") == spec.npos || spec.at(spec.length()-1) == ':' );
if(OPT_DEBUG) std::cout << "gridspec[" << dim << "]: " << spec << std::endl;
gridspec[dim] = spec;
}
irange ranges[3];
const char* range_str;
char *p;
int num_elements = 1;
for( int dim = 0; dim < 3; ++dim ){
range_str = gridspec[dim].c_str();
ranges[dim].first = strtol(range_str, &p, 10);
ranges[dim].second = strtol(p+1, NULL, 10);
if( ranges[dim].second != ranges[dim].first ){
num_elements *= ( ranges[dim].second - ranges[dim].first )+1;
}
}
std::vector<FillNode> elements;
for( int j = 0; j < num_elements; ++j ){
elements.push_back( parseFillNode( parent_deck, i, data.end(), degree_format ) );
i++;
}
i--;
fill = new ImmediateRef< Fill >( Fill( ranges[0], ranges[1], ranges[2], elements) );
}
else{ // no explicit grid; fill card is a single fill node
FillNode filler = parseFillNode( parent_deck, i, data.end(), degree_format );
fill = new ImmediateRef< Fill >( Fill(filler) );
}
} // token == {*}fill
}
// ensure data pointers are valid
if( !trcl ) {
trcl = new NullRef< Transform >();
}
if( !fill ){
fill = new NullRef< Fill > ();
}
}
bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
{
TiXmlDocument xml_doc;
xml_doc.Parse(sdfText);
if (xml_doc.Error())
{
logger->reportError(xml_doc.ErrorDesc());
xml_doc.ClearError();
return false;
}
TiXmlElement *sdf_xml = xml_doc.FirstChildElement("sdf");
if (!sdf_xml)
{
logger->reportError("expected an sdf element");
return false;
}
TiXmlElement *world_xml = sdf_xml->FirstChildElement("world");
if (!world_xml)
{
logger->reportError("expected a world element");
return false;
}
// Get all model (robot) elements
for (TiXmlElement* robot_xml = world_xml->FirstChildElement("model"); robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
{
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);
// Get robot name
const char *name = robot_xml->Attribute("name");
if (!name)
{
logger->reportError("Expected a name for robot");
return false;
}
localModel->m_name = name;
TiXmlElement* pose_xml = robot_xml->FirstChildElement("pose");
if (0==pose_xml)
{
localModel->m_rootTransformInWorld.setIdentity();
}
else
{
parseTransform(localModel->m_rootTransformInWorld,pose_xml,logger,m_parseSDF);
}
// Get all Material elements
for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
{
UrdfMaterial* material = new UrdfMaterial;
parseMaterial(*material, material_xml, logger);
UrdfMaterial** mat =localModel->m_materials.find(material->m_name.c_str());
if (mat)
{
logger->reportWarning("Duplicate material");
} else
{
localModel->m_materials.insert(material->m_name.c_str(),material);
}
}
// char msg[1024];
// sprintf(msg,"Num materials=%d", m_model.m_materials.size());
// logger->printMessage(msg);
for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
{
UrdfLink* link = new UrdfLink;
if (parseLink(*localModel, *link, link_xml,logger))
{
if (localModel->m_links.find(link->m_name.c_str()))
{
logger->reportError("Link name is not unique, link names in the same model have to be unique");
logger->reportError(link->m_name.c_str());
return false;
} else
{
//copy model material into link material, if link has no local material
for (int i=0;i<link->m_visualArray.size();i++)
{
UrdfVisual& vis = link->m_visualArray.at(i);
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
{
UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
if (mat && *mat)
{
vis.m_localMaterial = **mat;
} else
{
//logger->reportError("Cannot find material with name:");
//logger->reportError(vis.m_materialName.c_str());
}
}
}
localModel->m_links.insert(link->m_name.c_str(),link);
}
} else
{
logger->reportError("failed to parse link");
delete link;
return false;
}
}
if (localModel->m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
}
// Get all Joint elements
for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
{
UrdfJoint* joint = new UrdfJoint;
if (parseJoint(*joint, joint_xml,logger))
{
if (localModel->m_joints.find(joint->m_name.c_str()))
{
logger->reportError("joint '%s' is not unique.");
logger->reportError(joint->m_name.c_str());
return false;
}
else
{
localModel->m_joints.insert(joint->m_name.c_str(),joint);
}
}
else
{
logger->reportError("joint xml is not initialized correctly");
return false;
}
}
bool ok(initTreeAndRoot(*localModel,logger));
if (!ok)
{
return false;
}
m_sdfModels.push_back(localModel);
}
return true;
}
bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger)
{
inertia.m_linkLocalFrame.setIdentity();
inertia.m_mass = 0.f;
if(m_parseSDF)
{
TiXmlElement* pose = config->FirstChildElement("pose");
if (pose)
{
parseTransform(inertia.m_linkLocalFrame, pose,logger,m_parseSDF);
}
}
// Origin
TiXmlElement *o = config->FirstChildElement("origin");
if (o)
{
if (!parseTransform(inertia.m_linkLocalFrame,o,logger))
{
return false;
}
}
TiXmlElement *mass_xml = config->FirstChildElement("mass");
if (!mass_xml)
{
logger->reportError("Inertial element must have a mass element");
return false;
}
if (m_parseSDF)
{
inertia.m_mass = urdfLexicalCast<double>(mass_xml->GetText());
} else
{
if (!mass_xml->Attribute("value"))
{
logger->reportError("Inertial: mass element must have value attribute");
return false;
}
inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
}
TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
if (!inertia_xml)
{
logger->reportError("Inertial element must have inertia element");
return false;
}
if (m_parseSDF)
{
TiXmlElement* ixx = inertia_xml->FirstChildElement("ixx");
TiXmlElement* ixy = inertia_xml->FirstChildElement("ixy");
TiXmlElement* ixz = inertia_xml->FirstChildElement("ixz");
TiXmlElement* iyy = inertia_xml->FirstChildElement("iyy");
TiXmlElement* iyz = inertia_xml->FirstChildElement("iyz");
TiXmlElement* izz = inertia_xml->FirstChildElement("izz");
if (ixx && ixy && ixz && iyy && iyz && izz)
{
inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
inertia.m_ixy = urdfLexicalCast<double>(ixy->GetText());
inertia.m_ixz = urdfLexicalCast<double>(ixz->GetText());
inertia.m_iyy = urdfLexicalCast<double>(iyy->GetText());
inertia.m_iyz = urdfLexicalCast<double>(iyz->GetText());
inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
} else
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
return false;
}
} else
{
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")))
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
}
return true;
}
bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
{
logger->reportError("Link with no name");
return false;
}
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_linkTransformInWorld.setIdentity();
}
else
{
parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
}
}
{
//optional 'contact' parameters
TiXmlElement* ci = config->FirstChildElement("contact");
if (ci)
{
TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
if (damping_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
} else
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("Link/contact: damping element must have value attribute");
return false;
}
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
}
}
{
TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
if (friction_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
} else
{
if (!friction_xml->Attribute("value"))
{
logger->reportError("Link/contact: lateral_friction element must have value attribute");
return false;
}
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
}
}
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
if (!parseInertia(link.m_inertia, i,logger))
{
logger->reportError("Could not parse inertial element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
} else
{
if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0)
{
link.m_inertia.m_mass = 0.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 0.f;
link.m_inertia.m_iyy = 0.f;
link.m_inertia.m_izz= 0.f;
} else
{
logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
link.m_inertia.m_mass = 1.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 1.f;
link.m_inertia.m_iyy = 1.f;
link.m_inertia.m_izz= 1.f;
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
else
{
logger->reportError("Could not parse visual element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
UrdfCollision col;
if (parseCollision(col, col_xml,logger))
{
link.m_collisionArray.push_back(col);
}
else
{
logger->reportError("Could not parse collision element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
return true;
}
