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