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.
Files changed (81) hide show
  1. {pinker-1.2.0/pinker.egg-info → pinker-1.3.0}/PKG-INFO +2 -2
  2. {pinker-1.2.0 → pinker-1.3.0}/README.md +1 -1
  3. {pinker-1.2.0 → pinker-1.3.0}/pinker/__init__.py +1 -1
  4. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/__init__.py +2 -1
  5. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/geometry.py +16 -1
  6. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/mjcf.py +45 -15
  7. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/model.py +6 -0
  8. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/robot_wrapper.py +21 -5
  9. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/urdf.py +57 -54
  10. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/manipulability_task.py +6 -7
  11. {pinker-1.2.0 → pinker-1.3.0}/pinker/visualizer.py +93 -26
  12. {pinker-1.2.0 → pinker-1.3.0/pinker.egg-info}/PKG-INFO +2 -2
  13. {pinker-1.2.0 → pinker-1.3.0}/pinker.egg-info/SOURCES.txt +2 -1
  14. {pinker-1.2.0 → pinker-1.3.0}/tests/test_loaders.py +4 -2
  15. pinker-1.3.0/tests/test_visualizer.py +204 -0
  16. {pinker-1.2.0 → pinker-1.3.0}/LICENSE +0 -0
  17. {pinker-1.2.0 → pinker-1.3.0}/NOTICE +0 -0
  18. {pinker-1.2.0 → pinker-1.3.0}/pinker/barriers/__init__.py +0 -0
  19. {pinker-1.2.0 → pinker-1.3.0}/pinker/barriers/barrier.py +0 -0
  20. {pinker-1.2.0 → pinker-1.3.0}/pinker/barriers/body_spherical_barrier.py +0 -0
  21. {pinker-1.2.0 → pinker-1.3.0}/pinker/barriers/position_barrier.py +0 -0
  22. {pinker-1.2.0 → pinker-1.3.0}/pinker/configuration.py +0 -0
  23. {pinker-1.2.0 → pinker-1.3.0}/pinker/exceptions.py +0 -0
  24. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/_kinematics_c.c +0 -0
  25. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/algorithms.py +0 -0
  26. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/joints.py +0 -0
  27. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/se2.py +0 -0
  28. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/se3.py +0 -0
  29. {pinker-1.2.0 → pinker-1.3.0}/pinker/kinematics/so3.py +0 -0
  30. {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/__init__.py +0 -0
  31. {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/acceleration_limit.py +0 -0
  32. {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/configuration_limit.py +0 -0
  33. {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/floating_base_velocity_limit.py +0 -0
  34. {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/limit.py +0 -0
  35. {pinker-1.2.0 → pinker-1.3.0}/pinker/limits/velocity_limit.py +0 -0
  36. {pinker-1.2.0 → pinker-1.3.0}/pinker/loaders.py +0 -0
  37. {pinker-1.2.0 → pinker-1.3.0}/pinker/solve_ik.py +0 -0
  38. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/__init__.py +0 -0
  39. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/com_task.py +0 -0
  40. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/damping_task.py +0 -0
  41. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/frame_task.py +0 -0
  42. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/joint_coupling_task.py +0 -0
  43. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/joint_velocity_task.py +0 -0
  44. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/linear_holonomic_task.py +0 -0
  45. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/low_acceleration_task.py +0 -0
  46. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/omniwheel_task.py +0 -0
  47. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/posture_task.py +0 -0
  48. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/relative_frame_task.py +0 -0
  49. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/rolling_task.py +0 -0
  50. {pinker-1.2.0 → pinker-1.3.0}/pinker/tasks/task.py +0 -0
  51. {pinker-1.2.0 → pinker-1.3.0}/pinker/utils.py +0 -0
  52. {pinker-1.2.0 → pinker-1.3.0}/pinker.egg-info/dependency_links.txt +0 -0
  53. {pinker-1.2.0 → pinker-1.3.0}/pinker.egg-info/requires.txt +0 -0
  54. {pinker-1.2.0 → pinker-1.3.0}/pinker.egg-info/top_level.txt +0 -0
  55. {pinker-1.2.0 → pinker-1.3.0}/pyproject.toml +0 -0
  56. {pinker-1.2.0 → pinker-1.3.0}/setup.cfg +0 -0
  57. {pinker-1.2.0 → pinker-1.3.0}/setup.py +0 -0
  58. {pinker-1.2.0 → pinker-1.3.0}/tests/test_acceleration_limit.py +0 -0
  59. {pinker-1.2.0 → pinker-1.3.0}/tests/test_barrier.py +0 -0
  60. {pinker-1.2.0 → pinker-1.3.0}/tests/test_body_spherical_barrier.py +0 -0
  61. {pinker-1.2.0 → pinker-1.3.0}/tests/test_com_task.py +0 -0
  62. {pinker-1.2.0 → pinker-1.3.0}/tests/test_configuration.py +0 -0
  63. {pinker-1.2.0 → pinker-1.3.0}/tests/test_configuration_limit.py +0 -0
  64. {pinker-1.2.0 → pinker-1.3.0}/tests/test_damping_task.py +0 -0
  65. {pinker-1.2.0 → pinker-1.3.0}/tests/test_floating_base_velocity_limit.py +0 -0
  66. {pinker-1.2.0 → pinker-1.3.0}/tests/test_frame_task.py +0 -0
  67. {pinker-1.2.0 → pinker-1.3.0}/tests/test_jacobians.py +0 -0
  68. {pinker-1.2.0 → pinker-1.3.0}/tests/test_joint_coupling_task.py +0 -0
  69. {pinker-1.2.0 → pinker-1.3.0}/tests/test_joint_velocity_task.py +0 -0
  70. {pinker-1.2.0 → pinker-1.3.0}/tests/test_limits.py +0 -0
  71. {pinker-1.2.0 → pinker-1.3.0}/tests/test_linear_holonomic_task.py +0 -0
  72. {pinker-1.2.0 → pinker-1.3.0}/tests/test_low_acceleration_task.py +0 -0
  73. {pinker-1.2.0 → pinker-1.3.0}/tests/test_manipulability_task.py +0 -0
  74. {pinker-1.2.0 → pinker-1.3.0}/tests/test_omniwheel_task.py +0 -0
  75. {pinker-1.2.0 → pinker-1.3.0}/tests/test_position_barrier.py +0 -0
  76. {pinker-1.2.0 → pinker-1.3.0}/tests/test_posture_task.py +0 -0
  77. {pinker-1.2.0 → pinker-1.3.0}/tests/test_relative_frame_task.py +0 -0
  78. {pinker-1.2.0 → pinker-1.3.0}/tests/test_rolling_task.py +0 -0
  79. {pinker-1.2.0 → pinker-1.3.0}/tests/test_solve_ik.py +0 -0
  80. {pinker-1.2.0 → pinker-1.3.0}/tests/test_utils.py +0 -0
  81. {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.2.0
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.2.0},
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.2.0},
203
+ version = {1.3.0},
204
204
  year = {2026}
205
205
  }
206
206
  ```
@@ -20,7 +20,7 @@ from .tasks import (
20
20
  Task,
21
21
  )
22
22
 
23
- __version__ = "1.2.0"
23
+ __version__ = "1.3.0"
24
24
 
25
25
  __all__ = [
26
26
  "Configuration",
@@ -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 geometry attached to a robot model.
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.add_body_frame(name, 0, body_pose, 0)
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 build_geom_from_mjcf(model: Model, filename: str) -> "GeometryModel":
594
- """Parse the visual geometries of an MJCF file.
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
- Geoms that don't collide, or that collide but are in a group visible by
597
- default (0 to 2), are visual geometries. Mesh, box, sphere and cylinder
598
- geoms are supported, other shapes are skipped.
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 visual geom.
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 _geom_kind_is_collision(attributes):
680
+ if excluded(attributes):
655
681
  continue
656
- name = attributes.get("name", f"{body_name}_{index}")
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(model, filename, package_dirs)
69
- return RobotWrapper(model, visual_model)
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(model, filename)
89
- return RobotWrapper(model, visual_model)
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
- model.append_body_to_joint(0, _parse_inertial(links[root_link]))
185
- root_frame = model.add_body_frame(root_link, 0, SE3.Identity(), 0)
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> element.
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
- from .model import FrameType
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
- # BODY frames map each link to its supporting joint and placement
394
- link_frames = {
395
- frame.name: frame
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 link in robot.findall("link"):
402
- link_name = link.get("name")
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
- frame = link_frames[link_name]
406
- for index, visual in enumerate(link.findall("visual")):
407
- origin = _parse_origin(visual.find("origin"))
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 = visual.find("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 = visual.find("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
- obj = GeometryObject(
437
- name,
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 visual
465
- scale = np.array(
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
- obj = GeometryObject(
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
- "mesh",
475
+ shape,
476
+ size,
473
477
  mesh_path=mesh_path,
474
- mesh_scale=scale,
478
+ mesh_scale=mesh_scale,
475
479
  mesh_color=color,
480
+ parent_frame=frame_id,
476
481
  )
477
- else: # unknown geometry type: skip
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: Pinocchio robot 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: Pinocchio reference frame for Jacobian computation.
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: Pinocchio robot model. Used to verify that all joints on
111
- the kinematic path to the frame are revolute.
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: Pinocchio reference frame for Jacobian
121
- computation. LOCAL and WORLD are supported.
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