pinker 1.2.0__tar.gz → 1.3.0__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.
- {pinker-1.2.0/pinker.egg-info → pinker-1.3.0}/PKG-INFO +2 -2
- {pinker-1.2.0 → pinker-1.3.0}/README.md +1 -1
- {pinker-1.2.0 → pinker-1.3.0}/pinker/__init__.py +1 -1
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/__init__.py +2 -1
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/geometry.py +16 -1
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/mjcf.py +45 -15
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/model.py +6 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/robot_wrapper.py +21 -5
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/urdf.py +57 -54
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/manipulability_task.py +6 -7
- {pinker-1.2.0 → pinker-1.3.0}/pinker/visualizer.py +93 -26
- {pinker-1.2.0 → pinker-1.3.0/pinker.egg-info}/PKG-INFO +2 -2
- {pinker-1.2.0 → pinker-1.3.0}/pinker.egg-info/SOURCES.txt +2 -1
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_loaders.py +4 -2
- pinker-1.3.0/tests/test_visualizer.py +204 -0
- {pinker-1.2.0 → pinker-1.3.0}/LICENSE +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/NOTICE +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/barriers/__init__.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/barriers/barrier.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/barriers/body_spherical_barrier.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/barriers/position_barrier.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/configuration.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/exceptions.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/_kinematics_c.c +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/algorithms.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/joints.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/se2.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/se3.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/so3.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/__init__.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/acceleration_limit.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/configuration_limit.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/floating_base_velocity_limit.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/limit.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/velocity_limit.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/loaders.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/solve_ik.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/__init__.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/com_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/damping_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/frame_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/joint_coupling_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/joint_velocity_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/linear_holonomic_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/low_acceleration_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/omniwheel_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/posture_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/relative_frame_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/rolling_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker/utils.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker.egg-info/dependency_links.txt +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker.egg-info/requires.txt +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pinker.egg-info/top_level.txt +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/pyproject.toml +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/setup.cfg +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/setup.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_acceleration_limit.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_barrier.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_body_spherical_barrier.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_com_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_configuration.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_configuration_limit.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_damping_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_floating_base_velocity_limit.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_frame_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_jacobians.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_joint_coupling_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_joint_velocity_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_limits.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_linear_holonomic_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_low_acceleration_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_manipulability_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_omniwheel_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_position_barrier.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_posture_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_relative_frame_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_rolling_task.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_solve_ik.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_utils.py +0 -0
- {pinker-1.2.0 → pinker-1.3.0}/tests/test_velocity_limit.py +0 -0
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.4
|
|
2
2
|
Name: pinker
|
|
3
|
-
Version: 1.
|
|
3
|
+
Version: 1.3.0
|
|
4
4
|
Summary: Python inverse kinematics for embedded robots
|
|
5
5
|
Author-email: Stéphane Caron <stephane.caron@normalesup.org>
|
|
6
6
|
Maintainer-email: Stéphane Caron <stephane.caron@normalesup.org>
|
|
@@ -231,7 +231,7 @@ If you use Pinker in your scientific works, please cite it *e.g.* as follows:
|
|
|
231
231
|
author = {Caron, Stéphane and De Mont-Marin, Yann and Budhiraja, Rohan and Bang, Seung Hyeon and Domrachev, Ivan and Nedelchev, Simeon and Du, Peter and Escande, Adrien and Vaillant, Joris and Wingo, Bruce and Patapati, Santosh and San José Pro, Daniel and Marticorena Vidal, Nicolas Guillermo},
|
|
232
232
|
license = {Apache-2.0},
|
|
233
233
|
url = {https://github.com/pink-kinematics/pinker},
|
|
234
|
-
version = {1.
|
|
234
|
+
version = {1.3.0},
|
|
235
235
|
year = {2026}
|
|
236
236
|
}
|
|
237
237
|
```
|
|
@@ -200,7 +200,7 @@ If you use Pinker in your scientific works, please cite it *e.g.* as follows:
|
|
|
200
200
|
author = {Caron, Stéphane and De Mont-Marin, Yann and Budhiraja, Rohan and Bang, Seung Hyeon and Domrachev, Ivan and Nedelchev, Simeon and Du, Peter and Escande, Adrien and Vaillant, Joris and Wingo, Bruce and Patapati, Santosh and San José Pro, Daniel and Marticorena Vidal, Nicolas Guillermo},
|
|
201
201
|
license = {Apache-2.0},
|
|
202
202
|
url = {https://github.com/pink-kinematics/pinker},
|
|
203
|
-
version = {1.
|
|
203
|
+
version = {1.3.0},
|
|
204
204
|
year = {2026}
|
|
205
205
|
}
|
|
206
206
|
```
|
|
@@ -46,7 +46,7 @@ from .algorithms import (
|
|
|
46
46
|
neutral,
|
|
47
47
|
update_frame_placements,
|
|
48
48
|
)
|
|
49
|
-
from .geometry import GeometryModel, GeometryObject
|
|
49
|
+
from .geometry import GeometryModel, GeometryObject, GeometryType
|
|
50
50
|
from .joints import (
|
|
51
51
|
JointModel,
|
|
52
52
|
JointModelFreeFlyer,
|
|
@@ -92,6 +92,7 @@ __all__ = [
|
|
|
92
92
|
"FrameType",
|
|
93
93
|
"GeometryModel",
|
|
94
94
|
"GeometryObject",
|
|
95
|
+
"GeometryType",
|
|
95
96
|
"Inertia",
|
|
96
97
|
"Jlog6",
|
|
97
98
|
"Jlog_se2",
|
|
@@ -1,11 +1,12 @@
|
|
|
1
1
|
# SPDX-License-Identifier: Apache-2.0
|
|
2
2
|
|
|
3
|
-
"""Visual
|
|
3
|
+
"""Visual and collision geometries attached to a robot model.
|
|
4
4
|
|
|
5
5
|
Each geometry object carries a primitive shape (box, sphere, cylinder) or a
|
|
6
6
|
mesh file path, together with its placement in the frame of its parent joint.
|
|
7
7
|
"""
|
|
8
8
|
|
|
9
|
+
from enum import IntEnum
|
|
9
10
|
from typing import List, Optional
|
|
10
11
|
|
|
11
12
|
import numpy as np
|
|
@@ -15,6 +16,13 @@ from .se3 import SE3
|
|
|
15
16
|
DEFAULT_COLOR = np.array([0.7, 0.7, 0.7, 1.0])
|
|
16
17
|
|
|
17
18
|
|
|
19
|
+
class GeometryType(IntEnum):
|
|
20
|
+
"""Kinds of geometries, with the same values as in Pinocchio."""
|
|
21
|
+
|
|
22
|
+
VISUAL = 0
|
|
23
|
+
COLLISION = 1
|
|
24
|
+
|
|
25
|
+
|
|
18
26
|
class GeometryObject:
|
|
19
27
|
"""One displayable shape, attached to a joint of the model.
|
|
20
28
|
|
|
@@ -29,6 +37,7 @@ class GeometryObject:
|
|
|
29
37
|
mesh_path: Path to the mesh file, for mesh shapes.
|
|
30
38
|
mesh_scale: Scale applied to the mesh (3,).
|
|
31
39
|
mesh_color: RGBA color in [0, 1].
|
|
40
|
+
parent_frame: Index of the frame of the body the shape belongs to.
|
|
32
41
|
"""
|
|
33
42
|
|
|
34
43
|
def __init__(
|
|
@@ -41,6 +50,7 @@ class GeometryObject:
|
|
|
41
50
|
mesh_path: str = "",
|
|
42
51
|
mesh_scale: Optional[np.ndarray] = None,
|
|
43
52
|
mesh_color: Optional[np.ndarray] = None,
|
|
53
|
+
parent_frame: int = 0,
|
|
44
54
|
):
|
|
45
55
|
"""Initialize geometry object.
|
|
46
56
|
|
|
@@ -55,9 +65,12 @@ class GeometryObject:
|
|
|
55
65
|
mesh_path: Path to the mesh file, for mesh shapes.
|
|
56
66
|
mesh_scale: Scale applied to the mesh (3,), defaults to ones.
|
|
57
67
|
mesh_color: RGBA color in [0, 1], defaults to light gray.
|
|
68
|
+
parent_frame: Index of the frame of the body the shape belongs
|
|
69
|
+
to, defaults to the universe frame.
|
|
58
70
|
"""
|
|
59
71
|
self.name = name
|
|
60
72
|
self.parent_joint = parent_joint
|
|
73
|
+
self.parent_frame = parent_frame
|
|
61
74
|
self.placement = placement
|
|
62
75
|
self.shape = shape
|
|
63
76
|
self.size = np.zeros(0) if size is None else np.asarray(size)
|
|
@@ -83,6 +96,8 @@ class GeometryObject:
|
|
|
83
96
|
class GeometryModel:
|
|
84
97
|
"""Collection of geometry objects attached to a robot model."""
|
|
85
98
|
|
|
99
|
+
geometry_objects: List[GeometryObject]
|
|
100
|
+
|
|
86
101
|
def __init__(self):
|
|
87
102
|
"""Initialize an empty geometry model."""
|
|
88
103
|
self.geometry_objects: List[GeometryObject] = []
|
|
@@ -47,7 +47,7 @@ from .so3 import (
|
|
|
47
47
|
)
|
|
48
48
|
|
|
49
49
|
if TYPE_CHECKING:
|
|
50
|
-
from .geometry import GeometryModel
|
|
50
|
+
from .geometry import GeometryModel, GeometryType
|
|
51
51
|
|
|
52
52
|
ORIENTATION_KEYS = ("quat", "axisangle", "euler", "xyaxes", "zaxis")
|
|
53
53
|
"""Attributes that specify an orientation, at most one per element."""
|
|
@@ -471,7 +471,9 @@ def build_model_from_mjcf_xml(
|
|
|
471
471
|
)
|
|
472
472
|
elif parent is None and not joints:
|
|
473
473
|
model.append_body_to_joint(0, inertia, body_pose)
|
|
474
|
-
body_frame = model.
|
|
474
|
+
body_frame = model.add_frame(
|
|
475
|
+
Frame(name, 0, 0, body_pose, FrameType.BODY, inertia)
|
|
476
|
+
)
|
|
475
477
|
elif not joints:
|
|
476
478
|
support_joint = parent_frame.parent_joint
|
|
477
479
|
fixed_frame = model.add_frame(
|
|
@@ -481,6 +483,7 @@ def build_model_from_mjcf_xml(
|
|
|
481
483
|
parent_frame_id,
|
|
482
484
|
body_pose,
|
|
483
485
|
FrameType.FIXED_JOINT,
|
|
486
|
+
inertia,
|
|
484
487
|
)
|
|
485
488
|
)
|
|
486
489
|
model.append_body_to_joint(support_joint, inertia, body_pose)
|
|
@@ -590,21 +593,44 @@ def _geom_kind_is_collision(attributes: Dict[str, str]) -> bool:
|
|
|
590
593
|
return int(attributes.get("group", "0")) > 2
|
|
591
594
|
|
|
592
595
|
|
|
593
|
-
def
|
|
594
|
-
"""
|
|
596
|
+
def _geom_kind_is_visual(attributes: Dict[str, str]) -> bool:
|
|
597
|
+
"""Whether a geom is only meant for display, as Pinocchio reads it.
|
|
595
598
|
|
|
596
|
-
|
|
597
|
-
|
|
598
|
-
|
|
599
|
+
Args:
|
|
600
|
+
attributes: Attributes of the geom.
|
|
601
|
+
|
|
602
|
+
Returns:
|
|
603
|
+
True for geoms that don't collide, False otherwise.
|
|
604
|
+
"""
|
|
605
|
+
contype = int(attributes.get("contype", "1"))
|
|
606
|
+
conaffinity = int(attributes.get("conaffinity", "1"))
|
|
607
|
+
return contype == 0 and conaffinity == 0
|
|
608
|
+
|
|
609
|
+
|
|
610
|
+
def build_geom_from_mjcf(
|
|
611
|
+
model: Model,
|
|
612
|
+
filename: str,
|
|
613
|
+
geometry_type: Optional["GeometryType"] = None,
|
|
614
|
+
) -> "GeometryModel":
|
|
615
|
+
"""Parse the visual or collision geometries of an MJCF file.
|
|
599
616
|
|
|
600
617
|
Args:
|
|
601
618
|
model: Model previously built from the same MJCF.
|
|
602
619
|
filename: Path to the MJCF file.
|
|
620
|
+
geometry_type: Kind of geometries to parse, ``GeometryType.VISUAL``
|
|
621
|
+
(default) or ``GeometryType.COLLISION``.
|
|
603
622
|
|
|
604
623
|
Returns:
|
|
605
|
-
Geometry model with one object per
|
|
624
|
+
Geometry model with one object per geom of the requested kind.
|
|
606
625
|
"""
|
|
607
|
-
from .geometry import GeometryModel, GeometryObject
|
|
626
|
+
from .geometry import GeometryModel, GeometryObject, GeometryType
|
|
627
|
+
|
|
628
|
+
if geometry_type is None:
|
|
629
|
+
geometry_type = GeometryType.VISUAL
|
|
630
|
+
if geometry_type == GeometryType.VISUAL:
|
|
631
|
+
excluded = _geom_kind_is_collision
|
|
632
|
+
else:
|
|
633
|
+
excluded = _geom_kind_is_visual
|
|
608
634
|
|
|
609
635
|
with open(filename, "r", encoding="utf-8") as handle:
|
|
610
636
|
mjcf = _Mjcf(handle.read())
|
|
@@ -632,11 +658,11 @@ def build_geom_from_mjcf(model: Model, filename: str) -> "GeometryModel":
|
|
|
632
658
|
)
|
|
633
659
|
|
|
634
660
|
body_frames = {
|
|
635
|
-
frame.name: frame
|
|
636
|
-
for frame in model.frames
|
|
661
|
+
frame.name: (frame_id, frame)
|
|
662
|
+
for frame_id, frame in enumerate(model.frames)
|
|
637
663
|
if frame.type == FrameType.BODY
|
|
638
664
|
}
|
|
639
|
-
world_frame = model.frames[0]
|
|
665
|
+
world_frame = (0, model.frames[0])
|
|
640
666
|
elements = [
|
|
641
667
|
(worldbody, "world", "main", world_frame)
|
|
642
668
|
for worldbody in mjcf.root.findall("worldbody")
|
|
@@ -648,12 +674,12 @@ def build_geom_from_mjcf(model: Model, filename: str) -> "GeometryModel":
|
|
|
648
674
|
)
|
|
649
675
|
|
|
650
676
|
geometry_model = GeometryModel()
|
|
651
|
-
for body, body_name, childclass, frame in elements:
|
|
677
|
+
for body, body_name, childclass, (frame_id, frame) in elements:
|
|
652
678
|
for index, geom in enumerate(body.findall("geom")):
|
|
653
679
|
attributes = mjcf.attributes(geom, childclass)
|
|
654
|
-
if
|
|
680
|
+
if excluded(attributes):
|
|
655
681
|
continue
|
|
656
|
-
name = attributes.get("name", f"{body_name}
|
|
682
|
+
name = attributes.get("name", f"{body_name}Geom_{index}")
|
|
657
683
|
shape = attributes.get("type", "sphere")
|
|
658
684
|
size = _floats(attributes.get("size", "0 0 0"))
|
|
659
685
|
if "fromto" in attributes and shape == "cylinder":
|
|
@@ -680,6 +706,7 @@ def build_geom_from_mjcf(model: Model, filename: str) -> "GeometryModel":
|
|
|
680
706
|
"box",
|
|
681
707
|
2.0 * size[:3], # MJCF sizes are half extents
|
|
682
708
|
mesh_color=color,
|
|
709
|
+
parent_frame=frame_id,
|
|
683
710
|
)
|
|
684
711
|
elif shape == "sphere":
|
|
685
712
|
obj = GeometryObject(
|
|
@@ -689,6 +716,7 @@ def build_geom_from_mjcf(model: Model, filename: str) -> "GeometryModel":
|
|
|
689
716
|
"sphere",
|
|
690
717
|
size[:1],
|
|
691
718
|
mesh_color=color,
|
|
719
|
+
parent_frame=frame_id,
|
|
692
720
|
)
|
|
693
721
|
elif shape == "cylinder":
|
|
694
722
|
obj = GeometryObject(
|
|
@@ -698,6 +726,7 @@ def build_geom_from_mjcf(model: Model, filename: str) -> "GeometryModel":
|
|
|
698
726
|
"cylinder",
|
|
699
727
|
np.array([size[0], 2.0 * size[1]]), # half length
|
|
700
728
|
mesh_color=color,
|
|
729
|
+
parent_frame=frame_id,
|
|
701
730
|
)
|
|
702
731
|
elif shape == "mesh" and attributes.get("mesh") in meshes:
|
|
703
732
|
mesh_path, scale = meshes[attributes["mesh"]]
|
|
@@ -711,6 +740,7 @@ def build_geom_from_mjcf(model: Model, filename: str) -> "GeometryModel":
|
|
|
711
740
|
mesh_path=mesh_path,
|
|
712
741
|
mesh_scale=scale,
|
|
713
742
|
mesh_color=color,
|
|
743
|
+
parent_frame=frame_id,
|
|
714
744
|
)
|
|
715
745
|
else: # unsupported geometry type: skip
|
|
716
746
|
continue
|
|
@@ -43,6 +43,8 @@ class Frame:
|
|
|
43
43
|
parent_frame: Index of the previous frame in the tree.
|
|
44
44
|
placement: Pose of the frame in the support joint frame.
|
|
45
45
|
type: Frame type.
|
|
46
|
+
inertia: Inertia of the body the frame was created for, before it
|
|
47
|
+
was merged into its support joint, expressed in the frame.
|
|
46
48
|
"""
|
|
47
49
|
|
|
48
50
|
def __init__(
|
|
@@ -52,6 +54,7 @@ class Frame:
|
|
|
52
54
|
parent_frame: int,
|
|
53
55
|
placement: SE3,
|
|
54
56
|
type: FrameType,
|
|
57
|
+
inertia: Optional[Inertia] = None,
|
|
55
58
|
):
|
|
56
59
|
"""Initialize frame.
|
|
57
60
|
|
|
@@ -61,12 +64,15 @@ class Frame:
|
|
|
61
64
|
parent_frame: Index of the previous frame in the tree.
|
|
62
65
|
placement: Pose of the frame in the support joint frame.
|
|
63
66
|
type: Frame type.
|
|
67
|
+
inertia: Inertia of the body the frame was created for, defaults
|
|
68
|
+
to zero.
|
|
64
69
|
"""
|
|
65
70
|
self.name = name
|
|
66
71
|
self.parent_joint = parent_joint
|
|
67
72
|
self.parent_frame = parent_frame
|
|
68
73
|
self.placement = placement
|
|
69
74
|
self.type = type
|
|
75
|
+
self.inertia = Inertia.Zero() if inertia is None else inertia
|
|
70
76
|
|
|
71
77
|
def __repr__(self) -> str:
|
|
72
78
|
"""String representation.
|
|
@@ -7,7 +7,7 @@ from typing import List, Optional
|
|
|
7
7
|
import numpy as np
|
|
8
8
|
|
|
9
9
|
from .algorithms import neutral
|
|
10
|
-
from .geometry import GeometryModel
|
|
10
|
+
from .geometry import GeometryModel, GeometryType
|
|
11
11
|
from .joints import JointModel
|
|
12
12
|
from .mjcf import build_geom_from_mjcf, build_model_from_mjcf
|
|
13
13
|
from .model import Data, Model
|
|
@@ -23,28 +23,34 @@ class RobotWrapper:
|
|
|
23
23
|
q0: Neutral configuration.
|
|
24
24
|
visual_model: Geometry model with the robot's visuals, when built
|
|
25
25
|
from URDF or MJCF.
|
|
26
|
+
collision_model: Geometry model with the robot's collision shapes,
|
|
27
|
+
when built from URDF or MJCF.
|
|
26
28
|
"""
|
|
27
29
|
|
|
28
30
|
model: Model
|
|
29
31
|
data: Data
|
|
30
32
|
q0: np.ndarray
|
|
31
33
|
visual_model: Optional[GeometryModel]
|
|
34
|
+
collision_model: Optional[GeometryModel]
|
|
32
35
|
|
|
33
36
|
def __init__(
|
|
34
37
|
self,
|
|
35
38
|
model: Model,
|
|
36
39
|
visual_model: Optional[GeometryModel] = None,
|
|
40
|
+
collision_model: Optional[GeometryModel] = None,
|
|
37
41
|
):
|
|
38
42
|
"""Wrap a robot model.
|
|
39
43
|
|
|
40
44
|
Args:
|
|
41
45
|
model: Robot model to wrap.
|
|
42
46
|
visual_model: Geometry model with the robot's visuals.
|
|
47
|
+
collision_model: Geometry model with the robot's collision shapes.
|
|
43
48
|
"""
|
|
44
49
|
self.model = model
|
|
45
50
|
self.data = model.create_data()
|
|
46
51
|
self.q0 = neutral(model)
|
|
47
52
|
self.visual_model = visual_model
|
|
53
|
+
self.collision_model = collision_model
|
|
48
54
|
|
|
49
55
|
@staticmethod
|
|
50
56
|
def BuildFromURDF(
|
|
@@ -65,8 +71,13 @@ class RobotWrapper:
|
|
|
65
71
|
if isinstance(package_dirs, str):
|
|
66
72
|
package_dirs = [package_dirs]
|
|
67
73
|
model = build_model_from_urdf(filename, root_joint)
|
|
68
|
-
visual_model = build_geom_from_urdf(
|
|
69
|
-
|
|
74
|
+
visual_model = build_geom_from_urdf(
|
|
75
|
+
model, filename, package_dirs, GeometryType.VISUAL
|
|
76
|
+
)
|
|
77
|
+
collision_model = build_geom_from_urdf(
|
|
78
|
+
model, filename, package_dirs, GeometryType.COLLISION
|
|
79
|
+
)
|
|
80
|
+
return RobotWrapper(model, visual_model, collision_model)
|
|
70
81
|
|
|
71
82
|
@staticmethod
|
|
72
83
|
def BuildFromMJCF(
|
|
@@ -85,8 +96,13 @@ class RobotWrapper:
|
|
|
85
96
|
Robot wrapper.
|
|
86
97
|
"""
|
|
87
98
|
model = build_model_from_mjcf(filename, root_joint)
|
|
88
|
-
visual_model = build_geom_from_mjcf(
|
|
89
|
-
|
|
99
|
+
visual_model = build_geom_from_mjcf(
|
|
100
|
+
model, filename, GeometryType.VISUAL
|
|
101
|
+
)
|
|
102
|
+
collision_model = build_geom_from_mjcf(
|
|
103
|
+
model, filename, GeometryType.COLLISION
|
|
104
|
+
)
|
|
105
|
+
return RobotWrapper(model, visual_model, collision_model)
|
|
90
106
|
|
|
91
107
|
@property
|
|
92
108
|
def nq(self) -> int:
|
|
@@ -34,7 +34,7 @@ from .se3 import SE3, Inertia
|
|
|
34
34
|
from .so3 import rpy_to_matrix
|
|
35
35
|
|
|
36
36
|
if TYPE_CHECKING:
|
|
37
|
-
from .geometry import GeometryModel
|
|
37
|
+
from .geometry import GeometryModel, GeometryType
|
|
38
38
|
|
|
39
39
|
|
|
40
40
|
def _parse_origin(element: Optional[ET.Element]) -> SE3:
|
|
@@ -181,8 +181,18 @@ def build_model_from_xml(
|
|
|
181
181
|
)
|
|
182
182
|
root_entry = (root_link, root_id, SE3.Identity(), root_frame)
|
|
183
183
|
else:
|
|
184
|
-
|
|
185
|
-
|
|
184
|
+
root_inertia = _parse_inertial(links[root_link])
|
|
185
|
+
model.append_body_to_joint(0, root_inertia)
|
|
186
|
+
root_frame = model.add_frame(
|
|
187
|
+
Frame(
|
|
188
|
+
root_link,
|
|
189
|
+
0,
|
|
190
|
+
0,
|
|
191
|
+
SE3.Identity(),
|
|
192
|
+
FrameType.BODY,
|
|
193
|
+
root_inertia,
|
|
194
|
+
)
|
|
195
|
+
)
|
|
186
196
|
root_entry = (root_link, 0, SE3.Identity(), root_frame)
|
|
187
197
|
|
|
188
198
|
# Depth-first traversal with child joints in file order, like Pinocchio.
|
|
@@ -206,6 +216,7 @@ def build_model_from_xml(
|
|
|
206
216
|
link_frame,
|
|
207
217
|
placement,
|
|
208
218
|
FrameType.FIXED_JOINT,
|
|
219
|
+
child_inertia,
|
|
209
220
|
)
|
|
210
221
|
)
|
|
211
222
|
model.append_body_to_joint(
|
|
@@ -360,21 +371,30 @@ def build_geom_from_urdf(
|
|
|
360
371
|
model: Model,
|
|
361
372
|
filename: str,
|
|
362
373
|
package_dirs: Optional[List[str]] = None,
|
|
374
|
+
geometry_type: Optional["GeometryType"] = None,
|
|
363
375
|
) -> "GeometryModel":
|
|
364
|
-
"""Parse the visual geometries of a URDF file.
|
|
376
|
+
"""Parse the visual or collision geometries of a URDF file.
|
|
365
377
|
|
|
366
378
|
Args:
|
|
367
379
|
model: Model previously built from the same URDF.
|
|
368
380
|
filename: Path to the URDF file.
|
|
369
381
|
package_dirs: Directories where mesh files are looked up.
|
|
382
|
+
geometry_type: Kind of geometries to parse, ``GeometryType.VISUAL``
|
|
383
|
+
(default) for <visual> elements or ``GeometryType.COLLISION`` for
|
|
384
|
+
<collision> elements.
|
|
370
385
|
|
|
371
386
|
Returns:
|
|
372
|
-
Geometry model with one object per URDF <visual>
|
|
387
|
+
Geometry model with one object per URDF <visual> or <collision>
|
|
388
|
+
element. Geometries are named ``<link>_<index>``, where the index
|
|
389
|
+
counts elements of the same kind in the link, as in Pinocchio.
|
|
373
390
|
"""
|
|
374
391
|
import os
|
|
375
392
|
|
|
376
|
-
from .geometry import GeometryModel, GeometryObject
|
|
377
|
-
|
|
393
|
+
from .geometry import GeometryModel, GeometryObject, GeometryType
|
|
394
|
+
|
|
395
|
+
if geometry_type is None:
|
|
396
|
+
geometry_type = GeometryType.VISUAL
|
|
397
|
+
tag = "visual" if geometry_type == GeometryType.VISUAL else "collision"
|
|
378
398
|
|
|
379
399
|
with open(filename, "r", encoding="utf-8") as handle:
|
|
380
400
|
robot = ET.fromstring(handle.read())
|
|
@@ -390,25 +410,21 @@ def build_geom_from_urdf(
|
|
|
390
410
|
rgba = [float(x) for x in color.get("rgba").split()]
|
|
391
411
|
materials[material.get("name")] = np.array(rgba)
|
|
392
412
|
|
|
393
|
-
#
|
|
394
|
-
|
|
395
|
-
|
|
396
|
-
for frame in model.frames
|
|
397
|
-
if frame.type == FrameType.BODY
|
|
398
|
-
}
|
|
399
|
-
|
|
413
|
+
# Links are visited in the order of their BODY frames, that is, in the
|
|
414
|
+
# depth-first order of the kinematic tree, as in Pinocchio
|
|
415
|
+
links = {link.get("name"): link for link in robot.findall("link")}
|
|
400
416
|
geometry_model = GeometryModel()
|
|
401
|
-
for
|
|
402
|
-
|
|
403
|
-
if link_name not in link_frames:
|
|
417
|
+
for frame_id, frame in enumerate(model.frames):
|
|
418
|
+
if frame.type != FrameType.BODY or frame.name not in links:
|
|
404
419
|
continue
|
|
405
|
-
|
|
406
|
-
|
|
407
|
-
|
|
420
|
+
link_name = frame.name
|
|
421
|
+
link = links[link_name]
|
|
422
|
+
for index, element in enumerate(link.findall(tag)):
|
|
423
|
+
origin = _parse_origin(element.find("origin"))
|
|
408
424
|
placement = frame.placement * origin
|
|
409
425
|
name = f"{link_name}_{index}"
|
|
410
426
|
color = None
|
|
411
|
-
material =
|
|
427
|
+
material = element.find("material")
|
|
412
428
|
if material is not None:
|
|
413
429
|
color_elem = material.find("color")
|
|
414
430
|
if color_elem is not None:
|
|
@@ -417,64 +433,51 @@ def build_geom_from_urdf(
|
|
|
417
433
|
)
|
|
418
434
|
elif material.get("name") in materials:
|
|
419
435
|
color = materials[material.get("name")]
|
|
420
|
-
geometry =
|
|
436
|
+
geometry = element.find("geometry")
|
|
421
437
|
box = geometry.find("box")
|
|
422
438
|
sphere = geometry.find("sphere")
|
|
423
439
|
cylinder = geometry.find("cylinder")
|
|
424
440
|
mesh = geometry.find("mesh")
|
|
441
|
+
mesh_path = ""
|
|
442
|
+
mesh_scale = None
|
|
425
443
|
if box is not None:
|
|
444
|
+
shape = "box"
|
|
426
445
|
size = np.array([float(x) for x in box.get("size").split()])
|
|
427
|
-
obj = GeometryObject(
|
|
428
|
-
name,
|
|
429
|
-
frame.parent_joint,
|
|
430
|
-
placement,
|
|
431
|
-
"box",
|
|
432
|
-
size,
|
|
433
|
-
mesh_color=color,
|
|
434
|
-
)
|
|
435
446
|
elif sphere is not None:
|
|
436
|
-
|
|
437
|
-
|
|
438
|
-
frame.parent_joint,
|
|
439
|
-
placement,
|
|
440
|
-
"sphere",
|
|
441
|
-
np.array([float(sphere.get("radius"))]),
|
|
442
|
-
mesh_color=color,
|
|
443
|
-
)
|
|
447
|
+
shape = "sphere"
|
|
448
|
+
size = np.array([float(sphere.get("radius"))])
|
|
444
449
|
elif cylinder is not None:
|
|
450
|
+
shape = "cylinder"
|
|
445
451
|
size = np.array(
|
|
446
452
|
[
|
|
447
453
|
float(cylinder.get("radius")),
|
|
448
454
|
float(cylinder.get("length")),
|
|
449
455
|
]
|
|
450
456
|
)
|
|
451
|
-
obj = GeometryObject(
|
|
452
|
-
name,
|
|
453
|
-
frame.parent_joint,
|
|
454
|
-
placement,
|
|
455
|
-
"cylinder",
|
|
456
|
-
size,
|
|
457
|
-
mesh_color=color,
|
|
458
|
-
)
|
|
459
457
|
elif mesh is not None:
|
|
458
|
+
shape = "mesh"
|
|
459
|
+
size = None
|
|
460
460
|
mesh_path = _resolve_mesh_path(
|
|
461
461
|
mesh.get("filename"), urdf_dir, package_dirs
|
|
462
462
|
)
|
|
463
463
|
if not mesh_path:
|
|
464
|
-
continue # mesh file not found: skip this
|
|
465
|
-
|
|
464
|
+
continue # mesh file not found: skip this geometry
|
|
465
|
+
mesh_scale = np.array(
|
|
466
466
|
[float(x) for x in mesh.get("scale", "1 1 1").split()]
|
|
467
467
|
)
|
|
468
|
-
|
|
468
|
+
else: # unknown geometry type: skip
|
|
469
|
+
continue
|
|
470
|
+
geometry_model.add_geometry_object(
|
|
471
|
+
GeometryObject(
|
|
469
472
|
name,
|
|
470
473
|
frame.parent_joint,
|
|
471
474
|
placement,
|
|
472
|
-
|
|
475
|
+
shape,
|
|
476
|
+
size,
|
|
473
477
|
mesh_path=mesh_path,
|
|
474
|
-
mesh_scale=
|
|
478
|
+
mesh_scale=mesh_scale,
|
|
475
479
|
mesh_color=color,
|
|
480
|
+
parent_frame=frame_id,
|
|
476
481
|
)
|
|
477
|
-
|
|
478
|
-
continue
|
|
479
|
-
geometry_model.add_geometry_object(obj)
|
|
482
|
+
)
|
|
480
483
|
return geometry_model
|
|
@@ -27,7 +27,7 @@ def check_revolute_path(model: kin.Model, frame_name: str) -> None:
|
|
|
27
27
|
"""Check all joints from root to frame are revolute.
|
|
28
28
|
|
|
29
29
|
Args:
|
|
30
|
-
model:
|
|
30
|
+
model: Robot model.
|
|
31
31
|
frame_name: Name of the frame to check.
|
|
32
32
|
|
|
33
33
|
Raises:
|
|
@@ -70,7 +70,7 @@ class ManipulabilityTask(Task):
|
|
|
70
70
|
Attributes:
|
|
71
71
|
frame: Frame name for which to compute manipulability.
|
|
72
72
|
cost: Weight for the manipulability task in the QP objective.
|
|
73
|
-
reference_frame:
|
|
73
|
+
reference_frame: Reference frame for Jacobian computation.
|
|
74
74
|
manipulability_rate: Desired rate of change of manipulability
|
|
75
75
|
:math:`\dot{m}_\text{desired}` to achieve.
|
|
76
76
|
|
|
@@ -107,8 +107,8 @@ class ManipulabilityTask(Task):
|
|
|
107
107
|
frame: Frame name (from the robot model) for which to compute and
|
|
108
108
|
maximize manipulability. This is typically the end-effector
|
|
109
109
|
frame name.
|
|
110
|
-
model:
|
|
111
|
-
|
|
110
|
+
model: Robot model used to verify that all joints on the kinematic
|
|
111
|
+
path to the frame are revolute.
|
|
112
112
|
cost: Weight for the manipulability task in the QP objective.
|
|
113
113
|
Scales both the Hessian and linear terms. Higher values
|
|
114
114
|
prioritize manipulability over other tasks. Defaults to 1.0.
|
|
@@ -117,9 +117,8 @@ class ManipulabilityTask(Task):
|
|
|
117
117
|
gain: Task gain :math:`\alpha \in [0, 1]` for additional low-pass
|
|
118
118
|
filtering. Defaults to 1.0 (no filtering) for dead-beat
|
|
119
119
|
control.
|
|
120
|
-
reference_frame:
|
|
121
|
-
|
|
122
|
-
Defaults to LOCAL.
|
|
120
|
+
reference_frame: Reference frame for Jacobian computation. LOCAL
|
|
121
|
+
and WORLD are supported. Defaults to LOCAL.
|
|
123
122
|
manipulability_rate: Desired rate of change of manipulability to
|
|
124
123
|
achieve, in units of manipulability per second. This is the
|
|
125
124
|
target velocity for the manipulability gradient ascent. Higher
|