Skip to content

Commit 96d03bd

Browse files
tomsch420tomsch420
authored
Merge pull request #40 from ichumuh/body_when_possible
use Body instead of prefixedname when possible
2 parents ed1309b + 65980e2 commit 96d03bd

22 files changed

Lines changed: 1143 additions & 822 deletions

‎requirements.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,9 @@ casadi~=3.7.0
88
hypothesis
99
pytest
1010
scipy
11-
ormatic>=1.1.5
11+
ormatic>=1.1.8
1212
sqlacodegen
13-
ripple_down_rules
13+
ripple_down_rules>=0.6.51
1414
pytest-order
1515
sqlalchemy
1616
embreex # Hardware acceleration for trimesh raycasting

‎resources/urdf/simple_two_arm_robot.urdf‎

Lines changed: 172 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,172 @@
1+
<robot name="custom_robot">
2+
<!-- Base link -->
3+
<link name="base_link">
4+
<visual>
5+
<geometry>
6+
<box size="0.1 0.1 0.1"/>
7+
</geometry>
8+
</visual>
9+
<collision>
10+
<geometry>
11+
<box size="0.2 0.2 0.05"/>
12+
</geometry>
13+
</collision>
14+
</link>
15+
16+
<!-- Prismatic joint -->
17+
<joint name="prismatic_joint" type="prismatic">
18+
<parent link="base_link"/>
19+
<child link="prismatic_link"/>
20+
<origin xyz="0 0 0.05" rpy="0 0 0"/>
21+
<axis xyz="0 0 1"/>
22+
<limit lower="0.0" upper="1.0" effort="100.0" velocity="0.5"/>
23+
</joint>
24+
25+
<link name="prismatic_link">
26+
<visual>
27+
<geometry>
28+
<box size="0.1 0.1 0.5"/>
29+
</geometry>
30+
</visual>
31+
<collision>
32+
<geometry>
33+
<box size="0.1 0.1 0.5"/>
34+
</geometry>
35+
</collision>
36+
</link>
37+
38+
<!-- Revolute joints -->
39+
<!-- Assuming 6 revolute joints for illustration. Adjust number as needed. -->
40+
<!-- First Revolute Joint -->
41+
<joint name="r_joint_1" type="revolute">
42+
<parent link="prismatic_link"/>
43+
<child link="r_link_1"/>
44+
<origin xyz="0.2 0 0" rpy="0 0 0"/>
45+
<axis xyz="1 0 0"/>
46+
<limit lower="-1.57" upper="1.57" effort="50.0" velocity="1.0"/>
47+
</joint>
48+
<link name="r_link_1">
49+
<visual>
50+
<geometry>
51+
<box size="0.1 0.1 0.2"/>
52+
</geometry>
53+
</visual>
54+
<collision>
55+
<geometry>
56+
<box size="0.1 0.1 0.2"/>
57+
</geometry>
58+
</collision>
59+
</link>
60+
61+
<!-- Second Revolute Joint -->
62+
<joint name="r_joint_2" type="revolute">
63+
<parent link="r_link_1"/>
64+
<child link="r_link_2"/>
65+
<origin xyz="0 0 0.22" rpy="0 0 0"/>
66+
<axis xyz="1 0 0"/>
67+
<limit lower="-1.57" upper="1.57" effort="50.0" velocity="1.0"/>
68+
</joint>
69+
<link name="r_link_2">
70+
<visual>
71+
<geometry>
72+
<box size="0.1 0.1 0.2"/>
73+
</geometry>
74+
</visual>
75+
<collision>
76+
<geometry>
77+
<box size="0.1 0.1 0.2"/>
78+
</geometry>
79+
</collision>
80+
</link>
81+
82+
<!-- Third Revolute Joint -->
83+
<joint name="r_joint_3" type="revolute">
84+
<parent link="r_link_2"/>
85+
<child link="r_eef"/>
86+
<origin xyz="0 0 0.22" rpy="0 0 0"/>
87+
<axis xyz="1 0 0"/>
88+
<limit lower="-1.57" upper="1.57" effort="50.0" velocity="1.0"/>
89+
</joint>
90+
<link name="r_eef">
91+
<visual>
92+
<geometry>
93+
<box size="0.1 0.1 0.2"/>
94+
</geometry>
95+
</visual>
96+
<collision>
97+
<geometry>
98+
<box size="0.1 0.1 0.2"/>
99+
</geometry>
100+
</collision>
101+
</link>
102+
103+
<!-- Fourth Revolute Joint -->
104+
<joint name="l_joint_1" type="revolute">
105+
<parent link="prismatic_link"/>
106+
<child link="l_link_1"/>
107+
<origin xyz="-0.2 0 0" rpy="0 0 0"/>
108+
<axis xyz="1 0 0"/>
109+
<limit lower="-1.57" upper="1.57" effort="50.0" velocity="1.0"/>
110+
</joint>
111+
<link name="l_link_1">
112+
<visual>
113+
<geometry>
114+
<box size="0.1 0.1 0.2"/>
115+
</geometry>
116+
</visual>
117+
<collision>
118+
<geometry>
119+
<box size="0.1 0.1 0.2"/>
120+
</geometry>
121+
</collision>
122+
</link>
123+
124+
<!-- Fifth Revolute Joint -->
125+
<joint name="l_joint_2" type="revolute">
126+
<parent link="l_link_1"/>
127+
<child link="l_link_2"/>
128+
<origin xyz="0 0 0.22" rpy="0 0 0"/>
129+
<axis xyz="1 0 0"/>
130+
<limit lower="-1.57" upper="1.57" effort="50.0" velocity="1.0"/>
131+
</joint>
132+
<link name="l_link_2">
133+
<visual>
134+
<geometry>
135+
<box size="0.1 0.1 0.2"/>
136+
</geometry>
137+
</visual>
138+
<collision>
139+
<geometry>
140+
<box size="0.1 0.1 0.2"/>
141+
</geometry>
142+
</collision>
143+
</link>
144+
145+
<!-- Sixth Revolute Joint -->
146+
<joint name="l_joint_3" type="revolute">
147+
<parent link="l_link_2"/>
148+
<child link="l_link_3"/>
149+
<origin xyz="0 0 0.22" rpy="0 0 0"/>
150+
<axis xyz="1 0 0"/>
151+
<limit lower="-1.57" upper="1.57" effort="50.0" velocity="1.0"/>
152+
</joint>
153+
154+
<link name="l_link_3"/>
155+
156+
<joint name="l_dummy_joint" type="fixed">
157+
<parent link="l_link_3"/>
158+
<child link="l_eef"/>
159+
</joint>
160+
<link name="l_eef">
161+
<visual>
162+
<geometry>
163+
<box size="0.1 0.1 0.2"/>
164+
</geometry>
165+
</visual>
166+
<collision>
167+
<geometry>
168+
<box size="0.1 0.1 0.2"/>
169+
</geometry>
170+
</collision>
171+
</link>
172+
</robot>

‎scripts/generate_orm.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@
3838

3939
# remove classes that should not be mapped
4040
classes -= {ResetStateContextManager, WorldModelUpdateContextManager, HasUpdateState,
41-
World, ForwardKinematicsVisitor, Has1DOFState}
41+
World, ForwardKinematicsVisitor, Has1DOFState, DegreeOfFreedom}
4242
classes -= set(recursive_subclasses(Enum))
4343

4444
def generate_orm():

‎src/semantic_world/adapters/multi_parser.py‎

Lines changed: 28 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,8 @@
99
JointBuilder, JointType)
1010
from pxr import UsdUrdf
1111

12-
from ..connections import RevoluteConnection, PrismaticConnection, FixedConnection
13-
from ..spatial_types.derivatives import Derivatives
12+
from ..connections import RevoluteConnection, PrismaticConnection, FixedConnection, UnitVector
13+
from ..spatial_types.derivatives import DerivativeMap
1414
from ..prefixed_name import PrefixedName
1515
from ..spatial_types import spatial_types as cas
1616
from ..world import World, Body, Connection
@@ -115,13 +115,13 @@ def parse_joints(self, body_builder: BodyBuilder, world: World) -> list[Connecti
115115
transform = body_builder.xform.GetLocalTransformation()
116116
pos = transform.ExtractTranslation()
117117
quat = transform.ExtractRotationQuat()
118-
origin = cas.TransformationMatrix.from_xyz_quat(pos_x=pos[0],
119-
pos_y=pos[1],
120-
pos_z=pos[2],
121-
quat_w=quat.GetReal(),
122-
quat_x=quat.GetImaginary()[0],
123-
quat_y=quat.GetImaginary()[1],
124-
quat_z=quat.GetImaginary()[2])
118+
point_expr = cas.Point3((pos[0], pos[1], pos[2]))
119+
quaternion_expr = cas.Quaternion((quat.GetImaginary()[0],
120+
quat.GetImaginary()[1],
121+
quat.GetImaginary()[2],
122+
quat.GetReal()))
123+
origin = cas.TransformationMatrix.from_point_rotation_matrix(point=point_expr,
124+
rotation_matrix=quaternion_expr.to_rotation_matrix())
125125
connection = FixedConnection(parent=parent_body, child=child_body, origin_expression=origin)
126126
connections.append(connection)
127127

@@ -132,13 +132,13 @@ def parse_joint(self, joint_builder: JointBuilder, parent_body: Body, child_body
132132
joint_name = joint_prim.GetName()
133133
joint_pos = joint_builder.pos
134134
joint_quat = joint_builder.quat
135-
origin = cas.TransformationMatrix.from_xyz_quat(pos_x=joint_pos[0],
136-
pos_y=joint_pos[1],
137-
pos_z=joint_pos[2],
138-
quat_w=joint_quat.GetReal(),
139-
quat_x=joint_quat.GetImaginary()[0],
140-
quat_y=joint_quat.GetImaginary()[1],
141-
quat_z=joint_quat.GetImaginary()[2])
135+
point_expr = cas.Point3((joint_pos[0], joint_pos[1], joint_pos[2]))
136+
quaternion_expr = cas.Quaternion((joint_quat.GetImaginary()[0],
137+
joint_quat.GetImaginary()[1],
138+
joint_quat.GetImaginary()[2],
139+
joint_quat.GetReal()))
140+
origin = cas.TransformationMatrix.from_point_rotation_matrix(point=point_expr,
141+
rotation_matrix=quaternion_expr.to_rotation_matrix())
142142
free_variable_name = PrefixedName(joint_name)
143143
offset = None
144144
multiplier = None
@@ -153,16 +153,20 @@ def parse_joint(self, joint_builder: JointBuilder, parent_body: Body, child_body
153153
elif joint_builder.type == JointType.FIXED:
154154
return FixedConnection(parent=parent_body, child=child_body, origin_expression=origin)
155155
elif joint_builder.type in [JointType.REVOLUTE, JointType.CONTINUOUS, JointType.PRISMATIC]:
156-
axis = (float(joint_builder.axis.to_array()[0]),
157-
float(joint_builder.axis.to_array()[1]),
158-
float(joint_builder.axis.to_array()[2]))
159-
if free_variable_name in world.degrees_of_freedom:
160-
dof = world.degrees_of_freedom[free_variable_name]
161-
else:
156+
axis = UnitVector(float(joint_builder.axis.to_array()[0]),
157+
float(joint_builder.axis.to_array()[1]),
158+
float(joint_builder.axis.to_array()[2]))
159+
try:
160+
dof = world.get_degree_of_freedom_by_name(free_variable_name)
161+
except KeyError:
162162
if joint_builder.type == JointType.CONTINUOUS:
163+
lower_limits = DerivativeMap()
164+
lower_limits.position = joint_builder.joint.GetLowerLimitAttr().Get()
165+
upper_limits = DerivativeMap()
166+
upper_limits.position = joint_builder.joint.GetUpperLimitAttr().Get()
163167
dof = world.create_degree_of_freedom(name=PrefixedName(joint_name),
164-
lower_limits={Derivatives.position: joint_builder.joint.GetLowerLimitAttr().Get()},
165-
upper_limits={Derivatives.position: joint_builder.joint.GetUpperLimitAttr().Get()})
168+
lower_limits=lower_limits,
169+
upper_limits=upper_limits)
166170
else:
167171
dof = world.create_degree_of_freedom(name=PrefixedName(joint_name))
168172
if joint_builder.type in [JointType.REVOLUTE, JointType.CONTINUOUS]:

‎src/semantic_world/adapters/urdf.py‎

Lines changed: 21 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66

77
from ..connections import RevoluteConnection, PrismaticConnection, FixedConnection, UnitVector
88
from ..prefixed_name import PrefixedName
9-
from ..spatial_types.derivatives import Derivatives
9+
from ..spatial_types.derivatives import Derivatives, DerivativeMap
1010
from ..spatial_types.spatial_types import TransformationMatrix
1111
from ..utils import suppress_stdout_stderr, hacky_urdf_parser_fix
1212
from ..world import World, Body, Connection
@@ -21,24 +21,24 @@
2121
'fixed': FixedConnection}
2222

2323

24-
def urdf_joint_to_limits(urdf_joint: urdf.Joint) -> Tuple[Dict[Derivatives, float], Dict[Derivatives, float]]:
25-
lower_limits = {}
26-
upper_limits = {}
24+
def urdf_joint_to_limits(urdf_joint: urdf.Joint) -> Tuple[DerivativeMap[float], DerivativeMap[float]]:
25+
lower_limits = DerivativeMap()
26+
upper_limits = DerivativeMap()
2727
if not urdf_joint.type == 'continuous':
2828
try:
29-
lower_limits[Derivatives.position] = max(urdf_joint.safety_controller.soft_lower_limit,
29+
lower_limits.position = max(urdf_joint.safety_controller.soft_lower_limit,
3030
urdf_joint.limit.lower)
31-
upper_limits[Derivatives.position] = min(urdf_joint.safety_controller.soft_upper_limit,
31+
upper_limits.position = min(urdf_joint.safety_controller.soft_upper_limit,
3232
urdf_joint.limit.upper)
3333
except AttributeError:
3434
try:
35-
lower_limits[Derivatives.position] = urdf_joint.limit.lower
36-
upper_limits[Derivatives.position] = urdf_joint.limit.upper
35+
lower_limits.position = urdf_joint.limit.lower
36+
upper_limits.position = urdf_joint.limit.upper
3737
except AttributeError:
3838
pass
3939
try:
40-
lower_limits[Derivatives.velocity] = -urdf_joint.limit.velocity
41-
upper_limits[Derivatives.velocity] = urdf_joint.limit.velocity
40+
lower_limits.velocity = -urdf_joint.limit.velocity
41+
upper_limits.velocity = urdf_joint.limit.velocity
4242
except AttributeError:
4343
pass
4444
if urdf_joint.mimic is not None:
@@ -51,12 +51,12 @@ def urdf_joint_to_limits(urdf_joint: urdf.Joint) -> Tuple[Dict[Derivatives, floa
5151
else:
5252
offset = 0
5353
for d2 in Derivatives.range(Derivatives.position, Derivatives.velocity):
54-
lower_limits[d2] -= offset
55-
upper_limits[d2] -= offset
54+
lower_limits.data[d2] -= offset
55+
upper_limits.data[d2] -= offset
5656
if multiplier < 0:
57-
upper_limits[d2], lower_limits[d2] = lower_limits[d2], upper_limits[d2]
58-
upper_limits[d2] /= multiplier
59-
lower_limits[d2] /= multiplier
57+
upper_limits.data[d2], lower_limits.data[d2] = lower_limits.data[d2], upper_limits.data[d2]
58+
upper_limits.data[d2] /= multiplier
59+
lower_limits.data[d2] /= multiplier
6060
return lower_limits, upper_limits
6161

6262

@@ -144,13 +144,13 @@ def parse_joint(self, joint: urdf.Joint, parent: Body, child: Body, world: World
144144
else:
145145
offset = 0
146146

147-
free_variable_name = PrefixedName(joint.mimic.joint)
147+
dof_name = PrefixedName(joint.mimic.joint)
148148
else:
149-
free_variable_name = PrefixedName(joint.name)
149+
dof_name = PrefixedName(joint.name)
150150

151-
if free_variable_name in world.degrees_of_freedom:
152-
dof = world.degrees_of_freedom[free_variable_name]
153-
else:
151+
try:
152+
dof = world.get_degree_of_freedom_by_name(dof_name)
153+
except KeyError as e:
154154
dof = world.create_degree_of_freedom(name=PrefixedName(joint.name),
155155
lower_limits=lower_limits, upper_limits=upper_limits)
156156

@@ -185,14 +185,7 @@ def parse_geometry(self, geometry: Union[List[urdf.Collision], List[urdf.Visual]
185185
[material.color.rgba if material.color else None for material in
186186
self.parsed.materials]))
187187
for i, geom in enumerate(geometry):
188-
params = (*(geom.origin.xyz + geom.origin.rpy), parent_frame,
189-
PrefixedName(geom.__class__.__name__ + str(i), parent_frame.prefix)) if geom.origin else (0, 0, 0,
190-
0, 0, 0,
191-
parent_frame,
192-
PrefixedName(
193-
geom.__class__.__name__ + str(
194-
i),
195-
parent_frame.prefix))
188+
params = (*(geom.origin.xyz + geom.origin.rpy),) if geom.origin else (0, 0, 0, 0, 0, 0,)
196189
origin_transform = TransformationMatrix.from_xyz_rpy(*params)
197190
if isinstance(geom.geometry, urdf.Box):
198191
color = Color(*material_dict.get(geom.material.name,

0 commit comments

Comments
 (0)