warp-lang 0.11.0__py3-none-manylinux2014_x86_64.whl → 1.0.0__py3-none-manylinux2014_x86_64.whl

warp/sim/import_mjcf.py

@@ -25,8 +25,10 @@ def parse_mjcf(
     contact_ke=1000.0,
     contact_kd=100.0,
     contact_kf=100.0,
+    contact_ka=0.0,
     contact_mu=0.5,
     contact_restitution=0.5,
+    contact_thickness=0.0,
     limit_ke=100.0,
     limit_kd=10.0,
     scale=1.0,
@@ -48,13 +50,15 @@ def parse_mjcf(
         density (float): The density of the shapes in kg/m^3 which will be used to calculate the body mass and inertia.
         stiffness (float): The stiffness of the joints.
         damping (float): The damping of the joints.
-        contact_ke (float): The stiffness of the shape contacts (used by SemiImplicitIntegrator).
-        contact_kd (float): The damping of the shape contacts (used by SemiImplicitIntegrator).
-        contact_kf (float): The friction stiffness of the shape contacts (used by SemiImplicitIntegrator).
+        contact_ke (float): The stiffness of the shape contacts.
+        contact_kd (float): The damping of the shape contacts.
+        contact_kf (float): The friction stiffness of the shape contacts.
+        contact_ka (float): The adhesion distance of the shape contacts.
         contact_mu (float): The friction coefficient of the shape contacts.
         contact_restitution (float): The restitution coefficient of the shape contacts.
-        limit_ke (float): The stiffness of the joint limits (used by SemiImplicitIntegrator).
-        limit_kd (float): The damping of the joint limits (used by SemiImplicitIntegrator).
+        contact_thickness (float): The thickness to add to the shape geometry.
+        limit_ke (float): The stiffness of the joint limits.
+        limit_kd (float): The damping of the joint limits.
         scale (float): The scaling factor to apply to the imported mechanism.
         armature (float): Default joint armature to use if `armature` has not been defined for a joint in the MJCF.
         armature_scale (float): Scaling factor to apply to the MJCF-defined joint armature values.
@@ -73,6 +77,16 @@ def parse_mjcf(
     file = ET.parse(mjcf_filename)
     root = file.getroot()
 
+    contact_vars = dict(
+        ke=contact_ke,
+        kd=contact_kd,
+        kf=contact_kf,
+        ka=contact_ka,
+        mu=contact_mu,
+        restitution=contact_restitution,
+        thickness=contact_thickness,
+    )
+
     use_degrees = True  # angles are in degrees by default
     euler_seq = [1, 2, 3]  # XYZ by default
 
@@ -265,7 +279,7 @@ def parse_mjcf(
                     joint_type = wp.sim.JOINT_FIXED
                     break
                 is_angular = joint_type_str == "hinge"
-                mode = wp.sim.JOINT_MODE_LIMIT
+                mode = wp.sim.JOINT_MODE_FORCE
                 if stiffness > 0.0 or "stiffness" in joint_attrib:
                     mode = wp.sim.JOINT_MODE_TARGET_POSITION
                 axis_vec = parse_vec(joint_attrib, "axis", (0.0, 0.0, 0.0))
@@ -355,11 +369,7 @@ def parse_mjcf(
                     rot=geom_rot,
                     radius=geom_size[0],
                     density=geom_density,
-                    ke=contact_ke,
-                    kd=contact_kd,
-                    kf=contact_kf,
-                    mu=contact_mu,
-                    restitution=contact_restitution,
+                    **contact_vars,
                 )
 
             elif geom_type == "box":
@@ -371,11 +381,7 @@ def parse_mjcf(
                     hy=geom_size[1],
                     hz=geom_size[2],
                     density=geom_density,
-                    ke=contact_ke,
-                    kd=contact_kd,
-                    kf=contact_kf,
-                    mu=contact_mu,
-                    restitution=contact_restitution,
+                    **contact_vars,
                 )
 
             elif geom_type == "mesh" and parse_meshes:
@@ -393,10 +399,7 @@ def parse_mjcf(
                     mesh=mesh,
                     scale=mesh_scale,
                     density=density,
-                    ke=contact_ke,
-                    kd=contact_kd,
-                    kf=contact_kf,
-                    mu=contact_mu,
+                    **contact_vars,
                 )
 
             elif geom_type in {"capsule", "cylinder"}:
@@ -431,12 +434,8 @@ def parse_mjcf(
                         radius=geom_radius,
                         half_height=geom_height,
                         density=density,
-                        ke=contact_ke,
-                        kd=contact_kd,
-                        kf=contact_kf,
-                        mu=contact_mu,
-                        restitution=contact_restitution,
                         up_axis=geom_up_axis,
+                        **contact_vars,
                     )
                 else:
                     builder.add_shape_capsule(
@@ -446,12 +445,8 @@ def parse_mjcf(
                         radius=geom_radius,
                         half_height=geom_height,
                         density=density,
-                        ke=contact_ke,
-                        kd=contact_kd,
-                        kf=contact_kf,
-                        mu=contact_mu,
-                        restitution=contact_restitution,
                         up_axis=geom_up_axis,
+                        **contact_vars,
                     )
 
             else:

warp/sim/import_urdf.py

@@ -14,6 +14,8 @@ import numpy as np
 import warp as wp
 from warp.sim.model import Mesh
 
+from typing import Union
+
 
 def parse_urdf(
     urdf_filename,
@@ -25,16 +27,20 @@ def parse_urdf(
     stiffness=100.0,
     damping=10.0,
     armature=0.0,
-    shape_ke=1.0e4,
-    shape_kd=1.0e3,
-    shape_kf=1.0e2,
-    shape_mu=0.25,
-    shape_restitution=0.5,
-    shape_thickness=0.0,
+    contact_ke=1.0e4,
+    contact_kd=1.0e3,
+    contact_kf=1.0e2,
+    contact_ka=0.0,
+    contact_mu=0.25,
+    contact_restitution=0.5,
+    contact_thickness=0.0,
     limit_ke=100.0,
     limit_kd=10.0,
+    joint_limit_lower=-1e6,
+    joint_limit_upper=1e6,
     scale=1.0,
     parse_visuals_as_colliders=False,
+    force_show_colliders=False,
     enable_self_collisions=True,
     ignore_inertial_definitions=True,
     ensure_nonstatic_links=True,
@@ -54,16 +60,20 @@ def parse_urdf(
         stiffness (float): The stiffness of the joints.
         damping (float): The damping of the joints.
         armature (float): The armature of the joints (bias to add to the inertia diagonals that may stabilize the simulation).
-        shape_ke (float): The stiffness of the shape contacts (used by SemiImplicitIntegrator).
-        shape_kd (float): The damping of the shape contacts (used by SemiImplicitIntegrator).
-        shape_kf (float): The friction stiffness of the shape contacts (used by SemiImplicitIntegrator).
-        shape_mu (float): The friction coefficient of the shape contacts.
-        shape_restitution (float): The restitution coefficient of the shape contacts.
-        shape_thickness (float): The thickness to add to the shape geometry.
-        limit_ke (float): The stiffness of the joint limits (used by SemiImplicitIntegrator).
-        limit_kd (float): The damping of the joint limits (used by SemiImplicitIntegrator).
+        contact_ke (float): The stiffness of the shape contacts (used by the Euler integrators).
+        contact_kd (float): The damping of the shape contacts (used by the Euler integrators).
+        contact_kf (float): The friction stiffness of the shape contacts (used by the Euler integrators).
+        contact_ka (float): The adhesion distance of the shape contacts (used by the Euler integrators).
+        contact_mu (float): The friction coefficient of the shape contacts.
+        contact_restitution (float): The restitution coefficient of the shape contacts.
+        contact_thickness (float): The thickness to add to the shape geometry.
+        limit_ke (float): The stiffness of the joint limits (used by the Euler integrators).
+        limit_kd (float): The damping of the joint limits (used by the Euler integrators).
+        joint_limit_lower (float): The default lower joint limit if not specified in the URDF.
+        joint_limit_upper (float): The default upper joint limit if not specified in the URDF.
         scale (float): The scaling factor to apply to the imported mechanism.
-        parse_visuals_as_colliders (bool): If True, the geometry defined under the `<visual>` tags is used for collision handling instead of the `<collision>` geometries.
+        parse_visuals_as_colliders (bool): If True, the geometry defined under the `<visual>` tags is used for collision handling instead of the `<collision>` geoemtries.
+        force_show_colliders (bool): If True, the collision shapes are always shown, even if there are visual shapes.
         enable_self_collisions (bool): If True, self-collisions are enabled.
         ignore_inertial_definitions (bool): If True, the inertial parameters defined in the URDF are ignored and the inertia is calculated from the shape geometry.
         ensure_nonstatic_links (bool): If True, links with zero mass are given a small mass (see `static_link_mass`) to ensure they are dynamic.
@@ -74,6 +84,16 @@ def parse_urdf(
     file = ET.parse(urdf_filename)
     root = file.getroot()
 
+    contact_vars = dict(
+        ke=contact_ke,
+        kd=contact_kd,
+        kf=contact_kf,
+        ka=contact_ka,
+        mu=contact_mu,
+        restitution=contact_restitution,
+        thickness=contact_thickness,
+    )
+
     def parse_transform(element):
         if element is None or element.find("origin") is None:
             return wp.transform()
@@ -84,21 +104,22 @@ def parse_urdf(
         rpy = [float(x) for x in rpy.split()]
         return wp.transform(xyz, wp.quat_rpy(*rpy))
 
-    def parse_shapes(link, collisions, density, incoming_xform=None):
+    def parse_shapes(link, geoms, density, incoming_xform=None, visible=True, just_visual=False):
+        shapes = []
         # add geometry
-        for collision in collisions:
-            geo = collision.find("geometry")
+        for geom_group in geoms:
+            geo = geom_group.find("geometry")
             if geo is None:
                 continue
 
-            tf = parse_transform(collision)
+            tf = parse_transform(geom_group)
             if incoming_xform is not None:
                 tf = incoming_xform * tf
 
             for box in geo.findall("box"):
                 size = box.get("size") or "1 1 1"
                 size = [float(x) for x in size.split()]
-                builder.add_shape_box(
+                s = builder.add_shape_box(
                     body=link,
                     pos=wp.vec3(tf.p),
                     rot=wp.quat(tf.q),
@@ -106,45 +127,39 @@ def parse_urdf(
                     hy=size[1] * 0.5 * scale,
                     hz=size[2] * 0.5 * scale,
                     density=density,
-                    ke=shape_ke,
-                    kd=shape_kd,
-                    kf=shape_kf,
-                    mu=shape_mu,
-                    restitution=shape_restitution,
-                    thickness=shape_thickness,
+                    is_visible=visible,
+                    has_ground_collision=not just_visual,
+                    **contact_vars,
                 )
+                shapes.append(s)
 
             for sphere in geo.findall("sphere"):
-                builder.add_shape_sphere(
+                s = builder.add_shape_sphere(
                     body=link,
                     pos=wp.vec3(tf.p),
                     rot=wp.quat(tf.q),
                     radius=float(sphere.get("radius") or "1") * scale,
                     density=density,
-                    ke=shape_ke,
-                    kd=shape_kd,
-                    kf=shape_kf,
-                    mu=shape_mu,
-                    restitution=shape_restitution,
-                    thickness=shape_thickness,
+                    is_visible=visible,
+                    has_ground_collision=not just_visual,
+                    **contact_vars,
                 )
+                shapes.append(s)
 
             for cylinder in geo.findall("cylinder"):
-                builder.add_shape_capsule(
+                s = builder.add_shape_capsule(
                     body=link,
                     pos=wp.vec3(tf.p),
                     rot=wp.quat(tf.q),
                     radius=float(cylinder.get("radius") or "1") * scale,
                     half_height=float(cylinder.get("length") or "1") * 0.5 * scale,
                     density=density,
-                    ke=shape_ke,
-                    kd=shape_kd,
-                    kf=shape_kf,
-                    mu=shape_mu,
                     up_axis=2,  # cylinders in URDF are aligned with z-axis
-                    restitution=shape_restitution,
-                    thickness=shape_thickness,
+                    is_visible=visible,
+                    has_ground_collision=not just_visual,
+                    **contact_vars,
                 )
+                shapes.append(s)
 
             for mesh in geo.findall("mesh"):
                 filename = mesh.get("filename")
@@ -182,59 +197,70 @@ def parse_urdf(
                         vertices = np.array(geom.vertices, dtype=np.float32) * scaling
                         faces = np.array(geom.faces.flatten(), dtype=np.int32)
                         mesh = Mesh(vertices, faces)
-                        builder.add_shape_mesh(
+                        s = builder.add_shape_mesh(
                             body=link,
                             pos=wp.vec3(tf.p),
                             rot=wp.quat(tf.q),
                             mesh=mesh,
                             density=density,
-                            ke=shape_ke,
-                            kd=shape_kd,
-                            kf=shape_kf,
-                            mu=shape_mu,
-                            restitution=shape_restitution,
-                            thickness=shape_thickness,
+                            is_visible=visible,
+                            has_ground_collision=not just_visual,
+                            **contact_vars,
                         )
+                        shapes.append(s)
                 else:
                     # a single mesh
                     vertices = np.array(m.vertices, dtype=np.float32) * scaling
                     faces = np.array(m.faces.flatten(), dtype=np.int32)
                     mesh = Mesh(vertices, faces)
-                    builder.add_shape_mesh(
+                    s = builder.add_shape_mesh(
                         body=link,
                         pos=tf.p,
                         rot=tf.q,
                         mesh=mesh,
                         density=density,
-                        ke=shape_ke,
-                        kd=shape_kd,
-                        kf=shape_kf,
-                        mu=shape_mu,
-                        restitution=shape_restitution,
-                        thickness=shape_thickness,
+                        is_visible=visible,
+                        has_ground_collision=not just_visual,
+                        **contact_vars,
                     )
+                    shapes.append(s)
+
+        return shapes
 
     # maps from link name -> link index
     link_index = {}
 
+    visual_shapes = []
+
     builder.add_articulation()
 
     start_shape_count = len(builder.shape_geo_type)
 
     # add links
     for i, urdf_link in enumerate(root.findall("link")):
-        if parse_visuals_as_colliders:
-            colliders = urdf_link.findall("visual")
-        else:
-            colliders = urdf_link.findall("collision")
-
         name = urdf_link.get("name")
         link = builder.add_body(origin=wp.transform_identity(), armature=armature, name=name)
 
         # add ourselves to the index
         link_index[name] = link
 
-        parse_shapes(link, colliders, density=density)
+        visuals = urdf_link.findall("visual")
+        colliders = urdf_link.findall("collision")
+
+        if parse_visuals_as_colliders:
+            colliders = visuals
+        else:
+            s = parse_shapes(link, visuals, density=0.0, just_visual=True)
+            visual_shapes.extend(s)
+
+        show_colliders = force_show_colliders
+        if parse_visuals_as_colliders:
+            show_colliders = True
+        elif len(visuals) == 0:
+            # we need to show the collision shapes since there are no visual shapes
+            show_colliders = True
+
+        parse_shapes(link, colliders, density=density, visible=show_colliders)
         m = builder.body_mass[link]
         if not ignore_inertial_definitions and urdf_link.find("inertial") is not None:
             # overwrite inertial parameters if defined
@@ -264,6 +290,7 @@ def parse_urdf(
             m = static_link_mass
             # cube with side length 0.5
             I_m = wp.mat33(np.eye(3)) * m / 12.0 * (0.5 * scale) ** 2 * 2.0
+            I_m += wp.mat33(armature * np.eye(3))
             builder.body_mass[link] = m
             builder.body_inv_mass[link] = 1.0 / m
             builder.body_inertia[link] = I_m
@@ -289,8 +316,8 @@ def parse_urdf(
             "origin": parse_transform(joint),
             "damping": damping,
             "friction": 0.0,
-            "limit_lower": -1.0e6,
-            "limit_upper": 1.0e6,
+            "limit_lower": joint_limit_lower,
+            "limit_upper": joint_limit_upper,
         }
         if joint.find("axis") is not None:
             joint_data["axis"] = joint.find("axis").get("xyz")
@@ -408,7 +435,7 @@ def parse_urdf(
         parent_xform = joint["origin"]
         child_xform = wp.transform_identity()
 
-        joint_mode = wp.sim.JOINT_MODE_LIMIT
+        joint_mode = wp.sim.JOINT_MODE_FORCE
         if stiffness > 0.0:
             joint_mode = wp.sim.JOINT_MODE_TARGET_POSITION
 
@@ -476,6 +503,10 @@ def parse_urdf(
         else:
             raise Exception("Unsupported joint type: " + joint["type"])
 
+    for i in range(start_shape_count, end_shape_count):
+        for j in visual_shapes:
+            builder.shape_collision_filter_pairs.add((i, j))
+
     if not enable_self_collisions:
         for i in range(start_shape_count, end_shape_count):
             for j in range(i + 1, end_shape_count):

warp/sim/import_usd.py

@@ -18,14 +18,16 @@ def parse_usd(
     default_density=1.0e3,
     only_load_enabled_rigid_bodies=False,
     only_load_enabled_joints=True,
-    default_ke=1e5,
-    default_kd=250.0,
-    default_kf=500.0,
-    default_mu=0.6,
-    default_restitution=0.0,
-    default_thickness=0.0,
+    contact_ke=1e5,  # TODO rename to contact...
+    contact_kd=250.0,
+    contact_kf=500.0,
+    contact_ka=0.0,
+    contact_mu=0.6,
+    contact_restitution=0.0,
+    contact_thickness=0.0,
     joint_limit_ke=100.0,
     joint_limit_kd=10.0,
+    armature=0.0,
     invert_rotations=False,
     verbose=False,
     ignore_paths=[],
@@ -41,14 +43,16 @@ def parse_usd(
         default_density (float): The default density to use for bodies without a density attribute.
         only_load_enabled_rigid_bodies (bool): If True, only rigid bodies which do not have `physics:rigidBodyEnabled` set to False are loaded.
         only_load_enabled_joints (bool): If True, only joints which do not have `physics:jointEnabled` set to False are loaded.
-        default_ke (float): The default contact stiffness to use, only considered by SemiImplicitIntegrator.
-        default_kd (float): The default contact damping to use, only considered by SemiImplicitIntegrator.
-        default_kf (float): The default friction stiffness to use, only considered by SemiImplicitIntegrator.
-        default_mu (float): The default friction coefficient to use if a shape has not friction coefficient defined.
-        default_restitution (float): The default coefficient of restitution to use if a shape has not coefficient of restitution defined.
-        default_thickness (float): The thickness to add to the shape geometry.
-        joint_limit_ke (float): The default stiffness to use for joint limits, only considered by SemiImplicitIntegrator.
-        joint_limit_kd (float): The default damping to use for joint limits, only considered by SemiImplicitIntegrator.
+        contact_ke (float): The default contact stiffness to use, only considered by the Euler integrators.
+        contact_kd (float): The default contact damping to use, only considered by the Euler integrators.
+        contact_kf (float): The default friction stiffness to use, only considered by the Euler integrators.
+        contact_ka (float): The default adhesion distance to use, only considered by the Euler integrators.
+        contact_mu (float): The default friction coefficient to use if a shape has not friction coefficient defined.
+        contact_restitution (float): The default coefficient of restitution to use if a shape has not coefficient of restitution defined.
+        contact_thickness (float): The thickness to add to the shape geometry.
+        joint_limit_ke (float): The default stiffness to use for joint limits, only considered by the Euler integrators.
+        joint_limit_kd (float): The default damping to use for joint limits, only considered by the Euler integrators.
+        armature (float): The armature to use for the bodies.
         invert_rotations (bool): If True, inverts any rotations defined in the shape transforms.
         verbose (bool): If True, print additional information about the parsed USD file.
         ignore_paths (List[str]): A list of regular expressions matching prim paths to ignore.
@@ -150,14 +154,16 @@ def parse_usd(
     try:
         if UsdPhysics.StageHasAuthoredKilogramsPerUnit(stage):
             mass_unit = UsdPhysics.GetStageKilogramsPerUnit(stage)
-    except:
-        pass
+    except Exception as e:
+        if verbose:
+            print(f"Failed to get mass unit: {e}")
     linear_unit = 1.0
     try:
         if UsdGeom.StageHasAuthoredMetersPerUnit(stage):
             linear_unit = UsdGeom.GetStageMetersPerUnit(stage)
-    except:
-        pass
+    except Exception as e:
+        if verbose:
+            print(f"Failed to get linear unit: {e}")
 
     def parse_xform(prim):
         xform = UsdGeom.Xform(prim)
@@ -221,7 +227,8 @@ def parse_usd(
                 high = joint_data["upperLimit"]
             else:
                 high *= linear_unit
-        mode = wp.sim.JOINT_MODE_LIMIT
+
+        mode = wp.sim.JOINT_MODE_FORCE
         if f"DriveAPI:{type}" in schemas_str:
             if target_vel is not None and target_vel != 0.0:
                 mode = wp.sim.JOINT_MODE_TARGET_VELOCITY
@@ -234,7 +241,7 @@ def parse_usd(
             axis=(axis or joint_data["axis"]),
             limit_lower=low,
             limit_upper=high,
-            target=(target_pos or target_vel or (low + high) / 2),
+            action=(target_pos or target_vel or (low + high) / 2),
             target_ke=stiffness,
             target_kd=damping,
             mode=mode,
@@ -249,8 +256,9 @@ def parse_usd(
     axis_str = "Y"
     try:
         axis_str = UsdGeom.GetStageUpAxis(stage)
-    except:
-        pass
+    except Exception as e:
+        if verbose:
+            print(f"Failed to parse stage up axis: {e}")
     upaxis = str2axis(axis_str)
 
     shape_types = {"Cube", "Sphere", "Mesh", "Capsule", "Plane", "Cylinder", "Cone"}
@@ -344,11 +352,11 @@ def parse_usd(
             if has_attribute(prim, "physics:density"):
                 material["density"] = parse_float(prim, "physics:density") * mass_unit  # / (linear_unit**3)
             if has_attribute(prim, "physics:restitution"):
-                material["restitution"] = parse_float(prim, "physics:restitution", default_restitution)
+                material["restitution"] = parse_float(prim, "physics:restitution", contact_restitution)
             if has_attribute(prim, "physics:staticFriction"):
-                material["staticFriction"] = parse_float(prim, "physics:staticFriction", default_mu)
+                material["staticFriction"] = parse_float(prim, "physics:staticFriction", contact_mu)
             if has_attribute(prim, "physics:dynamicFriction"):
-                material["dynamicFriction"] = parse_float(prim, "physics:dynamicFriction", default_mu)
+                material["dynamicFriction"] = parse_float(prim, "physics:dynamicFriction", contact_mu)
             materials[path] = material
 
         elif type_name == "PhysicsScene":
@@ -364,8 +372,9 @@ def parse_usd(
                         builder.up_vector = -builder.up_vector
                 else:
                     builder.up_vector = upaxis
-            except:
-                pass
+            except Exception as e:
+                if verbose:
+                    print(f"Failed to parse physics scene: {e}")
 
     def parse_prim(prim, incoming_xform, incoming_scale, parent_body: int = -1):
         nonlocal builder
@@ -412,6 +421,7 @@ def parse_usd(
             body_id = builder.add_body(
                 origin=xform,
                 name=prim.GetName(),
+                armature=armature,
             )
             path_body_map[path] = body_id
             body_density[body_id] = 0.0
@@ -562,7 +572,12 @@ def parse_usd(
         elif type_name in shape_types:
             # parse shapes
             shape_params = dict(
-                ke=default_ke, kd=default_kd, kf=default_kf, mu=default_mu, restitution=default_restitution
+                ke=contact_ke,
+                kd=contact_kd,
+                kf=contact_kf,
+                ka=contact_ka,
+                mu=contact_mu,
+                restitution=contact_restitution,
             )
             if material is not None:
                 if "restitution" in material:
@@ -590,7 +605,7 @@ def parse_usd(
                     hy=extents[1] / 2,
                     hz=extents[2] / 2,
                     density=density,
-                    thickness=default_thickness,
+                    thickness=contact_thickness,
                     **shape_params,
                 )
             elif type_name == "Sphere":
@@ -626,7 +641,7 @@ def parse_usd(
                     rot=geo_rot,
                     width=width,
                     length=length,
-                    thickness=default_thickness,
+                    thickness=contact_thickness,
                     **shape_params,
                 )
             elif type_name == "Capsule":
@@ -699,7 +714,7 @@ def parse_usd(
                     scale=scale,
                     mesh=m,
                     density=density,
-                    thickness=default_thickness,
+                    thickness=contact_thickness,
                     **shape_params,
                 )
             else:
@@ -738,19 +753,19 @@ def parse_usd(
                     builder.body_inv_mass[body_id] = 0.0
                 # update inertia
                 builder.body_inertia[body_id] *= mass_ratio
-                if builder.body_inertia[body_id].any():
-                    builder.body_inv_inertia[body_id] = np.linalg.inv(builder.body_inertia[body_id])
+                if np.array(builder.body_inertia[body_id]).any():
+                    builder.body_inv_inertia[body_id] = wp.inverse(builder.body_inertia[body_id])
                 else:
-                    builder.body_inv_inertia[body_id] = np.zeros((3, 3), dtype=np.float32)
+                    builder.body_inv_inertia[body_id] = wp.mat33(*np.zeros((3, 3), dtype=np.float32))
 
             if np.linalg.norm(i_diag) > 0.0:
                 rot = np.array(wp.quat_to_matrix(i_rot), dtype=np.float32).reshape(3, 3)
                 inertia = rot @ np.diag(i_diag) @ rot.T
                 builder.body_inertia[body_id] = inertia
                 if inertia.any():
-                    builder.body_inv_inertia[body_id] = np.linalg.inv(inertia)
+                    builder.body_inv_inertia[body_id] = wp.inverse(wp.mat33(*inertia))
                 else:
-                    builder.body_inv_inertia[body_id] = np.zeros((3, 3), dtype=np.float32)
+                    builder.body_inv_inertia[body_id] = wp.mat33(*np.zeros((3, 3), dtype=np.float32))
 
     parse_prim(
         stage.GetDefaultPrim(), incoming_xform=wp.transform(), incoming_scale=np.ones(3, dtype=np.float32) * linear_unit

warp/sim/inertia.py

@@ -33,7 +33,7 @@ def triangle_inertia(
 
     Dm = wp.mat33(pcom[0], qcom[0], rcom[0], pcom[1], qcom[1], rcom[1], pcom[2], qcom[2], rcom[2])
 
-    volume = wp.determinant(Dm) / 6.0
+    volume = wp.abs(wp.determinant(Dm) / 6.0)
 
     # accumulate mass
     wp.atomic_add(mass, 0, 4.0 * density * volume)
@@ -260,6 +260,7 @@ def compute_mesh_inertia(
     """Computes mass, center of mass, 3x3 inertia matrix, and volume for a mesh."""
     com = wp.vec3(np.mean(vertices, 0))
 
+    indices = np.array(indices).flatten()
     num_tris = len(indices) // 3
 
     # compute signed inertia for each tetrahedron
@@ -305,7 +306,7 @@ def compute_mesh_inertia(
     # Extract mass and inertia and save to class attributes.
     mass = float(mass_warp.numpy()[0] * density)
     I = wp.mat33(*(I_warp.numpy()[0] * density))
-    volume = vol_warp.numpy()[0]
+    volume = float(vol_warp.numpy()[0])
     return mass, com, I, volume