cookie_description 1.0.0__py3-none-any.whl

cookie_description/__init__.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+
+"""Robot description for Cookie."""
+
+from .paths import MESHES_PATH, PATH, URDF_PATH, XACRO_PATH
+
+__version__ = "1.0.0"
+
+__all__ = [
+    "MESHES_PATH",
+    "PATH",
+    "URDF_PATH",
+    "XACRO_PATH",
+]

cookie_description/__main__.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+
+"""Entry point for ``uvx cookie_description <command>``."""
+
+import runpy
+import sys
+
+COMMANDS = {
+    "simulation": "cookie_description.simulation",
+    "visualizer": "cookie_description.visualizer",
+}
+
+
+def main():
+    """Run a cookie_description command."""
+    if len(sys.argv) < 2 or sys.argv[1] not in COMMANDS:
+        available = ", ".join(sorted(COMMANDS))
+        print("Usage: cookie_description <command>")
+        print(f"Available commands: {available}")
+        sys.exit(1)
+    command = sys.argv[1]
+    sys.argv = sys.argv[1:]  # shift so the submodule sees clean argv
+    runpy.run_module(COMMANDS[command], run_name="__main__")
+
+
+if __name__ == "__main__":
+    main()

cookie_description/collision_viser_visualizer.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+
+from pinocchio import visualize
+
+
+class CollisionViserVisualizer(visualize.ViserVisualizer):
+    """ViserVisualizer that renders collision geometries as wireframes.
+
+    While making Cookie's URDF, it is useful to overlay collision meshes on top
+    of the visual meshes to verify their fit. Viser's alpha-blending is
+    order-dependent and doesn't perform depth sorting, so transparent collision
+    meshes appear incorrectly from certain viewing angles. We thus render them
+    as wireframes.
+    """
+
+    _visual_opacity: float = 1.0
+    _visual_handles: list  # list of (handle, parent_frame_id) pairs
+    _collision_handles: list  # list of (handle, parent_frame_id) pairs
+
+    def loadViewerModel(self, *args, **kwargs):
+        """Reset tracked handles before (re)loading the model."""
+        self._visual_handles = []
+        self._collision_handles = []
+        super().loadViewerModel(*args, **kwargs)
+
+    def loadViewerGeometryObject(self, geometry_object, prefix="", color=None):
+        """Load geometry object with wireframe rendering for collision meshes.
+
+        Temporarily patches the Viser scene creation methods to inject
+        wireframe=True before delegating to the parent implementation. The
+        patch is scoped to a single geometry object and cleaned up in a finally
+        block.
+        """
+        scene = self.viewer.scene
+        _METHODS = ("add_box", "add_icosphere", "add_mesh_simple")
+
+        is_collision = "collision" in prefix
+        parent_frame = int(geometry_object.parentFrame)
+
+        for method_name in _METHODS:
+            original = getattr(scene, method_name)
+
+            def patched(
+                *args,
+                _orig=original,
+                _collision=is_collision,
+                _pf=parent_frame,
+                **kwargs,
+            ):
+                if _collision:
+                    kwargs["wireframe"] = True
+                    kwargs.pop("opacity", None)
+                else:
+                    kwargs["opacity"] = self._visual_opacity
+                handle = _orig(*args, **kwargs)
+                if _collision:
+                    self._collision_handles.append((handle, _pf))
+                else:
+                    self._visual_handles.append((handle, _pf))
+                return handle
+
+            setattr(scene, method_name, patched)
+
+        try:
+            super().loadViewerGeometryObject(geometry_object, prefix, color)
+        finally:
+            for method_name in _METHODS:
+                delattr(scene, method_name)
+
+    def set_visual_opacity(self, value: float) -> None:
+        """Update opacity of all loaded visual meshes uniformly."""
+        self._visual_opacity = value
+        for handle, _ in self._visual_handles:
+            handle.opacity = value
+
+    def set_joint_focus(
+        self, highlighted_frames: frozenset, dim_opacity: float = 0.2
+    ) -> None:
+        """Make parent and child link visuals opaque, dim everything else.
+
+        Also restricts collision mesh visibility to the highlighted frames.
+        """
+        for handle, parent_frame in self._visual_handles:
+            handle.opacity = (
+                1.0 if parent_frame in highlighted_frames else dim_opacity
+            )
+        for handle, parent_frame in self._collision_handles:
+            handle.visible = parent_frame in highlighted_frames
+
+    def set_all_collisions_visible(self, visible: bool) -> None:
+        """Set visibility on all individual collision handles."""
+        for handle, _ in self._collision_handles:
+            handle.visible = visible

cookie_description/paths.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+
+"""
+Paths to the URDF package and main description for Cookie.
+"""
+
+import os
+
+# Path to the cookie_description module
+PATH = os.path.dirname(os.path.realpath(__file__))
+
+# Path to the meshes folder
+MESHES_PATH = os.path.join(PATH, "meshes")
+
+# Path to the robot's URDF
+URDF_PATH = os.path.join(PATH, "urdf", "cookie.urdf")
+
+# Path to the robot's Xacro source
+XACRO_PATH = os.path.join(PATH, "xacro", "cookie.xacro")
+
+__all__ = [
+    "MESHES_PATH",
+    "PATH",
+    "URDF_PATH",
+    "XACRO_PATH",
+]

cookie_description/simulation.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+# SPDX-License-Identifier: Apache-2.0
+
+"""
+Load a robot description, specified from the command line, in PyBullet.
+
+This example requires PyBullet, which is installed by ``pip install pybullet``.
+"""
+
+import math
+import os
+
+import loop_rate_limiters
+import pybullet
+import pybullet_data
+
+from .paths import PATH, URDF_PATH
+
+if __name__ == "__main__":
+    pybullet.connect(pybullet.GUI)
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
+    pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SHADOWS, 0)
+    pybullet.setAdditionalSearchPath(PATH)
+    pybullet.setAdditionalSearchPath(os.path.dirname(PATH))
+    robot = pybullet.loadURDF(URDF_PATH)
+    pybullet.setGravity(0, 0, -9.81)
+    pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
+    plane = pybullet.loadURDF("plane.urdf")
+    pybullet.resetBasePositionAndOrientation(
+        robot, [0, 0, 1.5], [0, 0.5, 0.7, 0.3]
+    )
+
+    # Find hip and knee joint indices
+    hip_indices = []
+    knee_indices = []
+    for i in range(pybullet.getNumJoints(robot)):
+        joint_info = pybullet.getJointInfo(robot, i)
+        joint_name = joint_info[1].decode("utf-8")
+        if joint_info[2] != pybullet.JOINT_REVOLUTE:
+            continue
+        pybullet.setJointMotorControl2(
+            robot, i, pybullet.VELOCITY_CONTROL, force=0
+        )
+        if "hip" in joint_name:
+            hip_indices.append(i)
+        elif "knee" in joint_name:
+            knee_indices.append(i)
+
+    rate = loop_rate_limiters.RateLimiter(frequency=200.0)
+    pybullet.setTimeStep(rate.dt)
+    t = 0.0
+    while True:
+        hip_target = 0.4 * math.sin(2.0 * math.pi * 0.5 * t)
+        knee_target = -2.0 * hip_target
+        for idx in hip_indices:
+            pybullet.setJointMotorControl2(
+                robot, idx, pybullet.POSITION_CONTROL,
+                targetPosition=hip_target, force=10.0,
+            )
+        for idx in knee_indices:
+            pybullet.setJointMotorControl2(
+                robot, idx, pybullet.POSITION_CONTROL,
+                targetPosition=knee_target, force=10.0,
+            )
+        pybullet.stepSimulation()
+        t += rate.dt
+        rate.sleep()