public static URDFRobot BuildRobot(string urdfContent, Dictionary <string, string> packages, Options options = new Options()) { if (options.loadMeshCb == null) { options.loadMeshCb = LoadMesh; } // load the XML doc XmlDocument doc = new XmlDocument(); doc.LoadXml(urdfContent); // Store the information about the link and the objects // indexed by link name Dictionary <string, XmlNode> xmlLinks = new Dictionary <string, XmlNode>(); Dictionary <string, XmlNode> xmlJoints = new Dictionary <string, XmlNode>(); // indexed by joint name Dictionary <string, URDFJoint> urdfJoints = new Dictionary <string, URDFJoint>(); Dictionary <string, URDFLink> urdfLinks = new Dictionary <string, URDFLink>(); // indexed by joint name Dictionary <string, string> parentNames = new Dictionary <string, string>(); // first node is the robot node (for the full robot) foreach (XmlNode n in doc.ChildNodes) { // first node is expected to be the robot // cycle through and find all the links for the robot first foreach (XmlNode xLink in n.ChildNodes) { if (xLink.Name == "link") { // Store the XML node for hte link xmlLinks.Add(xLink.Attributes["name"].Value, xLink); // create the link gameobject URDFLink urdfLink = new URDFLink(); urdfLink.name = xLink.Attributes["name"].Value; urdfLink.transform = new GameObject(urdfLink.name).transform; urdfLinks.Add(urdfLink.name, urdfLink); // Get the geometry node and skip it if there isn't one XmlNode[] visualNodes = GetXmlNodeChildrenByName(xLink, "visual"); List <GameObject> renderers = new List <GameObject>(); // Iterate over all the visual nodes foreach (var vn in visualNodes) { XmlNode geomNode = GetXmlNodeChildByName(vn, "geometry"); if (geomNode == null) { continue; } XmlNode matNode = GetXmlNodeChildByName(vn, "material"); Color col = Color.white; if (matNode != null) { XmlNode colNode = GetXmlNodeChildByName(matNode, "color"); if (colNode != null) { col = TupleToColor(colNode.Attributes["rgba"].Value); } // TODO: Load the textures // XmlNode texNode = GetXmlNodeChildByName(matNode, "texture"); // if (texNode != null) { } } // Get the mesh and the origin nodes XmlNode meshNode = GetXmlNodeChildByName(geomNode, "mesh"); XmlNode visOriginNode = GetXmlNodeChildByName(vn, "origin"); // take the visual origin and place on the renderer // use the visual origin to set the pose if necessary Vector3 visPos = Vector3.zero; if (visOriginNode != null && visOriginNode.Attributes["xyz"] != null) { visPos = TupleToVector3(visOriginNode.Attributes["xyz"].Value); } visPos = URDFToUnityPos(visPos); Vector3 visRot = Vector3.zero; if (visOriginNode != null && visOriginNode.Attributes["rpy"] != null) { visRot = TupleToVector3(visOriginNode.Attributes["rpy"].Value); } visRot = URDFToUnityRot(visRot); try { // try to load primitives if there is no mesh if (meshNode == null) { XmlNode primitiveNode = GetXmlNodeChildByName(geomNode, "box") ?? GetXmlNodeChildByName(geomNode, "sphere") ?? GetXmlNodeChildByName(geomNode, "cylinder"); if (primitiveNode != null) { GameObject go = null; switch (primitiveNode.Name) { case "box": go = GameObject.CreatePrimitive(PrimitiveType.Cube); go.transform.localScale = URDFToUnityPos(TupleToVector3(primitiveNode.Attributes[0].Value)); break; case "sphere": go = GameObject.CreatePrimitive(PrimitiveType.Sphere); go.transform.localScale = Vector3.one * (float.Parse(primitiveNode.Attributes[0].Value) * 2); break; case "cylinder": go = new GameObject(); var cPrimitive = GameObject.CreatePrimitive(PrimitiveType.Cylinder); cPrimitive.transform.parent = go.transform; var length = float.Parse(primitiveNode.Attributes[0].Value); var radius = float.Parse(primitiveNode.Attributes[1].Value); go.transform.localScale = new Vector3(radius * 2, length / 2, radius * 2); break; } Renderer r = go.GetComponent <Renderer>(); if (r == null) { r = go.GetComponentInChildren <Renderer>(); } go.transform.parent = urdfLink.transform; go.transform.localPosition = visPos; go.transform.localRotation = Quaternion.Euler(visRot); go.name = urdfLink.name + " geometry " + primitiveNode.Name; if (r) { r.material.color = col; renderers.Add(r.gameObject); if (Application.isPlaying) { Destroy(r.GetComponent <Collider>()); Destroy(r.GetComponent <Rigidbody>()); } else { DestroyImmediate(r.GetComponent <Collider>()); DestroyImmediate(r.GetComponent <Rigidbody>()); } } } } else { // load the STL file if possible // get the file path and split it string fileName = ResolveMeshPath(meshNode.Attributes["filename"].Value, packages, options.workingPath); Vector3 meshScale = Vector3.one; if (meshNode.Attributes["scale"] != null) { meshScale = TupleToVector3(meshNode.Attributes["scale"].Value); } meshScale = URDFToUnityScale(meshScale); // load all meshes string ext = Path.GetExtension(fileName).ToLower().Replace(".", ""); options.loadMeshCb(fileName, ext, models => { // create the rest of the meshes and child them to the click target for (int i = 0; i < models.Length; i++) { var trans = models[i].transform; trans.parent = urdfLink.transform; trans.localPosition = visPos; trans.localRotation = Quaternion.Euler(visRot); trans.localScale = meshScale; trans.name = urdfLink.name + " geometry " + i; foreach (Renderer r in trans.GetComponentsInChildren <Renderer>()) { r.material.color = col; } renderers.Add(trans.gameObject); // allows the urdf parser to be called from editor scripts outside of runtime without throwing errors // TODO: traverse over the children and do this if (Application.isPlaying) { Destroy(trans.GetComponent <Collider>()); Destroy(trans.GetComponent <Rigidbody>()); } else { DestroyImmediate(trans.GetComponent <Collider>()); DestroyImmediate(trans.GetComponent <Rigidbody>()); } } }); // save the geometry in the link urdfLink.geometry = renderers; } } catch (Exception e) { Debug.LogError("Error loading model for " + urdfLink.name + " : " + e.Message); } } } } // find all the joints next foreach (XmlNode xJoint in n.ChildNodes) { if (xJoint.Name == "joint") { string jointName = xJoint.Attributes["name"].Value; // store the joints indexed by child name so we can find it later // to properly indicate the parents in the joint list xmlJoints.Add(jointName, xJoint); // Get the links by name URDFLink parentLink = urdfLinks[GetXmlNodeChildByName(xJoint, "parent").Attributes["link"].Value]; URDFLink childLink = urdfLinks[GetXmlNodeChildByName(xJoint, "child").Attributes["link"].Value]; // Create the joint URDFJoint urdfJoint = new URDFJoint(); urdfJoint.name = jointName; urdfJoint.parentLink = parentLink; urdfJoint.transform = new GameObject(urdfJoint.name).transform; urdfJoint.type = xJoint.Attributes["type"].Value; // set the tree hierarchy // Parent the joint to its parent link urdfJoint.parentLink = parentLink; urdfJoint.transform.parent = parentLink.transform; parentLink.children.Add(urdfJoint); // Parent the child link to this joint urdfJoint.childLink = childLink; childLink.transform.parent = urdfJoint.transform; childLink.parent = urdfJoint; childLink.transform.localPosition = Vector3.zero; childLink.transform.localRotation = Quaternion.identity; // position the origin if it's specified XmlNode transNode = GetXmlNodeChildByName(xJoint, "origin"); Vector3 pos = Vector3.zero; if (transNode != null && transNode.Attributes["xyz"] != null) { pos = TupleToVector3(transNode.Attributes["xyz"].Value); } pos = URDFToUnityPos(pos); Vector3 rot = Vector3.zero; if (transNode != null && transNode.Attributes["rpy"] != null) { rot = TupleToVector3(transNode.Attributes["rpy"].Value); } rot = URDFToUnityRot(rot); // parent the joint and name it urdfJoint.transform.localPosition = pos; urdfJoint.transform.localRotation = Quaternion.Euler(rot); urdfJoint.originalRotation = urdfJoint.transform.localRotation; XmlNode axisNode = GetXmlNodeChildByName(xJoint, "axis"); if (axisNode != null) { Vector3 axis = TupleToVector3(axisNode.Attributes["xyz"].Value); axis.Normalize(); axis = URDFToUnityPos(axis); urdfJoint.axis = axis; } XmlNode limitNode = GetXmlNodeChildByName(xJoint, "limit"); if (limitNode != null) { if (limitNode.Attributes["lower"] != null) { urdfJoint.minAngle = float.Parse(limitNode.Attributes["lower"].Value); } if (limitNode.Attributes["upper"] != null) { urdfJoint.maxAngle = float.Parse(limitNode.Attributes["upper"].Value); } } // save the URDF joint urdfJoints.Add(urdfJoint.name, urdfJoint); } } } // loop through all the transforms until we find the one that has no parent URDFRobot robot = options.target; foreach (KeyValuePair <string, URDFLink> kv in urdfLinks) { // TODO : if there are multiple robots described, then we'll only be getting // the one. Should update to return a list of jointlists if necessary if (kv.Value.parent == null) { // find the top most node and add a joint list to it if (robot == null) { robot = kv.Value.transform.gameObject.AddComponent <URDFRobot>(); } else { kv.Value.transform.parent = robot.transform; kv.Value.transform.localPosition = Vector3.zero; kv.Value.transform.localRotation = Quaternion.identity; } robot.links = urdfLinks; robot.joints = urdfJoints; robot.IsConsistent(); return(robot); } } return(null); }
public static URDFRobot Parse(string urdfContent, Dictionary <string, string> packages, Options options = new Options()) { if (options.loadMeshCb == null) { options.loadMeshCb = LoadMesh; } // Parse the XML doc XmlDocument doc = new XmlDocument(); doc.LoadXml(urdfContent); // Store the information about the link and the objects indexed by link name Dictionary <string, XmlNode> xmlLinks = new Dictionary <string, XmlNode>(); Dictionary <string, XmlNode> xmlJoints = new Dictionary <string, XmlNode>(); // Indexed by joint name Dictionary <string, URDFJoint> urdfJoints = new Dictionary <string, URDFJoint>(); Dictionary <string, URDFLink> urdfLinks = new Dictionary <string, URDFLink>(); Dictionary <string, Material> urdfMaterials = new Dictionary <string, Material>(); // Indexed by joint name Dictionary <string, string> parentNames = new Dictionary <string, string>(); // First node is the <robot> node XmlNode robotNode = GetXmlNodeChildByName(doc, "robot"); string robotName = robotNode.Attributes["name"].Value; XmlNode[] xmlLinksArray = GetXmlNodeChildrenByName(robotNode, "link"); XmlNode[] xmlJointsArray = GetXmlNodeChildrenByName(robotNode, "joint"); XmlNode[] xmlMaterialsArray = GetXmlNodeChildrenByName(robotNode, "material", true); foreach (XmlNode materialNode in xmlMaterialsArray) { if (materialNode.Attributes["name"] != null) { string materialName = materialNode.Attributes["name"].Value; if (!urdfMaterials.ContainsKey(materialName)) { Material material = new Material(Shader.Find("Standard")); Color color = Color.white; XmlNode colorNode = GetXmlNodeChildByName(materialNode, "color"); if (colorNode != null) { color = TupleToColor(colorNode.Attributes["rgba"].Value); } material.color = color; urdfMaterials.Add(materialName, material); } } } // Cycle through the links and instantiate the geometry foreach (XmlNode linkNode in xmlLinksArray) { // Store the XML node for the link string linkName = linkNode.Attributes["name"].Value; xmlLinks.Add(linkName, linkNode); // create the link gameobject GameObject gameObject = new GameObject(linkName); URDFLink urdfLink = new URDFLink(); urdfLink.name = linkName; urdfLink.transform = gameObject.transform; urdfLinks.Add(linkName, urdfLink); // Get the geometry node and skip it if there isn't one XmlNode[] visualNodesArray = GetXmlNodeChildrenByName(linkNode, "visual"); List <GameObject> renderers = new List <GameObject>(); urdfLink.geometry = renderers; // Iterate over all the visual nodes foreach (XmlNode xmlVisual in visualNodesArray) { XmlNode geomNode = GetXmlNodeChildByName(xmlVisual, "geometry"); if (geomNode == null) { continue; } Material material = null; XmlNode materialNode = GetXmlNodeChildByName(xmlVisual, "material"); if (materialNode != null) { if (materialNode.Attributes["name"] != null) { string materialName = materialNode.Attributes["name"].Value; material = urdfMaterials[materialName]; } else { Color color = Color.white; XmlNode colorNode = GetXmlNodeChildByName(materialNode, "color"); if (colorNode != null) { color = TupleToColor(colorNode.Attributes["rgba"].Value); } material = new Material(Shader.Find("Standard")); material.color = color; // TODO: Load the textures // XmlNode texNode = GetXmlNodeChildByName(materialNode, "texture"); // if (texNode != null) { } } } // Get the mesh and the origin nodes XmlNode originNode = GetXmlNodeChildByName(xmlVisual, "origin"); // Extract the position and rotation of the mesh Vector3 position = Vector3.zero; if (originNode != null && originNode.Attributes["xyz"] != null) { position = TupleToVector3(originNode.Attributes["xyz"].Value); } position = URDFToUnityPos(position); Vector3 rotation = Vector3.zero; if (originNode != null && originNode.Attributes["rpy"] != null) { rotation = TupleToVector3(originNode.Attributes["rpy"].Value); } rotation = URDFToUnityRot(rotation); XmlNode meshNode = GetXmlNodeChildByName(geomNode, "mesh") ?? GetXmlNodeChildByName(geomNode, "box") ?? GetXmlNodeChildByName(geomNode, "sphere") ?? GetXmlNodeChildByName(geomNode, "cylinder"); try { if (meshNode.Name == "mesh") { // Extract the mesh path string fileName = ResolveMeshPath(meshNode.Attributes["filename"].Value, packages, options.workingPath); // Extract the scale from the mesh node Vector3 scale = Vector3.one; if (meshNode.Attributes["scale"] != null) { scale = TupleToVector3(meshNode.Attributes["scale"].Value); } scale = URDFToUnityScale(scale); // load all meshes string extension = Path.GetExtension(fileName).ToLower().Replace(".", ""); options.loadMeshCb(fileName, extension, models => { // create the rest of the meshes and child them to the click target for (int i = 0; i < models.Length; i++) { GameObject modelGameObject = models[i]; Transform meshTransform = modelGameObject.transform; // Capture the original local transforms before parenting in case the loader or model came in // with existing pose information and then apply our transform on top of it. Vector3 originalLocalPosition = meshTransform.localPosition; Quaternion originalLocalRotation = meshTransform.localRotation; Vector3 originalLocalScale = meshTransform.localScale; Vector3 transformedScale = originalLocalScale; transformedScale.x *= scale.x; transformedScale.y *= scale.y; transformedScale.z *= scale.z; meshTransform.parent = urdfLink.transform; meshTransform.localPosition = originalLocalPosition + position; meshTransform.localRotation = Quaternion.Euler(rotation) * originalLocalRotation; meshTransform.localScale = transformedScale; modelGameObject.name = urdfLink.name + " geometry " + i; renderers.Add(modelGameObject); // allows the urdf parser to be called from editor scripts outside of runtime without throwing errors // TODO: traverse over the children and do this if (Application.isPlaying) { Destroy(meshTransform.GetComponent <Collider>()); Destroy(meshTransform.GetComponent <Rigidbody>()); } else { DestroyImmediate(meshTransform.GetComponent <Collider>()); DestroyImmediate(meshTransform.GetComponent <Rigidbody>()); } } }); } else { // Instantiate the primitive geometry XmlNode primitiveNode = meshNode; GameObject primitiveGameObject = null; Transform primitiveTransform = null; switch (primitiveNode.Name) { case "box": { primitiveGameObject = GameObject.CreatePrimitive(PrimitiveType.Cube); primitiveTransform = primitiveGameObject.transform; Vector3 boxScale = TupleToVector3(primitiveNode.Attributes["size"].Value); boxScale = URDFToUnityPos(boxScale); primitiveTransform.localScale = boxScale; break; } case "sphere": { primitiveGameObject = GameObject.CreatePrimitive(PrimitiveType.Sphere); primitiveTransform = primitiveGameObject.transform; float sphereRadius = float.Parse(primitiveNode.Attributes["radius"].Value); primitiveTransform.localScale = Vector3.one * sphereRadius * 2; break; } case "cylinder": { primitiveGameObject = new GameObject(); primitiveTransform = primitiveGameObject.transform; GameObject cylinderPrimitive = GameObject.CreatePrimitive(PrimitiveType.Cylinder); cylinderPrimitive.transform.parent = primitiveTransform; float length = float.Parse(primitiveNode.Attributes["length"].Value); float radius = float.Parse(primitiveNode.Attributes["radius"].Value); primitiveTransform.localScale = new Vector3(radius * 2, length / 2, radius * 2); break; } } // Position the transform primitiveTransform.parent = urdfLink.transform; primitiveTransform.localPosition = position; primitiveTransform.localRotation = Quaternion.Euler(rotation); primitiveGameObject.name = urdfLink.name + " geometry " + primitiveNode.Name; Renderer primitiveRenderer = primitiveGameObject.GetComponent <Renderer>(); if (primitiveRenderer == null) { primitiveRenderer = primitiveGameObject.GetComponentInChildren <Renderer>(); } if (material != null) { primitiveRenderer.material = material; } renderers.Add(primitiveGameObject); // Dispose of unneeded components if (Application.isPlaying) { Destroy(primitiveRenderer.GetComponent <Collider>()); Destroy(primitiveRenderer.GetComponent <Rigidbody>()); } else { DestroyImmediate(primitiveRenderer.GetComponent <Collider>()); DestroyImmediate(primitiveRenderer.GetComponent <Rigidbody>()); } } } catch (Exception e) { Debug.LogError("Error loading model for " + urdfLink.name + " : " + e.Message); } } } // Cycle through the joint nodes foreach (XmlNode jointNode in xmlJointsArray) { string jointName = jointNode.Attributes["name"].Value; // store the joints indexed by child name so we can find it later // to properly indicate the parents in the joint list xmlJoints.Add(jointName, jointNode); // Get the links by name XmlNode parentNode = GetXmlNodeChildByName(jointNode, "parent"); XmlNode childNode = GetXmlNodeChildByName(jointNode, "child"); string parentName = parentNode.Attributes["link"].Value; string childName = childNode.Attributes["link"].Value; URDFLink parentLink = urdfLinks[parentName]; URDFLink childLink = urdfLinks[childName]; // Create the joint GameObject jointGameObject = new GameObject(jointName); URDFJoint urdfJoint = new URDFJoint(); urdfJoint.name = jointName; urdfJoint.parentLink = parentLink; urdfJoint.transform = jointGameObject.transform; urdfJoint.type = jointNode.Attributes["type"].Value; // Set the tree hierarchy // Parent the joint to its parent link urdfJoint.parentLink = parentLink; urdfJoint.transform.parent = parentLink.transform; parentLink.children.Add(urdfJoint); // Parent the child link to this joint urdfJoint.childLink = childLink; childLink.transform.parent = urdfJoint.transform; childLink.parent = urdfJoint; childLink.transform.localPosition = Vector3.zero; childLink.transform.localRotation = Quaternion.identity; // Position the origin if it's specified XmlNode transformNode = GetXmlNodeChildByName(jointNode, "origin"); Vector3 position = Vector3.zero; if (transformNode != null && transformNode.Attributes["xyz"] != null) { position = TupleToVector3(transformNode.Attributes["xyz"].Value); } position = URDFToUnityPos(position); Vector3 rotation = Vector3.zero; if (transformNode != null && transformNode.Attributes["rpy"] != null) { rotation = TupleToVector3(transformNode.Attributes["rpy"].Value); } rotation = URDFToUnityRot(rotation); // parent the joint and name it urdfJoint.transform.localPosition = position; urdfJoint.transform.localRotation = Quaternion.Euler(rotation); urdfJoint.originalRotation = Quaternion.Euler(rotation); XmlNode axisNode = GetXmlNodeChildByName(jointNode, "axis"); if (axisNode != null) { Vector3 axis = TupleToVector3(axisNode.Attributes["xyz"].Value); axis = URDFToUnityPos(axis); axis.Normalize(); urdfJoint.axis = axis; } XmlNode limitNode = GetXmlNodeChildByName(jointNode, "limit"); if (limitNode != null) { // Use double.parse to handle particularly large values. if (limitNode.Attributes["lower"] != null) { urdfJoint.minAngle = (float)double.Parse(limitNode.Attributes["lower"].Value); } if (limitNode.Attributes["upper"] != null) { urdfJoint.maxAngle = (float)double.Parse(limitNode.Attributes["upper"].Value); } } // save the URDF joint urdfJoints.Add(urdfJoint.name, urdfJoint); } // loop through all the transforms until we find the one that has no parent URDFRobot robot = options.target; foreach (KeyValuePair <string, URDFLink> kv in urdfLinks) { if (kv.Value.parent == null) { // find the top most node and add a joint list to it if (robot == null) { robot = kv.Value.transform.gameObject.AddComponent <URDFRobot>(); } else { kv.Value.transform.parent = robot.transform; kv.Value.transform.localPosition = Vector3.zero; kv.Value.transform.localRotation = Quaternion.identity; } robot.links = urdfLinks; robot.joints = urdfJoints; robot.IsConsistent(); return(robot); } } robot.name = robotName; return(null); }