337 lines
12 KiB
C#
337 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
using System.Xml;
|
|
using UnityEngine;
|
|
|
|
namespace URDF
|
|
{
|
|
/// <summary>
|
|
/// Parser for URDF XML files
|
|
/// </summary>
|
|
public static class URDFParser
|
|
{
|
|
/// <summary>
|
|
/// Parse a URDF file and return a URDFRobot model
|
|
/// </summary>
|
|
public static URDFRobot ParseURDF(string filePath)
|
|
{
|
|
if (!File.Exists(filePath))
|
|
{
|
|
Debug.LogError($"URDF file not found: {filePath}");
|
|
return null;
|
|
}
|
|
|
|
XmlDocument doc = new XmlDocument();
|
|
try
|
|
{
|
|
doc.Load(filePath);
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
Debug.LogError($"Failed to load URDF XML: {e.Message}");
|
|
return null;
|
|
}
|
|
|
|
XmlNode robotNode = doc.SelectSingleNode("robot");
|
|
if (robotNode == null)
|
|
{
|
|
Debug.LogError("URDF file does not contain a <robot> root element");
|
|
return null;
|
|
}
|
|
|
|
string robotName = GetAttribute(robotNode, "name", "robot");
|
|
URDFRobot robot = new URDFRobot(robotName);
|
|
|
|
// Parse all links
|
|
XmlNodeList linkNodes = robotNode.SelectNodes("link");
|
|
foreach (XmlNode linkNode in linkNodes)
|
|
{
|
|
string linkName = GetAttribute(linkNode, "name");
|
|
if (string.IsNullOrEmpty(linkName))
|
|
{
|
|
Debug.LogWarning("Found link without name attribute, skipping");
|
|
continue;
|
|
}
|
|
|
|
URDFLink link = ParseLink(linkNode, linkName);
|
|
robot.links[linkName] = link;
|
|
}
|
|
|
|
// Parse all joints
|
|
XmlNodeList jointNodes = robotNode.SelectNodes("joint");
|
|
foreach (XmlNode jointNode in jointNodes)
|
|
{
|
|
string jointName = GetAttribute(jointNode, "name");
|
|
if (string.IsNullOrEmpty(jointName))
|
|
{
|
|
Debug.LogWarning("Found joint without name attribute, skipping");
|
|
continue;
|
|
}
|
|
|
|
URDFJoint joint = ParseJoint(jointNode, jointName);
|
|
robot.joints[jointName] = joint;
|
|
}
|
|
|
|
Debug.Log($"Parsed URDF: {robot.links.Count} links, {robot.joints.Count} joints");
|
|
return robot;
|
|
}
|
|
|
|
private static URDFLink ParseLink(XmlNode linkNode, string linkName)
|
|
{
|
|
URDFLink link = new URDFLink(linkName);
|
|
|
|
// Parse visual elements
|
|
XmlNodeList visualNodes = linkNode.SelectNodes("visual");
|
|
foreach (XmlNode visualNode in visualNodes)
|
|
{
|
|
URDFVisual visual = ParseVisual(visualNode);
|
|
if (visual != null)
|
|
{
|
|
link.visuals.Add(visual);
|
|
}
|
|
}
|
|
|
|
// Parse inertial properties
|
|
XmlNode inertialNode = linkNode.SelectSingleNode("inertial");
|
|
if (inertialNode != null)
|
|
{
|
|
XmlNode originNode = inertialNode.SelectSingleNode("origin");
|
|
if (originNode != null)
|
|
{
|
|
link.inertialOrigin = ParseOrigin(originNode);
|
|
}
|
|
|
|
XmlNode massNode = inertialNode.SelectSingleNode("mass");
|
|
if (massNode != null)
|
|
{
|
|
link.mass = ParseFloat(GetAttribute(massNode, "value"), 0f);
|
|
}
|
|
|
|
XmlNode inertiaNode = inertialNode.SelectSingleNode("inertia");
|
|
if (inertiaNode != null)
|
|
{
|
|
link.inertia = new Vector3(
|
|
ParseFloat(GetAttribute(inertiaNode, "ixx"), 0f),
|
|
ParseFloat(GetAttribute(inertiaNode, "iyy"), 0f),
|
|
ParseFloat(GetAttribute(inertiaNode, "izz"), 0f)
|
|
);
|
|
}
|
|
}
|
|
|
|
return link;
|
|
}
|
|
|
|
private static URDFVisual ParseVisual(XmlNode visualNode)
|
|
{
|
|
URDFVisual visual = new URDFVisual();
|
|
|
|
// Parse origin
|
|
XmlNode originNode = visualNode.SelectSingleNode("origin");
|
|
if (originNode != null)
|
|
{
|
|
visual.origin = ParseOrigin(originNode);
|
|
}
|
|
else
|
|
{
|
|
visual.origin = new URDFOrigin();
|
|
}
|
|
|
|
// Parse geometry
|
|
XmlNode geometryNode = visualNode.SelectSingleNode("geometry");
|
|
if (geometryNode != null)
|
|
{
|
|
visual.geometry = ParseGeometry(geometryNode);
|
|
}
|
|
|
|
// Parse material
|
|
XmlNode materialNode = visualNode.SelectSingleNode("material");
|
|
if (materialNode != null)
|
|
{
|
|
visual.materialName = GetAttribute(materialNode, "name");
|
|
XmlNode colorNode = materialNode.SelectSingleNode("color");
|
|
if (colorNode != null)
|
|
{
|
|
visual.color = ParseColor(GetAttribute(colorNode, "rgba"));
|
|
}
|
|
}
|
|
|
|
return visual;
|
|
}
|
|
|
|
private static URDFGeometry ParseGeometry(XmlNode geometryNode)
|
|
{
|
|
URDFGeometry geometry = new URDFGeometry();
|
|
|
|
// Check for mesh
|
|
XmlNode meshNode = geometryNode.SelectSingleNode("mesh");
|
|
if (meshNode != null)
|
|
{
|
|
geometry.type = URDFGeometry.GeometryType.Mesh;
|
|
geometry.meshFilename = GetAttribute(meshNode, "filename");
|
|
geometry.scale = ParseVector3(GetAttribute(meshNode, "scale"), Vector3.one);
|
|
}
|
|
// Check for box
|
|
else if (geometryNode.SelectSingleNode("box") != null)
|
|
{
|
|
geometry.type = URDFGeometry.GeometryType.Box;
|
|
XmlNode boxNode = geometryNode.SelectSingleNode("box");
|
|
geometry.size = ParseVector3(GetAttribute(boxNode, "size"), Vector3.one);
|
|
}
|
|
// Check for sphere
|
|
else if (geometryNode.SelectSingleNode("sphere") != null)
|
|
{
|
|
geometry.type = URDFGeometry.GeometryType.Sphere;
|
|
XmlNode sphereNode = geometryNode.SelectSingleNode("sphere");
|
|
geometry.radius = ParseFloat(GetAttribute(sphereNode, "radius"), 0.5f);
|
|
}
|
|
// Check for cylinder
|
|
else if (geometryNode.SelectSingleNode("cylinder") != null)
|
|
{
|
|
geometry.type = URDFGeometry.GeometryType.Cylinder;
|
|
XmlNode cylinderNode = geometryNode.SelectSingleNode("cylinder");
|
|
geometry.radius = ParseFloat(GetAttribute(cylinderNode, "radius"), 0.5f);
|
|
geometry.length = ParseFloat(GetAttribute(cylinderNode, "length"), 1f);
|
|
}
|
|
|
|
return geometry;
|
|
}
|
|
|
|
private static URDFJoint ParseJoint(XmlNode jointNode, string jointName)
|
|
{
|
|
URDFJoint joint = new URDFJoint(jointName);
|
|
|
|
// Parse joint type
|
|
string typeStr = GetAttribute(jointNode, "type", "fixed").ToLower();
|
|
switch (typeStr)
|
|
{
|
|
case "revolute":
|
|
joint.type = URDFJoint.JointType.Revolute;
|
|
break;
|
|
case "continuous":
|
|
joint.type = URDFJoint.JointType.Continuous;
|
|
break;
|
|
case "prismatic":
|
|
joint.type = URDFJoint.JointType.Prismatic;
|
|
break;
|
|
case "fixed":
|
|
joint.type = URDFJoint.JointType.Fixed;
|
|
break;
|
|
case "floating":
|
|
joint.type = URDFJoint.JointType.Floating;
|
|
break;
|
|
case "planar":
|
|
joint.type = URDFJoint.JointType.Planar;
|
|
break;
|
|
default:
|
|
joint.type = URDFJoint.JointType.Fixed;
|
|
break;
|
|
}
|
|
|
|
// Parse parent and child links
|
|
XmlNode parentNode = jointNode.SelectSingleNode("parent");
|
|
if (parentNode != null)
|
|
{
|
|
joint.parentLink = GetAttribute(parentNode, "link");
|
|
}
|
|
|
|
XmlNode childNode = jointNode.SelectSingleNode("child");
|
|
if (childNode != null)
|
|
{
|
|
joint.childLink = GetAttribute(childNode, "link");
|
|
}
|
|
|
|
// Parse origin
|
|
XmlNode originNode = jointNode.SelectSingleNode("origin");
|
|
if (originNode != null)
|
|
{
|
|
joint.origin = ParseOrigin(originNode);
|
|
}
|
|
|
|
// Parse axis
|
|
XmlNode axisNode = jointNode.SelectSingleNode("axis");
|
|
if (axisNode != null)
|
|
{
|
|
joint.axis = ParseVector3(GetAttribute(axisNode, "xyz"), Vector3.up);
|
|
}
|
|
|
|
// Parse limit
|
|
XmlNode limitNode = jointNode.SelectSingleNode("limit");
|
|
if (limitNode != null)
|
|
{
|
|
joint.limit = new URDFJointLimit
|
|
{
|
|
lower = ParseFloat(GetAttribute(limitNode, "lower"), -Mathf.PI),
|
|
upper = ParseFloat(GetAttribute(limitNode, "upper"), Mathf.PI),
|
|
effort = ParseFloat(GetAttribute(limitNode, "effort"), 1000f),
|
|
velocity = ParseFloat(GetAttribute(limitNode, "velocity"), 1f)
|
|
};
|
|
}
|
|
|
|
return joint;
|
|
}
|
|
|
|
private static URDFOrigin ParseOrigin(XmlNode originNode)
|
|
{
|
|
URDFOrigin origin = new URDFOrigin();
|
|
origin.xyz = ParseVector3(GetAttribute(originNode, "xyz"), Vector3.zero);
|
|
origin.rpy = ParseVector3(GetAttribute(originNode, "rpy"), Vector3.zero);
|
|
return origin;
|
|
}
|
|
|
|
private static Vector3 ParseVector3(string str, Vector3 defaultValue)
|
|
{
|
|
if (string.IsNullOrEmpty(str))
|
|
return defaultValue;
|
|
|
|
string[] parts = str.Trim().Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
|
|
if (parts.Length >= 3)
|
|
{
|
|
return new Vector3(
|
|
ParseFloat(parts[0], defaultValue.x),
|
|
ParseFloat(parts[1], defaultValue.y),
|
|
ParseFloat(parts[2], defaultValue.z)
|
|
);
|
|
}
|
|
return defaultValue;
|
|
}
|
|
|
|
private static Color ParseColor(string rgbaStr)
|
|
{
|
|
if (string.IsNullOrEmpty(rgbaStr))
|
|
return Color.white;
|
|
|
|
string[] parts = rgbaStr.Trim().Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
|
|
if (parts.Length >= 4)
|
|
{
|
|
return new Color(
|
|
ParseFloat(parts[0], 1f),
|
|
ParseFloat(parts[1], 1f),
|
|
ParseFloat(parts[2], 1f),
|
|
ParseFloat(parts[3], 1f)
|
|
);
|
|
}
|
|
return Color.white;
|
|
}
|
|
|
|
private static float ParseFloat(string str, float defaultValue)
|
|
{
|
|
if (string.IsNullOrEmpty(str))
|
|
return defaultValue;
|
|
|
|
if (float.TryParse(str, out float result))
|
|
return result;
|
|
|
|
return defaultValue;
|
|
}
|
|
|
|
private static string GetAttribute(XmlNode node, string attributeName, string defaultValue = "")
|
|
{
|
|
if (node?.Attributes?[attributeName] != null)
|
|
return node.Attributes[attributeName].Value;
|
|
return defaultValue;
|
|
}
|
|
}
|
|
}
|
|
|