rod 0.2.1.dev45__tar.gz → 0.2.1.dev57__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/PKG-INFO +1 -1
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/__init__.py +3 -3
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/builder/primitive_builder.py +42 -40
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/builder/primitives.py +1 -2
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/kinematics/kinematic_tree.py +6 -4
- rod-0.2.1.dev57/src/rod/kinematics/tree_transforms.py +120 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/logging.py +1 -2
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/pretty_printer.py +22 -21
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/element.py +3 -3
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/link.py +3 -5
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/model.py +9 -7
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/sdf.py +9 -7
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/world.py +3 -3
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/tree/directed_tree.py +5 -5
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/tree/tree_elements.py +15 -13
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/urdf/exporter.py +5 -5
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/utils/frame_convention.py +121 -113
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/utils/gazebo.py +35 -21
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/utils/resolve_frames.py +5 -3
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/utils/resolve_uris.py +1 -1
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod.egg-info/PKG-INFO +1 -1
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/tests/utils_models.py +1 -2
- rod-0.2.1.dev45/src/rod/kinematics/tree_transforms.py +0 -104
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/.github/workflows/ci_cd.yml +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/.github/workflows/style.yml +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/.gitignore +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/LICENSE +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/README.md +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/pyproject.toml +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/setup.cfg +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/setup.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/builder/__init__.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/kinematics/__init__.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/__init__.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/collision.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/common.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/geometry.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/joint.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/material.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/physics.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/scene.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/sdf/visual.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/tree/__init__.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/urdf/__init__.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod/utils/__init__.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod.egg-info/SOURCES.txt +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod.egg-info/dependency_links.txt +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod.egg-info/not-zip-safe +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod.egg-info/requires.txt +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/src/rod.egg-info/top_level.txt +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/tests/test_meshbuilder.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/tests/test_urdf_exporter.py +0 -0
- {rod-0.2.1.dev45 → rod-0.2.1.dev57}/tests/test_urdf_parsing.py +0 -0
|
@@ -115,9 +115,9 @@ def check_compatible_sdformat(specification_version: str) -> None:
|
|
|
115
115
|
|
|
116
116
|
if not GazeboHelper.has_gazebo():
|
|
117
117
|
return
|
|
118
|
-
|
|
119
|
-
|
|
120
|
-
|
|
118
|
+
|
|
119
|
+
cmdline = GazeboHelper.get_gazebo_executable()
|
|
120
|
+
logging.info(f"Calling sdformat through '{cmdline} sdf'")
|
|
121
121
|
|
|
122
122
|
output_sdf_version = packaging.version.Version(
|
|
123
123
|
xmltodict.parse(
|
|
@@ -1,6 +1,8 @@
|
|
|
1
|
+
from __future__ import annotations
|
|
2
|
+
|
|
1
3
|
import abc
|
|
2
4
|
import dataclasses
|
|
3
|
-
from typing import Optional
|
|
5
|
+
from typing import Optional
|
|
4
6
|
|
|
5
7
|
import numpy as np
|
|
6
8
|
import numpy.typing as npt
|
|
@@ -14,7 +16,7 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
14
16
|
name: str
|
|
15
17
|
mass: float
|
|
16
18
|
|
|
17
|
-
element:
|
|
19
|
+
element: rod.Model | rod.Link | rod.Inertial | rod.Collision | rod.Visual = (
|
|
18
20
|
dataclasses.field(
|
|
19
21
|
default=None, init=False, repr=False, hash=False, compare=False
|
|
20
22
|
)
|
|
@@ -22,7 +24,7 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
22
24
|
|
|
23
25
|
def build(
|
|
24
26
|
self,
|
|
25
|
-
) ->
|
|
27
|
+
) -> rod.Model | rod.Link | rod.Inertial | rod.Collision | rod.Visual:
|
|
26
28
|
return self.element
|
|
27
29
|
|
|
28
30
|
# ================
|
|
@@ -43,9 +45,9 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
43
45
|
|
|
44
46
|
def build_model(
|
|
45
47
|
self,
|
|
46
|
-
name:
|
|
47
|
-
pose:
|
|
48
|
-
) ->
|
|
48
|
+
name: str | None = None,
|
|
49
|
+
pose: rod.Pose | None = None,
|
|
50
|
+
) -> PrimitiveBuilder:
|
|
49
51
|
self._check_element()
|
|
50
52
|
|
|
51
53
|
self.element = self._model(name=name, pose=pose)
|
|
@@ -54,16 +56,16 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
54
56
|
|
|
55
57
|
def build_link(
|
|
56
58
|
self,
|
|
57
|
-
name:
|
|
58
|
-
pose:
|
|
59
|
-
) ->
|
|
59
|
+
name: str | None = None,
|
|
60
|
+
pose: rod.Pose | None = None,
|
|
61
|
+
) -> PrimitiveBuilder:
|
|
60
62
|
self._check_element()
|
|
61
63
|
|
|
62
64
|
self.element = self._link(name=name, pose=pose)
|
|
63
65
|
|
|
64
66
|
return self
|
|
65
67
|
|
|
66
|
-
def build_inertial(self, pose:
|
|
68
|
+
def build_inertial(self, pose: rod.Pose | None = None) -> PrimitiveBuilder:
|
|
67
69
|
self._check_element()
|
|
68
70
|
|
|
69
71
|
self.element = self._inertial(pose=pose)
|
|
@@ -72,9 +74,9 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
72
74
|
|
|
73
75
|
def build_visual(
|
|
74
76
|
self,
|
|
75
|
-
name:
|
|
76
|
-
pose:
|
|
77
|
-
) ->
|
|
77
|
+
name: str | None = None,
|
|
78
|
+
pose: rod.Pose | None = None,
|
|
79
|
+
) -> PrimitiveBuilder:
|
|
78
80
|
self._check_element()
|
|
79
81
|
|
|
80
82
|
self.element = self._visual(name=name, pose=pose)
|
|
@@ -83,9 +85,9 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
83
85
|
|
|
84
86
|
def build_collision(
|
|
85
87
|
self,
|
|
86
|
-
name:
|
|
87
|
-
pose:
|
|
88
|
-
) ->
|
|
88
|
+
name: str | None = None,
|
|
89
|
+
pose: rod.Pose | None = None,
|
|
90
|
+
) -> PrimitiveBuilder:
|
|
89
91
|
self._check_element()
|
|
90
92
|
|
|
91
93
|
self.element = self._collision(name=name, pose=pose)
|
|
@@ -98,10 +100,10 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
98
100
|
|
|
99
101
|
def add_link(
|
|
100
102
|
self,
|
|
101
|
-
name:
|
|
102
|
-
pose:
|
|
103
|
-
link:
|
|
104
|
-
) ->
|
|
103
|
+
name: str | None = None,
|
|
104
|
+
pose: rod.Pose | None = None,
|
|
105
|
+
link: rod.Link | None = None,
|
|
106
|
+
) -> PrimitiveBuilder:
|
|
105
107
|
if not isinstance(self.element, rod.Model):
|
|
106
108
|
raise ValueError(type(self.element))
|
|
107
109
|
|
|
@@ -116,9 +118,9 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
116
118
|
|
|
117
119
|
def add_inertial(
|
|
118
120
|
self,
|
|
119
|
-
pose:
|
|
120
|
-
inertial:
|
|
121
|
-
) ->
|
|
121
|
+
pose: rod.Pose | None = None,
|
|
122
|
+
inertial: rod.Inertial | None = None,
|
|
123
|
+
) -> PrimitiveBuilder:
|
|
122
124
|
if not isinstance(self.element, (rod.Model, rod.Link)):
|
|
123
125
|
raise ValueError(type(self.element))
|
|
124
126
|
|
|
@@ -144,11 +146,11 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
144
146
|
|
|
145
147
|
def add_visual(
|
|
146
148
|
self,
|
|
147
|
-
name:
|
|
149
|
+
name: str | None = None,
|
|
148
150
|
use_inertial_pose: bool = True,
|
|
149
|
-
pose:
|
|
150
|
-
visual:
|
|
151
|
-
) ->
|
|
151
|
+
pose: rod.Pose | None = None,
|
|
152
|
+
visual: rod.Visual | None = None,
|
|
153
|
+
) -> PrimitiveBuilder:
|
|
152
154
|
if not isinstance(self.element, (rod.Model, rod.Link)):
|
|
153
155
|
raise ValueError(type(self.element))
|
|
154
156
|
|
|
@@ -180,11 +182,11 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
180
182
|
|
|
181
183
|
def add_collision(
|
|
182
184
|
self,
|
|
183
|
-
name:
|
|
185
|
+
name: str | None = None,
|
|
184
186
|
use_inertial_pose: bool = True,
|
|
185
|
-
pose:
|
|
186
|
-
collision:
|
|
187
|
-
) ->
|
|
187
|
+
pose: rod.Pose | None = None,
|
|
188
|
+
collision: rod.Collision | None = None,
|
|
189
|
+
) -> PrimitiveBuilder:
|
|
188
190
|
if not isinstance(self.element, (rod.Model, rod.Link)):
|
|
189
191
|
raise ValueError(type(self.element))
|
|
190
192
|
|
|
@@ -224,8 +226,8 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
224
226
|
|
|
225
227
|
def _model(
|
|
226
228
|
self,
|
|
227
|
-
name:
|
|
228
|
-
pose:
|
|
229
|
+
name: str | None = None,
|
|
230
|
+
pose: rod.Pose | None = None,
|
|
229
231
|
) -> rod.Model:
|
|
230
232
|
name = name if name is not None else self.name
|
|
231
233
|
logging.debug(f"Building model '{name}'")
|
|
@@ -240,15 +242,15 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
240
242
|
|
|
241
243
|
def _link(
|
|
242
244
|
self,
|
|
243
|
-
name:
|
|
244
|
-
pose:
|
|
245
|
+
name: str | None = None,
|
|
246
|
+
pose: rod.Pose | None = None,
|
|
245
247
|
) -> rod.Link:
|
|
246
248
|
return rod.Link(
|
|
247
249
|
name=name if name is not None else f"{self.name}_link",
|
|
248
250
|
pose=pose,
|
|
249
251
|
)
|
|
250
252
|
|
|
251
|
-
def _inertial(self, pose:
|
|
253
|
+
def _inertial(self, pose: rod.Pose | None = None) -> rod.Inertial:
|
|
252
254
|
return rod.Inertial(
|
|
253
255
|
pose=pose,
|
|
254
256
|
mass=self.mass,
|
|
@@ -257,8 +259,8 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
257
259
|
|
|
258
260
|
def _visual(
|
|
259
261
|
self,
|
|
260
|
-
name:
|
|
261
|
-
pose:
|
|
262
|
+
name: str | None = None,
|
|
263
|
+
pose: rod.Pose | None = None,
|
|
262
264
|
) -> rod.Visual:
|
|
263
265
|
name = name if name is not None else f"{self.name}_visual"
|
|
264
266
|
|
|
@@ -271,7 +273,7 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
271
273
|
def _collision(
|
|
272
274
|
self,
|
|
273
275
|
name: Optional[str],
|
|
274
|
-
pose:
|
|
276
|
+
pose: rod.Pose | None = None,
|
|
275
277
|
) -> rod.Collision:
|
|
276
278
|
name = name if name is not None else f"{self.name}_collision"
|
|
277
279
|
|
|
@@ -297,7 +299,7 @@ class PrimitiveBuilder(abc.ABC):
|
|
|
297
299
|
relative_to: str = None,
|
|
298
300
|
degrees: bool = None,
|
|
299
301
|
rotation_format: str = None,
|
|
300
|
-
) ->
|
|
302
|
+
) -> rod.Pose | None:
|
|
301
303
|
if pos is None and rpy is None:
|
|
302
304
|
return rod.Pose.from_transform(transform=np.eye(4), relative_to=relative_to)
|
|
303
305
|
|
|
@@ -1,6 +1,5 @@
|
|
|
1
1
|
import dataclasses
|
|
2
2
|
import pathlib
|
|
3
|
-
from typing import Union
|
|
4
3
|
|
|
5
4
|
import trimesh
|
|
6
5
|
from numpy.typing import NDArray
|
|
@@ -63,7 +62,7 @@ class CylinderBuilder(PrimitiveBuilder):
|
|
|
63
62
|
|
|
64
63
|
@dataclasses.dataclass
|
|
65
64
|
class MeshBuilder(PrimitiveBuilder):
|
|
66
|
-
mesh_path:
|
|
65
|
+
mesh_path: str | pathlib.Path
|
|
67
66
|
scale: NDArray
|
|
68
67
|
|
|
69
68
|
def __post_init__(self) -> None:
|
|
@@ -1,7 +1,9 @@
|
|
|
1
|
+
from __future__ import annotations
|
|
2
|
+
|
|
1
3
|
import copy
|
|
2
4
|
import dataclasses
|
|
3
5
|
import functools
|
|
4
|
-
from typing import Dict, List, Sequence, Tuple
|
|
6
|
+
from typing import Dict, List, Sequence, Tuple
|
|
5
7
|
|
|
6
8
|
import numpy as np
|
|
7
9
|
|
|
@@ -12,7 +14,7 @@ from rod.tree import DirectedTree, DirectedTreeNode, TreeEdge, TreeFrame
|
|
|
12
14
|
|
|
13
15
|
@dataclasses.dataclass(frozen=True)
|
|
14
16
|
class KinematicTree(DirectedTree):
|
|
15
|
-
model:
|
|
17
|
+
model: rod.Model
|
|
16
18
|
|
|
17
19
|
joints: List[TreeEdge] = dataclasses.field(default_factory=list)
|
|
18
20
|
frames: List[TreeFrame] = dataclasses.field(default_factory=list)
|
|
@@ -46,7 +48,7 @@ class KinematicTree(DirectedTree):
|
|
|
46
48
|
return [joint.name() for joint in self.joints]
|
|
47
49
|
|
|
48
50
|
@staticmethod
|
|
49
|
-
def build(model:
|
|
51
|
+
def build(model: rod.Model, is_top_level: bool = True) -> KinematicTree:
|
|
50
52
|
logging.debug(msg=f"Building kinematic tree of model '{model.name}'")
|
|
51
53
|
|
|
52
54
|
if model.model is not None:
|
|
@@ -199,7 +201,7 @@ class KinematicTree(DirectedTree):
|
|
|
199
201
|
new_base_node, additional_frames = KinematicTree.remove_edge(
|
|
200
202
|
edge=world_to_base_edge, keep_parent=False
|
|
201
203
|
)
|
|
202
|
-
assert any(
|
|
204
|
+
assert any(f.name() == TreeFrame.WORLD for f in additional_frames)
|
|
203
205
|
|
|
204
206
|
# Replace the former base node with the new base node
|
|
205
207
|
nodes_links_dict[new_base_node.name()] = new_base_node
|
|
@@ -0,0 +1,120 @@
|
|
|
1
|
+
from __future__ import annotations
|
|
2
|
+
|
|
3
|
+
import copy
|
|
4
|
+
import dataclasses
|
|
5
|
+
|
|
6
|
+
import numpy as np
|
|
7
|
+
import numpy.typing as npt
|
|
8
|
+
|
|
9
|
+
import rod
|
|
10
|
+
from rod.kinematics.kinematic_tree import KinematicTree
|
|
11
|
+
from rod.tree import TreeFrame
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
@dataclasses.dataclass
|
|
15
|
+
class TreeTransforms:
|
|
16
|
+
kinematic_tree: KinematicTree = dataclasses.dataclass(init=False)
|
|
17
|
+
_transform_cache: dict[str, npt.NDArray] = dataclasses.field(default_factory=dict)
|
|
18
|
+
|
|
19
|
+
@staticmethod
|
|
20
|
+
def build(
|
|
21
|
+
model: rod.Model,
|
|
22
|
+
is_top_level: bool = True,
|
|
23
|
+
) -> TreeTransforms:
|
|
24
|
+
model = copy.deepcopy(model)
|
|
25
|
+
|
|
26
|
+
# Make sure that all elements have a pose attribute with explicit 'relative_to'.
|
|
27
|
+
model.resolve_frames(is_top_level=is_top_level, explicit_frames=True)
|
|
28
|
+
|
|
29
|
+
# Build the kinematic tree and return the TreeTransforms object.
|
|
30
|
+
return TreeTransforms(
|
|
31
|
+
kinematic_tree=KinematicTree.build(model=model, is_top_level=is_top_level)
|
|
32
|
+
)
|
|
33
|
+
|
|
34
|
+
def transform(self, name: str) -> npt.NDArray:
|
|
35
|
+
if name in self._transform_cache:
|
|
36
|
+
return self._transform_cache[name]
|
|
37
|
+
|
|
38
|
+
self._transform_cache[name] = self._compute_transform(name=name)
|
|
39
|
+
return self._transform_cache[name]
|
|
40
|
+
|
|
41
|
+
def _compute_transform(self, name: str) -> npt.NDArray:
|
|
42
|
+
match name:
|
|
43
|
+
case TreeFrame.WORLD:
|
|
44
|
+
|
|
45
|
+
return np.eye(4)
|
|
46
|
+
|
|
47
|
+
case name if name in {TreeFrame.MODEL, self.kinematic_tree.model.name}:
|
|
48
|
+
|
|
49
|
+
relative_to = self.kinematic_tree.model.pose.relative_to
|
|
50
|
+
assert relative_to in {None, ""}, (relative_to, name)
|
|
51
|
+
return self.kinematic_tree.model.pose.transform()
|
|
52
|
+
|
|
53
|
+
case name if name in self.kinematic_tree.joint_names():
|
|
54
|
+
|
|
55
|
+
edge = self.kinematic_tree.joints_dict[name]
|
|
56
|
+
assert edge.name() == name
|
|
57
|
+
|
|
58
|
+
# Get the pose of the frame in which the node's pose is expressed
|
|
59
|
+
assert edge._source.pose.relative_to not in {"", None}
|
|
60
|
+
x_H_E = edge._source.pose.transform()
|
|
61
|
+
W_H_x = self.transform(name=edge._source.pose.relative_to)
|
|
62
|
+
|
|
63
|
+
# Compute the world-to-node transform
|
|
64
|
+
# TODO: this assumes all joint positions to be 0
|
|
65
|
+
W_H_E = W_H_x @ x_H_E
|
|
66
|
+
|
|
67
|
+
return W_H_E
|
|
68
|
+
|
|
69
|
+
case name if name in self.kinematic_tree.link_names():
|
|
70
|
+
|
|
71
|
+
element = self.kinematic_tree.links_dict[name]
|
|
72
|
+
|
|
73
|
+
assert element.name() == name
|
|
74
|
+
assert element._source.pose.relative_to not in {"", None}
|
|
75
|
+
|
|
76
|
+
# Get the pose of the frame in which the link's pose is expressed.
|
|
77
|
+
x_H_L = element._source.pose.transform()
|
|
78
|
+
W_H_x = self.transform(name=element._source.pose.relative_to)
|
|
79
|
+
|
|
80
|
+
# Compute the world transform of the link.
|
|
81
|
+
W_H_L = W_H_x @ x_H_L
|
|
82
|
+
return W_H_L
|
|
83
|
+
|
|
84
|
+
case name if name in self.kinematic_tree.frame_names():
|
|
85
|
+
|
|
86
|
+
element = self.kinematic_tree.frames_dict[name]
|
|
87
|
+
|
|
88
|
+
assert element.name() == name
|
|
89
|
+
assert element._source.pose.relative_to not in {"", None}
|
|
90
|
+
|
|
91
|
+
# Get the pose of the frame in which the frame's pose is expressed.
|
|
92
|
+
x_H_F = element._source.pose.transform()
|
|
93
|
+
W_H_x = self.transform(name=element._source.pose.relative_to)
|
|
94
|
+
|
|
95
|
+
# Compute the world transform of the frame.
|
|
96
|
+
W_H_F = W_H_x @ x_H_F
|
|
97
|
+
return W_H_F
|
|
98
|
+
|
|
99
|
+
case _:
|
|
100
|
+
raise ValueError(name)
|
|
101
|
+
|
|
102
|
+
def relative_transform(self, relative_to: str, name: str) -> npt.NDArray:
|
|
103
|
+
|
|
104
|
+
world_H_name = self.transform(name=name)
|
|
105
|
+
world_H_relative_to = self.transform(name=relative_to)
|
|
106
|
+
|
|
107
|
+
return TreeTransforms.inverse(world_H_relative_to) @ world_H_name
|
|
108
|
+
|
|
109
|
+
@staticmethod
|
|
110
|
+
def inverse(transform: npt.NDArray) -> npt.NDArray:
|
|
111
|
+
|
|
112
|
+
R = transform[0:3, 0:3]
|
|
113
|
+
p = np.vstack(transform[0:3, 3])
|
|
114
|
+
|
|
115
|
+
return np.block(
|
|
116
|
+
[
|
|
117
|
+
[R.T, -R.T @ p],
|
|
118
|
+
[0, 0, 0, 1],
|
|
119
|
+
]
|
|
120
|
+
)
|
|
@@ -1,6 +1,5 @@
|
|
|
1
1
|
import enum
|
|
2
2
|
import logging
|
|
3
|
-
from typing import Union
|
|
4
3
|
|
|
5
4
|
import coloredlogs
|
|
6
5
|
|
|
@@ -20,7 +19,7 @@ def _logger() -> logging.Logger:
|
|
|
20
19
|
return logging.getLogger(name=LOGGER_NAME)
|
|
21
20
|
|
|
22
21
|
|
|
23
|
-
def set_logging_level(level:
|
|
22
|
+
def set_logging_level(level: int | LoggingLevel = LoggingLevel.WARNING):
|
|
24
23
|
if isinstance(level, int):
|
|
25
24
|
level = LoggingLevel(level)
|
|
26
25
|
|
|
@@ -30,7 +30,7 @@ class DataclassPrettyPrinter(abc.ABC):
|
|
|
30
30
|
]
|
|
31
31
|
|
|
32
32
|
return (
|
|
33
|
-
|
|
33
|
+
"[\n"
|
|
34
34
|
+ ",\n".join(f"{spacing_level}{el!s}" for el in list_str)
|
|
35
35
|
+ f",\n{spacing_level_up}]"
|
|
36
36
|
)
|
|
@@ -45,26 +45,27 @@ class DataclassPrettyPrinter(abc.ABC):
|
|
|
45
45
|
for field in dataclasses.fields(obj):
|
|
46
46
|
attr = getattr(obj, field.name)
|
|
47
47
|
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
67
|
-
|
|
48
|
+
match attr:
|
|
49
|
+
case None | "":
|
|
50
|
+
continue
|
|
51
|
+
|
|
52
|
+
case list():
|
|
53
|
+
list_str = DataclassPrettyPrinter.list_to_string(
|
|
54
|
+
obj=attr, level=level + 1
|
|
55
|
+
)
|
|
56
|
+
serialization += [(field.name, list_str)]
|
|
57
|
+
continue
|
|
58
|
+
|
|
59
|
+
case _ if dataclasses.is_dataclass(attr):
|
|
60
|
+
dataclass_str = DataclassPrettyPrinter.dataclass_to_str(
|
|
61
|
+
obj=attr, level=level + 1
|
|
62
|
+
)
|
|
63
|
+
serialization += [(field.name, dataclass_str)]
|
|
64
|
+
continue
|
|
65
|
+
|
|
66
|
+
case _:
|
|
67
|
+
serialization += [(field.name, f"{attr!s}")]
|
|
68
|
+
continue
|
|
68
69
|
|
|
69
70
|
spacing = " " * 4
|
|
70
71
|
spacing_level = spacing * level
|
|
@@ -1,5 +1,5 @@
|
|
|
1
1
|
import dataclasses
|
|
2
|
-
from typing import Any, Dict, List
|
|
2
|
+
from typing import Any, Dict, List
|
|
3
3
|
|
|
4
4
|
import mashumaro.config
|
|
5
5
|
import mashumaro.mixins.dict
|
|
@@ -37,7 +37,7 @@ class Element(mashumaro.mixins.dict.DataClassDictMixin, DataclassPrettyPrinter):
|
|
|
37
37
|
false_vals = {"0", "False", "false"}
|
|
38
38
|
assert data in true_vals.union(false_vals)
|
|
39
39
|
|
|
40
|
-
return
|
|
40
|
+
return data in true_vals
|
|
41
41
|
|
|
42
42
|
@staticmethod
|
|
43
43
|
def serialize_float(data: float) -> str:
|
|
@@ -50,7 +50,7 @@ class Element(mashumaro.mixins.dict.DataClassDictMixin, DataclassPrettyPrinter):
|
|
|
50
50
|
return " ".join(np.array(data, dtype=str))
|
|
51
51
|
|
|
52
52
|
@staticmethod
|
|
53
|
-
def deserialize_list(data: str, length:
|
|
53
|
+
def deserialize_list(data: str, length: int | None = None) -> List[float]:
|
|
54
54
|
assert isinstance(data, str)
|
|
55
55
|
array = np.atleast_1d(np.array(data.split(sep=" "), dtype=float).squeeze())
|
|
56
56
|
|
|
@@ -1,5 +1,5 @@
|
|
|
1
1
|
import dataclasses
|
|
2
|
-
from typing import List, Optional
|
|
2
|
+
from typing import List, Optional
|
|
3
3
|
|
|
4
4
|
import mashumaro
|
|
5
5
|
import numpy as np
|
|
@@ -73,11 +73,9 @@ class Link(Element):
|
|
|
73
73
|
|
|
74
74
|
inertial: Optional[Inertial] = dataclasses.field(default=None)
|
|
75
75
|
|
|
76
|
-
visual: Optional[
|
|
76
|
+
visual: Optional[Visual | List[Visual]] = dataclasses.field(default=None)
|
|
77
77
|
|
|
78
|
-
collision: Optional[
|
|
79
|
-
default=None
|
|
80
|
-
)
|
|
78
|
+
collision: Optional[Collision | List[Collision]] = dataclasses.field(default=None)
|
|
81
79
|
|
|
82
80
|
gravity: Optional[bool] = dataclasses.field(
|
|
83
81
|
default=None,
|
|
@@ -1,5 +1,7 @@
|
|
|
1
|
+
from __future__ import annotations
|
|
2
|
+
|
|
1
3
|
import dataclasses
|
|
2
|
-
from typing import List, Optional
|
|
4
|
+
from typing import List, Optional
|
|
3
5
|
|
|
4
6
|
import mashumaro
|
|
5
7
|
|
|
@@ -54,13 +56,13 @@ class Model(Element):
|
|
|
54
56
|
|
|
55
57
|
pose: Optional[Pose] = dataclasses.field(default=None)
|
|
56
58
|
|
|
57
|
-
model: Optional[
|
|
59
|
+
model: Optional[Model | List[Model]] = dataclasses.field(default=None)
|
|
58
60
|
|
|
59
|
-
frame: Optional[
|
|
61
|
+
frame: Optional[Frame | List[Frame]] = dataclasses.field(default=None)
|
|
60
62
|
|
|
61
|
-
link: Optional[
|
|
63
|
+
link: Optional[Link | List[Link]] = dataclasses.field(default=None)
|
|
62
64
|
|
|
63
|
-
joint: Optional[
|
|
65
|
+
joint: Optional[Joint | List[Joint]] = dataclasses.field(default=None)
|
|
64
66
|
|
|
65
67
|
def is_fixed_base(self) -> bool:
|
|
66
68
|
joints_having_world_parent = [j for j in self.joints() if j.parent == "world"]
|
|
@@ -80,7 +82,7 @@ class Model(Element):
|
|
|
80
82
|
|
|
81
83
|
return self.links()[0].name
|
|
82
84
|
|
|
83
|
-
def models(self) -> List[
|
|
85
|
+
def models(self) -> List[Model]:
|
|
84
86
|
if self.model is None:
|
|
85
87
|
return []
|
|
86
88
|
|
|
@@ -155,7 +157,7 @@ class Model(Element):
|
|
|
155
157
|
|
|
156
158
|
def switch_frame_convention(
|
|
157
159
|
self,
|
|
158
|
-
frame_convention:
|
|
160
|
+
frame_convention: rod.FrameConvention,
|
|
159
161
|
is_top_level: bool = True,
|
|
160
162
|
explicit_frames: bool = True,
|
|
161
163
|
attach_frames_to_links: bool = True,
|
|
@@ -1,7 +1,9 @@
|
|
|
1
|
+
from __future__ import annotations
|
|
2
|
+
|
|
1
3
|
import dataclasses
|
|
2
4
|
import os
|
|
3
5
|
import pathlib
|
|
4
|
-
from typing import List, Optional
|
|
6
|
+
from typing import List, Optional
|
|
5
7
|
|
|
6
8
|
import mashumaro
|
|
7
9
|
import packaging.specifiers
|
|
@@ -19,9 +21,9 @@ from .world import World
|
|
|
19
21
|
class Sdf(Element):
|
|
20
22
|
version: str = dataclasses.field(metadata=mashumaro.field_options(alias="@version"))
|
|
21
23
|
|
|
22
|
-
world: Optional[
|
|
24
|
+
world: Optional[World | List[World]] = dataclasses.field(default=None)
|
|
23
25
|
|
|
24
|
-
model: Optional[
|
|
26
|
+
model: Optional[Model | List[Model]] = dataclasses.field(default=None)
|
|
25
27
|
|
|
26
28
|
def worlds(self) -> List[World]:
|
|
27
29
|
if self.world is None:
|
|
@@ -44,7 +46,7 @@ class Sdf(Element):
|
|
|
44
46
|
return self.model
|
|
45
47
|
|
|
46
48
|
@staticmethod
|
|
47
|
-
def load(sdf:
|
|
49
|
+
def load(sdf: pathlib.Path | str, is_urdf: bool | None = None) -> Sdf:
|
|
48
50
|
"""
|
|
49
51
|
Load an SDF resource.
|
|
50
52
|
|
|
@@ -80,7 +82,7 @@ class Sdf(Element):
|
|
|
80
82
|
and len(sdf) <= MAX_PATH
|
|
81
83
|
and pathlib.Path(sdf).is_file()
|
|
82
84
|
):
|
|
83
|
-
sdf_string = pathlib.Path(sdf).read_text()
|
|
85
|
+
sdf_string = pathlib.Path(sdf).read_text(encoding="utf-8")
|
|
84
86
|
is_urdf = (
|
|
85
87
|
is_urdf if is_urdf is not None else pathlib.Path(sdf).suffix == ".urdf"
|
|
86
88
|
)
|
|
@@ -99,8 +101,8 @@ class Sdf(Element):
|
|
|
99
101
|
# Parse the SDF to dict
|
|
100
102
|
try:
|
|
101
103
|
xml_dict = xmltodict.parse(xml_input=sdf_string)
|
|
102
|
-
except Exception:
|
|
103
|
-
raise
|
|
104
|
+
except Exception as exc:
|
|
105
|
+
raise exc("Failed to parse 'sdf' argument")
|
|
104
106
|
|
|
105
107
|
# Look for the top-level <sdf> element
|
|
106
108
|
try:
|
|
@@ -1,5 +1,5 @@
|
|
|
1
1
|
import dataclasses
|
|
2
|
-
from typing import List, Optional
|
|
2
|
+
from typing import List, Optional
|
|
3
3
|
|
|
4
4
|
import mashumaro
|
|
5
5
|
|
|
@@ -34,9 +34,9 @@ class World(Element):
|
|
|
34
34
|
|
|
35
35
|
scene: Scene = dataclasses.field(default_factory=Scene)
|
|
36
36
|
|
|
37
|
-
model: Optional[
|
|
37
|
+
model: Optional[Model | List[Model]] = dataclasses.field(default=None)
|
|
38
38
|
|
|
39
|
-
frame: Optional[
|
|
39
|
+
frame: Optional[Frame | List[Frame]] = dataclasses.field(default=None)
|
|
40
40
|
|
|
41
41
|
def models(self) -> List[Model]:
|
|
42
42
|
if self.model is None:
|
|
@@ -1,7 +1,7 @@
|
|
|
1
1
|
import collections.abc
|
|
2
2
|
import dataclasses
|
|
3
3
|
import functools
|
|
4
|
-
from typing import Any, Callable, Dict, Iterable, List
|
|
4
|
+
from typing import Any, Callable, Dict, Iterable, List
|
|
5
5
|
|
|
6
6
|
from .tree_elements import DirectedTreeNode
|
|
7
7
|
|
|
@@ -33,7 +33,7 @@ class DirectedTree(collections.abc.Sequence):
|
|
|
33
33
|
@staticmethod
|
|
34
34
|
def breadth_first_search(
|
|
35
35
|
root: DirectedTreeNode,
|
|
36
|
-
sort_children:
|
|
36
|
+
sort_children: Callable[[Any], Any] | None = lambda node: node.name(),
|
|
37
37
|
) -> Iterable[DirectedTreeNode]:
|
|
38
38
|
queue = [root]
|
|
39
39
|
|
|
@@ -69,8 +69,8 @@ class DirectedTree(collections.abc.Sequence):
|
|
|
69
69
|
)
|
|
70
70
|
|
|
71
71
|
def __getitem__(
|
|
72
|
-
self, key:
|
|
73
|
-
) ->
|
|
72
|
+
self, key: int | slice | str
|
|
73
|
+
) -> DirectedTreeNode | List[DirectedTreeNode]:
|
|
74
74
|
# Get the nodes' dictionary (already inserted in order following BFS)
|
|
75
75
|
nodes_dict = self.nodes_dict
|
|
76
76
|
|
|
@@ -100,7 +100,7 @@ class DirectedTree(collections.abc.Sequence):
|
|
|
100
100
|
def __reversed__(self) -> Iterable[DirectedTreeNode]:
|
|
101
101
|
yield from reversed(self)
|
|
102
102
|
|
|
103
|
-
def __contains__(self, item:
|
|
103
|
+
def __contains__(self, item: str | DirectedTreeNode) -> bool:
|
|
104
104
|
if isinstance(item, str):
|
|
105
105
|
return item in self.nodes_dict.keys()
|
|
106
106
|
|