chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Speed up convex-convex collisions.

Browse Source 
PiperOrigin-RevId: 613450874
Change-Id: Ie66e0a078e2ff51a136b949dd9459847e1fe957d
This commit is contained in:
Baruch Tabanpour
2024-03-06 21:47:47 -08:00
committed by Copybara-Service
parent 106453fa2b
commit 22cd96fbbc
14 changed files with 1303 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mjx/mujoco/mjx/_src/collision_base.py
+2
View File
@@ -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):
mjx/mujoco/mjx/_src/collision_convex.py
+376 -173
View File
@@ -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
mjx/mujoco/mjx/_src/collision_driver.py
+47 -35
View File
@@ -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(
mjx/mujoco/mjx/_src/collision_driver_test.py
+28 -7
View File
@@ -371,15 +371,14 @@ class ConvexTest(absltest.TestCase):
_CONVEX_CONVEX = """
<mujoco>
<asset>
<mesh name="tetrahedron" file="meshes/tetrahedron.stl" scale="0.1 0.1 0.1" />
<mesh name="dodecahedron" file="meshes/dodecahedron.stl" scale="0.01 0.01 0.01" />
</asset>
<worldbody>
<body pos="0.0 2.0 0.096">
<joint axis="1 0 0" type="free"/>
<geom size="0.2 0.2 0.2" type="mesh" mesh="tetrahedron"/>
<geom size="0.2 0.2 0.2" type="mesh" mesh="dodecahedron"/>
</body>
<body pos="0.0 2.0 0.289" euler="0.1 -0.1 45">
<body pos="0.0 2.0 0.281" euler="0.1 -0.1 45">
<joint axis="1 0 0" type="free"/>
<geom size="0.1 0.1 0.1" type="mesh" mesh="dodecahedron"/>
</body>
@@ -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."""
mjx/mujoco/mjx/_src/io_test.py
+1 -1
View File
@@ -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)
mjx/mujoco/mjx/_src/mesh.py
+131 -23
View File
@@ -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}
mjx/mujoco/mjx/_src/mesh_test.py
+60 -3
View File
@@ -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]])
)
mjx/mujoco/mjx/_src/types.py
+6 -2
View File
@@ -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
mjx/mujoco/mjx/test_data/shadow_hand/assets/f_distal_pst_214.obj
+326
View File
@@ -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
mjx/mujoco/mjx/test_data/shadow_hand/assets/th_distal_pst_190.obj
+288
View File
@@ -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
mjx/mujoco/mjx/test_data/shadow_hand/right_hand.xml
+28 -26
View File
@@ -1,7 +1,7 @@
<mujoco model="right_shadow_hand">
<compiler angle="radian" meshdir="assets" autolimits="true"/>
<option impratio="10" iterations="1" ls_iterations="4">
<option impratio="10" iterations="1" ls_iterations="4" timestep="0.001">
<flag eulerdamp="disable"/>
</option>
@@ -74,7 +74,7 @@
<geom type="mesh" material="black" contype="0" conaffinity="0" group="2"/>
</default>
<default class="plastic_collision">
<geom group="3" contype="0" conaffinity="1"/>
<geom group="3" contype="1" conaffinity="1"/>
</default>
</default>
</default>
@@ -94,10 +94,12 @@
<mesh class="right_hand" file="f_proximal.obj"/>
<mesh class="right_hand" file="f_middle.obj"/>
<mesh class="right_hand" file="f_distal_pst.obj"/>
<mesh class="right_hand" file="f_distal_pst_214.obj"/>
<mesh class="right_hand" file="lf_metacarpal.obj"/>
<mesh class="right_hand" file="th_proximal.obj"/>
<mesh class="right_hand" file="th_middle.obj"/>
<mesh class="right_hand" file="th_distal_pst.obj"/>
<mesh class="right_hand" file="th_distal_pst_190.obj"/>
</asset>
<worldbody>
@@ -105,16 +107,16 @@
<inertial mass="3" pos="0 0 0.09" diaginertia="0.0138 0.0138 0.00744"/>
<geom class="plastic_visual" mesh="forearm_0" material="gray"/>
<geom class="plastic_visual" mesh="forearm_1"/>
<geom class="plastic_collision" type="mesh" mesh="forearm_collision" conaffinity="0"/>
<geom class="plastic_collision" type="mesh" mesh="forearm_collision" conaffinity="0" contype="0"/>
<geom class="plastic_collision" size="0.035 0.035 0.035" pos="0 -0.01 0.181" quat="0.924909 0 0.380188 0"
type="box" conaffinity="0"/>
<body name="rh_wrist" pos="0 -0.01 0.21301">
<inertial mass="0.1" pos="0 0 0.029" quat="0.5 0.5 0.5 0.5" diaginertia="6.4e-05 4.38e-05 3.5e-05"/>
<joint class="wrist_y" name="rh_WRJ2"/>
<geom class="plastic_visual" mesh="wrist" material="metallic"/>
<geom size="0.0135 0.015" quat="0.499998 0.5 0.5 -0.500002" type="cylinder" class="plastic_collision" conaffinity="0"/>
<geom size="0.011 0.005" pos="-0.026 0 0.034" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0"/>
<geom size="0.011 0.005" pos="0.031 0 0.034" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0"/>
<geom size="0.0135 0.015" quat="0.499998 0.5 0.5 -0.500002" type="cylinder" class="plastic_collision" conaffinity="0" contype="0"/>
<geom size="0.011 0.005" pos="-0.026 0 0.034" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0" contype="0"/>
<geom size="0.011 0.005" pos="0.031 0 0.034" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0" contype="0"/>
<geom size="0.0135 0.009 0.005" pos="-0.021 0 0.011" quat="0.923879 0 0.382684 0" type="box"
class="plastic_collision" conaffinity="0"/>
<geom size="0.0135 0.009 0.005" pos="0.026 0 0.01" quat="0.923879 0 -0.382684 0" type="box"
@@ -138,23 +140,23 @@
<inertial mass="0.008" pos="0 0 0" quat="0.5 0.5 -0.5 0.5" diaginertia="3.2e-07 2.6e-07 2.6e-07"/>
<joint name="rh_FFJ4" class="knuckle"/>
<geom pos="0 0 0.0005" class="plastic_visual" mesh="f_knuckle" material="metallic"/>
<geom size="0.009 0.009" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0"/>
<geom size="0.009 0.009" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0" contype="0"/>
<body name="rh_ffproximal">
<inertial mass="0.03" pos="0 0 0.0225" quat="1 0 0 1" diaginertia="1e-05 9.8e-06 1.8e-06"/>
<joint name="rh_FFJ3" class="proximal"/>
<geom class="plastic_visual" mesh="f_proximal"/>
<geom size="0.009 0.02" pos="0 0 0.025" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.02" pos="0 0 0.025" type="capsule" class="plastic_collision"/>
<body name="rh_ffmiddle" pos="0 0 0.045">
<inertial mass="0.017" pos="0 0 0.0125" quat="1 0 0 1" diaginertia="2.7e-06 2.6e-06 8.7e-07"/>
<joint name="rh_FFJ2" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_middle"/>
<geom size="0.009 0.0125" pos="0 0 0.0125" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.0125" pos="0 0 0.0125" type="capsule" class="plastic_collision"/>
<body name="rh_ffdistal" pos="0 0 0.025">
<inertial mass="0.013" pos="0 0 0.0130769" quat="1 0 0 1"
diaginertia="1.28092e-06 1.12092e-06 5.3e-07"/>
<joint name="rh_FFJ1" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214"/>
</body>
</body>
</body>
@@ -163,23 +165,23 @@
<inertial mass="0.008" pos="0 0 0" quat="0.5 0.5 -0.5 0.5" diaginertia="3.2e-07 2.6e-07 2.6e-07"/>
<joint name="rh_MFJ4" class="knuckle"/>
<geom pos="0 0 0.0005" class="plastic_visual" mesh="f_knuckle" material="metallic"/>
<geom size="0.009 0.009" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0"/>
<geom size="0.009 0.009" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0" contype="0"/>
<body name="rh_mfproximal">
<inertial mass="0.03" pos="0 0 0.0225" quat="1 0 0 1" diaginertia="1e-05 9.8e-06 1.8e-06"/>
<joint name="rh_MFJ3" class="proximal"/>
<geom class="plastic_visual" mesh="f_proximal"/>
<geom size="0.009 0.02" pos="0 0 0.025" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.02" pos="0 0 0.025" type="capsule" class="plastic_collision"/>
<body name="rh_mfmiddle" pos="0 0 0.045">
<inertial mass="0.017" pos="0 0 0.0125" quat="1 0 0 1" diaginertia="2.7e-06 2.6e-06 8.7e-07"/>
<joint name="rh_MFJ2" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_middle"/>
<geom size="0.009 0.0125" pos="0 0 0.0125" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.0125" pos="0 0 0.0125" type="capsule" class="plastic_collision"/>
<body name="rh_mfdistal" pos="0 0 0.025">
<inertial mass="0.013" pos="0 0 0.0130769" quat="1 0 0 1"
diaginertia="1.28092e-06 1.12092e-06 5.3e-07"/>
<joint name="rh_MFJ1" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214"/>
</body>
</body>
</body>
@@ -188,23 +190,23 @@
<inertial mass="0.008" pos="0 0 0" quat="0.5 0.5 -0.5 0.5" diaginertia="3.2e-07 2.6e-07 2.6e-07"/>
<joint name="rh_RFJ4" class="knuckle" axis="0 1 0"/>
<geom pos="0 0 0.0005" class="plastic_visual" mesh="f_knuckle" material="metallic"/>
<geom size="0.009 0.009" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0"/>
<geom size="0.009 0.009" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0" contype="0"/>
<body name="rh_rfproximal">
<inertial mass="0.03" pos="0 0 0.0225" quat="1 0 0 1" diaginertia="1e-05 9.8e-06 1.8e-06"/>
<joint name="rh_RFJ3" class="proximal"/>
<geom class="plastic_visual" mesh="f_proximal"/>
<geom size="0.009 0.02" pos="0 0 0.025" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.02" pos="0 0 0.025" type="capsule" class="plastic_collision"/>
<body name="rh_rfmiddle" pos="0 0 0.045">
<inertial mass="0.017" pos="0 0 0.0125" quat="1 0 0 1" diaginertia="2.7e-06 2.6e-06 8.7e-07"/>
<joint name="rh_RFJ2" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_middle"/>
<geom size="0.009 0.0125" pos="0 0 0.0125" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.0125" pos="0 0 0.0125" type="capsule" class="plastic_collision"/>
<body name="rh_rfdistal" pos="0 0 0.025">
<inertial mass="0.013" pos="0 0 0.0130769" quat="1 0 0 1"
diaginertia="1.28092e-06 1.12092e-06 5.3e-07"/>
<joint name="rh_RFJ1" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214"/>
</body>
</body>
</body>
@@ -218,23 +220,23 @@
<inertial mass="0.008" pos="0 0 0" quat="0.5 0.5 -0.5 0.5" diaginertia="3.2e-07 2.6e-07 2.6e-07"/>
<joint name="rh_LFJ4" class="knuckle" axis="0 1 0"/>
<geom pos="0 0 0.0005" class="plastic_visual" mesh="f_knuckle" material="metallic"/>
<geom size="0.009 0.009" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0"/>
<geom size="0.009 0.009" quat="1 0 1 0" type="cylinder" class="plastic_collision" conaffinity="0" contype="0"/>
<body name="rh_lfproximal">
<inertial mass="0.03" pos="0 0 0.0225" quat="1 0 0 1" diaginertia="1e-05 9.8e-06 1.8e-06"/>
<joint name="rh_LFJ3" class="proximal"/>
<geom class="plastic_visual" mesh="f_proximal"/>
<geom size="0.009 0.02" pos="0 0 0.025" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.02" pos="0 0 0.025" type="capsule" class="plastic_collision"/>
<body name="rh_lfmiddle" pos="0 0 0.045">
<inertial mass="0.017" pos="0 0 0.0125" quat="1 0 0 1" diaginertia="2.7e-06 2.6e-06 8.7e-07"/>
<joint name="rh_LFJ2" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_middle"/>
<geom size="0.009 0.0125" pos="0 0 0.0125" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.0125" pos="0 0 0.0125" type="capsule" class="plastic_collision"/>
<body name="rh_lfdistal" pos="0 0 0.025">
<inertial mass="0.013" pos="0 0 0.0130769" quat="1 0 0 1"
diaginertia="1.28092e-06 1.12092e-06 5.3e-07"/>
<joint name="rh_LFJ1" class="middle_distal"/>
<geom class="plastic_visual" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="f_distal_pst_214"/>
</body>
</body>
</body>
@@ -248,23 +250,23 @@
<inertial mass="0.04" pos="0 0 0.019" diaginertia="1.36e-05 1.36e-05 3.13e-06"/>
<joint name="rh_THJ4" class="thproximal"/>
<geom class="plastic_visual" mesh="th_proximal"/>
<geom class="plastic_collision" size="0.0105 0.009" pos="0 0 0.02" type="capsule" contype="1"/>
<geom class="plastic_collision" size="0.0105 0.009" pos="0 0 0.02" type="capsule"/>
<body name="rh_thhub" pos="0 0 0.038">
<inertial mass="0.005" pos="0 0 0" diaginertia="1e-06 1e-06 3e-07"/>
<joint name="rh_THJ3" class="thhub"/>
<geom size="0.011" class="plastic_collision" contype="1"/>
<geom size="0.011" class="plastic_collision"/>
<body name="rh_thmiddle">
<inertial mass="0.02" pos="0 0 0.016" diaginertia="5.1e-06 5.1e-06 1.21e-06"/>
<joint name="rh_THJ2" class="thmiddle"/>
<geom class="plastic_visual" mesh="th_middle"/>
<geom size="0.009 0.009" pos="0 0 0.012" type="capsule" class="plastic_collision" contype="1"/>
<geom size="0.009 0.009" pos="0 0 0.012" type="capsule" class="plastic_collision"/>
<geom size="0.01" pos="0 0 0.03" class="plastic_collision"/>
<body name="rh_thdistal" pos="0 0 0.032" quat="1 0 0 -1">
<inertial mass="0.017" pos="0 0 0.0145588" quat="1 0 0 1"
diaginertia="2.37794e-06 2.27794e-06 1e-06"/>
<joint name="rh_THJ1" class="thdistal"/>
<geom class="plastic_visual" mesh="th_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="th_distal_pst"/>
<geom class="plastic_collision" type="mesh" mesh="th_distal_pst_190"/>
</body>
</body>
</body>
mjx/mujoco/mjx/viewer.py
-1
View File
@@ -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