public void Convert(UrdfRobot robot, string baseDirectory)
{
foreach (UrdfLink link in robot.Links)
{
LoadLink(link, baseDirectory);
}
}
private static IDictionary <UrdfLink, UrdfJoint> FindLinkParents(UrdfRobot robot)
{
var linkToParent = new Dictionary <UrdfLink, UrdfJoint>();
foreach (UrdfLink link in robot.Links.Values)
{
UrdfJoint parentJoint = robot.Joints.FirstOrDefault(joint => joint.Child == link);
linkToParent[link] = parentJoint;
}
return(linkToParent);
}
public void Convert(UrdfRobot robot, string baseDirectory)
{
_linkToParentJoint = FindLinkParents(robot);
var rootLink = _linkToParentJoint.FirstOrDefault(link => link.Value == null).Key;
if (rootLink != null)
{
LoadLink(rootLink, Matrix.Identity, baseDirectory);
}
}
private static UrdfRobot ParseRobot(XmlElement element)
{
var robot = new UrdfRobot
{
Name = element.GetAttribute("name")
};
foreach (XmlElement linkElement in element.SelectNodes("link"))
{
var link = ParseLink(linkElement);
robot.Links.Add(link);
}
return(robot);
}
private static UrdfJoint ParseJoint(XmlElement jointElement, UrdfRobot robot)
{
XmlNode parentNode = jointElement.SelectSingleNode("parent");
XmlNode childNode = jointElement.SelectSingleNode("child");
XmlNode originNode = jointElement.SelectSingleNode("origin");
string parentId = parentNode.Attributes["link"].Value;
string childId = childNode.Attributes["link"].Value;
UrdfJoint joint;
string type = jointElement.GetAttribute("type");
switch (type)
{
case "continuous":
joint = new UrdfContinuousJoint();
break;
case "prismatic":
joint = new UrdfPrismaticJoint();
break;
case "fixed":
joint = new UrdfFixedJoint();
break;
case "revolute":
joint = new UrdfRevoluteJoint();
break;
case "spherical":
// Not in URDF specification
case "floating":
default:
throw new NotSupportedException();
}
joint.Origin = ParseOrigin(originNode);
joint.Parent = robot.Links[parentId];
joint.Child = robot.Links[childId];
return(joint);
}
private static UrdfRobot ParseRobot(XmlElement element)
{
var robot = new UrdfRobot
{
Name = element.GetAttribute("name")
};
foreach (XmlElement linkElement in element.SelectNodes("link"))
{
UrdfLink link = ParseLink(linkElement);
robot.Links[link.Name] = link;
}
foreach (XmlElement jointElement in element.SelectNodes("joint"))
{
UrdfJoint joint = ParseJoint(jointElement, robot);
robot.Joints.Add(joint);
}
return(robot);
}
