Speed up convex-convex collisions.
PiperOrigin-RevId: 613450874 Change-Id: Ie66e0a078e2ff51a136b949dd9459847e1fe957d
This commit is contained in:
Baruch Tabanpour
committed by
Copybara-Service
Copybara-Service
parent
106453fa2b
commit
22cd96fbbc
@@ -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)
|
||||
-----------------------------------
|
||||
|
||||
|
||||
+2
-2
@@ -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 <https://github.com/google-deepmind/mujoco/tree/main/mjx/mujoco/mjx/benchmark/model/shadow_hand>`__
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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(
|
||||
|
||||
@@ -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."""
|
||||
|
||||
@@ -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)
|
||||
|
||||
+131
-23
@@ -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}
|
||||
|
||||
|
||||
@@ -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]])
|
||||
)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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>
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user