├── +urdf
├── +joints
│ ├── Continuous.m
│ ├── Fixed.m
│ ├── Floating.m
│ ├── Planar.m
│ ├── Prismatic.m
│ └── Revolute.m
├── +shapes
│ ├── Box.m
│ ├── Cylinder.m
│ ├── Mesh.m
│ └── Sphere.m
├── +util
│ ├── compareURDF.m
│ ├── findNodeFromRobotRoot.m
│ ├── findNodesByPattern.m
│ ├── isNullTag.m
│ ├── readXML.m
│ └── writeToURDFFile.m
├── Axis.m
├── Builder.m
├── Collision.m
├── Component.m
├── Geometry.m
├── Inertial.m
├── Joint.m
├── Link.m
├── Material.m
├── Origin.m
├── Robot.m
├── URDFTag.m
└── Visual.m
├── .devcontainer
└── devcontainer.json
├── .gitattributes
├── .gitignore
├── LICENSE
├── README.md
├── RobotBuilder_matlab.prj
├── minion.mlx
├── resources
└── project
│ ├── 3CZsW2g7OlSQ0x9yuNPOmB5IOKw
│ ├── 5bdXy6yV3tBkNrw67bdbMMu5Xusd.xml
│ ├── 5bdXy6yV3tBkNrw67bdbMMu5Xusp.xml
│ ├── 6diEYHjS2-gX6Gm0q020IiEYQC0d.xml
│ ├── 6diEYHjS2-gX6Gm0q020IiEYQC0p.xml
│ ├── 7DaGURu-Rl0SNakAZllRLU2okbAd.xml
│ ├── 7DaGURu-Rl0SNakAZllRLU2okbAp.xml
│ ├── 7qi7QPLUDzJJcR3utBf_IyUecuYd.xml
│ ├── 7qi7QPLUDzJJcR3utBf_IyUecuYp.xml
│ ├── AUt6rrDEyduF9FkM126mG_iSatsd.xml
│ ├── AUt6rrDEyduF9FkM126mG_iSatsp.xml
│ ├── BDvzuMREN2EnEuHGSFCRCWmjqbcd.xml
│ ├── BDvzuMREN2EnEuHGSFCRCWmjqbcp.xml
│ ├── F0JCeQKpi8N_H4x6HP1P3jvQcxUd.xml
│ ├── F0JCeQKpi8N_H4x6HP1P3jvQcxUp.xml
│ ├── Jnb29F8mFlHoTW_X59EJyh7BRfMd.xml
│ ├── Jnb29F8mFlHoTW_X59EJyh7BRfMp.xml
│ ├── QPKarK7d9eMPg-Fszh0xqz04KWgd.xml
│ ├── QPKarK7d9eMPg-Fszh0xqz04KWgp.xml
│ ├── Tn3kQTmMKAHYlCmAW2_FGtPZK8Ad.xml
│ ├── Tn3kQTmMKAHYlCmAW2_FGtPZK8Ap.xml
│ ├── YDtaz24FDqeRWXk2FJTpeNzuVYod.xml
│ ├── YDtaz24FDqeRWXk2FJTpeNzuVYop.xml
│ ├── bvuCWAVBVigrwQVnx_C4ppNh2Ysd.xml
│ ├── bvuCWAVBVigrwQVnx_C4ppNh2Ysp.xml
│ ├── d0FP3cR6X2M-DTZhoVMWYB4TvBMd.xml
│ ├── d0FP3cR6X2M-DTZhoVMWYB4TvBMp.xml
│ ├── o5YLlby1X55yRprnjX5LjXisJ94d.xml
│ ├── o5YLlby1X55yRprnjX5LjXisJ94p.xml
│ ├── wpBrtCcS1qtcYgFNDOBQb_TUu1gd.xml
│ └── wpBrtCcS1qtcYgFNDOBQb_TUu1gp.xml
│ ├── 5bdXy6yV3tBkNrw67bdbMMu5Xus
│ ├── DBBL6SRS8HeYro-PCHA6SHLu2U4d.xml
│ ├── DBBL6SRS8HeYro-PCHA6SHLu2U4p.xml
│ ├── GzqJIrhwKb19IztmZakYs3VDjNkd.xml
│ ├── GzqJIrhwKb19IztmZakYs3VDjNkp.xml
│ ├── PT6ZyH7HUQbKNAMrFVc18EBQpScd.xml
│ ├── PT6ZyH7HUQbKNAMrFVc18EBQpScp.xml
│ ├── V2O8QYlYi9EWBwyCC6hxDqXWP3Md.xml
│ ├── V2O8QYlYi9EWBwyCC6hxDqXWP3Mp.xml
│ ├── rKjvnAB-Apn4DeznEt898Ltk2UQd.xml
│ └── rKjvnAB-Apn4DeznEt898Ltk2UQp.xml
│ ├── 7qi7QPLUDzJJcR3utBf_IyUecuY
│ ├── -fEBdkpHv-CTASg0QT1Bl0j-Dysd.xml
│ ├── -fEBdkpHv-CTASg0QT1Bl0j-Dysp.xml
│ ├── B3Y5jegPNHopzioh1maRhvV93vEd.xml
│ ├── B3Y5jegPNHopzioh1maRhvV93vEp.xml
│ ├── E6j8uBF0jkXZSuwIS0rAohgmemQd.xml
│ ├── E6j8uBF0jkXZSuwIS0rAohgmemQp.xml
│ ├── Gbn2jCTbZzv5X5vUJiucphCKqCUd.xml
│ ├── Gbn2jCTbZzv5X5vUJiucphCKqCUp.xml
│ ├── zxxU5a5enkI9VBaPbFwAQXlJthMd.xml
│ └── zxxU5a5enkI9VBaPbFwAQXlJthMp.xml
│ ├── EEtUlUb-dLAdf0KpMVivaUlztwA
│ ├── gOb6R7uWizd9E9UZfHk2YVzrcqUd.xml
│ └── gOb6R7uWizd9E9UZfHk2YVzrcqUp.xml
│ ├── NjSPEMsIuLUyIpr2u1Js5bVPsOs
│ ├── 2kj09UetkV_lru3gvSPXnY6-nM4d.xml
│ ├── 2kj09UetkV_lru3gvSPXnY6-nM4p.xml
│ ├── KKyDJtbdIBOlaeHmIZd5VX6vqx8d.xml
│ ├── KKyDJtbdIBOlaeHmIZd5VX6vqx8p.xml
│ ├── QWNDYJD5mGW1bWYvPx9DtKnxzw4d.xml
│ ├── QWNDYJD5mGW1bWYvPx9DtKnxzw4p.xml
│ ├── R1RggVhA72agIvELiuhWPRS8F0Id.xml
│ ├── R1RggVhA72agIvELiuhWPRS8F0Ip.xml
│ ├── aEHSZBIY-yve10yGis12Zr5DLZod.xml
│ ├── aEHSZBIY-yve10yGis12Zr5DLZop.xml
│ ├── j4xwF_j8iFTVayUMfxLgMnTbencd.xml
│ ├── j4xwF_j8iFTVayUMfxLgMnTbencp.xml
│ ├── r8LR4nLmg9ai3oHrW1r_-KocQzkd.xml
│ └── r8LR4nLmg9ai3oHrW1r_-KocQzkp.xml
│ ├── Project.xml
│ ├── bvuCWAVBVigrwQVnx_C4ppNh2Ys
│ ├── JXuVlLBSOK3njbSIht8l4-UZjtId.xml
│ ├── JXuVlLBSOK3njbSIht8l4-UZjtIp.xml
│ ├── qnIB_pkcUpZGhQxDEfcxpCbqEx4d.xml
│ ├── qnIB_pkcUpZGhQxDEfcxpCbqEx4p.xml
│ ├── y05kef22xdBH7oh1uWS6f1iojsMd.xml
│ └── y05kef22xdBH7oh1uWS6f1iojsMp.xml
│ ├── fjRQtWiSIy7hIlj-Kmk87M7s21k
│ ├── NjSPEMsIuLUyIpr2u1Js5bVPsOsd.xml
│ └── NjSPEMsIuLUyIpr2u1Js5bVPsOsp.xml
│ ├── qaw0eS1zuuY1ar9TdPn1GMfrjbQ
│ ├── 3CZsW2g7OlSQ0x9yuNPOmB5IOKwd.xml
│ ├── 3CZsW2g7OlSQ0x9yuNPOmB5IOKwp.xml
│ ├── TMK4UzWHdRLhy_w-CHt9y11Q8XAd.xml
│ ├── TMK4UzWHdRLhy_w-CHt9y11Q8XAp.xml
│ ├── _N6hx4NG-rjx-xgGeMGZcdrZQrQd.xml
│ ├── _N6hx4NG-rjx-xgGeMGZcdrZQrQp.xml
│ ├── bTEJeU_8R4R4qC77t7aJSjzV1F0d.xml
│ ├── bTEJeU_8R4R4qC77t7aJSjzV1F0p.xml
│ ├── qD-kr16wmwlzR-nIg1IG_vvRrWkd.xml
│ └── qD-kr16wmwlzR-nIg1IG_vvRrWkp.xml
│ ├── root
│ ├── EEtUlUb-dLAdf0KpMVivaUlztwAp.xml
│ ├── GiiBklLgTxteCEmomM8RCvWT0nQd.xml
│ ├── GiiBklLgTxteCEmomM8RCvWT0nQp.xml
│ ├── fjRQtWiSIy7hIlj-Kmk87M7s21kp.xml
│ └── qaw0eS1zuuY1ar9TdPn1GMfrjbQp.xml
│ ├── rootp.xml
│ └── uuid-61d4c966-f84d-4cb0-829c-73fa50223d73.xml
└── robotBuilder.slreqx
/+urdf/+joints/Continuous.m:
--------------------------------------------------------------------------------
1 | classdef Continuous < urdf.Joint
2 | %CONTINUOUS Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | Property1
7 | end
8 |
9 | methods
10 | function obj = Continuous(name, parentLink,childLink)
11 | obj@urdf.Joint(name, 'continuous', parentLink, childLink);
12 | end
13 | end
14 |
15 | methods(Static)
16 | function obj = buildFromURDF(node)
17 | name = char(node.getAttribute('name'));
18 | parentName = '';
19 | childName = '';
20 | for i = 0:(node.getLength()-1)
21 | child = node.item(i);
22 | switch char(child.getNodeName())
23 | case 'parent'
24 | parentName = char(child.getAttribute('link'));
25 | case 'child'
26 | childName = char(child.getAttribute('link'));
27 | end
28 | end
29 | obj = urdf.joints.Continuous(name, parentName, childName);
30 | end
31 | end
32 | end
33 |
34 |
--------------------------------------------------------------------------------
/+urdf/+joints/Fixed.m:
--------------------------------------------------------------------------------
1 | classdef Fixed < urdf.Joint
2 | %FIXED Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | Property1
7 | end
8 |
9 | methods
10 | function obj = Fixed(name, parentLink,childLink)
11 | obj@urdf.Joint(name, 'fixed', parentLink, childLink);
12 | end
13 | end
14 |
15 | methods(Static)
16 | function obj = buildFromURDF(node)
17 | name = char(node.getAttribute('name'));
18 | parentName = '';
19 | childName = '';
20 | for i = 0:(node.getLength()-1)
21 | child = node.item(i);
22 | switch char(child.getNodeName())
23 | case 'parent'
24 | parentName = char(child.getAttribute('link'));
25 | case 'child'
26 | childName = char(child.getAttribute('link'));
27 | end
28 | end
29 | obj = urdf.joints.Fixed(name, parentName, childName);
30 | end
31 | end
32 | end
33 |
34 |
--------------------------------------------------------------------------------
/+urdf/+joints/Floating.m:
--------------------------------------------------------------------------------
1 | classdef Floating < urdf.Joint
2 | %FLOATING Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | Property1
7 | end
8 |
9 | methods
10 | function obj = Floating(name, parentLink,childLink)
11 | obj@urdf.Joint(name, 'floating', parentLink, childLink);
12 | end
13 | end
14 |
15 | methods(Static)
16 | function obj = buildFromURDF(node)
17 | name = char(node.getAttribute('name'));
18 | parentName = '';
19 | childName = '';
20 | for i = 0:(node.getLength()-1)
21 | child = node.item(i);
22 | switch char(child.getNodeName())
23 | case 'parent'
24 | parentName = char(child.getAttribute('link'));
25 | case 'child'
26 | childName = char(child.getAttribute('link'));
27 | end
28 | end
29 | obj = urdf.joints.Floating(name, parentName, childName);
30 | end
31 | end
32 | end
33 |
34 |
--------------------------------------------------------------------------------
/+urdf/+joints/Planar.m:
--------------------------------------------------------------------------------
1 | classdef Planar < urdf.Joint
2 | %UNTITLED Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | Property1
7 | end
8 |
9 | methods
10 | function obj = Planar(name, parentLink,childLink)
11 | obj@urdf.Joint(name, 'planar', parentLink, childLink);
12 | end
13 | end
14 |
15 | methods(Static)
16 | function obj = buildFromURDF(node)
17 | name = char(node.getAttribute('name'));
18 | parentName = '';
19 | childName = '';
20 | for i = 0:(node.getLength()-1)
21 | child = node.item(i);
22 | switch char(child.getNodeName())
23 | case 'parent'
24 | parentName = char(child.getAttribute('link'));
25 | case 'child'
26 | childName = char(child.getAttribute('link'));
27 | end
28 | end
29 | obj = urdf.joints.Planar(name, parentName, childName);
30 | end
31 | end
32 | end
33 |
34 |
--------------------------------------------------------------------------------
/+urdf/+joints/Prismatic.m:
--------------------------------------------------------------------------------
1 | classdef Prismatic < urdf.Joint
2 | %PRISMATIC Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | Property1
7 | end
8 |
9 | methods
10 | function obj = Prismatic(name, parentLink,childLink)
11 | obj@urdf.Joint(name, 'prismatic', parentLink, childLink);
12 | end
13 | end
14 |
15 | methods(Static)
16 | function obj = buildFromURDF(node)
17 | name = char(node.getAttribute('name'));
18 | parentName = '';
19 | childName = '';
20 | for i = 0:(node.getLength()-1)
21 | child = node.item(i);
22 | switch char(child.getNodeName())
23 | case 'parent'
24 | parentName = char(child.getAttribute('link'));
25 | case 'child'
26 | childName = char(child.getAttribute('link'));
27 | end
28 | end
29 | obj = urdf.joints.Prismatic(name, parentName, childName);
30 | end
31 | end
32 | end
33 |
34 |
--------------------------------------------------------------------------------
/+urdf/+joints/Revolute.m:
--------------------------------------------------------------------------------
1 | classdef Revolute < urdf.Joint
2 | % urdf.joints.Revolute class implements the revolute joint which has
3 | % properties such as lower, upper, effort and velocity.
4 |
5 | methods
6 | function obj = Revolute(name, parentLink, childLink, lower, upper, effort, velocity, axis)
7 | obj@urdf.Joint(name, 'revolute', parentLink, childLink);
8 |
9 | limit = urdf.URDFTag('limit');
10 | limit.addAttribute('lower', num2str(lower));
11 | limit.addAttribute('upper', num2str(upper));
12 | limit.addAttribute('effort', num2str(effort));
13 | limit.addAttribute('velocity', num2str(velocity));
14 |
15 | obj.addChild(limit);
16 |
17 | if nargin == 8
18 | axisTag = urdf.Axis(axis(1), axis(2), axis(3));
19 | obj.addChild(axisTag);
20 | end
21 | end
22 |
23 | function setLimits(obj, lower, upper)
24 | obj.addAttribute('lower', num2str(lower));
25 | obj.addAttribute('upper', num2str(upper));
26 | end
27 |
28 | function setEffort(obj, effort)
29 | obj.addAttribute('effort', num2str(effort));
30 | end
31 |
32 | function setVelocity(obj, velocity)
33 | obj.addAttribute('velocity', num2str(velocity));
34 | end
35 |
36 | function setAxis(obj, axis)
37 | foundChild = obj.findChild('axis');
38 | if isempty(foundChild)
39 | axisTag = urdf.Axis(axis(1), axis(2), axis(3));
40 | obj.addChild(axisTag);
41 | else
42 | obj.children(foundChild.name)
43 | end
44 | end
45 | end
46 |
47 | methods(Static)
48 | function obj = buildFromURDF(node)
49 | name = char(node.getAttribute('name'));
50 | parentName = '';
51 | childName = '';
52 | lower = '';
53 | upper = '';
54 | effort = '';
55 | velocity = '';
56 | for i = 0:(node.getLength()-1)
57 | child = node.item(i);
58 | switch char(child.getNodeName())
59 | case 'parent'
60 | parentName = char(child.getAttribute('link'));
61 | case 'child'
62 | childName = char(child.getAttribute('link'));
63 | case 'limit'
64 | lower = str2double(child.getAttribute('lower'));
65 | upper = str2double(child.getAttribute('upper'));
66 | effort = str2double(child.getAttribute('effort'));
67 | velocity = str2double(child.getAttribute('velocity'));
68 | end
69 | end
70 | obj = urdf.joints.Revolute(name, parentName, childName, lower, upper, effort, velocity);
71 | end
72 | end
73 | end
74 |
75 |
--------------------------------------------------------------------------------
/+urdf/+shapes/Box.m:
--------------------------------------------------------------------------------
1 | classdef Box < urdf.URDFTag
2 | %Box Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | methods
6 | function obj = Box(x, y, z)
7 | obj@urdf.URDFTag('box');
8 | obj.addAttribute('size', sprintf('%s %s %s', x, y, z));
9 | end
10 | end
11 |
12 | methods(Static)
13 | function obj = buildFromURDF(node)
14 | size = node.getAttribute('size');
15 | size = strsplit(size,' ');
16 | obj = urdf.shapes.Cylinder(str2double(size(1)), str2double(size(2)), str2double(size(3)));
17 | end
18 | end
19 | end
20 |
21 |
22 |
--------------------------------------------------------------------------------
/+urdf/+shapes/Cylinder.m:
--------------------------------------------------------------------------------
1 | classdef Cylinder < urdf.URDFTag
2 | %CYLINDER Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | methods
6 | function obj = Cylinder(radius, length)
7 | obj@urdf.URDFTag('cylinder');
8 | obj.addAttribute('radius', num2str(radius));
9 | obj.addAttribute('length', num2str(length));
10 | end
11 | end
12 |
13 | methods(Static)
14 | function obj = buildFromURDF(node)
15 | radius = node.getAttribute('radius');
16 | length = node.getAttribute('length');
17 | obj = urdf.shapes.Cylinder(str2double(radius), str2double(length));
18 | end
19 | end
20 | end
21 |
22 |
23 |
--------------------------------------------------------------------------------
/+urdf/+shapes/Mesh.m:
--------------------------------------------------------------------------------
1 | classdef Mesh
2 | %MESH Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | Property1
7 | end
8 |
9 | methods
10 | function obj = Mesh(inputArg1,inputArg2)
11 | %MESH Construct an instance of this class
12 | % Detailed explanation goes here
13 | obj.Property1 = inputArg1 + inputArg2;
14 | end
15 |
16 | function outputArg = method1(obj,inputArg)
17 | %METHOD1 Summary of this method goes here
18 | % Detailed explanation goes here
19 | outputArg = obj.Property1 + inputArg;
20 | end
21 | end
22 | end
23 |
24 |
--------------------------------------------------------------------------------
/+urdf/+shapes/Sphere.m:
--------------------------------------------------------------------------------
1 | classdef Sphere < urdf.URDFTag
2 | %CYLINDER Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | methods
6 | function obj = Sphere(radius)
7 | obj@urdf.URDFTag('sphere');
8 | obj.addAttribute('radius', num2str(radius));
9 | end
10 | end
11 |
12 | methods(Static)
13 | function obj = buildFromURDF(node)
14 | radius = node.getAttribute('radius');
15 | obj = urdf.shapes.Cylinder(str2double(radius));
16 | end
17 | end
18 | end
19 |
20 |
21 |
--------------------------------------------------------------------------------
/+urdf/+util/compareURDF.m:
--------------------------------------------------------------------------------
1 | function differences = compareURDF(file1, file2)
2 | % Compare two URDF files accounting for different tag ordering
3 | % Input: file1, file2 - paths to URDF files
4 | % Output: differences - struct containing differences found
5 |
6 | % Read XML files
7 | try
8 | xml1 = xmlread(file1);
9 | xml2 = xmlread(file2);
10 | catch
11 | error('Error reading URDF files. Make sure both files exist and are valid XML.');
12 | end
13 |
14 | % Initialize differences struct
15 | differences.missing_in_file1 = {};
16 | differences.missing_in_file2 = {};
17 | differences.attribute_differences = {};
18 |
19 | % Get root elements
20 | root1 = xml1.getDocumentElement();
21 | root2 = xml2.getDocumentElement();
22 |
23 | % Compare complete hierarchies
24 | differences = compareElements(root1, root2, '', differences);
25 | end
26 |
27 | function differences = compareElements(element1, element2, path, differences)
28 | % Compare two XML elements recursively
29 |
30 | % Get all child nodes
31 | children1 = element1.getChildNodes();
32 | children2 = element2.getChildNodes();
33 |
34 | % Create maps to store elements by their identifying attributes
35 | elements1 = containers.Map();
36 | elements2 = containers.Map();
37 |
38 | % Process children of first element
39 | for i = 0:children1.getLength()-1
40 | child = children1.item(i);
41 | if child.getNodeType() == child.ELEMENT_NODE
42 | key = generateElementKey(child);
43 | if ~isKey(elements1, key)
44 | elements1(key) = child;
45 | else
46 | % Handle duplicate keys by appending index
47 | j = 1;
48 | while isKey(elements1, [key '_' num2str(j)])
49 | j = j + 1;
50 | end
51 | elements1([key '_' num2str(j)]) = child;
52 | end
53 | end
54 | end
55 |
56 | % Process children of second element
57 | for i = 0:children2.getLength()-1
58 | child = children2.item(i);
59 | if child.getNodeType() == child.ELEMENT_NODE
60 | key = generateElementKey(child);
61 | if ~isKey(elements2, key)
62 | elements2(key) = child;
63 | else
64 | % Handle duplicate keys by appending index
65 | j = 1;
66 | while isKey(elements2, [key '_' num2str(j)])
67 | j = j + 1;
68 | end
69 | elements2([key '_' num2str(j)]) = child;
70 | end
71 | end
72 | end
73 |
74 | % Compare elements
75 | keys1 = elements1.keys();
76 | keys2 = elements2.keys();
77 |
78 | % Find elements missing in file2
79 | for i = 1:length(keys1)
80 | key = keys1{i};
81 | if ~isKey(elements2, key)
82 | currentPath = [path '/' char(elements1(key).getNodeName())];
83 | differences.missing_in_file2{end+1} = currentPath;
84 | end
85 | end
86 |
87 | % Find elements missing in file1
88 | for i = 1:length(keys2)
89 | key = keys2{i};
90 | if ~isKey(elements1, key)
91 | currentPath = [path '/' char(elements2(key).getNodeName())];
92 | differences.missing_in_file1{end+1} = currentPath;
93 | end
94 | end
95 |
96 | % Compare common elements
97 | commonKeys = intersect(keys1, keys2);
98 | for i = 1:length(commonKeys)
99 | key = commonKeys{i};
100 | elem1 = elements1(key);
101 | elem2 = elements2(key);
102 |
103 | % Compare attributes
104 | attrs1 = elem1.getAttributes();
105 | attrs2 = elem2.getAttributes();
106 |
107 | if attrs1.getLength() ~= attrs2.getLength()
108 | currentPath = [path '/' char(elem1.getNodeName())];
109 | differences.attribute_differences{end+1} = sprintf('%s: Different number of attributes', currentPath);
110 | else
111 | for j = 0:attrs1.getLength()-1
112 | attr1 = attrs1.item(j);
113 | attr2 = attrs2.getNamedItem(attr1.getName());
114 |
115 | if isempty(attr2) || ~strcmp(attr1.getValue(), attr2.getValue())
116 | currentPath = [path '/' char(elem1.getNodeName())];
117 | differences.attribute_differences{end+1} = sprintf('%s: Attribute "%s" differs', ...
118 | currentPath, char(attr1.getName()));
119 | end
120 | end
121 | end
122 |
123 | % Recurse into child elements
124 | differences = compareElements(elem1, elem2, [path '/' char(elem1.getNodeName())], differences);
125 | end
126 | end
127 |
128 | function key = generateElementKey(element)
129 | % Generate a unique key for an element based on its name and identifying attributes
130 |
131 | % Start with element name
132 | key = char(element.getNodeName());
133 |
134 | % Add important attributes that help identify the element
135 | attrs = element.getAttributes();
136 | if ~isempty(attrs)
137 | % Common identifying attributes in URDF
138 | idAttrs = {'name', 'joint', 'link', 'id'};
139 |
140 | for i = 1:length(idAttrs)
141 | attrNode = attrs.getNamedItem(idAttrs{i});
142 | if ~isempty(attrNode)
143 | key = [key '_' char(attrNode.getValue())];
144 | break; % Use first matching identifier found
145 | end
146 | end
147 | end
148 | end
--------------------------------------------------------------------------------
/+urdf/+util/findNodeFromRobotRoot.m:
--------------------------------------------------------------------------------
1 | function node = findNodeFromRobotRoot(model, path)
2 | node = urdf.URDFTag;
3 | levels = strsplit(path, '.');
4 | currentNode = model;
5 |
6 | % If the path only contains the root-level node of the model, simply
7 | % return the model.
8 | if isequal(length(levels), 1) && strcmp(currentNode.getName(), levels{1})
9 | node = currentNode;
10 | return
11 | end
12 |
13 | rootLevel = currentNode.getName();
14 | % Root level should match the model name.
15 | if ~strcmp(rootLevel, levels{1})
16 | return
17 | end
18 |
19 | % If the model is a Robot instance, serialize it once to populate its
20 | % children.
21 | if isa(model, "urdf.Robot")
22 | model.serialize();
23 | end
24 |
25 | % Check if the model has children
26 | if ~isConfigured(currentNode.children)
27 | % If the model has no children, then check if the second level is
28 | % an attribute. If yes, they return the currentNode as the found
29 | % node.
30 | attribute = currentNode.attributes.lookup(levels{2}, "FallbackValue", '');
31 | if ~isempty(attribute)
32 | node = currentNode;
33 | end
34 | return
35 | end
36 |
37 | % If the path contains more levels, traverse the levels and check if
38 | % the model contains children that match the level name.
39 | for index = 2:length(levels)
40 | level = levels{index};
41 | if ~isConfigured(currentNode.children)
42 | attribute = currentNode.attributes.lookup(level, "FallbackValue", '');
43 | if ~isempty(attribute)
44 | node = currentNode;
45 | end
46 | return
47 | else
48 | childNode = currentNode.children.lookup(level, "FallbackValue", {urdf.URDFTag});
49 | if isa(childNode, "cell")
50 | childNode = childNode{1};
51 | end
52 | if urdf.util.isNullTag(childNode)
53 | % if the child node returned as NULL, check if the level
54 | % actually is a type and not a name.
55 | childNode = currentNode.findChild(level);
56 | if isa(childNode, "cell")
57 | childNode = childNode{1};
58 | end
59 |
60 | % If we did not find the level with the type information as
61 | % well, perhaps its an attribute.
62 | if urdf.util.isNullTag(childNode)
63 | attribute = currentNode.attributes.lookup(level, "FallbackValue", '');
64 | if isempty(attribute)
65 | return
66 | else
67 | % Make sure that the attribute is always the last level
68 | % in the path. If its not, then the path is badly
69 | % formed and we should return nothing.
70 | if ~isequal(index, length(levels))
71 | return
72 | end
73 |
74 | % We have indeed reached an attribute which is the last
75 | % level on the path. We can safely return the current
76 | % node as the desired node.
77 | node = currentNode;
78 | end
79 | end
80 | end
81 | currentNode = childNode;
82 | end
83 | end
84 | node = currentNode;
85 | end
86 |
87 |
--------------------------------------------------------------------------------
/+urdf/+util/findNodesByPattern.m:
--------------------------------------------------------------------------------
1 | function nodes = findNodesByPattern(rootNode, pattern, options)
2 | % FINDNODESBYPATTERN Find all URDF nodes whose names match the specified pattern
3 | %
4 | % Usage:
5 | % nodes = urdf.util.findNodesByPattern(rootNode, pattern)
6 | % nodes = urdf.util.findNodesByPattern(rootNode, pattern, options)
7 | %
8 | % Parameters:
9 | % rootNode - URDFTag object to start search from
10 | % pattern - String containing the search pattern (can include wildcards * and ?)
11 | % options - Optional struct with search parameters:
12 | % .recursive (logical): Search recursively through children (default: true)
13 | % .caseSensitive (logical): Case-sensitive search (default: false)
14 | % .regularExpression (logical): Use regular expression pattern (default: false)
15 | % .typeFilter (string): Filter by node type (default: '')
16 | % .maxDepth (numeric): Maximum depth to search (default: inf)
17 | %
18 | % Returns:
19 | % nodes - Cell array of matching URDFTag objects
20 |
21 | % Input validation
22 | validateattributes(rootNode, {'urdf.URDFTag'}, {'scalar'}, 'findNodesByPattern', 'rootNode');
23 | validateattributes(pattern, {'char', 'string'}, {'scalartext'}, 'findNodesByPattern', 'pattern');
24 |
25 | % Set default options if not provided
26 | if nargin < 3
27 | options = struct();
28 | end
29 | validateattributes(options, {'struct'}, {}, 'findNodesByPattern', 'options');
30 |
31 | % Default option values
32 | if ~isfield(options, 'recursive')
33 | options.recursive = true;
34 | end
35 | if ~isfield(options, 'caseSensitive')
36 | options.caseSensitive = false;
37 | end
38 | if ~isfield(options, 'regularExpression')
39 | options.regularExpression = false;
40 | end
41 | if ~isfield(options, 'typeFilter')
42 | options.typeFilter = '';
43 | end
44 | if ~isfield(options, 'maxDepth')
45 | options.maxDepth = inf;
46 | end
47 |
48 | % Convert pattern to regex if not using regex mode
49 | if ~options.regularExpression
50 | % Escape special regex characters
51 | pattern = regexptranslate('escape', pattern);
52 | % Convert wildcards to regex patterns
53 | pattern = strrep(pattern, '\*', '.*');
54 | pattern = strrep(pattern, '\?', '.');
55 | end
56 |
57 | % Set case sensitivity flag for regex
58 | if options.caseSensitive
59 | regexFlags = '';
60 | else
61 | regexFlags = 'ignorecase';
62 | end
63 |
64 | % Start recursive search
65 | nodes = searchNodes(rootNode, pattern, regexFlags, options, 1, {});
66 | end
67 |
68 | function nodes = searchNodes(node, pattern, regexFlags, options, depth, nodes)
69 | % Recursive search function
70 |
71 | % Check if current node matches
72 | if matchesPattern(node, pattern, regexFlags, options)
73 | nodes{end+1} = node;
74 | end
75 |
76 | % Stop if we've reached max depth or recursion is disabled
77 | if depth >= options.maxDepth || ~options.recursive
78 | return;
79 | end
80 |
81 | % Search through children if they exist
82 | if isConfigured(node.children)
83 | childKeys = keys(node.children);
84 | for i = 1:numel(childKeys)
85 | child = node.children(childKeys{i});
86 | child = child{1}; % Get the actual child object from cell
87 | nodes = searchNodes(child, pattern, regexFlags, options, depth + 1, nodes);
88 | end
89 | end
90 | end
91 |
92 | function matches = matchesPattern(node, pattern, regexFlags, options)
93 | % Check if node matches search criteria
94 |
95 | % Check type filter first if specified
96 | if ~isempty(options.typeFilter) && ~strcmp(node.type, options.typeFilter)
97 | matches = false;
98 | return;
99 | end
100 |
101 | % Get node name
102 | nodeName = node.getName();
103 |
104 | % Check if name matches pattern
105 | matches = ~isempty(regexp(nodeName, pattern, 'once', regexFlags));
106 | end
--------------------------------------------------------------------------------
/+urdf/+util/isNullTag.m:
--------------------------------------------------------------------------------
1 | function flag = isNullTag(tag)
2 | flag = strcmp(tag.type, 'NULL');
3 | end
4 |
5 |
--------------------------------------------------------------------------------
/+urdf/+util/readXML.m:
--------------------------------------------------------------------------------
1 | function robotNode = readXML(filename)
2 | try
3 | node = xmlread(filename);
4 | robotNode = node.getDocumentElement();
5 | catch
6 | error('Failed to read XML file %s.', filename);
7 | end
8 | end
9 |
10 |
--------------------------------------------------------------------------------
/+urdf/+util/writeToURDFFile.m:
--------------------------------------------------------------------------------
1 | function writeToURDFFile(robot, filename)
2 | file = fopen(filename, 'w+');
3 | fprintf(file, '\n%s', robot.serialize());
4 | fclose(file);
5 | end
6 |
7 |
--------------------------------------------------------------------------------
/+urdf/Axis.m:
--------------------------------------------------------------------------------
1 | classdef Axis < urdf.URDFTag
2 | methods
3 | function obj = Axis(x, y, z)
4 | obj@urdf.URDFTag('axis');
5 | obj.addAttribute('xyz', sprintf('%s %s %s', x, y, z));
6 | end
7 |
8 | function reset(obj, x, y, z)
9 | obj.attributes('xyz') = sprintf('%s %s %s', x, y, z);
10 | end
11 | end
12 |
13 | methods(Static)
14 | function obj = buildFromURDF(node)
15 | xyz = node.getAttribute('xyz');
16 | xyz_s = xyz.split(' ');
17 | obj = urdf.Axis(str2double(xyz_s(1)),...
18 | str2double(xyz_s(2)),...
19 | str2double(xyz_s(3)));
20 | end
21 | end
22 | end
23 |
24 |
--------------------------------------------------------------------------------
/+urdf/Builder.m:
--------------------------------------------------------------------------------
1 | classdef Builder
2 | %URDFBUILDER URDFBuilder helps building a robot model using the urdf
3 | %format.
4 | % The URDFBuilder can be used to build a robot model using a
5 | % step-by-step construction process, by defining links, geometries,
6 | % joints etc. Once all the elements have been added, the model can be
7 | % compiled into a .urdf file that can be loaded onto Gazebo and used
8 | % for simulations.
9 |
10 | properties
11 | Property1
12 | end
13 |
14 | methods
15 | function obj = Builder(inputArg1,inputArg2)
16 | %URDFBUILDER Construct an instance of this class
17 | % Detailed explanation goes here
18 | obj.Property1 = inputArg1 + inputArg2;
19 | end
20 |
21 | function outputArg = method1(obj,inputArg)
22 | %METHOD1 Summary of this method goes here
23 | % Detailed explanation goes here
24 | outputArg = obj.Property1 + inputArg;
25 | end
26 | end
27 | end
28 |
29 |
--------------------------------------------------------------------------------
/+urdf/Collision.m:
--------------------------------------------------------------------------------
1 | classdef Collision < urdf.URDFTag
2 | %VISUAL class represents the visual component of the link node
3 | properties
4 | geometry
5 | origin
6 | end
7 |
8 | methods
9 | function obj = Collision()
10 | obj@urdf.URDFTag('collision');
11 | obj.geometry = urdf.Geometry();
12 | obj.origin = urdf.Origin();
13 | end
14 |
15 | function addVisualComponent(obj, element)
16 | obj.geometry.addVisualComponent(element);
17 | end
18 |
19 | function setOrigin(obj, roll, pitch, yaw, x, y, z)
20 | obj.origin.reset(roll, pitch, yaw, x, y, z);
21 | end
22 |
23 | function setOriginObj(obj, origin)
24 | obj.origin = origin;
25 | end
26 |
27 | function outputArg = serialize(obj)
28 | obj.clearChildren();
29 | obj.addChild(obj.geometry);
30 | obj.addChild(obj.origin);
31 | outputArg = serialize@urdf.URDFTag(obj);
32 | end
33 | end
34 |
35 | methods(Static)
36 | function obj = buildFromURDF(node)
37 | obj = urdf.Collision();
38 | for i = 0:(node.getLength()-1)
39 | child = node.item(i);
40 | switch char(child.getNodeName())
41 | case 'geometry'
42 | obj.geometry = urdf.Geometry.buildFromURDF(child);
43 | case 'origin'
44 | obj.setOriginObj(urdf.Origin.buildFromURDF(child));
45 | end
46 | end
47 | end
48 | end
49 | end
50 |
--------------------------------------------------------------------------------
/+urdf/Component.m:
--------------------------------------------------------------------------------
1 | classdef Component < handle
2 | properties
3 | shadowRobot % Robot object containing component definition
4 | parameters % Map of parameter name to {defaultValue, appliesTo} struct
5 | attachPoint % Attachment point for the component
6 | tabs
7 | end
8 |
9 | methods
10 | function obj = Component(name)
11 | obj.tabs = 0;
12 | obj.shadowRobot = urdf.Robot(name);
13 | obj.parameters = containers.Map('KeyType', 'char', 'ValueType', 'any');
14 | obj.attachPoint = '';
15 |
16 | % Increment the shadow robot's tabs to indent it by one level.
17 | obj.shadowRobot.tabs = 1;
18 | end
19 |
20 | function addLink(obj, link)
21 | obj.shadowRobot.addLink(link);
22 | end
23 |
24 | function addJoint(obj, joint)
25 | obj.shadowRobot.addJoint(joint);
26 | end
27 |
28 | function addParameter(obj, name, defaultValue, appliesTo)
29 | if obj.parameters.isKey(name)
30 | error('Parameter %s already exists', name);
31 | end
32 | obj.parameters(name) = struct('defaultValue', defaultValue, ...
33 | 'appliesTo', appliesTo);
34 | end
35 |
36 | function setAttachPoint(obj, point)
37 | obj.attachPoint = point;
38 | end
39 |
40 | function targetRobot = createInstance(obj, targetRobot, instanceName, paramValues)
41 | % Create instance copy
42 | robotCopy = obj.shadowRobot.clone();
43 | robotCopy.serialize();
44 |
45 |
46 | % Apply parameter values
47 | if ~isempty(paramValues)
48 | paramNames = fieldnames(paramValues);
49 | for i = 1:numel(paramNames)
50 | param = paramNames{i};
51 | % Get path and apply value
52 | paramDef = obj.parameters(param);
53 | path = paramDef.appliesTo;
54 | if ~robotCopy.applyParameterValue(path, paramValues.(param))
55 | error('Failed to apply parameter %s to path %s', ...
56 | param, path);
57 | end
58 | end
59 | end
60 |
61 | % Add the instance name as prefix to the robot copy nodes.
62 | robotCopy.addPrefixToChildren(instanceName);
63 |
64 | % Merge the robot copy into the target robot.
65 | targetRobot.merge(robotCopy);
66 | end
67 |
68 | function xml = serialize(obj)
69 | shadowRobotXML = obj.shadowRobot.serialize();
70 |
71 | end
72 | end
73 |
74 | methods(Static)
75 | function obj = buildFromURDF(node)
76 | % Create component from XML node
77 | name = char(node.getAttribute('name'));
78 | obj = urdf.Component(name);
79 |
80 | % Parse parameters
81 | paramNodes = node.getElementsByTagName('parameter');
82 | for i = 0:paramNodes.getLength()-1
83 | paramNode = paramNodes.item(i);
84 |
85 | nameNode = paramNode.getElementsByTagName('name').item(0);
86 | defaultNode = paramNode.getElementsByTagName('defaultValue').item(0);
87 | appliesToNode = paramNode.getElementsByTagName('appliesTo').item(0);
88 |
89 | name = char(nameNode.getTextContent());
90 | defaultValue = char(defaultNode.getTextContent());
91 | appliesTo = char(appliesToNode.getTextContent());
92 |
93 | obj.addParameter(name, defaultValue, appliesTo);
94 | end
95 |
96 | % Parse attachPoint
97 | attachNodes = node.getElementsByTagName('attachPoint');
98 | if attachNodes.getLength() > 0
99 | attachPoint = char(attachNodes.item(0).getTextContent());
100 | obj.setAttachPoint(attachPoint);
101 | end
102 | end
103 | end
104 | end
--------------------------------------------------------------------------------
/+urdf/Geometry.m:
--------------------------------------------------------------------------------
1 | classdef Geometry < urdf.URDFTag
2 | %GEOMETRY Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | methods
6 | function obj = Geometry()
7 | %GEOMETRY Construct an instance of this class
8 | obj@urdf.URDFTag('geometry');
9 | end
10 |
11 | function addVisualComponent(obj, element)
12 | obj.addChild(element);
13 | end
14 | end
15 |
16 | methods(Static)
17 | function obj = buildFromURDF(node)
18 | obj = urdf.Geometry();
19 | for i = 0:(node.getLength()-1)
20 | child = node.item(i);
21 | if strcmp(char(child.getNodeName()), 'cylinder')
22 | obj.addVisualComponent(urdf.shapes.Cylinder.buildFromURDF(child));
23 | end
24 | end
25 | end
26 | end
27 | end
28 |
29 |
--------------------------------------------------------------------------------
/+urdf/Inertial.m:
--------------------------------------------------------------------------------
1 | classdef Inertial < urdf.URDFTag
2 | %INERTIAL Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | Property1
7 | end
8 |
9 | methods
10 | function obj = Inertial(inputArg1,inputArg2)
11 | %INERTIAL Construct an instance of this class
12 | % Detailed explanation goes here
13 | obj.Property1 = inputArg1 + inputArg2;
14 | end
15 |
16 | function outputArg = method1(obj,inputArg)
17 | %METHOD1 Summary of this method goes here
18 | % Detailed explanation goes here
19 | outputArg = obj.Property1 + inputArg;
20 | end
21 | end
22 | end
23 |
24 |
--------------------------------------------------------------------------------
/+urdf/Joint.m:
--------------------------------------------------------------------------------
1 | classdef Joint < urdf.URDFTag
2 | %JOINT Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | jointType
7 | origin
8 | parentLink
9 | childLink
10 | end
11 |
12 | methods
13 | function obj = Joint(name, jointType, parentName, childName, elements)
14 | arguments (Input)
15 | % Required properties
16 | name (1,1) string
17 | jointType (1,1) string
18 | parentName (1,1) string
19 | childName (1,1) string
20 |
21 | % Optional elements - sub-classes to enforce elements as required.
22 | elements.origin (1,1) struct = struct('roll', 0, 'pitch', 0, 'yaw', 0, 'x', 0, 'y', 0, 'z', 0)
23 | elements.axis (1,3) = [1, 0, 1]
24 | elements.calibration (1,1) struct = struct('rising', [], 'falling', [])
25 | elements.dynamics (1,1) struct = struct('damping', 0, 'friction', 0)
26 | elements.limit (1,1) struct = struct('lower', 0, 'upper', 0, 'effort', 0, 'velocity', 0)
27 | elements.safety_controller (1,1) struct = struct('soft_lower_limit', 0, 'soft_upper_limit', 0, 'k_position', 0, 'k_velocity', 0)
28 | elements.mimic (1,1) struct = struct('joint', '', 'multiplier', 0, 'offset', 0)
29 | end
30 |
31 | % Construct the urdftag.
32 | obj@urdf.URDFTag('joint', name);
33 |
34 | % Set the joint type.
35 | obj.addAttribute('type', jointType);
36 |
37 | % Configure the parent and child links.
38 | obj.parentLink = urdf.URDFTag('parent');
39 | obj.parentLink.addAttribute('link', parentName);
40 | obj.addChild(obj.parentLink);
41 | obj.childLink = urdf.URDFTag('child');
42 | obj.childLink.addAttribute('link', childName);
43 | obj.addChild(obj.childLink);
44 |
45 | % Add the joint elements.
46 | obj.addElements(elements)
47 | end
48 |
49 | function updateParentLink(obj, parentName)
50 | obj.parentLink.addAttribute('link', parentName);
51 | end
52 |
53 | function updateChildLink(obj, childName)
54 | obj.childLink.addAttribute('link', childName)
55 | end
56 |
57 | function addElements(elements)
58 |
59 | end
60 | end
61 |
62 | methods(Static)
63 | function obj = buildFromURDF(node)
64 | type = char(node.getAttribute('type'));
65 | obj = [];
66 | switch(type)
67 | case 'revolute'
68 | obj = urdf.joints.Revolute.buildFromURDF(node);
69 | case 'prismatic'
70 | obj = urdf.joints.Prismatic.buildFromURDF(node);
71 | case 'planar'
72 | obj = urdf.joints.Planar.buildFromURDF(node);
73 | case 'floating'
74 | obj = urdf.joints.Floating.buildFromURDF(node);
75 | case 'fixed'
76 | obj = urdf.joints.Fixed.buildFromURDF(node);
77 | case 'continuous'
78 | obj = urdf.joints.Continuous.buildFromURDF(node);
79 | end
80 | if ~isempty(obj)
81 | for i = 0:(node.getLength()-1)
82 | child = node.item(i);
83 | if strcmp(char(child.getNodeName()), 'origin')
84 | obj.setOriginObj(urdf.Origin.buildFromURDF(child));
85 | end
86 | end
87 | end
88 | end
89 | end
90 | end
91 |
92 |
--------------------------------------------------------------------------------
/+urdf/Link.m:
--------------------------------------------------------------------------------
1 | classdef Link < urdf.URDFTag
2 | %LINK class represents the LINK node in the URDF file.
3 | % Link node represents a physical element in the robot.
4 | properties
5 | visual
6 | collision
7 | end
8 | methods
9 | function obj = Link(name)
10 | obj@urdf.URDFTag('link', name);
11 | obj.visual = urdf.Visual();
12 | obj.collision = urdf.Collision();
13 | end
14 |
15 | function addChild(obj, child)
16 | if ~isa(child, "urdf.Visual") && ~isa(child, "urdf.Collision")
17 | error("Invalid child for a Link node");
18 | end
19 | addChild@urdf.URDFTag(obj, child);
20 | end
21 |
22 | function addVisual(obj, element)
23 | obj.visual.addVisualComponent(element);
24 | end
25 |
26 | function addCollision(obj, element)
27 | obj.collision.addVisualComponent(element);
28 | end
29 |
30 | function setOrigin(obj, roll, pitch, yaw, x, y, z)
31 | obj.visual.setOrigin(roll, pitch, yaw, x, y, z);
32 | obj.collision.setOrigin(roll, pitch, yaw, x, y, z);
33 | end
34 |
35 | function outputArg = serialize(obj)
36 | obj.clearChildren();
37 | obj.addChild(obj.visual);
38 | obj.addChild(obj.collision);
39 | outputArg = serialize@urdf.URDFTag(obj);
40 | end
41 | end
42 |
43 | methods(Static)
44 | function obj = buildFromURDF(node)
45 | linkName = node.getAttribute('name');
46 | obj = urdf.Link(linkName);
47 | for i = 0:(node.getLength()-1)
48 | child = node.item(i);
49 | if strcmp(child.getNodeName(), 'visual')
50 | obj.visual = urdf.Visual.buildFromURDF(child);
51 | elseif strcmp(child.getNodeName(), 'collision')
52 | obj.collision = urdf.Collision.buildFromURDF(child);
53 | end
54 | end
55 | end
56 | end
57 | end
58 |
59 |
--------------------------------------------------------------------------------
/+urdf/Material.m:
--------------------------------------------------------------------------------
1 | classdef Material < urdf.URDFTag
2 | %MATERIAL Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | Property1
7 | end
8 |
9 | methods
10 | function obj = Material(inputArg1,inputArg2)
11 | %MATERIAL Construct an instance of this class
12 | % Detailed explanation goes here
13 | obj.Property1 = inputArg1 + inputArg2;
14 | end
15 |
16 | function outputArg = method1(obj,inputArg)
17 | %METHOD1 Summary of this method goes here
18 | % Detailed explanation goes here
19 | outputArg = obj.Property1 + inputArg;
20 | end
21 | end
22 | end
23 |
24 |
--------------------------------------------------------------------------------
/+urdf/Origin.m:
--------------------------------------------------------------------------------
1 | classdef Origin < urdf.URDFTag
2 | %ORIGIN This class specifies the origin for a link node
3 |
4 | properties
5 | pitch
6 | yaw
7 | roll
8 | x
9 | y
10 | z
11 | end
12 |
13 | methods
14 | function obj = Origin(roll, pitch, yaw, x, y, z)
15 | obj@urdf.URDFTag('origin');
16 | obj.pitch = '0';
17 | obj.yaw = '0';
18 | obj.roll = '0';
19 | obj.x = '0';
20 | obj.y = '0';
21 | obj.z = '0';
22 | if exist("roll", "var") ...
23 | && exist("pitch", "var") ...
24 | && exist("yaw", "var") ...
25 | && exist("x", "var") ...
26 | && exist("y", "var") ...
27 | && exist("z", "var")
28 | obj.reset(roll, pitch, yaw, x, y, z);
29 | end
30 | end
31 |
32 | function reset(obj, roll, pitch, yaw, x, y, z)
33 | obj.setPitch(pitch);
34 | obj.setYaw(yaw);
35 | obj.setRoll(roll);
36 | obj.setX(x);
37 | obj.setY(y);
38 | obj.setZ(z);
39 | end
40 |
41 | function setPitch(obj, pitch)
42 | if isnumeric(pitch)
43 | pitch = num2str(pitch);
44 | end
45 | obj.pitch = pitch;
46 | end
47 |
48 | function setYaw(obj, yaw)
49 |
50 | if isnumeric(yaw)
51 | yaw = num2str(yaw);
52 | end
53 | obj.yaw = yaw;
54 | end
55 |
56 | function setRoll(obj, roll)
57 |
58 | if isnumeric(roll)
59 | roll = num2str(roll);
60 | end
61 | obj.roll = roll;
62 | end
63 |
64 | function setX(obj, x)
65 |
66 | if isnumeric(x)
67 | x = num2str(x);
68 | end
69 | obj.x = x;
70 | end
71 |
72 | function setY(obj, y)
73 |
74 | if isnumeric(y)
75 | y = num2str(y);
76 | end
77 | obj.y = y;
78 | end
79 |
80 | function setZ(obj, z)
81 |
82 | if isnumeric(z)
83 | z = num2str(z);
84 | end
85 | obj.z = z;
86 | end
87 |
88 | function outputArg = serialize(obj)
89 | obj.addAttribute('rpy', sprintf('%s %s %s', obj.roll, obj.pitch, obj.yaw));
90 | obj.addAttribute('xyz', sprintf('%s %s %s', obj.x, obj.y, obj.z));
91 | outputArg = serialize@urdf.URDFTag(obj);
92 | end
93 | end
94 |
95 | methods(Static)
96 | function obj = buildFromURDF(node)
97 | rpy = node.getAttribute('rpy');
98 | xyz = node.getAttribute('xyz');
99 | rpy_s = rpy.split(' ');
100 | xyz_s = xyz.split(' ');
101 | obj = urdf.Origin(...
102 | str2double(rpy_s(1)),...
103 | str2double(rpy_s(2)),...
104 | str2double(rpy_s(3)),...
105 | str2double(xyz_s(1)),...
106 | str2double(xyz_s(2)),...
107 | str2double(xyz_s(3)));
108 | end
109 | end
110 | end
111 |
112 |
--------------------------------------------------------------------------------
/+urdf/Robot.m:
--------------------------------------------------------------------------------
1 | classdef Robot < urdf.URDFTag
2 | %ROBOT Summary of this class goes here
3 | % Detailed explanation goes here
4 |
5 | properties
6 | links
7 | joints
8 | end
9 |
10 | methods
11 | function obj = Robot(name)
12 | obj@urdf.URDFTag('robot', name);
13 | obj.links = dictionary;
14 | obj.joints = dictionary;
15 | end
16 |
17 | function newRobot = clone(obj)
18 | urdf.util.writeToURDFFile(obj, 'tempFile.urdf');
19 | newRobot = urdf.Robot.buildFromURDF('tempFile.urdf');
20 | delete tempFile.urdf;
21 | end
22 |
23 | function addLink(obj, link)
24 | obj.links(link.getName()) = {link};
25 | end
26 |
27 | function removeLink(obj, linkName)
28 | obj.links = remove(obj.links, linkName);
29 | end
30 |
31 | function link = getLink(obj, linkName)
32 | link = obj.links(linkName);
33 | if isa(link, 'cell')
34 | link = link{1};
35 | end
36 | end
37 |
38 | function addJoint(obj, joint)
39 | obj.joints(joint.getName()) = {joint};
40 | end
41 |
42 | function removeJoint(obj, jointName)
43 | remove(obj.joints, jointName);
44 | end
45 |
46 | function joint = getJoint(obj, jointName)
47 | joint = obj.joints(jointName);
48 | if isa(joint, 'cell')
49 | joint = joint{1};
50 | end
51 | end
52 |
53 | function configureJoint(obj, jointName, lower, upper, effort, velocity)
54 | joint = obj.getJoint(jointName);
55 | joint.setLimits(lower, upper);
56 | joint.setEffort(effort);
57 | joint.setVelocity(velocity);
58 | end
59 |
60 | function addPrefixToChildren(obj, prefix)
61 | % Input validation
62 | validateattributes(prefix, {'char', 'string'}, {'scalartext'}, 'addPrefix', 'prefix');
63 |
64 | % Collect all original names for reference updating
65 | nameMap = containers.Map('KeyType', 'char', 'ValueType', 'char');
66 |
67 | % Update link names
68 | if isConfigured(obj.links)
69 | linkKeys = keys(obj.links);
70 | for i = 1:numel(linkKeys)
71 | link = obj.getLink(linkKeys{i});
72 | oldName = link.getName();
73 | newName = strcat(prefix, "_", oldName);
74 |
75 | % Store name mapping for joint reference updates
76 | nameMap(oldName) = newName;
77 |
78 | % Update link name
79 | if link.useName
80 | link.attributes('name') = newName;
81 | else
82 | link.name = newName;
83 | end
84 |
85 | % Update link key in robot's links map
86 | obj.removeLink(oldName);
87 | obj.addLink(link);
88 | end
89 | end
90 |
91 | % Update joint names and references
92 | if isConfigured(obj.joints)
93 | jointKeys = keys(obj.joints);
94 | for i = 1:numel(jointKeys)
95 | joint = obj.getJoint(jointKeys{i});
96 | oldName = joint.getName();
97 | newName = strcat(prefix, "_", oldName);
98 |
99 | % Update joint name
100 | if joint.useName
101 | joint.attributes('name') = newName;
102 | else
103 | joint.name = newName;
104 | end
105 |
106 | % Update parent link reference
107 | if isa(joint.parentLink, 'urdf.URDFTag') && joint.parentLink.hasAttribute('link')
108 | parentName = joint.parentLink.attributes('link');
109 | if nameMap.isKey(parentName)
110 | joint.parentLink.attributes('link') = nameMap(parentName);
111 | end
112 | end
113 |
114 | % Update child link reference
115 | if isa(joint.child, 'urdf.URDFTag') && joint.child.hasAttribute('link')
116 | childName = joint.child.attributes('link');
117 | if nameMap.isKey(childName)
118 | joint.child.attributes('link') = nameMap(childName);
119 | end
120 | end
121 |
122 | % Update joint key in robot's joints map
123 | obj.removeJoint(oldName);
124 | obj.addJoint(joint);
125 | end
126 | end
127 | end
128 |
129 | function merge(obj, otherRobot)
130 | if isConfigured(otherRobot.links)
131 | linkKeys = keys(otherRobot.links);
132 | for i = 1:numel(linkKeys)
133 | link = otherRobot.getLink(linkKeys{i});
134 | obj.addLink(link);
135 | end
136 | end
137 | if isConfigured(otherRobot.joints)
138 | jointKeys = keys(otherRobot.joints);
139 | for i = 1:numel(jointKeys)
140 | joint = otherRobot.getJoint(jointKeys{i});
141 | obj.addJoint(joint);
142 | end
143 | end
144 | % Merge material and other nodes here.
145 | % TO DO
146 | end
147 |
148 | function outputArg = serialize(obj)
149 | obj.resetChildren();
150 | obj.populateChildren();
151 | outputArg = serialize@urdf.URDFTag(obj);
152 | end
153 |
154 | function populateFromXML(obj, node)
155 | for i = 0:(node.getLength()-1)
156 | child = node.item(i);
157 | switch char(child.getNodeName())
158 | case 'link'
159 | obj.addLink(urdf.Link.buildFromURDF(child));
160 | case 'joint'
161 | obj.addJoint(urdf.Joint.buildFromURDF(child));
162 | end
163 | end
164 | end
165 |
166 | function success = applyParameterValue(obj, path, value)
167 | % Apply parameter value to specified path in robot
168 | try
169 | pathParts = strsplit(path, '.');
170 | node = obj;
171 |
172 | % Since the first part of the path has to be a named
173 | % element, we can search for it first.
174 | node = node.findChild('urdf.URDFTag',pathParts{1});
175 | if isa(node, 'cell')
176 | node = node{1};
177 | end
178 |
179 | % Navigate path
180 | for i = 2:length(pathParts)-1
181 | temp = node.findChild(pathParts{i});
182 | if isa(temp, 'cell')
183 | node = temp{1};
184 | end
185 | end
186 |
187 | % Set value
188 | node.addAttribute(pathParts{end}, value);
189 | success = true;
190 | catch
191 | success = false;
192 | end
193 | end
194 | end
195 |
196 | methods(Access = private)
197 | function resetChildren(obj)
198 | if isConfigured(obj.children)
199 | obj.children = remove(obj.children, keys(obj.children));
200 | end
201 | end
202 |
203 | function populateChildren(obj)
204 | linkEntries = values(obj.links);
205 | jointEntries = values(obj.joints);
206 |
207 | for index = 1:numel(linkEntries)
208 | link = linkEntries{index};
209 | obj.addChild(link);
210 | end
211 |
212 | for index = 1:numel(jointEntries)
213 | joint = jointEntries{index};
214 | obj.addChild(joint);
215 | end
216 | end
217 | end
218 |
219 | methods(Static)
220 | function robot = buildFromURDF(filename)
221 | node = urdf.util.readXML(filename);
222 | robot = urdf.Robot(node.getAttribute('name'));
223 | robot.populateFromXML(node);
224 | end
225 | end
226 | end
227 |
228 |
--------------------------------------------------------------------------------
/+urdf/URDFTag.m:
--------------------------------------------------------------------------------
1 | classdef URDFTag < handle
2 | %URDFTAG URDFTag is the main urdf element xml tag base class
3 | % URDFTag base class provides all the XML and common urdf element
4 | % properties. All urdf elements are subclasses of this class and
5 | % override specific functionality as needed.
6 |
7 | properties
8 | name
9 | type
10 | parent
11 | children
12 | attributes
13 | tabs
14 | end
15 |
16 | properties(Access = protected)
17 | useName = true
18 | end
19 |
20 | methods
21 | function obj = URDFTag(type, name)
22 | %URDFTAG Construct an instance of this class
23 |
24 | % If a type is specified, assign it to the object and create
25 | % the containers. If type is not specified, treat it as a null
26 | % tag.
27 | if exist('type', 'var')
28 | obj.type = type;
29 | obj.attributes = dictionary;
30 | obj.children = dictionary;
31 |
32 | if exist('name','var')
33 | if isa(name, 'java.lang.String')
34 | name = string(name);
35 | end
36 | obj.name = name;
37 | obj.attributes('name') = name;
38 | else
39 | [~, randStr] = fileparts(tempname);
40 | obj.name = strcat(obj.type, '_', randStr);
41 | obj.useName = false;
42 | end
43 | obj.tabs = 0;
44 | else
45 | obj.type = 'NULL';
46 | end
47 | end
48 |
49 | function name = getName(obj)
50 | if obj.useName
51 | name = obj.attributes('name');
52 | else
53 | name = obj.name;
54 | end
55 | end
56 |
57 | function addAttribute(obj, name, value)
58 | obj.attributes(name) = value;
59 | end
60 |
61 | function addAttributes(obj, attrs)
62 | for index = 1:numel(attrs)
63 | obj.addAttribute(attrs(index).name, attrs(index).value);
64 | end
65 | end
66 |
67 | function flag = hasAttribute(obj, attrName)
68 | flag = false;
69 | if ~urdf.util.isNullTag(obj) && isConfigured(obj.attributes)
70 | flag = isKey(obj.attributes, attrName);
71 | end
72 | end
73 |
74 | function addParent(obj, parent)
75 | obj.parent = parent;
76 | obj.tabs = parent.tabs + 1;
77 | if numEntries(obj.children) > 0
78 | childKeys = keys(obj.children);
79 | for index = 1:numel(childKeys)
80 | child = obj.children(childKeys(index));
81 | child = child{1};
82 | child.addParent(obj);
83 | end
84 | end
85 | end
86 |
87 | function addChild(obj, child)
88 | child.addParent(obj);
89 |
90 | % Check for duplicates and throw an error if the child already
91 | % was added earlier.
92 | foundChild = obj.findChild(child.type, child.name);
93 | if ~urdf.util.isNullTag(foundChild)
94 | error(sprintf('There is already a child of type %s and name %s', child.type, child.name));
95 | else
96 | obj.children(child.getName()) = {child};
97 | end
98 | end
99 |
100 | function child = findChild(obj, type, name)
101 | child = urdf.URDFTag;
102 | if ~isConfigured(obj.children)
103 | return;
104 | end
105 |
106 | childType = type;
107 | isUnNamed = true;
108 | childName = '';
109 | if nargin == 3
110 | childName = name;
111 | else
112 | isUnNamed = false;
113 | end
114 |
115 | if ~isUnNamed
116 | childKeys = keys(obj.children);
117 | for index = 1:numel(childKeys)
118 | currentChild = obj.children(childKeys(index));
119 | currentChild = currentChild{1};
120 | if strcmp(currentChild.type, childType) && ~currentChild.useName
121 | childName = currentChild.name;
122 | break;
123 | end
124 | end
125 | end
126 |
127 | if ~isempty(childName) && obj.children.isKey(childName)
128 | child = obj.children(childName);
129 | end
130 | end
131 |
132 | function clearChildren(obj)
133 | if isConfigured(obj.children)
134 | obj.children = remove(obj.children, obj.children.keys);
135 | end
136 | end
137 |
138 | function outputArg = serialize(obj)
139 |
140 | %SERIALIZE serialize the tag into a valid xml tag
141 | % Object will be serialized into an inline xml tag with the format:
142 | %
4 | A MATLAB toolkit for programmatically creating, modifying, and managing URDF (Unified Robot Description Format) files. This library provides an object-oriented interface for robot model construction and manipulation. 5 |
6 |
7 |
8 |
9 |
10 |
11 |
13 |
14 |15 | 16 |
17 |