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; }