diff --git a/doc/changelog.rst b/doc/changelog.rst index e6d9e56f..be50cbb0 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,14 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +MJX +^^^ + +1. Improved performance of SAT for convex collisions. + Version 3.1.3 (March 5th, 2024) ----------------------------------- diff --git a/doc/mjx.rst b/doc/mjx.rst index af935449..3d1d42d2 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -299,8 +299,8 @@ Collisions between large meshes SAT works well for smaller meshes but suffers in both runtime and memory for larger meshes. For - collisions between convex meshes and primitives (spheres, capsules, planes), use **3000 vertices or less** for your convex meshes. - For collisions between convex meshes and other convex meshes, use **30 vertices or less**. + collisions with convex meshes, the convex decompositon of the mesh should have + roughly **200 vertices or less** for reasonable performance. With careful tuning, MJX can simulate scenes with mesh collisions -- see the MJX `shadow hand `__ diff --git a/mjx/mujoco/mjx/_src/collision_base.py b/mjx/mujoco/mjx/_src/collision_base.py index fbad8609..cb6293da 100644 --- a/mjx/mujoco/mjx/_src/collision_base.py +++ b/mjx/mujoco/mjx/_src/collision_base.py @@ -52,6 +52,8 @@ class GeomInfo(PyTreeNode): vert: Optional[jax.Array] = None edge: Optional[jax.Array] = None facenorm: Optional[jax.Array] = None + face_edge: Optional[jax.Array] = None + face_edge_normal: Optional[jax.Array] = None class SolverParams(PyTreeNode): diff --git a/mjx/mujoco/mjx/_src/collision_convex.py b/mjx/mujoco/mjx/_src/collision_convex.py index 5428f53c..196b5b78 100644 --- a/mjx/mujoco/mjx/_src/collision_convex.py +++ b/mjx/mujoco/mjx/_src/collision_convex.py @@ -14,6 +14,7 @@ # ============================================================================== """Convex collisions.""" +import functools from typing import Tuple import jax @@ -177,6 +178,174 @@ def _manifold_points( return jp.array([a_idx, b_idx, c_idx, d_idx]) +def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact: + """Calculates contacts between a plane and a convex object.""" + vert = convex.vert + + # get points in the convex frame + plane_pos = convex.mat.T @ (plane.pos - convex.pos) + n = convex.mat.T @ plane.mat[:, 2] + support = (plane_pos - vert) @ n + idx = _manifold_points(vert, support > 0, n) + pos = vert[idx] + + # convert to world frame + pos = convex.pos + pos @ convex.mat.T + n = plane.mat[:, 2] + + frame = jp.stack([math.make_frame(n)] * 4, axis=0) + unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1 + dist = jp.where(unique, -support[idx], 1) + pos = pos - 0.5 * dist[:, None] * n + return dist, pos, frame + + +def sphere_convex(sphere: GeomInfo, convex: GeomInfo) -> Contact: + """Calculates contact between a sphere and a convex object.""" + faces = convex.face + normals = convex.facenorm + + # Put sphere in convex frame. + sphere_pos = convex.mat.T @ (sphere.pos - convex.pos) + + # Get support from face normals. + @jax.vmap + def get_support(faces, normal): + pos = sphere_pos - normal * sphere.size[0] + return jp.dot(pos - faces[0], normal) + + support = get_support(faces, normals) + + # Pick the face with minimal penetration as long as it has support. + support = jp.where(support >= 0, -1e12, support) + best_idx = support.argmax() + face = faces[best_idx] + normal = normals[best_idx] + + # Get closest point between the polygon face and the sphere center point. + # Project the sphere center point onto poly plane. If it's inside polygon + # edge normals, then we're done. + pt = _project_pt_onto_plane(sphere_pos, face[0], normal) + edge_p0 = jp.roll(face, 1, axis=0) + edge_p1 = face + edge_normals = jax.vmap(jp.cross, in_axes=[0, None])( + edge_p1 - edge_p0, + normal, + ) + edge_dist = jax.vmap( + lambda plane_pt, plane_norm: (pt - plane_pt).dot(plane_norm) + )(edge_p0, edge_normals) + inside = jp.all(edge_dist <= 0) # lte to handle degenerate edges + + # If the point is outside edge normals, project onto the closest edge plane + # that the point is in front of. + degenerate_edge = jp.all(edge_normals == 0, axis=1) + behind = edge_dist < 0.0 + edge_dist = jp.where(degenerate_edge | behind, 1e12, edge_dist) + idx = edge_dist.argmin() + edge_pt = math.closest_segment_point(edge_p0[idx], edge_p1[idx], pt) + + pt = jp.where(inside, pt, edge_pt) + + # Get the normal, dist, and contact position. + n, d = math.normalize_with_norm(pt - sphere_pos) + spt = sphere_pos + n * sphere.size[0] + dist = d - sphere.size[0] + pos = (pt + spt) * 0.5 + + # Go back to world frame. + n = convex.mat @ n + pos = convex.mat @ pos + convex.pos + + return jax.tree_map( + lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n)) + ) + + +def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact: + """Calculates contacts between a capsule and a convex object.""" + # Get convex transformed normals, faces, and vertices. + faces = convex.face + normals = convex.facenorm + + # Put capsule in convex frame. + cap_pos = convex.mat.T @ (cap.pos - convex.pos) + axis, length = cap.mat[:, 2], cap.size[1] + axis = convex.mat.T @ axis + seg = axis * length + cap_pts = jp.array([ + cap_pos - seg, + cap_pos + seg, + ]) + + # Get support from face normals. + @jax.vmap + def get_support(face, normal): + pts = cap_pts - normal * cap.size[0] + sup = jax.vmap(lambda x: jp.dot(x - face[0], normal))(pts) + return sup.min() + + support = get_support(faces, normals) + has_support = jp.all(support < 0) + + # Pick the face with minimal penetration as long as it has support. + support = jp.where(support >= 0, -1e12, support) + best_idx = support.argmax() + face = faces[best_idx] + normal = normals[best_idx] + + # Clip the edge against side planes and create two contact points against the + # face. + edge_p0 = jp.roll(face, 1, axis=0) + edge_p1 = face + edge_normals = jax.vmap(jp.cross, in_axes=[0, None])( + edge_p1 - edge_p0, + normal, + ) + cap_pts_clipped, mask = _clip_edge_to_planes( + cap_pts[0], cap_pts[1], edge_p0, edge_normals + ) + cap_pts_clipped = cap_pts_clipped - normal * cap.size[0] + face_pts = jax.vmap(_project_pt_onto_plane, in_axes=[0, None, None])( + cap_pts_clipped, face[0], normal + ) + # Create variables for the face contact. + pos = (cap_pts_clipped + face_pts) * 0.5 + norm = jp.stack([normal] * 2, 0) + penetration = jp.where( + mask & has_support, jp.dot(face_pts - cap_pts_clipped, normal), -1 + ) + + # Get a potential edge contact. + edge_closest, cap_closest = jax.vmap( + math.closest_segment_to_segment_points, in_axes=[0, 0, None, None] + )(edge_p0, edge_p1, cap_pts[0], cap_pts[1]) + e_idx = ((edge_closest - cap_closest) ** 2).sum(axis=1).argmin() + cap_closest_pt, edge_closest_pt = cap_closest[e_idx], edge_closest[e_idx] + edge_axis = cap_closest_pt - edge_closest_pt + edge_axis, edge_dist = math.normalize_with_norm(edge_axis) + edge_pos = ( + edge_closest_pt + (cap_closest_pt - edge_axis * cap.size[0]) + ) * 0.5 + edge_norm = edge_axis + edge_penetration = cap.size[0] - edge_dist + has_edge_contact = edge_penetration > 0 + + # Get the contact info. + pos = jp.where(has_edge_contact, pos.at[0].set(edge_pos), pos) + n = -jp.where(has_edge_contact, norm.at[0].set(edge_norm), norm) + + # Go back to world frame. + pos = convex.pos + pos @ convex.mat.T + n = n @ convex.mat.T + + dist = -jp.where( + has_edge_contact, penetration.at[0].set(edge_penetration), penetration + ) + frame = jax.vmap(math.make_frame)(n) + return dist, pos, frame + + def _project_pt_onto_plane( pt: jax.Array, plane_pt: jax.Array, plane_normal: jax.Array ) -> jax.Array: @@ -396,7 +565,7 @@ def _create_contact_manifold( return dist, pos, normal -def _sat_hull_hull( +def _sat_bruteforce( faces_a: jax.Array, faces_b: jax.Array, vertices_a: jax.Array, @@ -414,13 +583,16 @@ def _sat_hull_hull( We return both the edge and face contacts. Valid contacts can be checked with dist < 0. Resulting edge contacts should be preferred over face contacts. + This method checks all unique edge-pairs and is thus costly to run over large + meshes, but is more performant for smaller meshes (boxes, tetrahedra, etc.). + Args: - faces_a: An ndarray of hull A's polygon faces. - faces_b: An ndarray of hull B's polygon faces. + faces_a: Faces for hull A. + faces_b: Faces for hull B. vertices_a: Vertices for hull A. vertices_b: Vertices for hull B. - normals_a: Normal vectors for hull A's polygon faces. - normals_b: Normal vectors for hull B's polygon faces. + normals_a: Normal vectors for hull A faces. + normals_b: Normal vectors for hull B faces. unique_edges_a: Unique edges for hull A. unique_edges_b: Unique edges for hull B. @@ -428,30 +600,36 @@ def _sat_hull_hull( tuple of dist, pos, and normal """ # get the separating axes - edge_dir_a = unique_edges_a[:, 0] - unique_edges_a[:, 1] - edge_dir_b = unique_edges_b[:, 0] - unique_edges_b[:, 1] + v_norm = jax.vmap(math.normalize) + edge_dir_a = v_norm(unique_edges_a[:, 0] - unique_edges_a[:, 1]) + edge_dir_b = v_norm(unique_edges_b[:, 0] - unique_edges_b[:, 1]) edge_dir_a_r = jp.tile(edge_dir_a, reps=(unique_edges_b.shape[0], 1)) edge_dir_b_r = jp.repeat(edge_dir_b, repeats=unique_edges_a.shape[0], axis=0) - edge_edge_axes = jax.vmap(jp.cross)(edge_dir_a_r, edge_dir_b_r) - edge_edge_axes = jax.vmap(lambda x: math.normalize(x, axis=0))( - edge_edge_axes + edge_axes = jax.vmap(jp.cross)(edge_dir_a_r, edge_dir_b_r) + degenerate_edge_axes = (edge_axes**2).sum(axis=1) < 1e-6 + edge_axes = jax.vmap(lambda x: math.normalize(x, axis=0))(edge_axes) + n_norm = normals_a.shape[0] + normals_b.shape[0] + degenerate_axes = jp.concatenate( + [jp.array([False] * n_norm), degenerate_edge_axes] ) - axes = jp.concatenate([normals_a, normals_b, edge_edge_axes]) + axes = jp.concatenate([normals_a, normals_b, edge_axes]) # for each separating axis, get the support @jax.vmap - def get_support(axis): - support_a = jax.vmap(jp.dot, in_axes=[None, 0])(axis, vertices_a) - support_b = jax.vmap(jp.dot, in_axes=[None, 0])(axis, vertices_b) + def get_support(axis, is_degenerate): + # the matmul here is more performant with vmap(dot) + dot = functools.partial(jp.dot, precision=jax.lax.Precision.HIGH) + support_a = jax.vmap(dot, in_axes=[None, 0])(axis, vertices_a) + support_b = jax.vmap(dot, in_axes=[None, 0])(axis, vertices_b) dist1 = support_a.max() - support_b.min() dist2 = support_b.max() - support_a.min() sign = jp.where(dist1 > dist2, -1, 1) dist = jp.minimum(dist1, dist2) - dist = jp.where(~jp.all(axis == 0.0), dist, 1e6) # degenerate axis + dist = jp.where(~is_degenerate, dist, 1e6) # degenerate axis return dist, sign - support, sign = get_support(axes) + support, sign = get_support(axes, degenerate_axes) # choose the best separating axis best_idx = jp.argmin(support) @@ -460,8 +638,8 @@ def _sat_hull_hull( is_edge_contact = best_idx >= (normals_a.shape[0] + normals_b.shape[0]) # get the (reference) face most aligned with the separating axis - dist_a = jax.vmap(jp.dot, in_axes=[None, 0])(best_axis, normals_a) - dist_b = jax.vmap(jp.dot, in_axes=[None, 0])(best_axis, normals_b) + dist_a = normals_a @ best_axis + dist_b = normals_b @ best_axis a_max = dist_a.argmax() b_max = dist_b.argmax() a_min = dist_a.argmin() @@ -496,172 +674,172 @@ def _sat_hull_hull( return dist, pos, normal -def plane_convex(plane: GeomInfo, convex: GeomInfo) -> Contact: - """Calculates contacts between a plane and a convex object.""" - vert = convex.vert - - # get points in the convex frame - plane_pos = convex.mat.T @ (plane.pos - convex.pos) - n = convex.mat.T @ plane.mat[:, 2] - support = (plane_pos - vert) @ n - idx = _manifold_points(vert, support > 0, n) - pos = vert[idx] - - # convert to world frame - pos = convex.pos + pos @ convex.mat.T - n = plane.mat[:, 2] - - frame = jp.stack([math.make_frame(n)] * 4, axis=0) - unique = jp.tril(idx == idx[:, None]).sum(axis=1) == 1 - dist = jp.where(unique, -support[idx], 1) - pos = pos - 0.5 * dist[:, None] * n - return dist, pos, frame +def _arcs_intersect( + a: jax.Array, b: jax.Array, c: jax.Array, d: jax.Array +) -> jax.Array: + """Tests if arcs AB and CD on the unit sphere intersect.""" + ba, dc = jp.cross(b, a), jp.cross(d, c) + cba, dba = jp.dot(c, ba), jp.dot(d, ba) + adc, bdc = jp.dot(a, dc), jp.dot(b, dc) + return (cba * dba < 0) & (adc * bdc < 0) & (cba * bdc > 0) -def sphere_convex(sphere: GeomInfo, convex: GeomInfo) -> Contact: - """Calculates contact between a sphere and a convex object.""" - faces = convex.face - normals = convex.facenorm +def _sat_approx( + centroid_a: jax.Array, + faces_a: jax.Array, + faces_b: jax.Array, + vertices_a: jax.Array, + vertices_b: jax.Array, + normals_a: jax.Array, + normals_b: jax.Array, + face_edges_a: jax.Array, + face_edges_b: jax.Array, + face_edge_normals_a: jax.Array, + face_edge_normals_b: jax.Array, +) -> Tuple[jax.Array, jax.Array, jax.Array]: + """Runs the Separating Axis Test for a pair of hulls. - # Put sphere in convex frame. - sphere_pos = convex.mat.T @ (sphere.pos - convex.pos) + Runs the separating axis test for all faces. After obtaining a reference + and incident face, tests edge separating axes via edge intersections + on gauss maps. + + Certain meshes can have nearly parallel coplanar faces that are not merged. + Thus reference/incident faces can be non-overlapping, and face contacts will + not get generated. Since we only check edge separating axes on + reference/incident faces, the correct edge separating axes may also be + missing. We mitigate this issue by checking nearly coplanar anti-parallel + reference/incident faces for support, hence why this method is "approximate". + + Args: + centroid_a: Centroid of hull A. + faces_a: Faces for hull A. + faces_b: Faces for hull B. + vertices_a: Vertices for hull A. + vertices_b: Vertices for hull B. + normals_a: Normal vectors for hull A faces. + normals_b: Normal vectors for hull B faces. + face_edges_a: Edges for faces in hull A. + face_edges_b: Edges for faces in hull B. + face_edge_normals_a: Edge normals for faces in hull A. + face_edge_normals_b: Edge normals for faces in hull B. + + Returns: + tuple of dist, pos, and normal + """ + # Handle face separating axes. + axes = jp.concatenate([normals_a, -normals_b]) - # Get support from face normals. @jax.vmap - def get_support(faces, normal): - pos = sphere_pos - normal * sphere.size[0] - return jp.dot(pos - faces[0], normal) + def get_support(axis): + # the matmul here is more performant with vmap(dot) + dot = functools.partial(jp.dot, precision=jax.lax.Precision.HIGH) + support_a = jax.vmap(dot, in_axes=[None, 0])(axis, vertices_a) + support_b = jax.vmap(dot, in_axes=[None, 0])(axis, vertices_b) + dist = support_a.max() - support_b.min() + separating = dist < 0 + dist = jp.where(dist < 0, 1e6, dist) + return dist, separating - support = get_support(faces, normals) + support, separating = get_support(axes) + is_face_separating = separating.any() - # Pick the face with minimal penetration as long as it has support. - support = jp.where(support >= 0, -1e12, support) - best_idx = support.argmax() - face = faces[best_idx] - normal = normals[best_idx] + # choose the best separating axis + best_idx = jp.argmin(support) + best_axis = axes[best_idx] - # Get closest point between the polygon face and the sphere center point. - # Project the sphere center point onto poly plane. If it's inside polygon - # edge normals, then we're done. - pt = _project_pt_onto_plane(sphere_pos, face[0], normal) - edge_p0 = jp.roll(face, 1, axis=0) - edge_p1 = face - edge_normals = jax.vmap(jp.cross, in_axes=[0, None])( - edge_p1 - edge_p0, - normal, - ) - edge_dist = jax.vmap( - lambda plane_pt, plane_norm: (pt - plane_pt).dot(plane_norm) - )(edge_p0, edge_normals) - inside = jp.all(edge_dist <= 0) # lte to handle degenerate edges + # get the (reference) face most aligned with the separating axis + dist_a = normals_a @ best_axis + dist_b = normals_b @ -best_axis + face_a_idx = dist_a.argmax() + face_b_idx = dist_b.argmax() - # If the point is outside edge normals, project onto the closest edge plane - # that the point is in front of. - degenerate_edge = jp.all(edge_normals == 0, axis=1) - behind = edge_dist < 0.0 - edge_dist = jp.where(degenerate_edge | behind, 1e12, edge_dist) - idx = edge_dist.argmin() - edge_pt = math.closest_segment_point(edge_p0[idx], edge_p1[idx], pt) - - pt = jp.where(inside, pt, edge_pt) - - # Get the normal, dist, and contact position. - n, d = math.normalize_with_norm(pt - sphere_pos) - spt = sphere_pos + n * sphere.size[0] - dist = d - sphere.size[0] - pos = (pt + spt) * 0.5 - - # Go back to world frame. - n = convex.mat @ n - pos = convex.mat @ pos + convex.pos - - return jax.tree_map( - lambda x: jp.expand_dims(x, axis=0), (dist, pos, math.make_frame(n)) + cond = best_idx < normals_a.shape[0] + ref_face = jp.where(cond, faces_a[face_a_idx], faces_b[face_b_idx]) + incident_face = jp.where(cond, faces_b[face_b_idx], faces_a[face_a_idx]) + ref_face_norm = jp.where(cond, normals_a[face_a_idx], normals_b[face_b_idx]) + incident_face_norm = jp.where( + cond, normals_b[face_b_idx], normals_a[face_a_idx] ) + dist, pos, normal = _create_contact_manifold( + ref_face, + incident_face, + ref_face_norm, + incident_face_norm, + -best_axis, + ) -def capsule_convex(cap: GeomInfo, convex: GeomInfo) -> Contact: - """Calculates contacts between a capsule and a convex object.""" - # Get convex transformed normals, faces, and vertices. - faces = convex.face - normals = convex.facenorm + # Handle edge separating axes by checking edge pairs on the reference and + # incident faces. In principle, the correct edge-edge separating axis can be + # created from edge pairs on the reference and incident faces. + # first get the edge directions and face edge normals + face_edges_a = face_edges_a[face_a_idx] + v_norm = jax.vmap(math.normalize) + face_edges_dir_a = v_norm(face_edges_a[:, 0] - face_edges_a[:, 1]) + face_edges_b = face_edges_b[face_b_idx] + face_edges_dir_b = v_norm(face_edges_b[:, 0] - face_edges_b[:, 1]) + face_edge_normals_a = face_edge_normals_a[face_a_idx] + face_edge_normals_b = face_edge_normals_b[face_b_idx] - # Put capsule in convex frame. - cap_pos = convex.mat.T @ (cap.pos - convex.pos) - axis, length = cap.mat[:, 2], cap.size[1] - axis = convex.mat.T @ axis - seg = axis * length - cap_pts = jp.array([ - cap_pos - seg, - cap_pos + seg, - ]) + # scatter to all edge pairs + edge_idx_a = jp.repeat( + jp.arange(face_edges_a.shape[0]), face_edges_b.shape[0] + ) + edge_idx_b = jp.tile(jp.arange(face_edges_b.shape[0]), face_edges_a.shape[0]) + face_edges_dir_a = face_edges_dir_a[edge_idx_a] + face_edges_dir_b = face_edges_dir_b[edge_idx_b] + face_edges_pt_a = face_edges_a[:, 0][edge_idx_a] + face_edges_pt_b = face_edges_b[:, 0][edge_idx_b] + face_edge_normals_a = face_edge_normals_a[edge_idx_a] + face_edge_normals_b = face_edge_normals_b[edge_idx_b] - # Get support from face normals. @jax.vmap - def get_support(face, normal): - pts = cap_pts - normal * cap.size[0] - sup = jax.vmap(lambda x: jp.dot(x - face[0], normal))(pts) - return sup.min() + def get_normals(a_dir, a_pt, b_dir): + edge_axis = math.normalize(jp.cross(a_dir, b_dir)) + # correct normal to point from a to b, object b is at the origin + sign = jp.where(jp.dot(edge_axis, a_pt - centroid_a) > 0.0, 1.0, -1.0) + return edge_axis * sign - support = get_support(faces, normals) - has_support = jp.all(support < 0) + edge_axes = get_normals(face_edges_dir_a, face_edges_pt_a, face_edges_dir_b) + edge_dist = jax.vmap(jp.dot)(edge_axes, face_edges_pt_b - face_edges_pt_a) + # handle degenerate axes + edge_dist = jp.where((edge_axes**2).sum(axis=1) < 1e-6, 1e6, edge_dist) + # ensure edges create a minkowski face by testing intersection on gauss maps + is_minkowski_face = jax.vmap(_arcs_intersect)( + face_edge_normals_a[:, 0], + face_edge_normals_a[:, 1], + -face_edge_normals_b[:, 0], + -face_edge_normals_b[:, 1], + ) + edge_dist = jp.where(is_minkowski_face, edge_dist, 1e6) + edge_dist = jp.where(edge_dist > 0, -1e6, edge_dist) - # Pick the face with minimal penetration as long as it has support. - support = jp.where(support >= 0, -1e12, support) - best_idx = support.argmax() - face = faces[best_idx] - normal = normals[best_idx] + best_edge_idx = edge_dist.argmax() + best_edge_dist = edge_dist[best_edge_idx] + # prefer edge over face contacts as long as we have a valid edge contact + is_edge_contact = (best_edge_dist > dist.min() + 1e-6) & (best_edge_dist < 0) + normal = jp.where(is_edge_contact, edge_axes[best_edge_idx], normal) + dist = jp.where(is_edge_contact, jp.array([best_edge_dist, 1, 1, 1]), dist) - # Clip the edge against side planes and create two contact points against the - # face. - edge_p0 = jp.roll(face, 1, axis=0) - edge_p1 = face - edge_normals = jax.vmap(jp.cross, in_axes=[0, None])( - edge_p1 - edge_p0, - normal, - ) - cap_pts_clipped, mask = _clip_edge_to_planes( - cap_pts[0], cap_pts[1], edge_p0, edge_normals - ) - cap_pts_clipped = cap_pts_clipped - normal * cap.size[0] - face_pts = jax.vmap(_project_pt_onto_plane, in_axes=[0, None, None])( - cap_pts_clipped, face[0], normal - ) - # Create variables for the face contact. - pos = (cap_pts_clipped + face_pts) * 0.5 - norm = jp.stack([normal] * 2, 0) - penetration = jp.where( - mask & has_support, jp.dot(face_pts - cap_pts_clipped, normal), -1 + # A failure mode occurs if faces are very narrow and nearly parallel + # (i.e. faces did not get merged properly as coplanar faces). The face + # contacts will be empty since the reference/incident faces may not overlap. + # An edge-edge separating axis will not be found, since the reference and + # incident faces will also not overlap or necessarily create a minkowski face. + # Thus, we approximate the contact for anti-parallel faces with support. + anti_parallel = incident_face_norm.dot(ref_face_norm) < -0.97 + dist = dist.at[0].set( + jp.where( + anti_parallel + & ~is_face_separating + & (dist > 0).all() + & ~is_edge_contact, + -support[best_idx], + dist[0], + ) ) - # Get a potential edge contact. - edge_closest, cap_closest = jax.vmap( - math.closest_segment_to_segment_points, in_axes=[0, 0, None, None] - )(edge_p0, edge_p1, cap_pts[0], cap_pts[1]) - e_idx = ((edge_closest - cap_closest) ** 2).sum(axis=1).argmin() - cap_closest_pt, edge_closest_pt = cap_closest[e_idx], edge_closest[e_idx] - edge_axis = cap_closest_pt - edge_closest_pt - edge_axis, edge_dist = math.normalize_with_norm(edge_axis) - edge_pos = ( - edge_closest_pt + (cap_closest_pt - edge_axis * cap.size[0]) - ) * 0.5 - edge_norm = edge_axis - edge_penetration = cap.size[0] - edge_dist - has_edge_contact = edge_penetration > 0 - - # Get the contact info. - pos = jp.where(has_edge_contact, pos.at[0].set(edge_pos), pos) - n = -jp.where(has_edge_contact, norm.at[0].set(edge_norm), norm) - - # Go back to world frame. - pos = convex.pos + pos @ convex.mat.T - n = n @ convex.mat.T - - dist = -jp.where( - has_edge_contact, penetration.at[0].set(edge_penetration), penetration - ) - frame = jax.vmap(math.make_frame)(n) - return dist, pos, frame + return dist, pos, normal def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact: @@ -699,16 +877,41 @@ def convex_convex(c1: GeomInfo, c2: GeomInfo) -> Contact: unique_edges1 = jp.take(vertices1, c1.edge, axis=0) unique_edges2 = jp.take(vertices2, c2.edge, axis=0) - dist, pos, normal = _sat_hull_hull( - faces1, - faces2, - vertices1, - vertices2, - normals1, - normals2, - unique_edges1, - unique_edges2, + face_edges1 = jp.take(vertices1, c1.face_edge, axis=0) + face_edges2 = jp.take(vertices2, c2.face_edge, axis=0) + + face_edge_normals1 = c1.face_edge_normal @ to_local_mat.T + face_edge_normals2 = c2.face_edge_normal + + enable_bruteforce = ( + unique_edges1.shape[0] * unique_edges2.shape[0] + < face_edges1[0].shape[0] * face_edges2[0].shape[0] ) + if enable_bruteforce: + dist, pos, normal = _sat_bruteforce( + faces1, + faces2, + vertices1, + vertices2, + normals1, + normals2, + unique_edges1, + unique_edges2, + ) + else: + dist, pos, normal = _sat_approx( + to_local_pos, + faces1, + faces2, + vertices1, + vertices2, + normals1, + normals2, + face_edges1, + face_edges2, + face_edge_normals1, + face_edge_normals2, + ) # Go back to world frame. pos = c2.pos + pos @ c2.mat.T diff --git a/mjx/mujoco/mjx/_src/collision_driver.py b/mjx/mujoco/mjx/_src/collision_driver.py index 87ef925f..d492d69b 100644 --- a/mjx/mujoco/mjx/_src/collision_driver.py +++ b/mjx/mujoco/mjx/_src/collision_driver.py @@ -20,8 +20,8 @@ import jax from jax import numpy as jp import mujoco from mujoco.mjx._src import collision_base +from mujoco.mjx._src import mesh from mujoco.mjx._src import support -from mujoco.mjx._src import math # pylint: disable=g-importing-member from mujoco.mjx._src.collision_base import Candidate from mujoco.mjx._src.collision_base import CandidateSet @@ -43,7 +43,6 @@ from mujoco.mjx._src.types import GeomType from mujoco.mjx._src.types import Model # pylint: enable=g-importing-member - # pair-wise collision functions _COLLISION_FUNC = { (GeomType.PLANE, GeomType.SPHERE): plane_sphere, @@ -91,7 +90,18 @@ def _add_candidate( def mesh_key(i): convex_data = [[None] * m.ngeom] * 3 if isinstance(m, Model): - convex_data = [m.geom_convex_face, m.geom_convex_vert, m.geom_convex_edge] + convex_data = [ + m.geom_convex_face, + m.geom_convex_vert, + m.geom_convex_edge_dir, + ] + elif isinstance(m, mujoco.MjModel): + kwargs = mesh.get(m) + convex_data = [ + kwargs['geom_convex_face'], + kwargs['geom_convex_vert'], + kwargs['geom_convex_edge_dir'], + ] key = tuple((-1,) if v[i] is None else v[i].shape for v in convex_data) return key @@ -181,36 +191,39 @@ def _pair_info( m: Model, d: Data, geom1: Sequence[int], geom2: Sequence[int] ) -> Tuple[GeomInfo, GeomInfo, Sequence[Dict[str, Optional[int]]]]: """Returns geom pair info for calculating collision.""" - g1, g2 = jp.array(geom1), jp.array(geom2) - info1 = GeomInfo( - g1, - d.geom_xpos[g1], - d.geom_xmat[g1], - m.geom_size[g1], - ) - info2 = GeomInfo( - g2, - d.geom_xpos[g2], - d.geom_xmat[g2], - m.geom_size[g2], - ) - in_axes1 = in_axes2 = jax.tree_map(lambda x: 0, info1) - if m.geom_convex_face[geom1[0]] is not None: - info1 = info1.replace( - face=jp.stack([m.geom_convex_face[i] for i in geom1]), - vert=jp.stack([m.geom_convex_vert[i] for i in geom1]), - edge=jp.stack([m.geom_convex_edge[i] for i in geom1]), - facenorm=jp.stack([m.geom_convex_facenormal[i] for i in geom1]), + def mesh_info(geom): + g = jp.array(geom) + info = GeomInfo( + g, + d.geom_xpos[g], + d.geom_xmat[g], + m.geom_size[g], ) - in_axes1 = in_axes1.replace(face=0, vert=0, edge=0, facenorm=0) - if m.geom_convex_face[geom2[0]] is not None: - info2 = info2.replace( - face=jp.stack([m.geom_convex_face[i] for i in geom2]), - vert=jp.stack([m.geom_convex_vert[i] for i in geom2]), - edge=jp.stack([m.geom_convex_edge[i] for i in geom2]), - facenorm=jp.stack([m.geom_convex_facenormal[i] for i in geom2]), - ) - in_axes2 = in_axes2.replace(face=0, vert=0, edge=0, facenorm=0) + in_axes = jax.tree_map(lambda x: 0, info) + is_mesh = m.geom_convex_face[geom[0]] is not None + if is_mesh: + info = info.replace( + face=jp.stack([m.geom_convex_face[i] for i in geom]), + vert=jp.stack([m.geom_convex_vert[i] for i in geom]), + edge=jp.stack([m.geom_convex_edge_dir[i] for i in geom]), + facenorm=jp.stack([m.geom_convex_facenormal[i] for i in geom]), + face_edge_normal=jp.stack( + [m.geom_convex_face_edge_normal[i] for i in geom] + ), + face_edge=jp.stack([m.geom_convex_face_edge[i] for i in geom]), + ) + in_axes = in_axes.replace( + face=0, + vert=0, + edge=0, + facenorm=0, + face_edge=0, + face_edge_normal=0, + ) + return info, in_axes + + info1, in_axes1 = mesh_info(geom1) + info2, in_axes2 = mesh_info(geom2) return info1, info2, [in_axes1, in_axes2] @@ -289,9 +302,8 @@ def _collide_geoms( n_pairs = max_pairs if run_broadphase else len(geom1) if run_broadphase: # broadphase over geom pairs, using bounding spheres - size1 = jp.max(m.geom_size[jp.array(geom1)], axis=-1) - size2 = jp.max(m.geom_size[jp.array(geom2)], axis=-1) - # TODO(btaba): consider re-using collision info for (sphere, sphere) + size1 = jp.max(m.geom_size[g1.geom_id], axis=-1) + size2 = jp.max(m.geom_size[g2.geom_id], axis=-1) dists = jax.vmap(jp.linalg.norm)(g2.pos - g1.pos) - (size1 + size2) _, idx = jax.lax.top_k(-dists, k=n_pairs) g1, g2, params = jax.tree_map( diff --git a/mjx/mujoco/mjx/_src/collision_driver_test.py b/mjx/mujoco/mjx/_src/collision_driver_test.py index c57f38fb..164b1e86 100644 --- a/mjx/mujoco/mjx/_src/collision_driver_test.py +++ b/mjx/mujoco/mjx/_src/collision_driver_test.py @@ -371,15 +371,14 @@ class ConvexTest(absltest.TestCase): _CONVEX_CONVEX = """ - - + - + @@ -388,12 +387,9 @@ class ConvexTest(absltest.TestCase): """ def test_convex_convex(self): - """Tests generic convex-convex collision.""" + """Tests generic convex-convex collision via _sat_approx.""" directory = epath.resource_path('mujoco.mjx') assets = { - 'meshes/tetrahedron.stl': ( - directory / 'test_data' / 'meshes/tetrahedron.stl' - ).read_bytes(), 'meshes/dodecahedron.stl': ( directory / 'test_data' / 'meshes/dodecahedron.stl' ).read_bytes(), @@ -492,6 +488,31 @@ class NconTest(parameterized.TestCase): ncon = collision_driver.ncon(m) self.assertEqual(ncon, 0) + def test_ncon_meshes(self): + m = test_util.load_test_file('shadow_hand/scene_right.xml') + + ncon = collision_driver.ncon(m) + self.assertEqual(ncon, 15) + + mx = mjx.put_model(m) + ncon = collision_driver.ncon(mx) + self.assertEqual(ncon, 15) + + # get rid of max_contact_points, test only max_geom_pairs + for i in range(m.nnumeric): + name_ = ( + m.names[m.name_numericadr[i] :].decode('utf-8').split('\x00', 1)[0] + ) + if name_ == 'max_contact_points': + m.numeric_data[m.numeric_adr[i]] = -1 + + ncon = collision_driver.ncon(m) + self.assertEqual(ncon, 307) + + mx = mjx.put_model(m) + ncon = collision_driver.ncon(mx) + self.assertEqual(ncon, 307) + class TopKContactTest(absltest.TestCase): """Tests top-k contacts.""" diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index 7e71db5d..9ceca6b0 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -110,7 +110,7 @@ class ModelIOTest(parameterized.TestCase): np.testing.assert_almost_equal(mx.geom_pos, m.geom_pos) self.assertLen(mx.geom_convex_face, 6) self.assertLen(mx.geom_convex_vert, 6) - self.assertLen(mx.geom_convex_edge, 6) + self.assertLen(mx.geom_convex_edge_dir, 6) self.assertLen(mx.geom_convex_facenormal, 6) np.testing.assert_allclose(mx.jnt_type, m.jnt_type) diff --git a/mjx/mujoco/mjx/_src/mesh.py b/mjx/mujoco/mjx/_src/mesh.py index eb7a79c1..13af828f 100644 --- a/mjx/mujoco/mjx/_src/mesh.py +++ b/mjx/mujoco/mjx/_src/mesh.py @@ -14,8 +14,11 @@ # ============================================================================== """Mesh processing.""" +import collections +import dataclasses import itertools -from typing import Dict, Optional, Sequence, Tuple +from typing import Dict, List, Optional, Sequence, Tuple +import warnings import mujoco # pylint: disable=g-importing-member @@ -44,8 +47,10 @@ _CONVEX_CACHE: Dict[Tuple[int, int], Dict[str, np.ndarray]] = {} _DERIVED_ARGS = [ 'geom_convex_face', 'geom_convex_vert', - 'geom_convex_edge', + 'geom_convex_edge_dir', 'geom_convex_facenormal', + 'geom_convex_face_edge', + 'geom_convex_face_edge_normal', ] DERIVED = {(Model, d) for d in _DERIVED_ARGS} @@ -73,8 +78,8 @@ def _get_face_norm(vert: np.ndarray, face: np.ndarray) -> np.ndarray: return face_norm -def _get_unique_edges(vert: np.ndarray, face: np.ndarray) -> np.ndarray: - """Returns unique edges. +def _get_unique_edge_dir(vert: np.ndarray, face: np.ndarray) -> np.ndarray: + """Returns unique edge directions. Args: vert: (n_vert, 3) vertices @@ -109,6 +114,52 @@ def _get_unique_edges(vert: np.ndarray, face: np.ndarray) -> np.ndarray: return edges[unique_edge_idx] +def _get_face_edge_normals( + face: np.ndarray, face_norm: np.ndarray +) -> Tuple[np.ndarray, np.ndarray]: + """Returns face edges and face edge normals.""" + # get face edges and scatter the face norms + r_face = np.roll(face, 1, axis=1) + face_edge = np.array([face, r_face]).transpose((1, 2, 0)) + face_edge.sort(axis=2) + face_edge_flat = np.concatenate(face_edge) + edge_face_idx = np.repeat(np.arange(face.shape[0]), face.shape[1]) + edge_face_norm = face_norm[edge_face_idx] + + # get the edge normals associated with each edge + edge_map_list = collections.defaultdict(list) + for i in range(face_edge_flat.shape[0]): + if face_edge_flat[i][0] == face_edge_flat[i][1]: + continue + edge_map_list[tuple(face_edge_flat[i])].append(edge_face_norm[i]) + + edge_map = {} + for k, v in edge_map_list.items(): + v = np.array(v) + if len(v) > 2: + # Meshes can be of poor quality and contain edges adjacent to more than + # two faces. We take the first two unique face normals. + v = np.unique(v, axis=0)[:2] + elif len(v) == 1: + # Some edges are either degenerate or _MAX_HULL_FACE_VERTICES was hit + # and face vertices were down sampled. In either case, we ignore these + # edges. + continue + edge_map[k] = v + + # for each face, list the edge normals + face_edge_normal = [] + for face_idx in range(face_edge.shape[0]): + normals = [] + for edge in face_edge[face_idx]: + k = tuple(edge) + normals.append(edge_map.get(k, np.zeros((2, 3)))) + face_edge_normal.append(np.array(normals)) + face_edge_normal = np.array(face_edge_normal) + + return face_edge, face_edge_normal + + def _convex_hull_2d(points: np.ndarray, normal: np.ndarray) -> np.ndarray: """Calculates the convex hull for a set of points on a plane.""" # project points onto the closest axis plane @@ -128,7 +179,16 @@ def _convex_hull_2d(points: np.ndarray, normal: np.ndarray) -> np.ndarray: return hull_point_idx -def _merge_coplanar(tm: trimesh.Trimesh) -> np.ndarray: +@dataclasses.dataclass +class MeshInfo: + name: str + vert: np.ndarray + face: np.ndarray + convex_vert: Optional[np.ndarray] + convex_face: Optional[np.ndarray] + + +def _merge_coplanar(tm: trimesh.Trimesh, mesh_info: MeshInfo) -> np.ndarray: """Merges coplanar facets.""" if not tm.facets: return tm.faces.copy() # no facets @@ -152,6 +212,13 @@ def _merge_coplanar(tm: trimesh.Trimesh) -> np.ndarray: face = point_idx[hull_point_idx] # resize faces that exceed max polygon vertices + if face.shape[0] > _MAX_HULL_FACE_VERTICES: + warnings.warn( + f'Mesh "{mesh_info.name}" has a coplanar face with more than' + f' {_MAX_HULL_FACE_VERTICES} vertices. This may lead to performance ' + 'issues and inaccuracies in collision detection. Consider ' + 'decimating the mesh.' + ) every = face.shape[0] // _MAX_HULL_FACE_VERTICES + 1 face = face[::every] facets.append(face) @@ -173,48 +240,82 @@ def _merge_coplanar(tm: trimesh.Trimesh) -> np.ndarray: return np.concatenate([faces, facets]) -def _get_faces_verts( +def _mesh_info( m: mujoco.MjModel, -) -> Tuple[Sequence[np.ndarray], Sequence[np.ndarray]]: - """Extracts mesh faces and vertices from MjModel.""" - verts, faces = [], [] +) -> List[MeshInfo]: + """Extracts mesh info from MjModel.""" + mesh_infos = [] for i in range(m.nmesh): + name = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_MESH.value, i) + last = (i + 1) >= m.nmesh face_start = m.mesh_faceadr[i] face_end = m.mesh_faceadr[i + 1] if not last else m.mesh_face.shape[0] face = m.mesh_face[face_start:face_end] - faces.append(face) vert_start = m.mesh_vertadr[i] vert_end = m.mesh_vertadr[i + 1] if not last else m.mesh_vert.shape[0] vert = m.mesh_vert[vert_start:vert_end] - verts.append(vert) - return verts, faces + + graphadr = m.mesh_graphadr[i] + if graphadr < 0: + mesh_infos.append(MeshInfo(name, vert, face, None, None)) + continue + + graph = m.mesh_graph[graphadr:] + numvert, numface = graph[0], graph[1] + + # unused vert_edgeadr + # vert_edgeadr = graph[2 : numvert + 2] + last_idx = numvert + 2 + + vert_globalid = graph[last_idx : last_idx + numvert] + last_idx += numvert + + # unused edge_localid + # edge_localid = graph[last_idx : last_idx + numvert + 3 * numface] + last_idx += numvert + 3 * numface + + face_globalid = graph[last_idx : last_idx + 3 * numface] + face_globalid = face_globalid.reshape((numface, 3)) + + convex_vert = vert[vert_globalid] + vertex_map = dict(zip(vert_globalid, np.arange(vert_globalid.shape[0]))) + convex_face = np.vectorize(vertex_map.get)(face_globalid) + mesh_infos.append(MeshInfo(name, vert, face, convex_vert, convex_face)) + + return mesh_infos def _geom_mesh_kwargs( - vert: np.ndarray, face: np.ndarray + mesh_info: MeshInfo, ) -> Dict[str, np.ndarray]: """Generates convex mesh attributes for mjx.Model.""" - tm = trimesh.Trimesh(vertices=vert, faces=face) - tm_convex = trimesh.convex.convex_hull(tm) + tm_convex = trimesh.Trimesh( + vertices=mesh_info.convex_vert, faces=mesh_info.convex_face + ) vert = np.array(tm_convex.vertices) - face = _merge_coplanar(tm_convex) + face = _merge_coplanar(tm_convex, mesh_info) + facenormal = _get_face_norm(vert, face) + face_edge, face_edge_normal = _get_face_edge_normals(face, facenormal) return { 'geom_convex_face': vert[face], 'geom_convex_face_vert_idx': face, 'geom_convex_vert': vert, - 'geom_convex_edge': _get_unique_edges(vert, face), - 'geom_convex_facenormal': _get_face_norm(vert, face), + 'geom_convex_edge_dir': _get_unique_edge_dir(vert, face), + 'geom_convex_facenormal': facenormal, + 'geom_convex_face_edge': face_edge, + 'geom_convex_face_edge_normal': face_edge_normal, } def get(m: mujoco.MjModel) -> Dict[str, Sequence[Optional[np.ndarray]]]: """Derives geom mesh attributes for mjx.Model from MjModel.""" kwargs = {k: [] for k in _DERIVED_ARGS} - verts, faces = _get_faces_verts(m) + mesh_infos = _mesh_info(m) geom_con = m.geom_conaffinity | m.geom_contype for geomid in range(m.ngeom): + mesh_info = None dataid = m.geom_dataid[geomid] if not geom_con[geomid]: # ignore visual-only meshes @@ -222,15 +323,22 @@ def get(m: mujoco.MjModel) -> Dict[str, Sequence[Optional[np.ndarray]]]: continue elif m.geom_type[geomid] == GeomType.BOX: vert, face = _box(m.geom_size[geomid]) - elif dataid >= 0: - vert, face = verts[dataid], faces[dataid] - else: + mesh_info = MeshInfo( + name='box', + vert=vert, + face=face, + convex_vert=vert, + convex_face=face, + ) + elif dataid < 0: kwargs = {k: kwargs[k] + [None] for k in _DERIVED_ARGS} continue + mesh_info = mesh_info or mesh_infos[dataid] + vert, face = mesh_info.vert, mesh_info.face key = (hash(vert.data.tobytes()), hash(face.data.tobytes())) if key not in _CONVEX_CACHE: - _CONVEX_CACHE[key] = _geom_mesh_kwargs(vert, face) + _CONVEX_CACHE[key] = _geom_mesh_kwargs(mesh_info) kwargs = {k: kwargs[k] + [_CONVEX_CACHE[key][k]] for k in _DERIVED_ARGS} diff --git a/mjx/mujoco/mjx/_src/mesh_test.py b/mjx/mujoco/mjx/_src/mesh_test.py index 0ff9785e..233f3baa 100644 --- a/mjx/mujoco/mjx/_src/mesh_test.py +++ b/mjx/mujoco/mjx/_src/mesh_test.py @@ -17,6 +17,7 @@ from absl.testing import absltest from mujoco.mjx._src import mesh import numpy as np +import trimesh class GeomMeshKwargsTest(absltest.TestCase): @@ -33,7 +34,18 @@ class GeomMeshKwargsTest(absltest.TestCase): face = np.array( [[0, 1, 2], [0, 3, 1], [0, 4, 3], [0, 2, 4], [2, 1, 4], [1, 3, 4]] ) - h = mesh._geom_mesh_kwargs(vert, face) + tm = trimesh.Trimesh(vertices=vert, faces=face) + tm_convex = trimesh.convex.convex_hull(tm) + convex_vert = np.array(tm_convex.vertices) + convex_face = np.array(tm_convex.faces) + mesh_info = mesh.MeshInfo( + name='test', + vert=vert, + face=face, + convex_vert=convex_vert, + convex_face=convex_face, + ) + h = mesh._geom_mesh_kwargs(mesh_info) # get index of vertices in h['geom_convex_vert'] for vertices in vert dist = np.repeat(vert, vert.shape[0], axis=0) - np.tile( @@ -57,7 +69,7 @@ class GeomMeshKwargsTest(absltest.TestCase): ) # check edges - unique_edge = np.vectorize(map_.get)(h['geom_convex_edge']) + unique_edge = np.vectorize(map_.get)(h['geom_convex_edge_dir']) unique_edge = np.array(sorted(unique_edge.tolist())) np.testing.assert_array_equal( unique_edge, @@ -67,6 +79,51 @@ class GeomMeshKwargsTest(absltest.TestCase): # face normals self.assertEqual(h['geom_convex_facenormal'].shape, (5, 3)) + # face edges + edges = np.concatenate(h['geom_convex_face_edge']) + edges = np.vectorize(map_.get)(edges) + mask = edges[:, 0] != edges[:, 1] + edges = edges[mask] + sort_col_idx = np.argsort(edges, axis=1) + edges = np.take_along_axis(edges, sort_col_idx, axis=1) + sort_row_idx = np.lexsort((edges[:, 1], edges[:, 0])) + edges = edges[sort_row_idx] + np.testing.assert_array_equal( + edges, + np.array([ + [0, 2], + [0, 2], + [0, 3], + [0, 3], + [0, 4], + [0, 4], + [1, 2], + [1, 2], + [1, 3], + [1, 3], + [1, 4], + [1, 4], + [2, 4], + [2, 4], + [3, 4], + [3, 4], + ]), + ) + + # face edge normals + edge_normal = h['geom_convex_face_edge_normal'] + edge_normal = np.concatenate(edge_normal) + edge_normal = edge_normal[mask] + edge_normal = np.take_along_axis( + edge_normal, sort_col_idx[..., None], axis=1 + ) + edge_normal = edge_normal[sort_row_idx] + edge_normal_02 = np.array([[0.4472136, -0.0, 0.89442719], [-1.0, 0.0, 0.0]]) + np.testing.assert_array_almost_equal( + edge_normal[:2], + np.array([edge_normal_02, edge_normal_02]), + ) + class ConvexHull2DTest(absltest.TestCase): @@ -109,7 +166,7 @@ class UniqueEdgesTest(absltest.TestCase): [[-0.1, 0.0, -0.1], [0.0, 0.1, 0.1], [0.1, 0.0, -0.1], [0.0, -0.1, 0.1]] ) face = np.array([[0, 1, 2], [0, 2, 3], [0, 3, 1], [2, 1, 3]]) - idx = mesh._get_unique_edges(vert, face) + idx = mesh._get_unique_edge_dir(vert, face) np.testing.assert_array_equal( idx, np.array([[0, 1], [0, 2], [0, 3], [1, 2], [1, 3], [2, 3]]) ) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 19085ff5..92b04a72 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -394,8 +394,10 @@ class Model(PyTreeNode): mesh_face: vertex face data (nmeshface, 3) geom_convex_face: vertex face data, MJX only (ngeom,) geom_convex_vert: vertex data, MJX only (ngeom,) - geom_convex_edge: unique edge data, MJX only (ngeom,) + geom_convex_edge_dir: unique edge direction, MJX only (ngeom,) geom_convex_facenormal: normal face data, MJX only (ngeom,) + geom_convex_face_edge: edges for each face (ngeom,) + geom_convex_face_edge_normal: edge normals for each face (ngeom,) pair_dim: contact dimensionality (npair,) pair_geom1: id of geom1 (npair,) pair_geom2: id of geom2 (npair,) @@ -539,8 +541,10 @@ class Model(PyTreeNode): pair_geom2: np.ndarray geom_convex_face: List[Optional[jax.Array]] geom_convex_vert: List[Optional[jax.Array]] - geom_convex_edge: List[Optional[jax.Array]] + geom_convex_edge_dir: List[Optional[jax.Array]] geom_convex_facenormal: List[Optional[jax.Array]] + geom_convex_face_edge: List[Optional[jax.Array]] + geom_convex_face_edge_normal: List[Optional[jax.Array]] pair_solref: jax.Array pair_solreffriction: jax.Array pair_solimp: jax.Array diff --git a/mjx/mujoco/mjx/test_data/shadow_hand/assets/f_distal_pst_214.obj b/mjx/mujoco/mjx/test_data/shadow_hand/assets/f_distal_pst_214.obj new file mode 100644 index 00000000..98d2d3c5 --- /dev/null +++ b/mjx/mujoco/mjx/test_data/shadow_hand/assets/f_distal_pst_214.obj @@ -0,0 +1,326 @@ +# https://github.com/mikedh/trimesh +v 6.50390586 0.64350290 9.24793311 +v 5.30661760 -0.31024313 9.73751972 +v 4.94829845 3.15382325 9.11860457 +v 6.33721589 4.12708360 8.09188972 +v 4.89932115 1.87489667 8.51418670 +v 4.70769594 6.29147507 5.98527472 +v -3.34707744 6.97764180 19.92969765 +v 3.37781185 6.95828391 19.93485428 +v 3.86552899 6.99640846 4.70996617 +v -3.86635821 6.99562104 4.70428710 +v 7.87627282 -4.43532761 11.05140915 +v 5.15111169 -2.50918618 29.19889183 +v 5.21323338 -5.31973481 21.52245554 +v 7.23443379 -5.57507698 7.08196652 +v 3.55631959 -7.25167066 13.72878573 +v 3.07783375 -6.06397783 22.26222936 +v 6.79721197 0.78554179 28.07666368 +v 7.34199933 -2.39018025 21.29567778 +v 8.46922360 -2.75276442 10.83537109 +v 5.70007033 -6.65083634 12.11134503 +v 6.65417384 -4.34381538 20.30772765 +v 2.40619519 -4.04642312 29.14441797 +v -3.14414705 -7.26638904 13.85199291 +v -2.63835983 -6.00383154 22.58289427 +v -3.00959200 -4.30725036 28.45014698 +v -8.67648316 -1.39026155 6.96999792 +v -4.06466345 -7.30215421 7.07949375 +v -6.28665038 -6.40156394 7.07810144 +v -6.54392136 -6.18002794 10.50626076 +v -8.30555380 -3.95818407 7.11847429 +v 3.99831418 -7.31980111 7.07815211 +v 8.55120535 -1.75383243 6.99438213 +v 8.56780262 0.94971194 7.19002689 +v 8.43784203 -0.11071443 11.68166065 +v 5.95701350 -2.93345893 8.95484023 +v 5.48357694 -5.40361293 7.09865875 +v 2.51696542 -1.82616354 31.30618217 +v 5.23352468 -0.09383099 30.37120968 +v 2.54165736 -0.08445142 31.87502021 +v -2.07591824 -2.80877817 30.61779698 +v -2.68516470 -0.30738259 31.87551034 +v -5.88866912 -2.35051278 27.97823005 +v -5.61433545 -0.28582589 30.06682621 +v -6.88747168 -0.60927090 26.91366651 +v -4.52763552 -2.99362370 29.28895007 +v -5.12471544 -6.68446634 14.74402446 +v -7.19221015 -4.66949421 16.33102903 +v -8.04709503 -2.58759069 16.47269914 +v -5.09882985 -6.05347300 19.08687005 +v -8.46397644 -0.09998855 12.28854257 +v -5.96767427 -3.59595155 8.64092197 +v -5.81328100 -5.28758722 7.07790139 +v -6.27336927 1.27569430 9.39815533 +v 5.02206109 4.76332516 29.37171859 +v 4.15314519 5.23168516 21.95940541 +v 5.17313368 5.59165002 26.74885525 +v 8.06735094 4.13382332 5.94027701 +v 1.84103550 0.11588066 9.57800292 +v -5.10987421 -1.53844923 9.52260374 +v -4.00466120 5.32536649 21.27395753 +v -5.14488595 5.62968259 26.54913621 +v -4.93618877 4.82608602 29.39774940 +v 0.13861477 5.60451047 30.84654114 +v 5.17690385 4.16565385 29.34090177 +v -2.33512516 5.71259775 31.51929105 +v 3.02924934 5.52483519 31.19856340 +v 0.33599790 6.02786602 31.69972194 +v 8.40308173 2.37008374 12.43632136 +v -6.67781474 2.44604496 28.40686319 +v -7.92231657 2.23071969 18.08938295 +v -8.67465140 2.37217143 6.62526268 +v -8.12663140 4.10835071 5.90795101 +v -6.58245892 3.80002568 8.12575754 +v 1.92380381 5.90630332 19.63364340 +v -1.94537491 5.85504527 19.75550715 +v 6.24046765 4.00344013 27.68537316 +v -4.76083302 0.19773252 9.56727057 +v -4.75478027 3.99592541 8.50326258 +v -4.75356295 5.97374027 6.11144414 +v -6.19869154 4.20600270 27.39431646 +v -5.20052903 4.17754391 29.34088490 +v 0.01018935 4.18887797 30.19189119 +v 7.34531069 4.04378806 18.31192060 +v 2.64767041 4.15626461 31.19604383 +v 2.91940624 1.81677046 31.93396132 +v 5.78947517 2.14245583 29.91917121 +v -1.87245751 4.05085463 31.50866574 +v -4.49162410 2.16490945 31.16573852 +v -7.42203042 4.05853555 17.84804552 +v 7.29060088 5.35550655 5.82194916 +v 6.19559178 6.24745900 5.92513728 +v -3.40716650 6.29775703 6.47339043 +v 3.31059343 6.36237014 6.86894461 +v -6.35243334 6.14850129 6.12121855 +v -6.78456749 5.15932446 15.32211577 +v -4.05237357 5.34508952 18.89352070 +v -4.05927069 5.50641088 11.20495569 +v -4.23292864 4.98830910 8.61492912 +v -3.66587380 6.49086575 7.52061777 +v 4.13979503 5.18578137 8.51712956 +v 4.05000000 4.33266300 8.79873500 +v 4.06449151 5.74468123 11.12190375 +v 4.05098944 5.34622564 18.89279302 +v 2.60006316 5.62573869 11.89468890 +v 2.88494951 6.45245533 7.52121533 +v -2.33032286 6.21437152 7.68457736 +v -1.89095108 5.87781213 8.80650730 +v -2.41607436 5.75672005 11.75714274 +v -3.08457284 6.61379088 11.24407430 +v 2.84135055 6.51445336 11.33588395 +v 1.87137543 5.90276285 8.81500425 +f 1 2 3 +f 1 3 4 +f 3 5 6 +f 8 9 7 +f 7 9 10 +f 18 19 17 +f 12 22 16 +f 13 12 16 +f 13 20 21 +f 21 12 13 +f 14 11 20 +f 20 11 21 +f 20 13 15 +f 16 15 13 +f 21 11 18 +f 18 11 19 +f 21 18 12 +f 12 18 17 +f 23 15 16 +f 23 16 24 +f 24 16 25 +f 25 16 22 +f 27 29 28 +f 29 30 28 +f 27 31 23 +f 23 31 15 +f 32 19 14 +f 14 19 11 +f 14 20 31 +f 15 31 20 +f 33 34 19 +f 33 19 32 +f 1 33 35 +f 35 33 32 +f 35 32 36 +f 35 2 1 +f 19 34 17 +f 12 38 37 +f 37 38 39 +f 37 22 12 +f 17 38 12 +f 25 22 40 +f 40 22 37 +f 39 41 37 +f 37 41 40 +f 25 40 45 +f 43 44 42 +f 42 45 43 +f 41 43 45 +f 41 45 40 +f 27 23 46 +f 30 48 26 +f 29 27 46 +f 49 23 24 +f 49 46 23 +f 47 29 49 +f 24 25 49 +f 46 49 29 +f 29 47 30 +f 47 42 44 +f 47 44 48 +f 25 45 49 +f 49 45 42 +f 49 42 47 +f 47 48 30 +f 50 48 44 +f 50 26 48 +f 26 51 52 +f 51 26 53 +f 36 14 31 +f 28 52 27 +f 36 31 52 +f 52 31 27 +f 14 36 32 +f 30 26 52 +f 30 52 28 +f 54 56 55 +f 33 1 4 +f 33 4 57 +f 59 58 2 +f 36 52 51 +f 36 51 35 +f 35 51 59 +f 35 59 2 +f 5 3 2 +f 51 53 59 +f 65 63 66 +f 66 63 64 +f 68 17 34 +f 70 71 50 +f 70 50 44 +f 70 44 69 +f 53 26 71 +f 72 73 71 +f 71 73 53 +f 74 67 66 +f 54 55 66 +f 66 55 74 +f 67 74 75 +f 67 75 65 +f 65 75 60 +f 60 62 65 +f 68 34 33 +f 77 58 59 +f 58 77 79 +f 2 58 5 +f 5 58 79 +f 5 79 6 +f 59 53 77 +f 77 53 78 +f 53 73 78 +f 60 61 62 +f 65 62 81 +f 63 65 81 +f 82 63 81 +f 63 82 64 +f 67 65 66 +f 66 64 54 +f 54 64 76 +f 17 68 83 +f 17 83 76 +f 68 57 83 +f 84 86 64 +f 84 85 86 +f 86 76 64 +f 86 17 76 +f 38 86 85 +f 38 85 39 +f 17 86 38 +f 41 39 85 +f 41 85 87 +f 87 85 84 +f 81 88 87 +f 41 87 88 +f 41 88 43 +f 69 44 43 +f 88 81 69 +f 88 69 43 +f 69 81 80 +f 69 80 89 +f 69 89 70 +f 71 70 89 +f 71 89 72 +f 26 50 71 +f 60 7 61 +f 55 56 8 +f 54 76 56 +f 90 57 4 +f 57 68 33 +f 4 6 91 +f 4 91 90 +f 6 4 3 +f 76 83 56 +f 8 91 9 +f 91 8 56 +f 91 56 90 +f 90 56 83 +f 90 83 57 +f 79 77 78 +f 92 6 93 +f 93 6 79 +f 93 79 92 +f 73 72 94 +f 73 94 78 +f 92 10 6 +f 6 10 9 +f 10 92 94 +f 79 78 94 +f 92 79 94 +f 91 6 9 +f 7 60 75 +f 74 8 7 +f 74 7 75 +f 55 8 74 +f 62 61 80 +f 80 81 62 +f 84 82 87 +f 82 84 64 +f 82 81 87 +f 7 10 94 +f 80 61 95 +f 94 72 95 +f 95 72 89 +f 89 80 95 +f 95 61 94 +f 94 61 7 +f 97 96 98 +f 97 98 99 +f 102 100 103 +f 102 103 104 +f 105 101 100 +f 101 105 98 +f 98 105 106 +f 98 106 99 +f 107 108 106 +f 106 108 109 +f 106 109 99 +f 97 99 109 +f 100 102 105 +f 105 102 110 +f 104 111 105 +f 104 105 110 +f 96 97 108 +f 102 104 110 +f 105 111 106 +f 111 107 106 +f 108 97 109 +f 101 98 107 +f 107 111 101 +f 101 111 100 +f 108 98 96 +f 98 108 107 +f 104 103 100 +f 100 111 104 diff --git a/mjx/mujoco/mjx/test_data/shadow_hand/assets/th_distal_pst_190.obj b/mjx/mujoco/mjx/test_data/shadow_hand/assets/th_distal_pst_190.obj new file mode 100644 index 00000000..64fcd08b --- /dev/null +++ b/mjx/mujoco/mjx/test_data/shadow_hand/assets/th_distal_pst_190.obj @@ -0,0 +1,288 @@ +# https://github.com/mikedh/trimesh +v -3.87874325 8.97064419 20.09696708 +v 3.33570154 8.98693768 20.00768455 +v 3.97136533 8.99677039 3.41064184 +v -4.40959718 8.98399823 3.59941665 +v 5.04763382 5.62557226 9.36068667 +v -5.02436772 5.66797804 9.33833176 +v 8.42356732 7.11834272 5.65561215 +v 7.16907807 4.98106463 9.48168643 +v 6.93022084 7.11209326 7.67445153 +v 6.00313716 8.54341758 5.23598706 +v -7.17607178 4.97689550 9.48109597 +v -7.53652139 7.70583276 6.21388495 +v -6.33457230 8.38034856 5.54912333 +v -6.36194547 7.11256003 7.72303997 +v -7.41801011 -7.02747341 9.48715299 +v -9.00250133 -6.21476019 10.39855164 +v -10.56978774 -2.49716127 10.09559906 +v -10.62194494 2.32975955 10.22490268 +v -10.00924604 1.94777627 9.50486470 +v -9.28319438 -4.73784570 9.49027691 +v 7.15147452 -7.02207443 9.48858316 +v -9.34805732 1.62432463 28.12775447 +v -8.41997159 3.26874043 30.22096769 +v -8.38566584 0.49128714 30.36535708 +v -9.99533054 -1.50299841 20.01519589 +v -10.12283126 3.11061820 17.96099483 +v -5.02698431 -8.76602969 11.28385625 +v 0.11482147 2.98923697 34.94012405 +v 3.25105044 2.19295301 34.54920903 +v 5.77625527 2.34301591 33.22697054 +v 7.01215516 0.22222499 31.98819736 +v 8.68817366 2.25705075 30.03040785 +v -5.94953357 1.96627038 33.13643548 +v -3.10200136 1.12817921 34.55661511 +v -6.94802143 -2.98867327 29.90330874 +v -3.47954158 -4.98943428 29.95337100 +v -5.59633128 -1.82989710 32.29260356 +v -8.67271027 -1.94975485 27.24043087 +v 7.14653336 -7.76840294 10.18077314 +v 10.51833151 -2.80994991 10.10868653 +v 10.54668917 3.02343530 9.94316907 +v 9.44180322 -4.52842458 9.49224422 +v 9.97350136 4.97225815 9.53086440 +v 9.38518238 2.54311294 28.25864799 +v -3.66496455 -8.34115937 17.12281688 +v -3.21069565 -6.86293492 24.40581797 +v -7.62899787 -6.84366752 15.55638216 +v -9.34593926 -4.80200843 15.86489224 +v -5.95568387 -5.51044178 25.65788219 +v -7.53474573 -5.29387205 22.74512105 +v -5.18778033 -7.66186405 19.12469359 +v 3.78124546 -8.84686073 11.50506732 +v 3.09549482 -2.18508034 33.34215978 +v 0.78768397 -0.02478223 34.72533610 +v 2.68990799 -4.86100483 30.49338459 +v -1.41877649 -2.77285241 33.25112683 +v 6.19646739 -2.41296064 31.29605596 +v 4.73049801 -4.38124345 30.16120258 +v 9.75367743 -4.99253587 10.49735095 +v 10.10181125 -1.60374544 18.71064990 +v 10.13395072 3.11384449 17.82424622 +v 7.29850531 -2.94041893 29.32535499 +v 8.74400152 -0.63822020 28.98680478 +v 3.18013135 -6.80211841 24.62651460 +v 4.32813053 -8.27393555 17.35176564 +v 8.80732741 -4.05818076 21.89903631 +v 8.24643786 -6.12345471 16.96933119 +v 5.84426791 -6.26782767 23.59689283 +v 7.25462912 5.25085258 30.81490723 +v -7.00354422 5.23761054 31.02656238 +v 8.53569245 4.96773714 28.52913924 +v -1.81751594 5.12508889 34.07456292 +v -5.53904331 5.06936882 32.51863349 +v 3.49520113 5.05850645 33.65201689 +v -8.80327393 4.92438086 27.71651001 +v -9.86364913 4.99133321 9.63484711 +v 9.92455642 4.99944390 5.81479466 +v -10.02391542 5.04284006 5.60395859 +v 0.45968813 7.52595152 19.96784167 +v 4.00318133 7.10315704 20.34590709 +v 5.55947365 8.34241193 21.24125621 +v 6.52790384 6.33851034 22.92923670 +v 7.37650233 7.00282503 24.49933456 +v -6.71762637 7.61275679 22.70424477 +v -6.27464471 6.43274232 22.14945120 +v -3.25255024 7.24845029 20.11592765 +v 1.91096432 7.43638768 34.84101849 +v 5.17432765 6.75352949 33.87613774 +v 7.52432026 5.93372585 30.18058739 +v -3.24835546 7.28503241 34.62798014 +v -7.16568535 5.90046745 31.83913660 +v -7.54060967 6.36598267 28.22611851 +v -8.67169909 6.20329766 15.65355212 +v 4.56589868 6.56222219 34.44826307 +v -4.37970367 6.60081864 34.41987413 +v 3.35922860 6.54988825 33.55138553 +v -1.58205973 6.84531224 34.12029778 +f 2 3 1 +f 1 3 4 +f 10 7 9 +f 14 6 11 +f 11 12 14 +f 14 12 13 +f 6 14 5 +f 4 3 10 +f 8 5 9 +f 9 5 14 +f 9 14 10 +f 10 14 13 +f 10 13 4 +f 17 18 19 +f 19 20 17 +f 25 26 17 +f 17 26 18 +f 22 26 25 +f 17 20 16 +f 16 20 15 +f 16 15 27 +f 24 23 22 +f 22 38 24 +f 24 35 37 +f 24 38 35 +f 36 37 35 +f 39 27 15 +f 15 21 39 +f 42 43 41 +f 42 41 40 +f 16 27 47 +f 16 47 48 +f 16 48 17 +f 22 25 38 +f 49 35 50 +f 50 35 38 +f 46 36 49 +f 49 36 35 +f 50 48 47 +f 50 47 51 +f 25 17 48 +f 47 27 51 +f 51 27 45 +f 49 50 51 +f 49 51 46 +f 38 25 48 +f 38 48 50 +f 46 51 45 +f 52 27 39 +f 53 56 55 +f 54 29 28 +f 53 58 57 +f 29 54 53 +f 29 53 31 +f 31 53 57 +f 54 56 53 +f 55 58 53 +f 30 29 31 +f 24 37 33 +f 54 28 34 +f 36 56 37 +f 55 56 36 +f 56 54 34 +f 34 33 37 +f 34 37 56 +f 40 59 42 +f 42 59 39 +f 42 39 21 +f 40 41 60 +f 60 41 61 +f 61 44 60 +f 31 57 62 +f 58 62 57 +f 62 63 31 +f 32 31 63 +f 44 32 63 +f 64 55 36 +f 64 36 46 +f 64 46 65 +f 65 46 45 +f 27 52 45 +f 45 52 65 +f 62 66 63 +f 59 67 39 +f 39 65 52 +f 62 58 68 +f 68 58 55 +f 67 66 68 +f 65 68 64 +f 60 44 63 +f 60 63 66 +f 66 62 68 +f 64 68 55 +f 40 60 59 +f 59 60 66 +f 59 66 67 +f 39 67 65 +f 65 67 68 +f 30 32 69 +f 31 32 30 +f 29 74 28 +f 28 74 72 +f 30 69 74 +f 30 74 29 +f 28 72 34 +f 34 72 73 +f 34 73 33 +f 24 33 23 +f 23 33 73 +f 23 73 70 +f 75 23 70 +f 75 22 23 +f 76 18 26 +f 76 26 75 +f 26 22 75 +f 8 43 42 +f 8 42 21 +f 15 8 21 +f 11 19 76 +f 11 5 8 +f 11 6 5 +f 11 8 15 +f 11 15 20 +f 11 20 19 +f 76 19 18 +f 43 61 41 +f 44 61 43 +f 44 43 71 +f 32 44 71 +f 32 71 69 +f 77 43 8 +f 76 78 11 +f 79 80 2 +f 2 80 81 +f 81 80 82 +f 81 82 83 +f 84 85 1 +f 1 85 86 +f 79 87 80 +f 89 82 88 +f 87 88 80 +f 80 88 82 +f 87 79 90 +f 90 79 86 +f 85 91 90 +f 90 86 85 +f 89 69 71 +f 83 82 89 +f 7 77 9 +f 77 8 9 +f 1 86 79 +f 1 79 2 +f 10 81 7 +f 43 77 7 +f 83 89 71 +f 83 71 43 +f 3 2 10 +f 10 2 81 +f 7 81 83 +f 83 43 7 +f 92 85 84 +f 70 92 75 +f 78 12 11 +f 93 92 84 +f 93 76 75 +f 76 93 78 +f 93 12 78 +f 1 4 13 +f 1 13 12 +f 1 12 84 +f 84 12 93 +f 93 75 92 +f 94 88 87 +f 91 95 90 +f 94 89 88 +f 69 89 94 +f 96 94 97 +f 97 94 95 +f 97 95 91 +f 90 95 87 +f 95 94 87 +f 72 97 73 +f 73 97 70 +f 74 97 72 +f 74 96 97 +f 74 69 96 +f 85 92 91 +f 70 91 92 +f 91 70 97 +f 94 96 69 diff --git a/mjx/mujoco/mjx/test_data/shadow_hand/right_hand.xml b/mjx/mujoco/mjx/test_data/shadow_hand/right_hand.xml index a5907d8b..13bc5435 100644 --- a/mjx/mujoco/mjx/test_data/shadow_hand/right_hand.xml +++ b/mjx/mujoco/mjx/test_data/shadow_hand/right_hand.xml @@ -1,7 +1,7 @@ - @@ -74,7 +74,7 @@ - + @@ -94,10 +94,12 @@ + + @@ -105,16 +107,16 @@ - + - - - + + + - + - + - + - + @@ -163,23 +165,23 @@ - + - + - + - + @@ -188,23 +190,23 @@ - + - + - + - + @@ -218,23 +220,23 @@ - + - + - + - + @@ -248,23 +250,23 @@ - + - + - + - + diff --git a/mjx/mujoco/mjx/viewer.py b/mjx/mujoco/mjx/viewer.py index c29357eb..c4d7c607 100644 --- a/mjx/mujoco/mjx/viewer.py +++ b/mjx/mujoco/mjx/viewer.py @@ -52,7 +52,6 @@ def _main(argv: Sequence[str]) -> None: while True: start = time.time() - # TODO(robotics-simulation): debug xfrc_applied sometimes causing NaN # TODO(robotics-simulation): recompile when changing disable flags, etc. dx = dx.replace(ctrl=d.ctrl, xfrc_applied=d.xfrc_applied) dx = dx.replace(qpos=d.qpos, qvel=d.qvel, time=d.time) # handle resets