diff --git a/URDF.m b/URDF.m index 3bc76df3..31503aa7 100644 --- a/URDF.m +++ b/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 = []; diff --git a/structA.m b/structA.m new file mode 100644 index 00000000..f2e77102 --- /dev/null +++ b/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 + \ No newline at end of file diff --git a/structFindPath.m b/structFindPath.m new file mode 100644 index 00000000..732f0bac --- /dev/null +++ b/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 \ No newline at end of file