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