URDF fixed lookup of elements as children of root and not at any level

URDF.m

@@ -17,18 +17,19 @@
 
             dom = xmlread(urdffile);
 
-            links = dom.getElementsByTagName('link');
-            joints = dom.getElementsByTagName('joint');
-            properties = dom.getElementsByTagName('property');
             robot = dom.getElementsByTagName('robot');
-            urdf.attr = urdf.getattr(robot.item(0))
+            ro = robot.item(0);
+            links = URDF.getChildElementsByTagName(ro,'link');
+            joints = URDF.getChildElementsByTagName(ro,'joint');
+            properties = URDF.getChildElementsByTagName(ro,'property');
+            urdf.attr = urdf.getattr(robot.item(0));
 
             % get the properties
-            fprintf('%d properties\n', properties.getLength);
+            fprintf('%d properties\n', length(properties));
 
             Props = [];
-            for k = 0:properties.getLength-1
-                property = properties.item(k)
+            for k = 1:length(properties)
+                property = properties(k);
                 if property.hasAttributes
                     attributes = property.getAttributes;
 
@@ -77,11 +78,69 @@
                 link = urdf.joints{jj}.child.link;
             end
         end
-
+
+        % So far Link/SerialLink is not enough expressive (really?) for supporting URDF
+        % the following code exports a links and joints structure that can
+        % be easily solved. This structure separates links from joints
+        % allowing to hide fixed joints
+        function r = asstructure(urdf)
+            links = {};
+            njoints = 0;
+            linksbyname = [];    
+            linkparent = zeros(urdf.nlinks);
+            linkjoint = zeros(urdf.nlinks);
+            joints = [];
+            for i=1:urdf.nlinks
+                linksbyname.(urdf.links{i}.name) = i;
+            end
+            for i=1:urdf.njoints
+                T = rpy2tr(urdf.joints{i}.origin.rpy);
+                T(1:3,4) = urdf.joints{i}.origin.xyz;
+                s = [];
+                s.rel = T;
+                s.type = urdf.joints{i}.type;
+                s.name = urdf.joints{i}.child.link;
+                s.parent = urdf.joints{i}.parent.link;
+                s.id = length(links)+1;
+                s.parentid = linksbyname.(s.parent);
+
+                if isfield(urdf.joints{i},'axis')
+                    s.axis = urdf.joints{i}.axis.xyz;
+                end
+
+                switch(urdf.joints{i}.type)
+                    case 'revolute'
+                        njoints = njoints+1;
+                        s.joint = njoints;
+                        joints.(s.name) = njoints;
+                        linkjoint(s.id,njoints) = 1;
+                    case 'fixed'
+                        s.joint = 0;
+                    otherwise
+                        error(['Joint type unsupported ' urdf.joints{i}.type]);
+                end                
+                linkparent(s.id,s.parentid) = 1;
+                links{end+1} = s;
+                linksbyname.(s.name) = s.id;
+            end
+            linkjoint = linkjoint(:,1:njoints);
+            r = [];
+            r.name = urdf.attr.name;
+            r.links = links; % struct of links
+            r.jointsbyname = joints; % struct child link name => joint identifier
+            r.njoints = njoints;
+            r.linksbyname = linksbyname; % links byname
+            r.linkjoint = linkjoint;
+            r.linkparent = linkparent; % parent relationship
+        end
+
         function r = robot(urdf)
+            links = [];
             for i=1:urdf.njoints
-                % create link objects
-                % attach the STL model to them
+                switch(urdf.joints{i}.type)
+                    otherwise
+                        error(['Joint type unsupported ' urdf.joints{i}.type]);
+                end
             end
             r = SerialLink(links, 'name', urdf.attr.name);
         end
@@ -125,14 +184,28 @@ function display(urdf)
     end
 
     methods (Static)
+       function r = getChildElementsByTagName(node,name)
+            r = [];
+            nodeChildren = node.getChildNodes;
+            for i=1:nodeChildren.getLength
+                child = nodeChildren.item(i-1);
+                if child.getNodeType == 3
+                    continue;
+                end
+                if strcmp(char(child.getNodeName),name)
+                    r = [r; child];
+                end
+            end
+        end
+
         function List = get_elements(doc, elname, props)
-            elements  = doc.getElementsByTagName(elname);
+            elements  = URDF.getChildElementsByTagName(doc.getDocumentElement(),elname);
             % get the links
             List = {};
-            fprintf('%d %s\n', elements.getLength, elname);
+            fprintf('%d %s\n', length(elements), elname);
             % step through the list of  elements found
-            for k = 1:elements.getLength
-                element = elements.item(k-1);
+            for k = 1: length(elements)
+                element = elements(k);
 
                 info = URDF.descend(element, props);
                 info = URDF.getattr(element, info, props);
@@ -163,6 +236,10 @@ function display(urdf)
             end
         end
 
+
+
+
+
         function att = getattr(node, att, props)
             if nargin < 2
                 att = [];

structA.m

@@ -0,0 +1,58 @@
+% similar to robotA
+% with option to not compose
+function T = structA(robotstruct,llist,allq, notcompose)
+    if nargin < 4
+        notcompose = 0;
+    end
+    if notcompose == 1
+        To = cell(length(llist),1);
+        for I=1:length(llist)
+            li = robotstruct.links{I};
+            if isfield(li,'rel')
+                T = li.rel;
+            else
+                T = eye(4);
+            end            
+            switch(li.type)
+                case 'fixed'
+                case 'revolute'
+                    T = T*angvec2tr(allq(li.joint),li.axis);
+                otherwise
+                    error('Not Implemented');
+            end
+            To{I} = T;
+        end
+        T = To;
+    else
+        if length(llist) > 1
+            T = eye(4);
+            for I=1:length(llist)
+                % check if has real joint and solve it
+                li = robotstruct.links{llist(I)};
+                if li.joint < 1
+                    q = 0;
+                else
+                    q = allq(li.joint);
+                end
+                T = T*structA(robotstruct,llist(I),q);
+            end
+        else
+            j = llist;
+            q = allq;
+
+            li = robotstruct.links{j};
+            if isfield(li,'rel')
+                T = li.rel;
+            else
+                T = eye(4);
+            end
+            switch(li.type)
+                case 'fixed'
+                case 'revolute'
+                    T = T*angvec2tr(q,li.axis);
+                otherwise
+                    error('Not Implemented');
+            end        
+        end
+    end
+

structFindPath.m

@@ -0,0 +1,36 @@
+function p = structFindPath(robotstruct,start,target)
+
+% finds the path of link indices to go from start to end
+% the path can be used with structA(robotstruct,p,allq)
+A = robotstruct.linkparent;
+A = (A' + A)*0.5; % symmetric
+
+unusedNodes=ones(1,size(A,1));
+emptyPath=[];
+
+p = findpaths(A,unusedNodes,emptyPath,start,target);
+
+end
+
+% http://stackoverflow.com/questions/28824640/find-all-possible-paths-in-a-graph-using-matlab-brute-force-search
+function paths = findpaths(Adj, nodes, currentPath, start, target)
+   paths = {};
+   nodes(start) = 0;
+   currentPath = [currentPath start];
+   childAdj = Adj(start,:) & nodes;
+   childList = find(childAdj);
+   childCount = numel(childList);
+   if childCount == 0 || start == target
+      if start == target
+         paths = [paths; currentPath];
+      end
+      return;
+   end
+   for idx = 1:childCount
+      currentNode = childList(idx);
+      newNodes = nodes;
+      newNodes(currentNode) = 0;
+      newPaths = findpaths(Adj, newNodes, currentPath, currentNode, target);
+      paths = [paths; newPaths];
+   end
+end