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;
}
Exemple #2
0
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;
}
Exemple #3
0
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;
}
Exemple #4
0
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;
    }
Exemple #7
0
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;
}
Exemple #15
0
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 );
}
Exemple #16
0
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;
    }
Exemple #18
0
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);
}
Exemple #19
0
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);
}
Exemple #20
0
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);
}
Exemple #21
0
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);
}
Exemple #22
0
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);
}
Exemple #23
0
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);
}
Exemple #24
0
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;
}
Exemple #25
0
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;
}
Exemple #26
0
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 );
}
Exemple #27
0
  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 > ();
    }

  }
Exemple #28
0
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;
}
Exemple #29
0
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;
    
}
Exemple #30
0
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;
}