diff --git a/README.md b/README.md index 44eb0bf..a0faac6 100644 --- a/README.md +++ b/README.md @@ -106,6 +106,28 @@ sole, from the heel to the toe (`cad/scad/_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/_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//... 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/.urdf # flat URDF (GENERATED; the kinematic HUB) mjcf/.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 `` block. diff --git a/lite_description/__init__.py b/lite_description/__init__.py index a09e1c7..93bc886 100644 --- a/lite_description/__init__.py +++ b/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}.") diff --git a/lite_description/robots/lite_biped/meshes/collision/left_foot_hull.stl b/lite_description/robots/lite_biped/meshes/collision/left_foot_hull.stl new file mode 100644 index 0000000..ad822cb Binary files /dev/null and b/lite_description/robots/lite_biped/meshes/collision/left_foot_hull.stl differ diff --git a/lite_description/robots/lite_biped/meshes/collision/right_foot_hull.stl b/lite_description/robots/lite_biped/meshes/collision/right_foot_hull.stl new file mode 100644 index 0000000..4001ad2 Binary files /dev/null and b/lite_description/robots/lite_biped/meshes/collision/right_foot_hull.stl differ diff --git a/lite_description/workflow/foot_hull.py b/lite_description/workflow/foot_hull.py new file mode 100644 index 0000000..bbf2643 --- /dev/null +++ b/lite_description/workflow/foot_hull.py @@ -0,0 +1,202 @@ +"""Build the optional sole hull of a shoe: ``meshes/collision/_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", " list[Path]: + """Write ``meshes/collision/{left,right}_foot_hull.stl`` for one ``robots//`` 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() diff --git a/pyproject.toml b/pyproject.toml index 6eb079b..ce0b9cb 100644 --- a/pyproject.toml +++ b/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 = [ diff --git a/tests/test_foot_hull.py b/tests/test_foot_hull.py new file mode 100644 index 0000000..1e39c00 --- /dev/null +++ b/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")