Skip to content

Commit 40aa0e9

Browse files
gonzalocasasgonzalocasas
authored
Merge pull request #83 from compas-dev/fix/sematic-nested-groups
Fix semantic parsing of nested groups
2 parents b7b482a + a867cbc commit 40aa0e9

File tree

5 files changed

+390
-31
lines changed

5 files changed

+390
-31
lines changed

‎CHANGELOG.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ Unreleased
2323
**Fixed**
2424

2525
* Use WorldXY's origin as default for robots that are have no parent join on their base
26+
* Fixed parsing of semantics (SRDF) containing nested groups
2627

2728
**Deprecated**
2829

‎src/compas_fab/robots/semantics.py‎

Lines changed: 11 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -44,21 +44,21 @@ def from_srdf_string(cls, text, urdf_robot):
4444

4545
def __get_group_link_names(self, group):
4646
link_names = []
47-
for link in group.iter('link'):
47+
for link in group.findall('link'):
4848
name = link.attrib['name']
4949
if name not in link_names:
5050
link_names.append(name)
51-
for chain in group.iter('chain'):
51+
for chain in group.findall('chain'):
5252
for link in self.urdf_robot.iter_link_chain(chain.attrib['base_link'], chain.attrib['tip_link']):
5353
if link.name not in link_names:
5454
link_names.append(link.name)
55-
for joint in group.iter('joint'):
55+
for joint in group.findall('joint'):
5656
joint = self.urdf_robot.get_joint_by_name(joint.attrib['name'])
5757
if joint:
5858
name = joint.parent.link
5959
if name not in link_names:
6060
link_names.append(name)
61-
for subgroup in group.iter('group'):
61+
for subgroup in group.findall('group'):
6262
if subgroup.attrib['name'] != group.attrib['name']:
6363
subgroup_link_names = self.__get_group_link_names(subgroup)
6464
for name in subgroup_link_names:
@@ -68,19 +68,19 @@ def __get_group_link_names(self, group):
6868

6969
def __get_group_joint_names(self, group):
7070
joint_names = []
71-
for link in group.iter('link'):
71+
for link in group.findall('link'):
7272
link = self.urdf_robot.get_link_by_name(link.attrib['name'])
7373
for joint in link.joints:
7474
if joint.name not in joint_names:
7575
joint_names.append(joint.name)
76-
for chain in group.iter('chain'):
76+
for chain in group.findall('chain'):
7777
for joint in self.urdf_robot.iter_joint_chain(chain.attrib['base_link'], chain.attrib['tip_link']):
7878
if joint.name not in joint_names:
7979
joint_names.append(joint.name)
80-
for joint in group.iter('joint'):
80+
for joint in group.findall('joint'):
8181
if joint.attrib['name'] not in joint_names:
8282
joint_names.append(joint.attrib['name'])
83-
for subgroup in group.iter('group'):
83+
for subgroup in group.findall('group'):
8484
if subgroup.attrib['name'] != group.attrib['name']:
8585
subgroup_joint_names = self.__get_group_joint_names(subgroup)
8686
for name in subgroup_joint_names:
@@ -94,7 +94,7 @@ def __source_attributes(self):
9494
glenth = []
9595

9696
group_dict = {}
97-
for group in self.root.iter('group'):
97+
for group in self.root.findall('group'):
9898
name = group.attrib['name']
9999
group_dict[name] = {}
100100
link_names = self.__get_group_link_names(group)
@@ -110,13 +110,13 @@ def __source_attributes(self):
110110
self.main_group_name = gnames[idx]
111111

112112
def __get_group_names(self):
113-
return [group.attrib['name'] for group in self.root.iter('group')]
113+
return [group.attrib['name'] for group in self.root.findall('group')]
114114

115115
def __get_passive_joints(self):
116116
return [pjoint.attrib['name'] for pjoint in self.root.iter('passive_joint')]
117117

118118
def __get_end_effectors(self):
119-
return [ee.attrib['parent_link'] for ee in self.root.iter('end_effector')]
119+
return [ee.attrib['parent_link'] for ee in self.root.findall('end_effector')]
120120

121121
def get_end_effector_link_name(self, group=None):
122122
if not group:
@@ -141,23 +141,3 @@ def get_configurable_joints(self, group=None):
141141

142142
def get_configurable_joint_names(self, group=None):
143143
return [joint.name for joint in self.get_configurable_joints(group)]
144-
145-
146-
if __name__ == "__main__":
147-
148-
import compas_fab
149-
from compas.robots import RobotModel
150-
151-
urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur5.urdf')
152-
srdf_filename = compas_fab.get('universal_robot/ur5_moveit_config/config/ur5.srdf')
153-
154-
model = RobotModel.from_urdf_file(urdf_filename)
155-
semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
156-
157-
print(model.name)
158-
print("group_names:", semantics.group_names)
159-
print("main_group_name:", semantics.main_group_name)
160-
print("base_link_name:", semantics.get_base_link_name())
161-
print("ee_link_name:", semantics.get_end_effector_link_name())
162-
print("configurable_joints:", semantics.get_configurable_joint_names())
163-
print()

‎tests/robots/fixtures/panda.urdf‎

Lines changed: 224 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,224 @@
1+
<?xml version="1.0" ?>
2+
<!-- =================================================================================== -->
3+
<!-- | This document was autogenerated by xacro from /opt/ros/kinetic/share/franka_description/robots/panda_arm_hand.urdf.xacro | -->
4+
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
5+
<!-- =================================================================================== -->
6+
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
7+
<link name="panda_link0">
8+
<visual>
9+
<geometry>
10+
<mesh filename="package://franka_description/meshes/visual/link0.dae"/>
11+
</geometry>
12+
</visual>
13+
<collision>
14+
<geometry>
15+
<mesh filename="package://franka_description/meshes/collision/link0.stl"/>
16+
</geometry>
17+
</collision>
18+
</link>
19+
<link name="panda_link1">
20+
<visual>
21+
<geometry>
22+
<mesh filename="package://franka_description/meshes/visual/link1.dae"/>
23+
</geometry>
24+
</visual>
25+
<collision>
26+
<geometry>
27+
<mesh filename="package://franka_description/meshes/collision/link1.stl"/>
28+
</geometry>
29+
</collision>
30+
</link>
31+
<joint name="panda_joint1" type="revolute">
32+
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
33+
<origin rpy="0 0 0" xyz="0 0 0.333"/>
34+
<parent link="panda_link0"/>
35+
<child link="panda_link1"/>
36+
<axis xyz="0 0 1"/>
37+
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
38+
</joint>
39+
<link name="panda_link2">
40+
<visual>
41+
<geometry>
42+
<mesh filename="package://franka_description/meshes/visual/link2.dae"/>
43+
</geometry>
44+
</visual>
45+
<collision>
46+
<geometry>
47+
<mesh filename="package://franka_description/meshes/collision/link2.stl"/>
48+
</geometry>
49+
</collision>
50+
</link>
51+
<joint name="panda_joint2" type="revolute">
52+
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
53+
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
54+
<parent link="panda_link1"/>
55+
<child link="panda_link2"/>
56+
<axis xyz="0 0 1"/>
57+
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
58+
</joint>
59+
<link name="panda_link3">
60+
<visual>
61+
<geometry>
62+
<mesh filename="package://franka_description/meshes/visual/link3.dae"/>
63+
</geometry>
64+
</visual>
65+
<collision>
66+
<geometry>
67+
<mesh filename="package://franka_description/meshes/collision/link3.stl"/>
68+
</geometry>
69+
</collision>
70+
</link>
71+
<joint name="panda_joint3" type="revolute">
72+
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
73+
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
74+
<parent link="panda_link2"/>
75+
<child link="panda_link3"/>
76+
<axis xyz="0 0 1"/>
77+
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
78+
</joint>
79+
<link name="panda_link4">
80+
<visual>
81+
<geometry>
82+
<mesh filename="package://franka_description/meshes/visual/link4.dae"/>
83+
</geometry>
84+
</visual>
85+
<collision>
86+
<geometry>
87+
<mesh filename="package://franka_description/meshes/collision/link4.stl"/>
88+
</geometry>
89+
</collision>
90+
</link>
91+
<joint name="panda_joint4" type="revolute">
92+
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
93+
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
94+
<parent link="panda_link3"/>
95+
<child link="panda_link4"/>
96+
<axis xyz="0 0 1"/>
97+
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
98+
</joint>
99+
<link name="panda_link5">
100+
<visual>
101+
<geometry>
102+
<mesh filename="package://franka_description/meshes/visual/link5.dae"/>
103+
</geometry>
104+
</visual>
105+
<collision>
106+
<geometry>
107+
<mesh filename="package://franka_description/meshes/collision/link5.stl"/>
108+
</geometry>
109+
</collision>
110+
</link>
111+
<joint name="panda_joint5" type="revolute">
112+
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
113+
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
114+
<parent link="panda_link4"/>
115+
<child link="panda_link5"/>
116+
<axis xyz="0 0 1"/>
117+
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
118+
</joint>
119+
<link name="panda_link6">
120+
<visual>
121+
<geometry>
122+
<mesh filename="package://franka_description/meshes/visual/link6.dae"/>
123+
</geometry>
124+
</visual>
125+
<collision>
126+
<geometry>
127+
<mesh filename="package://franka_description/meshes/collision/link6.stl"/>
128+
</geometry>
129+
</collision>
130+
</link>
131+
<joint name="panda_joint6" type="revolute">
132+
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
133+
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
134+
<parent link="panda_link5"/>
135+
<child link="panda_link6"/>
136+
<axis xyz="0 0 1"/>
137+
<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
138+
</joint>
139+
<link name="panda_link7">
140+
<visual>
141+
<geometry>
142+
<mesh filename="package://franka_description/meshes/visual/link7.dae"/>
143+
</geometry>
144+
</visual>
145+
<collision>
146+
<geometry>
147+
<mesh filename="package://franka_description/meshes/collision/link7.stl"/>
148+
</geometry>
149+
</collision>
150+
</link>
151+
<joint name="panda_joint7" type="revolute">
152+
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
153+
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
154+
<parent link="panda_link6"/>
155+
<child link="panda_link7"/>
156+
<axis xyz="0 0 1"/>
157+
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
158+
</joint>
159+
<link name="panda_link8"/>
160+
<joint name="panda_joint8" type="fixed">
161+
<origin rpy="0 0 0" xyz="0 0 0.107"/>
162+
<parent link="panda_link7"/>
163+
<child link="panda_link8"/>
164+
<axis xyz="0 0 0"/>
165+
</joint>
166+
<joint name="panda_hand_joint" type="fixed">
167+
<parent link="panda_link8"/>
168+
<child link="panda_hand"/>
169+
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
170+
</joint>
171+
<link name="panda_hand">
172+
<visual>
173+
<geometry>
174+
<mesh filename="package://franka_description/meshes/visual/hand.dae"/>
175+
</geometry>
176+
</visual>
177+
<collision>
178+
<geometry>
179+
<mesh filename="package://franka_description/meshes/collision/hand.stl"/>
180+
</geometry>
181+
</collision>
182+
</link>
183+
<link name="panda_leftfinger">
184+
<visual>
185+
<geometry>
186+
<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
187+
</geometry>
188+
</visual>
189+
<collision>
190+
<geometry>
191+
<mesh filename="package://franka_description/meshes/collision/finger.stl"/>
192+
</geometry>
193+
</collision>
194+
</link>
195+
<link name="panda_rightfinger">
196+
<visual>
197+
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
198+
<geometry>
199+
<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
200+
</geometry>
201+
</visual>
202+
<collision>
203+
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
204+
<geometry>
205+
<mesh filename="package://franka_description/meshes/collision/finger.stl"/>
206+
</geometry>
207+
</collision>
208+
</link>
209+
<joint name="panda_finger_joint1" type="prismatic">
210+
<parent link="panda_hand"/>
211+
<child link="panda_leftfinger"/>
212+
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
213+
<axis xyz="0 1 0"/>
214+
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
215+
</joint>
216+
<joint name="panda_finger_joint2" type="prismatic">
217+
<parent link="panda_hand"/>
218+
<child link="panda_rightfinger"/>
219+
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
220+
<axis xyz="0 -1 0"/>
221+
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
222+
<mimic joint="panda_finger_joint1"/>
223+
</joint>
224+
</robot>

0 commit comments

Comments
 (0)