warp-lang 1.9.0__py3-none-win_amd64.whl → 1.10.0rc2__py3-none-win_amd64.whl

warp/sim/inertia.py

@@ -1,357 +0,0 @@
-# SPDX-FileCopyrightText: Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
-# SPDX-License-Identifier: Apache-2.0
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-"""Helper functions for computing rigid body inertia properties."""
-
-from __future__ import annotations
-
-import numpy as np
-
-import warp as wp
-
-
-def compute_sphere_inertia(density: float, r: float) -> tuple[float, wp.vec3, wp.mat33]:
-    """Helper to compute mass and inertia of a solid sphere
-
-    Args:
-        density: The sphere density
-        r: The sphere radius
-
-    Returns:
-
-        A tuple of (mass, inertia) with inertia specified around the origin
-    """
-
-    v = 4.0 / 3.0 * wp.pi * r * r * r
-
-    m = density * v
-    Ia = 2.0 / 5.0 * m * r * r
-
-    I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ia]])
-
-    return (m, wp.vec3(), I)
-
-
-def compute_capsule_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]:
-    """Helper to compute mass and inertia of a solid capsule extending along the y-axis
-
-    Args:
-        density: The capsule density
-        r: The capsule radius
-        h: The capsule height (full height of the interior cylinder)
-
-    Returns:
-
-        A tuple of (mass, inertia) with inertia specified around the origin
-    """
-
-    ms = density * (4.0 / 3.0) * wp.pi * r * r * r
-    mc = density * wp.pi * r * r * h
-
-    # total mass
-    m = ms + mc
-
-    # adapted from ODE
-    Ia = mc * (0.25 * r * r + (1.0 / 12.0) * h * h) + ms * (0.4 * r * r + 0.375 * r * h + 0.25 * h * h)
-    Ib = (mc * 0.5 + ms * 0.4) * r * r
-
-    I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ib, 0.0], [0.0, 0.0, Ia]])
-
-    return (m, wp.vec3(), I)
-
-
-def compute_cylinder_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]:
-    """Helper to compute mass and inertia of a solid cylinder extending along the y-axis
-
-    Args:
-        density: The cylinder density
-        r: The cylinder radius
-        h: The cylinder height (extent along the y-axis)
-
-    Returns:
-
-        A tuple of (mass, inertia) with inertia specified around the origin
-    """
-
-    m = density * wp.pi * r * r * h
-
-    Ia = 1 / 12 * m * (3 * r * r + h * h)
-    Ib = 1 / 2 * m * r * r
-
-    I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ib, 0.0], [0.0, 0.0, Ia]])
-
-    return (m, wp.vec3(), I)
-
-
-def compute_cone_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]:
-    """Helper to compute mass and inertia of a solid cone extending along the y-axis
-
-    Args:
-        density: The cone density
-        r: The cone radius
-        h: The cone height (extent along the y-axis)
-
-    Returns:
-
-        A tuple of (mass, inertia) with inertia specified around the origin
-    """
-
-    m = density * wp.pi * r * r * h / 3.0
-
-    Ia = 1 / 20 * m * (3 * r * r + 2 * h * h)
-    Ib = 3 / 10 * m * r * r
-
-    I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ib, 0.0], [0.0, 0.0, Ia]])
-
-    return (m, wp.vec3(), I)
-
-
-def compute_box_inertia(density: float, w: float, h: float, d: float) -> tuple[float, wp.vec3, wp.mat33]:
-    """Helper to compute mass and inertia of a solid box
-
-    Args:
-        density: The box density
-        w: The box width along the x-axis
-        h: The box height along the y-axis
-        d: The box depth along the z-axis
-
-    Returns:
-
-        A tuple of (mass, inertia) with inertia specified around the origin
-    """
-
-    v = w * h * d
-    m = density * v
-
-    Ia = 1.0 / 12.0 * m * (h * h + d * d)
-    Ib = 1.0 / 12.0 * m * (w * w + d * d)
-    Ic = 1.0 / 12.0 * m * (w * w + h * h)
-
-    I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ib, 0.0], [0.0, 0.0, Ic]])
-
-    return (m, wp.vec3(), I)
-
-
-@wp.func
-def triangle_inertia(
-    v0: wp.vec3,
-    v1: wp.vec3,
-    v2: wp.vec3,
-):
-    vol = wp.dot(v0, wp.cross(v1, v2)) / 6.0  # tetra volume (0,v0,v1,v2)
-    first = vol * (v0 + v1 + v2) / 4.0  # first-order integral
-
-    # second-order integral (symmetric)
-    o00, o11, o22 = wp.outer(v0, v0), wp.outer(v1, v1), wp.outer(v2, v2)
-    o01, o02, o12 = wp.outer(v0, v1), wp.outer(v0, v2), wp.outer(v1, v2)
-    o01t, o02t, o12t = wp.transpose(o01), wp.transpose(o02), wp.transpose(o12)
-
-    second = (vol / 10.0) * (o00 + o11 + o22)
-    second += (vol / 20.0) * (o01 + o01t + o02 + o02t + o12 + o12t)
-
-    return vol, first, second
-
-
-@wp.kernel
-def compute_solid_mesh_inertia(
-    indices: wp.array(dtype=int),
-    vertices: wp.array(dtype=wp.vec3),
-    # outputs
-    volume: wp.array(dtype=float),
-    first: wp.array(dtype=wp.vec3),
-    second: wp.array(dtype=wp.mat33),
-):
-    i = wp.tid()
-    p = vertices[indices[i * 3 + 0]]
-    q = vertices[indices[i * 3 + 1]]
-    r = vertices[indices[i * 3 + 2]]
-
-    v, f, s = triangle_inertia(p, q, r)
-    wp.atomic_add(volume, 0, v)
-    wp.atomic_add(first, 0, f)
-    wp.atomic_add(second, 0, s)
-
-
-@wp.kernel
-def compute_hollow_mesh_inertia(
-    indices: wp.array(dtype=int),
-    vertices: wp.array(dtype=wp.vec3),
-    thickness: wp.array(dtype=float),
-    # outputs
-    volume: wp.array(dtype=float),
-    first: wp.array(dtype=wp.vec3),
-    second: wp.array(dtype=wp.mat33),
-):
-    tid = wp.tid()
-    i = indices[tid * 3 + 0]
-    j = indices[tid * 3 + 1]
-    k = indices[tid * 3 + 2]
-
-    vi = vertices[i]
-    vj = vertices[j]
-    vk = vertices[k]
-
-    normal = -wp.normalize(wp.cross(vj - vi, vk - vi))
-    ti = normal * thickness[i]
-    tj = normal * thickness[j]
-    tk = normal * thickness[k]
-
-    # wedge vertices
-    vi0 = vi - ti
-    vi1 = vi + ti
-    vj0 = vj - tj
-    vj1 = vj + tj
-    vk0 = vk - tk
-    vk1 = vk + tk
-
-    v_total = 0.0
-    f_total = wp.vec3(0.0)
-    s_total = wp.mat33(0.0)
-
-    v, f, s = triangle_inertia(vi0, vj0, vk0)
-    v_total += v
-    f_total += f
-    s_total += s
-    v, f, s = triangle_inertia(vj0, vk1, vk0)
-    v_total += v
-    f_total += f
-    s_total += s
-    v, f, s = triangle_inertia(vj0, vj1, vk1)
-    v_total += v
-    f_total += f
-    s_total += s
-    v, f, s = triangle_inertia(vj0, vi1, vj1)
-    v_total += v
-    f_total += f
-    s_total += s
-    v, f, s = triangle_inertia(vj0, vi0, vi1)
-    v_total += v
-    f_total += f
-    s_total += s
-    v, f, s = triangle_inertia(vj1, vi1, vk1)
-    v_total += v
-    f_total += f
-    s_total += s
-    v, f, s = triangle_inertia(vi1, vi0, vk0)
-    v_total += v
-    f_total += f
-    s_total += s
-    v, f, s = triangle_inertia(vi1, vk0, vk1)
-    v_total += v
-    f_total += f
-    s_total += s
-
-    wp.atomic_add(volume, 0, v_total)
-    wp.atomic_add(first, 0, f_total)
-    wp.atomic_add(second, 0, s_total)
-
-
-def compute_mesh_inertia(
-    density: float,
-    vertices: list,
-    indices: list,
-    is_solid: bool = True,
-    thickness: list[float] | float = 0.001,
-) -> tuple[float, wp.vec3, wp.mat33, float]:
-    """
-    Compute the mass, center of mass, inertia, and volume of a triangular mesh.
-
-    Args:
-        density: The density of the mesh material.
-        vertices: A list of vertex positions (3D coordinates).
-        indices: A list of triangle indices (each triangle is defined by 3 vertex indices).
-        is_solid: If True, compute inertia for a solid mesh; if False, for a hollow mesh using the given thickness.
-        thickness: Thickness of the mesh if it is hollow. Can be a single value or a list of values for each vertex.
-
-    Returns:
-        A tuple containing:
-            - mass: The mass of the mesh.
-            - com: The center of mass (3D coordinates).
-            - I: The inertia tensor (3x3 matrix).
-            - volume: The signed volume of the mesh.
-    """
-
-    indices = np.array(indices).flatten()
-    num_tris = len(indices) // 3
-
-    # Allocating for mass and inertia
-    com_warp = wp.zeros(1, dtype=wp.vec3)
-    I_warp = wp.zeros(1, dtype=wp.mat33)
-    vol_warp = wp.zeros(1, dtype=float)
-
-    wp_vertices = wp.array(vertices, dtype=wp.vec3)
-    wp_indices = wp.array(indices, dtype=int)
-
-    if is_solid:
-        wp.launch(
-            kernel=compute_solid_mesh_inertia,
-            dim=num_tris,
-            inputs=[
-                wp_indices,
-                wp_vertices,
-            ],
-            outputs=[
-                vol_warp,
-                com_warp,
-                I_warp,
-            ],
-        )
-    else:
-        if isinstance(thickness, float):
-            thickness = [thickness] * len(vertices)
-        wp.launch(
-            kernel=compute_hollow_mesh_inertia,
-            dim=num_tris,
-            inputs=[
-                wp_indices,
-                wp_vertices,
-                wp.array(thickness, dtype=float),
-            ],
-            outputs=[
-                vol_warp,
-                com_warp,
-                I_warp,
-            ],
-        )
-
-    V_tot = float(vol_warp.numpy()[0])  # signed volume
-    F_tot = com_warp.numpy()[0]  # first moment
-    S_tot = I_warp.numpy()[0]  # second moment
-
-    # If the winding is inward, flip signs
-    if V_tot < 0:
-        V_tot = -V_tot
-        F_tot = -F_tot
-        S_tot = -S_tot
-
-    mass = density * V_tot
-    if V_tot > 0.0:
-        com = F_tot / V_tot
-    else:
-        com = F_tot
-
-    S_tot *= density  # include density
-    I_origin = np.trace(S_tot) * np.eye(3) - S_tot  # inertia about origin
-    r = com
-    I_com = I_origin - mass * ((r @ r) * np.eye(3) - np.outer(r, r))
-
-    return mass, wp.vec3(*com), wp.mat33(*I_com), V_tot
-
-
-def transform_inertia(m, I, p, q) -> wp.mat33:
-    R = wp.quat_to_matrix(q)
-
-    # Steiner's theorem
-    return R @ I @ wp.transpose(R) + m * (wp.dot(p, p) * wp.mat33(np.eye(3)) - wp.outer(p, p))

warp/sim/integrator.py

@@ -1,245 +0,0 @@
-# SPDX-FileCopyrightText: Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
-# SPDX-License-Identifier: Apache-2.0
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import warp as wp
-
-from .model import PARTICLE_FLAG_ACTIVE, Control, Model, State
-
-
-@wp.kernel
-def integrate_particles(
-    x: wp.array(dtype=wp.vec3),
-    v: wp.array(dtype=wp.vec3),
-    f: wp.array(dtype=wp.vec3),
-    w: wp.array(dtype=float),
-    particle_flags: wp.array(dtype=wp.uint32),
-    gravity: wp.vec3,
-    dt: float,
-    v_max: float,
-    x_new: wp.array(dtype=wp.vec3),
-    v_new: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-    x0 = x[tid]
-    v0 = v[tid]
-
-    if (particle_flags[tid] & PARTICLE_FLAG_ACTIVE) == 0:
-        x_new[tid] = x0
-        v_new[tid] = wp.vec3(0.0)
-        return
-
-    f0 = f[tid]
-
-    inv_mass = w[tid]
-
-    # simple semi-implicit Euler. v1 = v0 + a dt, x1 = x0 + v1 dt
-    v1 = v0 + (f0 * inv_mass + gravity * wp.step(-inv_mass)) * dt
-    # enforce velocity limit to prevent instability
-    v1_mag = wp.length(v1)
-    if v1_mag > v_max:
-        v1 *= v_max / v1_mag
-    x1 = x0 + v1 * dt
-
-    x_new[tid] = x1
-    v_new[tid] = v1
-
-
-@wp.func
-def integrate_rigid_body(
-    q: wp.transform,
-    qd: wp.spatial_vector,
-    f: wp.spatial_vector,
-    com: wp.vec3,
-    inertia: wp.mat33,
-    inv_mass: float,
-    inv_inertia: wp.mat33,
-    gravity: wp.vec3,
-    angular_damping: float,
-    dt: float,
-):
-    # unpack transform
-    x0 = wp.transform_get_translation(q)
-    r0 = wp.transform_get_rotation(q)
-
-    # unpack spatial twist
-    w0 = wp.spatial_top(qd)
-    v0 = wp.spatial_bottom(qd)
-
-    # unpack spatial wrench
-    t0 = wp.spatial_top(f)
-    f0 = wp.spatial_bottom(f)
-
-    x_com = x0 + wp.quat_rotate(r0, com)
-
-    # linear part
-    v1 = v0 + (f0 * inv_mass + gravity * wp.nonzero(inv_mass)) * dt
-    x1 = x_com + v1 * dt
-
-    # angular part (compute in body frame)
-    wb = wp.quat_rotate_inv(r0, w0)
-    tb = wp.quat_rotate_inv(r0, t0) - wp.cross(wb, inertia * wb)  # coriolis forces
-
-    w1 = wp.quat_rotate(r0, wb + inv_inertia * tb * dt)
-    r1 = wp.normalize(r0 + wp.quat(w1, 0.0) * r0 * 0.5 * dt)
-
-    # angular damping
-    w1 *= 1.0 - angular_damping * dt
-
-    q_new = wp.transform(x1 - wp.quat_rotate(r1, com), r1)
-    qd_new = wp.spatial_vector(w1, v1)
-
-    return q_new, qd_new
-
-
-# semi-implicit Euler integration
-@wp.kernel
-def integrate_bodies(
-    body_q: wp.array(dtype=wp.transform),
-    body_qd: wp.array(dtype=wp.spatial_vector),
-    body_f: wp.array(dtype=wp.spatial_vector),
-    body_com: wp.array(dtype=wp.vec3),
-    m: wp.array(dtype=float),
-    I: wp.array(dtype=wp.mat33),
-    inv_m: wp.array(dtype=float),
-    inv_I: wp.array(dtype=wp.mat33),
-    gravity: wp.vec3,
-    angular_damping: float,
-    dt: float,
-    # outputs
-    body_q_new: wp.array(dtype=wp.transform),
-    body_qd_new: wp.array(dtype=wp.spatial_vector),
-):
-    tid = wp.tid()
-
-    # positions
-    q = body_q[tid]
-    qd = body_qd[tid]
-    f = body_f[tid]
-
-    # masses
-    inv_mass = inv_m[tid]  # 1 / mass
-
-    inertia = I[tid]
-    inv_inertia = inv_I[tid]  # inverse of 3x3 inertia matrix
-
-    com = body_com[tid]
-
-    q_new, qd_new = integrate_rigid_body(
-        q,
-        qd,
-        f,
-        com,
-        inertia,
-        inv_mass,
-        inv_inertia,
-        gravity,
-        angular_damping,
-        dt,
-    )
-
-    body_q_new[tid] = q_new
-    body_qd_new[tid] = qd_new
-
-
-class Integrator:
-    """
-    Generic base class for integrators. Provides methods to integrate rigid bodies and particles.
-    """
-
-    def integrate_bodies(
-        self,
-        model: Model,
-        state_in: State,
-        state_out: State,
-        dt: float,
-        angular_damping: float = 0.0,
-    ):
-        """
-        Integrate the rigid bodies of the model.
-
-        Args:
-            model (Model): The model to integrate.
-            state_in (State): The input state.
-            state_out (State): The output state.
-            dt (float): The time step (typically in seconds).
-            angular_damping (float, optional): The angular damping factor. Defaults to 0.0.
-        """
-        if model.body_count:
-            wp.launch(
-                kernel=integrate_bodies,
-                dim=model.body_count,
-                inputs=[
-                    state_in.body_q,
-                    state_in.body_qd,
-                    state_in.body_f,
-                    model.body_com,
-                    model.body_mass,
-                    model.body_inertia,
-                    model.body_inv_mass,
-                    model.body_inv_inertia,
-                    model.gravity,
-                    angular_damping,
-                    dt,
-                ],
-                outputs=[state_out.body_q, state_out.body_qd],
-                device=model.device,
-            )
-
-    def integrate_particles(
-        self,
-        model: Model,
-        state_in: State,
-        state_out: State,
-        dt: float,
-    ):
-        """
-        Integrate the particles of the model.
-
-        Args:
-            model (Model): The model to integrate.
-            state_in (State): The input state.
-            state_out (State): The output state.
-            dt (float): The time step (typically in seconds).
-        """
-        if model.particle_count:
-            wp.launch(
-                kernel=integrate_particles,
-                dim=model.particle_count,
-                inputs=[
-                    state_in.particle_q,
-                    state_in.particle_qd,
-                    state_in.particle_f,
-                    model.particle_inv_mass,
-                    model.particle_flags,
-                    model.gravity,
-                    dt,
-                    model.particle_max_velocity,
-                ],
-                outputs=[state_out.particle_q, state_out.particle_qd],
-                device=model.device,
-            )
-
-    def simulate(self, model: Model, state_in: State, state_out: State, dt: float, control: Control = None):
-        """
-        Simulate the model for a given time step using the given control input.
-
-        Args:
-            model (Model): The model to simulate.
-            state_in (State): The input state.
-            state_out (State): The output state.
-            dt (float): The time step (typically in seconds).
-            control (Control): The control input. Defaults to `None` which means the control values from the :class:`Model` are used.
-        """
-        raise NotImplementedError()