Ship an optional flat-bottomed sole hull for lite_biped

README.md

@@ -106,6 +106,28 @@ sole, from the heel to the toe (`cad/scad/<side>_foot.scad`). The five capsules
 flat sole are flush with it, 65 mm below the foot origin. The last two follow the toe
 spring. `lite_biped_debug` keeps its flat debug box.
 
+`lite_biped` also ships an optional convex sole hull with an exactly flat bottom,
+`meshes/collision/<side>_foot_hull.stl`, in the frame of the foot body. The MJCF does not
+reference it. To collide through the hull, replace the capsules of each foot with it:
+
+```python
+import mujoco
+from lite_description import get_foot_hull_path, get_mjcf_path
+
+spec = mujoco.MjSpec.from_file(str(get_mjcf_path("lite_biped")))
+for side in ("left", "right"):
+    foot = spec.body(f"{side}_foot")
+    for geom in [g for g in foot.geoms if g.contype or g.conaffinity]:
+        spec.delete(geom)
+    spec.add_mesh(name=f"{side}_foot_hull", file=str(get_foot_hull_path("lite_biped", side)))
+    # The contact parameters of the lite_biped sole capsules.
+    foot.add_geom(name=f"{side}_foot_collision_0", type=mujoco.mjtGeom.mjGEOM_MESH,
+                  meshname=f"{side}_foot_hull", group=3, condim=3, priority=1,
+                  friction=[0.6, 0.005, 0.0001])
+```
+
+The foot bodies hold their own inertials, so the swap does not change the mass properties.
+
 The `uv.lock` of the consuming project records the commit of this repository. The assets
 thus change only when that project upgrades `lite-description`. No ROS install is required.
 
@@ -127,7 +149,7 @@ Lite-Description/                  # repo root == ament package "lite_descriptio
   package.xml  CMakeLists.txt      # ament (colcon); installs lite_description/robots/<variant>/...
   pyproject.toml                   # pip/uv: builds the lite-description wheel
   lite_description/                # Python package
-    __init__.py                    #   ROBOTS_DIR, VARIANTS, get_urdf_path(), get_mjcf_path()
+    __init__.py                    #   ROBOTS_DIR, VARIANTS, get_urdf_path(), get_mjcf_path(), get_foot_hull_path()
     actuators/                     #   actuator spec tables (velocity/effort/armature)
     workflow/                      #   the generator stages (needs the `cad` extra)
     robots/                        #   per-variant assets, inside the package so a wheel carries them
@@ -139,6 +161,7 @@ Lite-Description/                  # repo root == ament package "lite_descriptio
         urdf/<variant>.urdf        #     flat URDF (GENERATED; the kinematic HUB)
         mjcf/<variant>.xml         #     MJCF (GENERATED; MuJoCo training + deployment sim)
         meshes/visual/*.stl        #     one shared mesh copy
+        meshes/collision/*.stl     #     lite_biped: optional foot hulls, not in the MJCF (GENERATED)
         cad/                       #     generation INPUTS (not in the wheel, not installed):
           config.json              #       Onshape document + export options
           joint_properties.json    #       sim tuning: armature / friction / effort_limit
@@ -186,6 +209,13 @@ uv run lite-description-generate lite_dummy --force
 
 The Onshape stage costs about 1000 API requests, which is why it is skipped by default.
 
+The optional foot hull has its own command. It reads the shoe mesh from the generated MJCF
+and writes `meshes/collision/`:
+
+```bash
+uv run lite-description-foot-hull lite_biped
+```
+
 A variant without a `ros2_control.json` generates a model-only package: a description
 macro and a thin assembly, with no `<ros2_control>` block.
 

lite_description/__init__.py

@@ -9,7 +9,7 @@
 ROBOTS_DIR = Path(__file__).parent / "robots"
 VARIANTS = tuple(sorted(path.name for path in ROBOTS_DIR.iterdir() if path.is_dir()))
 
-__all__ = ["ROBOTS_DIR", "VARIANTS", "get_mjcf_path", "get_urdf_path"]
+__all__ = ["ROBOTS_DIR", "VARIANTS", "get_foot_hull_path", "get_mjcf_path", "get_urdf_path"]
 
 
 def get_urdf_path(variant: str) -> Path:
@@ -22,6 +22,17 @@ def get_mjcf_path(variant: str) -> Path:
     return _variant_dir(variant) / "mjcf" / f"{variant}.xml"
 
 
+def get_foot_hull_path(variant: str, side: str) -> Path:
+    """Return the optional sole hull of the ``side`` foot, an STL in the foot frame.
+
+    The MJCF does not reference the hull. Only ``lite_biped`` ships one.
+    """
+    path = _variant_dir(variant) / "meshes" / "collision" / f"{side}_foot_hull.stl"
+    if not path.is_file():
+        raise FileNotFoundError(f"{variant} ships no {side} foot hull at {path}.")
+    return path
+
+
 def _variant_dir(variant: str) -> Path:
     if variant not in VARIANTS:
         raise ValueError(f"Unknown variant {variant!r}. Expected one of {VARIANTS}.")

lite_description/workflow/foot_hull.py

@@ -0,0 +1,202 @@
+"""Build the optional sole hull of a shoe: ``meshes/collision/<side>_foot_hull.stl``.
+
+The hull is the convex hull of the shoe up to 25 mm above the sole, with 32 vertices. The
+MJCF does not reference it. MuJoCo collides a mesh through its convex hull. A plain
+simplified hull puts part of the sole on a facet that tilts by up to 3 deg, because an
+outline vertex is a fraction of a millimetre off the sole plane. Thus this module puts the
+outline of the flat sole on the plane as one 10-vertex polygon. Then it adds the other 22
+vertices from the shoe, farthest out first, as the incremental build of qhull does. The
+right hull is the mirror of the left hull.
+
+Usage:
+  lite-description-foot-hull lite_biped
+"""
+
+import argparse
+from pathlib import Path
+
+import numpy as np
+
+try:
+    import mujoco
+except ModuleNotFoundError as error:
+    raise ModuleNotFoundError(
+        "MuJoCo is needed to build the foot hull, and ships with the CAD toolchain rather than "
+        "the base install. Install it with `uv sync --extra cad`."
+    ) from error
+
+from . import robot_model
+
+VERTICES = 32  # all hull vertices
+BOTTOM_VERTICES = 10  # of them on the sole plane
+SOLE_REGION = 0.025  # m above the sole plane that the hull covers
+FLAT_TOLERANCE = 0.0007  # m: a face this close to the plane is part of the flat sole
+DOWN_FACING = 0.9  # minimum y of the unit normal of a sole face (+y is down)
+
+
+def geom_rotation(model: mujoco.MjModel, geom: int) -> np.ndarray:
+    """Return the rotation of ``geom`` in its body frame, shape (3, 3)."""
+    rotation = np.zeros(9)
+    mujoco.mju_quat2Mat(rotation, model.geom_quat[geom])
+    return rotation.reshape(3, 3)
+
+
+def mesh_geom_vertices(model: mujoco.MjModel, geom: int) -> np.ndarray:
+    """Return the mesh vertices of ``geom`` in its body frame, shape (n, 3)."""
+    mesh = model.geom_dataid[geom]
+    start = model.mesh_vertadr[mesh]
+    vertices = model.mesh_vert[start:start + model.mesh_vertnum[mesh]]
+    return vertices @ geom_rotation(model, geom).T + model.geom_pos[geom]
+
+
+def shoe_triangles(mjcf: Path) -> np.ndarray:
+    """Return the visual triangles of the left foot in the foot frame, shape (n, 3, 3)."""
+    model = mujoco.MjModel.from_xml_path(str(mjcf))
+    body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "left_foot")
+    triangles = []
+    for geom in range(model.ngeom):
+        is_mesh = model.geom_type[geom] == mujoco.mjtGeom.mjGEOM_MESH
+        is_visual = model.geom_contype[geom] == 0 and model.geom_conaffinity[geom] == 0
+        if model.geom_bodyid[geom] != body or not (is_mesh and is_visual):
+            continue
+        mesh = model.geom_dataid[geom]
+        start = model.mesh_faceadr[mesh]
+        faces = model.mesh_face[start:start + model.mesh_facenum[mesh]]
+        triangles.append(mesh_geom_vertices(model, geom)[faces])
+    return np.vstack(triangles).astype(float)
+
+
+def hull_faces(points: np.ndarray) -> list[tuple[np.ndarray, np.ndarray]]:
+    """Return the faces of the convex hull of ``points`` as (vertex indices, outward normal).
+
+    MuJoCo builds the hull with qhull and merges coplanar triangles into polygons. It
+    stores the mesh recentred and turned to its principal axes, so the geom pose maps the
+    polygons back onto ``points``.
+    """
+    spec = mujoco.MjSpec()
+    spec.add_mesh(name="hull", uservert=points.ravel().tolist())
+    spec.worldbody.add_geom(type=mujoco.mjtGeom.mjGEOM_MESH, meshname="hull")
+    model = spec.compile()
+    rotation = geom_rotation(model, 0)
+    vertices = mesh_geom_vertices(model, 0)
+    index = {}  # MuJoCo vertex -> the input point it came from (MuJoCo stores float32)
+
+    def point(vertex: int) -> int:
+        if vertex not in index:
+            index[vertex] = int(np.argmin(np.linalg.norm(points - vertices[vertex], axis=1)))
+        return index[vertex]
+
+    faces = []
+    for polygon in range(model.mesh_polynum[0]):
+        start = model.mesh_polyvertadr[polygon]
+        ring = model.mesh_polyvert[start:start + model.mesh_polyvertnum[polygon]]
+        normal = rotation @ model.mesh_polynormal[polygon]
+        faces.append((np.array([point(v) for v in ring]), normal))
+    return faces
+
+
+def cross2(a: np.ndarray, b: np.ndarray) -> float:
+    """Return the z component of the cross product of two 2-D vectors."""
+    return a[0] * b[1] - a[1] * b[0]
+
+
+def convex_ring(points: np.ndarray) -> np.ndarray:
+    """Return the 2-D convex hull of ``points`` in counter-clockwise order (monotone chain)."""
+    points = np.unique(points, axis=0)
+
+    def half(sequence):
+        chain = []
+        for point in sequence:
+            while len(chain) >= 2 and cross2(chain[-1] - chain[-2], point - chain[-2]) <= 0:
+                chain.pop()
+            chain.append(point)
+        return chain[:-1]
+
+    return np.array(half(points) + half(points[::-1]))
+
+
+def flat_bottom_hull(triangles: np.ndarray) -> np.ndarray:
+    """Return the vertices of the flat-bottomed sole hull (foot frame: +y down, +z forward)."""
+    normals = np.cross(triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0])
+    normals /= np.maximum(np.linalg.norm(normals, axis=1, keepdims=True), 1e-15)
+    vertices = triangles.reshape(-1, 3)
+    sole = vertices[:, 1].max()
+
+    # The bottom: the outline of the flat sole faces, on the sole plane. Each step removes
+    # the outline vertex whose removal loses the smallest area.
+    on_sole = triangles[:, :, 1].min(axis=1) > sole - FLAT_TOLERANCE
+    flat = (normals[:, 1] > DOWN_FACING) & on_sole
+    outline = list(convex_ring(triangles[flat].reshape(-1, 3)[:, [0, 2]]))
+    while len(outline) > BOTTOM_VERTICES:
+        loss = []
+        for i in range(len(outline)):
+            before, after = outline[i - 1], outline[(i + 1) % len(outline)]
+            loss.append(abs(cross2(outline[i] - before, after - before)) / 2)
+        outline.pop(int(np.argmin(loss)))
+    points = [np.array([x, sole, z]) for x, z in outline]
+
+    # The other vertices come from the shoe vertices in the sole region that are clear of
+    # the plane. Only the vertices on the hull of that region can be farthest out.
+    above = (vertices[:, 1] > sole - SOLE_REGION) & (vertices[:, 1] < sole - FLAT_TOLERANCE)
+    region = np.unique(vertices[above], axis=0)
+    region = region[np.unique(np.concatenate([face for face, _ in hull_faces(region)]))]
+    # First the extreme vertex along each axis, but not straight down (+y). Then, each
+    # time, the vertex farthest outside the hull so far.
+    for direction in np.vstack([np.eye(3), -np.eye(3)]):
+        candidate = region[np.argmax(region @ direction)]
+        if direction[1] <= 0 and not any(np.allclose(candidate, point) for point in points):
+            points.append(candidate)
+    while len(points) < VERTICES:
+        hull = np.array(points)
+        distance = np.full(len(region), -np.inf)
+        for ring, normal in hull_faces(hull):
+            distance = np.maximum(distance, (region - hull[ring[0]]) @ normal)
+        best = int(np.argmax(distance))
+        if distance[best] < 1e-4:
+            break
+        points.append(region[best])
+    return np.array(points)
+
+
+def write_stl(path: Path, points: np.ndarray) -> None:
+    """Write the convex hull of ``points`` as a binary STL, each polygon as a triangle fan."""
+    normals, corners = [], []
+    for ring, normal in hull_faces(points):
+        for i in range(1, len(ring) - 1):
+            triangle = points[[ring[0], ring[i], ring[i + 1]]]
+            if np.cross(triangle[1] - triangle[0], triangle[2] - triangle[0]) @ normal < 0:
+                triangle = triangle[[0, 2, 1]]
+            normals.append(normal)
+            corners.append(triangle)
+    record = np.dtype([("normal", "<f4", 3), ("vertices", "<f4", (3, 3)), ("attribute", "<u2")])
+    data = np.zeros(len(corners), record)
+    data["normal"] = normals
+    data["vertices"] = corners
+    path.parent.mkdir(parents=True, exist_ok=True)
+    with path.open("wb") as stream:
+        stream.write(b"lite_description foot hull".ljust(80, b" "))
+        stream.write(np.uint32(len(data)).tobytes())
+        stream.write(data.tobytes())
+
+
+def generate(robot_dir: Path) -> list[Path]:
+    """Write ``meshes/collision/{left,right}_foot_hull.stl`` for one ``robots/<robot>/`` dir."""
+    left = flat_bottom_hull(shoe_triangles(robot_dir / "mjcf" / f"{robot_dir.name}.xml"))
+    written = []
+    for side, mirror in (("left", 1.0), ("right", -1.0)):
+        path = robot_dir / "meshes" / "collision" / f"{side}_foot_hull.stl"
+        write_stl(path, left * np.array([mirror, 1.0, 1.0]))
+        written.append(path)
+    return written
+
+
+def main(argv: list[str] | None = None) -> None:
+    parser = argparse.ArgumentParser(description="Build the optional sole hull of a robot's shoes.")
+    parser.add_argument("robot", help="Variant name (under lite_description/robots/) or a path to a robot dir.")
+    args = parser.parse_args(argv)
+    for path in generate(robot_model.resolve_robot_dir(args.robot)):
+        print(f"Wrote {path}")
+
+
+if __name__ == "__main__":
+    main()

pyproject.toml

@@ -25,6 +25,8 @@ lite-description-onshape-to-urdf = "lite_description.workflow.onshape_to_urdf:ma
 lite-description-finalize-urdf = "lite_description.workflow.finalize_urdf:main"
 lite-description-urdf-to-mjcf = "lite_description.workflow.urdf_to_mjcf:main"
 lite-description-urdf-to-xacro = "lite_description.workflow.urdf_to_xacro:main"
+# The optional sole hull, which the MJCF does not reference.
+lite-description-foot-hull = "lite_description.workflow.foot_hull:main"
 
 [dependency-groups]
 test = [

tests/test_foot_hull.py

@@ -0,0 +1,60 @@
+"""Check the optional foot hull of lite_biped against the capsules that it replaces."""
+import mujoco
+import numpy as np
+import pytest
+
+from lite_description import get_foot_hull_path, get_mjcf_path
+from lite_description.workflow.foot_hull import geom_rotation, mesh_geom_vertices
+
+
+def hull_model(side: str) -> mujoco.MjModel:
+    """Load the hull as a training config does: one mesh geom in the foot frame."""
+    spec = mujoco.MjSpec()
+    spec.add_mesh(name="hull", file=str(get_foot_hull_path("lite_biped", side)))
+    spec.worldbody.add_geom(type=mujoco.mjtGeom.mjGEOM_MESH, meshname="hull")
+    return spec.compile()
+
+
+def sole_plane(side: str) -> float:
+    """Return the y of the lowest point of the sole capsules (+y is down)."""
+    model = mujoco.MjModel.from_xml_path(str(get_mjcf_path("lite_biped")))
+    body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, f"{side}_foot")
+    lowest = -np.inf
+    for geom in range(model.ngeom):
+        collides = model.geom_contype[geom] or model.geom_conaffinity[geom]
+        if model.geom_bodyid[geom] != body or not collides:
+            continue
+        assert model.geom_type[geom] == mujoco.mjtGeom.mjGEOM_CAPSULE, "the shoe must be capsules"
+        axis = geom_rotation(model, geom)[:, 2] * model.geom_size[geom][1]
+        ends = model.geom_pos[geom] + np.array([axis, -axis])
+        lowest = max(lowest, ends[:, 1].max() + model.geom_size[geom][0])
+    return lowest
+
+
+@pytest.mark.parametrize("side", ("left", "right"))
+def test_hull_bottom_is_one_level_face_on_the_sole_plane(side):
+    model = hull_model(side)
+    normals = model.mesh_polynormal[:model.mesh_polynum[0]] @ geom_rotation(model, 0).T
+    level = np.flatnonzero(normals[:, 1] > 1 - 1e-9)
+    # One polygon faces down, and no other polygon faces down within 3 deg of it.
+    assert len(level) == 1
+    assert np.sum(normals[:, 1] > np.cos(np.radians(3))) == 1
+    vertices = mesh_geom_vertices(model, 0)
+    start = model.mesh_polyvertadr[level[0]]
+    bottom = vertices[model.mesh_polyvert[start:start + model.mesh_polyvertnum[level[0]]]]
+    assert np.ptp(bottom[:, 1]) < 1e-6
+    # The bottom is the lowest part of the hull, at the lowest point of the capsules.
+    assert bottom[:, 1].max() == pytest.approx(vertices[:, 1].max(), abs=1e-9)
+    assert bottom[0, 1] == pytest.approx(sole_plane(side), abs=1e-4)
+
+
+def test_hulls_mirror():
+    left = mesh_geom_vertices(hull_model("left"), 0)
+    right = mesh_geom_vertices(hull_model("right"), 0) * np.array([-1.0, 1.0, 1.0])
+    distance = np.linalg.norm(left[:, None] - right[None], axis=2)
+    assert distance.min(axis=1).max() < 1e-6
+
+
+def test_variant_without_a_hull_raises():
+    with pytest.raises(FileNotFoundError):
+        get_foot_hull_path("lite_biped_debug", "left")