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

warp/sim/integrator_euler.py

@@ -1,2000 +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.
-
-"""This module contains time-integration objects for simulating
-models + state forward in time.
-
-"""
-
-import warp as wp
-
-from .collide import triangle_closest_point_barycentric
-from .integrator import Integrator
-from .model import PARTICLE_FLAG_ACTIVE, Control, Model, ModelShapeGeometry, ModelShapeMaterials, State
-from .particles import eval_particle_forces
-from .utils import quat_decompose, quat_twist
-
-
-@wp.kernel
-def eval_springs(
-    x: wp.array(dtype=wp.vec3),
-    v: wp.array(dtype=wp.vec3),
-    spring_indices: wp.array(dtype=int),
-    spring_rest_lengths: wp.array(dtype=float),
-    spring_stiffness: wp.array(dtype=float),
-    spring_damping: wp.array(dtype=float),
-    f: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-
-    i = spring_indices[tid * 2 + 0]
-    j = spring_indices[tid * 2 + 1]
-
-    if i == -1 or j == -1:
-        return
-
-    ke = spring_stiffness[tid]
-    kd = spring_damping[tid]
-    rest = spring_rest_lengths[tid]
-
-    xi = x[i]
-    xj = x[j]
-
-    vi = v[i]
-    vj = v[j]
-
-    xij = xi - xj
-    vij = vi - vj
-
-    l = wp.length(xij)
-    l_inv = 1.0 / l
-
-    # normalized spring direction
-    dir = xij * l_inv
-
-    c = l - rest
-    dcdt = wp.dot(dir, vij)
-
-    # damping based on relative velocity
-    fs = dir * (ke * c + kd * dcdt)
-
-    wp.atomic_sub(f, i, fs)
-    wp.atomic_add(f, j, fs)
-
-
-@wp.kernel
-def eval_triangles(
-    x: wp.array(dtype=wp.vec3),
-    v: wp.array(dtype=wp.vec3),
-    indices: wp.array2d(dtype=int),
-    pose: wp.array(dtype=wp.mat22),
-    activation: wp.array(dtype=float),
-    materials: wp.array2d(dtype=float),
-    f: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-
-    k_mu = materials[tid, 0]
-    k_lambda = materials[tid, 1]
-    k_damp = materials[tid, 2]
-    k_drag = materials[tid, 3]
-    k_lift = materials[tid, 4]
-
-    i = indices[tid, 0]
-    j = indices[tid, 1]
-    k = indices[tid, 2]
-
-    x0 = x[i]  # point zero
-    x1 = x[j]  # point one
-    x2 = x[k]  # point two
-
-    v0 = v[i]  # vel zero
-    v1 = v[j]  # vel one
-    v2 = v[k]  # vel two
-
-    x10 = x1 - x0  # barycentric coordinates (centered at p)
-    x20 = x2 - x0
-
-    v10 = v1 - v0
-    v20 = v2 - v0
-
-    Dm = pose[tid]
-
-    inv_rest_area = wp.determinant(Dm) * 2.0  # 1 / det(A) = det(A^-1)
-    rest_area = 1.0 / inv_rest_area
-
-    # scale stiffness coefficients to account for area
-    k_mu = k_mu * rest_area
-    k_lambda = k_lambda * rest_area
-    k_damp = k_damp * rest_area
-
-    # F = Xs*Xm^-1
-    F1 = x10 * Dm[0, 0] + x20 * Dm[1, 0]
-    F2 = x10 * Dm[0, 1] + x20 * Dm[1, 1]
-
-    # dFdt = Vs*Xm^-1
-    dFdt1 = v10 * Dm[0, 0] + v20 * Dm[1, 0]
-    dFdt2 = v10 * Dm[0, 1] + v20 * Dm[1, 1]
-
-    # deviatoric PK1 + damping term
-    P1 = F1 * k_mu + dFdt1 * k_damp
-    P2 = F2 * k_mu + dFdt2 * k_damp
-
-    # -----------------------------
-    # St. Venant-Kirchoff
-
-    # # Green strain, F'*F-I
-    # e00 = dot(f1, f1) - 1.0
-    # e10 = dot(f2, f1)
-    # e01 = dot(f1, f2)
-    # e11 = dot(f2, f2) - 1.0
-
-    # E = wp.mat22(e00, e01,
-    #              e10, e11)
-
-    # # local forces (deviatoric part)
-    # T = wp.mul(E, wp.transpose(Dm))
-
-    # # spatial forces, F*T
-    # fq = (f1*T[0,0] + f2*T[1,0])*k_mu*2.0
-    # fr = (f1*T[0,1] + f2*T[1,1])*k_mu*2.0
-    # alpha = 1.0
-
-    # -----------------------------
-    # Baraff & Witkin, note this model is not isotropic
-
-    # c1 = length(f1) - 1.0
-    # c2 = length(f2) - 1.0
-    # f1 = normalize(f1)*c1*k1
-    # f2 = normalize(f2)*c2*k1
-
-    # fq = f1*Dm[0,0] + f2*Dm[0,1]
-    # fr = f1*Dm[1,0] + f2*Dm[1,1]
-
-    # -----------------------------
-    # Neo-Hookean (with rest stability)
-
-    # force = P*Dm'
-    f1 = P1 * Dm[0, 0] + P2 * Dm[0, 1]
-    f2 = P1 * Dm[1, 0] + P2 * Dm[1, 1]
-    alpha = 1.0 + k_mu / k_lambda
-
-    # -----------------------------
-    # Area Preservation
-
-    n = wp.cross(x10, x20)
-    area = wp.length(n) * 0.5
-
-    # actuation
-    act = activation[tid]
-
-    # J-alpha
-    c = area * inv_rest_area - alpha + act
-
-    # dJdx
-    n = wp.normalize(n)
-    dcdq = wp.cross(x20, n) * inv_rest_area * 0.5
-    dcdr = wp.cross(n, x10) * inv_rest_area * 0.5
-
-    f_area = k_lambda * c
-
-    # -----------------------------
-    # Area Damping
-
-    dcdt = wp.dot(dcdq, v1) + wp.dot(dcdr, v2) - wp.dot(dcdq + dcdr, v0)
-    f_damp = k_damp * dcdt
-
-    f1 = f1 + dcdq * (f_area + f_damp)
-    f2 = f2 + dcdr * (f_area + f_damp)
-    f0 = f1 + f2
-
-    # -----------------------------
-    # Lift + Drag
-
-    vmid = (v0 + v1 + v2) * 0.3333
-    vdir = wp.normalize(vmid)
-
-    f_drag = vmid * (k_drag * area * wp.abs(wp.dot(n, vmid)))
-    f_lift = n * (k_lift * area * (wp.HALF_PI - wp.acos(wp.dot(n, vdir)))) * wp.dot(vmid, vmid)
-
-    f0 = f0 - f_drag - f_lift
-    f1 = f1 + f_drag + f_lift
-    f2 = f2 + f_drag + f_lift
-
-    # apply forces
-    wp.atomic_add(f, i, f0)
-    wp.atomic_sub(f, j, f1)
-    wp.atomic_sub(f, k, f2)
-
-
-# @wp.func
-# def triangle_closest_point(a: wp.vec3, b: wp.vec3, c: wp.vec3, p: wp.vec3):
-#     ab = b - a
-#     ac = c - a
-#     ap = p - a
-
-#     d1 = wp.dot(ab, ap)
-#     d2 = wp.dot(ac, ap)
-
-#     if (d1 <= 0.0 and d2 <= 0.0):
-#         return a
-
-#     bp = p - b
-#     d3 = wp.dot(ab, bp)
-#     d4 = wp.dot(ac, bp)
-
-#     if (d3 >= 0.0 and d4 <= d3):
-#         return b
-
-#     vc = d1 * d4 - d3 * d2
-#     v = d1 / (d1 - d3)
-#     if (vc <= 0.0 and d1 >= 0.0 and d3 <= 0.0):
-#         return a + ab * v
-
-#     cp = p - c
-#     d5 = dot(ab, cp)
-#     d6 = dot(ac, cp)
-
-#     if (d6 >= 0.0 and d5 <= d6):
-#         return c
-
-#     vb = d5 * d2 - d1 * d6
-#     w = d2 / (d2 - d6)
-#     if (vb <= 0.0 and d2 >= 0.0 and d6 <= 0.0):
-#         return a + ac * w
-
-#     va = d3 * d6 - d5 * d4
-#     w = (d4 - d3) / ((d4 - d3) + (d5 - d6))
-#     if (va <= 0.0 and (d4 - d3) >= 0.0 and (d5 - d6) >= 0.0):
-#         return b + (c - b) * w
-
-#     denom = 1.0 / (va + vb + vc)
-#     v = vb * denom
-#     w = vc * denom
-
-#     return a + ab * v + ac * w
-
-
-@wp.kernel
-def eval_triangles_contact(
-    # idx : wp.array(dtype=int), # list of indices for colliding particles
-    num_particles: int,  # size of particles
-    x: wp.array(dtype=wp.vec3),
-    v: wp.array(dtype=wp.vec3),
-    indices: wp.array2d(dtype=int),
-    materials: wp.array2d(dtype=float),
-    particle_radius: wp.array(dtype=float),
-    f: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-    face_no = tid // num_particles  # which face
-    particle_no = tid % num_particles  # which particle
-
-    # k_mu = materials[face_no, 0]
-    # k_lambda = materials[face_no, 1]
-    # k_damp = materials[face_no, 2]
-    # k_drag = materials[face_no, 3]
-    # k_lift = materials[face_no, 4]
-
-    # at the moment, just one particle
-    pos = x[particle_no]
-
-    i = indices[face_no, 0]
-    j = indices[face_no, 1]
-    k = indices[face_no, 2]
-
-    if i == particle_no or j == particle_no or k == particle_no:
-        return
-
-    p = x[i]  # point zero
-    q = x[j]  # point one
-    r = x[k]  # point two
-
-    # vp = v[i] # vel zero
-    # vq = v[j] # vel one
-    # vr = v[k] # vel two
-
-    # qp = q-p # barycentric coordinates (centered at p)
-    # rp = r-p
-
-    bary = triangle_closest_point_barycentric(p, q, r, pos)
-    closest = p * bary[0] + q * bary[1] + r * bary[2]
-
-    diff = pos - closest
-    dist = wp.dot(diff, diff)
-    n = wp.normalize(diff)
-    c = wp.min(dist - particle_radius[particle_no], 0.0)  # 0 unless within particle's contact radius
-    # c = wp.leaky_min(dot(n, x0)-0.01, 0.0, 0.0)
-    fn = n * c * 1e5
-
-    wp.atomic_sub(f, particle_no, fn)
-
-    # # apply forces (could do - f / 3 here)
-    wp.atomic_add(f, i, fn * bary[0])
-    wp.atomic_add(f, j, fn * bary[1])
-    wp.atomic_add(f, k, fn * bary[2])
-
-
-@wp.kernel
-def eval_triangles_body_contacts(
-    num_particles: int,  # number of particles (size of contact_point)
-    x: wp.array(dtype=wp.vec3),  # position of particles
-    v: wp.array(dtype=wp.vec3),
-    indices: wp.array(dtype=int),  # triangle indices
-    body_x: wp.array(dtype=wp.vec3),  # body body positions
-    body_r: wp.array(dtype=wp.quat),
-    body_v: wp.array(dtype=wp.vec3),
-    body_w: wp.array(dtype=wp.vec3),
-    contact_body: wp.array(dtype=int),
-    contact_point: wp.array(dtype=wp.vec3),  # position of contact points relative to body
-    contact_dist: wp.array(dtype=float),
-    contact_mat: wp.array(dtype=int),
-    materials: wp.array(dtype=float),
-    #   body_f : wp.array(dtype=wp.vec3),
-    #   body_t : wp.array(dtype=wp.vec3),
-    tri_f: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-
-    face_no = tid // num_particles  # which face
-    particle_no = tid % num_particles  # which particle
-
-    # -----------------------
-    # load body body point
-    c_body = contact_body[particle_no]
-    c_point = contact_point[particle_no]
-    c_dist = contact_dist[particle_no]
-    c_mat = contact_mat[particle_no]
-
-    # hard coded surface parameter tensor layout (ke, kd, kf, mu)
-    ke = materials[c_mat * 4 + 0]  # restitution coefficient
-    kd = materials[c_mat * 4 + 1]  # damping coefficient
-    kf = materials[c_mat * 4 + 2]  # friction coefficient
-    mu = materials[c_mat * 4 + 3]  # coulomb friction
-
-    x0 = body_x[c_body]  # position of colliding body
-    r0 = body_r[c_body]  # orientation of colliding body
-
-    v0 = body_v[c_body]
-    w0 = body_w[c_body]
-
-    # transform point to world space
-    pos = x0 + wp.quat_rotate(r0, c_point)
-    # use x0 as center, everything is offset from center of mass
-
-    # moment arm
-    r = pos - x0  # basically just c_point in the new coordinates
-    rhat = wp.normalize(r)
-    pos = pos + rhat * c_dist  # add on 'thickness' of shape, e.g.: radius of sphere/capsule
-
-    # contact point velocity
-    dpdt = v0 + wp.cross(w0, r)  # this is body velocity cross offset, so it's the velocity of the contact point.
-
-    # -----------------------
-    # load triangle
-    i = indices[face_no * 3 + 0]
-    j = indices[face_no * 3 + 1]
-    k = indices[face_no * 3 + 2]
-
-    p = x[i]  # point zero
-    q = x[j]  # point one
-    r = x[k]  # point two
-
-    vp = v[i]  # vel zero
-    vq = v[j]  # vel one
-    vr = v[k]  # vel two
-
-    bary = triangle_closest_point_barycentric(p, q, r, pos)
-    closest = p * bary[0] + q * bary[1] + r * bary[2]
-
-    diff = pos - closest  # vector from tri to point
-    dist = wp.dot(diff, diff)  # squared distance
-    n = wp.normalize(diff)  # points into the object
-    c = wp.min(dist - 0.05, 0.0)  # 0 unless within 0.05 of surface
-    # c = wp.leaky_min(wp.dot(n, x0)-0.01, 0.0, 0.0)
-    # fn = n * c * 1e6    # points towards cloth (both n and c are negative)
-
-    # wp.atomic_sub(tri_f, particle_no, fn)
-
-    fn = c * ke  # normal force (restitution coefficient * how far inside for ground) (negative)
-
-    vtri = vp * bary[0] + vq * bary[1] + vr * bary[2]  # bad approximation for centroid velocity
-    vrel = vtri - dpdt
-
-    vn = wp.dot(n, vrel)  # velocity component of body in negative normal direction
-    vt = vrel - n * vn  # velocity component not in normal direction
-
-    # contact damping
-    fd = -wp.max(vn, 0.0) * kd * wp.step(c)  # again, negative, into the ground
-
-    # # viscous friction
-    # ft = vt*kf
-
-    # Coulomb friction (box)
-    lower = mu * (fn + fd)
-    upper = -lower
-
-    nx = wp.cross(n, wp.vec3(0.0, 0.0, 1.0))  # basis vectors for tangent
-    nz = wp.cross(n, wp.vec3(1.0, 0.0, 0.0))
-
-    vx = wp.clamp(wp.dot(nx * kf, vt), lower, upper)
-    vz = wp.clamp(wp.dot(nz * kf, vt), lower, upper)
-
-    ft = (nx * vx + nz * vz) * (-wp.step(c))  # wp.vec3(vx, 0.0, vz)*wp.step(c)
-
-    # # Coulomb friction (smooth, but gradients are numerically unstable around |vt| = 0)
-    # #ft = wp.normalize(vt)*wp.min(kf*wp.length(vt), -mu*c*ke)
-
-    f_total = n * (fn + fd) + ft
-
-    wp.atomic_add(tri_f, i, f_total * bary[0])
-    wp.atomic_add(tri_f, j, f_total * bary[1])
-    wp.atomic_add(tri_f, k, f_total * bary[2])
-
-
-@wp.kernel
-def eval_bending(
-    x: wp.array(dtype=wp.vec3),
-    v: wp.array(dtype=wp.vec3),
-    indices: wp.array2d(dtype=int),
-    rest: wp.array(dtype=float),
-    bending_properties: wp.array2d(dtype=float),
-    f: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-    ke = bending_properties[tid, 0]
-    kd = bending_properties[tid, 1]
-
-    i = indices[tid, 0]
-    j = indices[tid, 1]
-    k = indices[tid, 2]
-    l = indices[tid, 3]
-
-    if i == -1 or j == -1 or k == -1 or l == -1:
-        return
-
-    rest_angle = rest[tid]
-
-    x1 = x[i]
-    x2 = x[j]
-    x3 = x[k]
-    x4 = x[l]
-
-    v1 = v[i]
-    v2 = v[j]
-    v3 = v[k]
-    v4 = v[l]
-
-    n1 = wp.cross(x3 - x1, x4 - x1)  # normal to face 1
-    n2 = wp.cross(x4 - x2, x3 - x2)  # normal to face 2
-
-    n1_length = wp.length(n1)
-    n2_length = wp.length(n2)
-
-    if n1_length < 1.0e-6 or n2_length < 1.0e-6:
-        return
-
-    rcp_n1 = 1.0 / n1_length
-    rcp_n2 = 1.0 / n2_length
-
-    cos_theta = wp.dot(n1, n2) * rcp_n1 * rcp_n2
-
-    n1 = n1 * rcp_n1 * rcp_n1
-    n2 = n2 * rcp_n2 * rcp_n2
-
-    e = x4 - x3
-    e_hat = wp.normalize(e)
-    e_length = wp.length(e)
-
-    s = wp.sign(wp.dot(wp.cross(n2, n1), e_hat))
-    angle = wp.acos(cos_theta) * s
-
-    d1 = n1 * e_length
-    d2 = n2 * e_length
-    d3 = n1 * wp.dot(x1 - x4, e_hat) + n2 * wp.dot(x2 - x4, e_hat)
-    d4 = n1 * wp.dot(x3 - x1, e_hat) + n2 * wp.dot(x3 - x2, e_hat)
-
-    # elastic
-    f_elastic = ke * (angle - rest_angle)
-
-    # damping
-    f_damp = kd * (wp.dot(d1, v1) + wp.dot(d2, v2) + wp.dot(d3, v3) + wp.dot(d4, v4))
-
-    # total force, proportional to edge length
-    f_total = -e_length * (f_elastic + f_damp)
-
-    wp.atomic_add(f, i, d1 * f_total)
-    wp.atomic_add(f, j, d2 * f_total)
-    wp.atomic_add(f, k, d3 * f_total)
-    wp.atomic_add(f, l, d4 * f_total)
-
-
-@wp.kernel
-def eval_tetrahedra(
-    x: wp.array(dtype=wp.vec3),
-    v: wp.array(dtype=wp.vec3),
-    indices: wp.array2d(dtype=int),
-    pose: wp.array(dtype=wp.mat33),
-    activation: wp.array(dtype=float),
-    materials: wp.array2d(dtype=float),
-    f: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-
-    i = indices[tid, 0]
-    j = indices[tid, 1]
-    k = indices[tid, 2]
-    l = indices[tid, 3]
-
-    act = activation[tid]
-
-    k_mu = materials[tid, 0]
-    k_lambda = materials[tid, 1]
-    k_damp = materials[tid, 2]
-
-    x0 = x[i]
-    x1 = x[j]
-    x2 = x[k]
-    x3 = x[l]
-
-    v0 = v[i]
-    v1 = v[j]
-    v2 = v[k]
-    v3 = v[l]
-
-    x10 = x1 - x0
-    x20 = x2 - x0
-    x30 = x3 - x0
-
-    v10 = v1 - v0
-    v20 = v2 - v0
-    v30 = v3 - v0
-
-    Ds = wp.matrix_from_cols(x10, x20, x30)
-    Dm = pose[tid]
-
-    inv_rest_volume = wp.determinant(Dm) * 6.0
-    rest_volume = 1.0 / inv_rest_volume
-
-    alpha = 1.0 + k_mu / k_lambda - k_mu / (4.0 * k_lambda)
-
-    # scale stiffness coefficients to account for area
-    k_mu = k_mu * rest_volume
-    k_lambda = k_lambda * rest_volume
-    k_damp = k_damp * rest_volume
-
-    # F = Xs*Xm^-1
-    F = Ds * Dm
-    dFdt = wp.matrix_from_cols(v10, v20, v30) * Dm
-
-    col1 = wp.vec3(F[0, 0], F[1, 0], F[2, 0])
-    col2 = wp.vec3(F[0, 1], F[1, 1], F[2, 1])
-    col3 = wp.vec3(F[0, 2], F[1, 2], F[2, 2])
-
-    # -----------------------------
-    # Neo-Hookean (with rest stability [Smith et al 2018])
-
-    Ic = wp.dot(col1, col1) + wp.dot(col2, col2) + wp.dot(col3, col3)
-
-    # deviatoric part
-    P = F * k_mu * (1.0 - 1.0 / (Ic + 1.0)) + dFdt * k_damp
-    H = P * wp.transpose(Dm)
-
-    f1 = wp.vec3(H[0, 0], H[1, 0], H[2, 0])
-    f2 = wp.vec3(H[0, 1], H[1, 1], H[2, 1])
-    f3 = wp.vec3(H[0, 2], H[1, 2], H[2, 2])
-
-    # -----------------------------
-    # C_sqrt
-
-    # alpha = 1.0
-
-    # r_s = wp.sqrt(wp.abs(dot(col1, col1) + dot(col2, col2) + dot(col3, col3) - 3.0))
-
-    # f1 = wp.vec3()
-    # f2 = wp.vec3()
-    # f3 = wp.vec3()
-
-    # if (r_s > 0.0):
-    #     r_s_inv = 1.0/r_s
-
-    #     C = r_s
-    #     dCdx = F*wp.transpose(Dm)*r_s_inv*wp.sign(r_s)
-
-    #     grad1 = vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
-    #     grad2 = vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
-    #     grad3 = vec3(dCdx[0,2], dCdx[1,2], dCdx[2,2])
-
-    #     f1 = grad1*C*k_mu
-    #     f2 = grad2*C*k_mu
-    #     f3 = grad3*C*k_mu
-
-    # -----------------------------
-    # C_spherical
-
-    # alpha = 1.0
-
-    # r_s = wp.sqrt(dot(col1, col1) + dot(col2, col2) + dot(col3, col3))
-    # r_s_inv = 1.0/r_s
-
-    # C = r_s - wp.sqrt(3.0)
-    # dCdx = F*wp.transpose(Dm)*r_s_inv
-
-    # grad1 = vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
-    # grad2 = vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
-    # grad3 = vec3(dCdx[0,2], dCdx[1,2], dCdx[2,2])
-
-    # f1 = grad1*C*k_mu
-    # f2 = grad2*C*k_mu
-    # f3 = grad3*C*k_mu
-
-    # ----------------------------
-    # C_D
-
-    # alpha = 1.0
-
-    # r_s = wp.sqrt(dot(col1, col1) + dot(col2, col2) + dot(col3, col3))
-
-    # C = r_s*r_s - 3.0
-    # dCdx = F*wp.transpose(Dm)*2.0
-
-    # grad1 = vec3(dCdx[0,0], dCdx[1,0], dCdx[2,0])
-    # grad2 = vec3(dCdx[0,1], dCdx[1,1], dCdx[2,1])
-    # grad3 = vec3(dCdx[0,2], dCdx[1,2], dCdx[2,2])
-
-    # f1 = grad1*C*k_mu
-    # f2 = grad2*C*k_mu
-    # f3 = grad3*C*k_mu
-
-    # ----------------------------
-    # Hookean
-
-    # alpha = 1.0
-
-    # I = wp.matrix_from_cols(wp.vec3(1.0, 0.0, 0.0),
-    #                         wp.vec3(0.0, 1.0, 0.0),
-    #                         wp.vec3(0.0, 0.0, 1.0))
-
-    # P = (F + wp.transpose(F) + I*(0.0-2.0))*k_mu
-    # H = P * wp.transpose(Dm)
-
-    # f1 = wp.vec3(H[0, 0], H[1, 0], H[2, 0])
-    # f2 = wp.vec3(H[0, 1], H[1, 1], H[2, 1])
-    # f3 = wp.vec3(H[0, 2], H[1, 2], H[2, 2])
-
-    # hydrostatic part
-    J = wp.determinant(F)
-
-    # print(J)
-    s = inv_rest_volume / 6.0
-    dJdx1 = wp.cross(x20, x30) * s
-    dJdx2 = wp.cross(x30, x10) * s
-    dJdx3 = wp.cross(x10, x20) * s
-
-    f_volume = (J - alpha + act) * k_lambda
-    f_damp = (wp.dot(dJdx1, v1) + wp.dot(dJdx2, v2) + wp.dot(dJdx3, v3)) * k_damp
-
-    f_total = f_volume + f_damp
-
-    f1 = f1 + dJdx1 * f_total
-    f2 = f2 + dJdx2 * f_total
-    f3 = f3 + dJdx3 * f_total
-    f0 = -(f1 + f2 + f3)
-
-    # apply forces
-    wp.atomic_sub(f, i, f0)
-    wp.atomic_sub(f, j, f1)
-    wp.atomic_sub(f, k, f2)
-    wp.atomic_sub(f, l, f3)
-
-
-@wp.kernel
-def eval_particle_ground_contacts(
-    particle_x: wp.array(dtype=wp.vec3),
-    particle_v: wp.array(dtype=wp.vec3),
-    particle_radius: wp.array(dtype=float),
-    particle_flags: wp.array(dtype=wp.uint32),
-    ke: float,
-    kd: float,
-    kf: float,
-    mu: float,
-    ground: wp.array(dtype=float),
-    # outputs
-    f: wp.array(dtype=wp.vec3),
-):
-    tid = wp.tid()
-    if (particle_flags[tid] & PARTICLE_FLAG_ACTIVE) == 0:
-        return
-
-    x = particle_x[tid]
-    v = particle_v[tid]
-    radius = particle_radius[tid]
-
-    n = wp.vec3(ground[0], ground[1], ground[2])
-    c = wp.min(wp.dot(n, x) + ground[3] - radius, 0.0)
-
-    vn = wp.dot(n, v)
-    jn = c * ke
-
-    if c >= 0.0:
-        return
-
-    jd = min(vn, 0.0) * kd
-
-    # contact force
-    fn = jn + jd
-
-    # friction force
-    vt = v - n * vn
-    vs = wp.length(vt)
-
-    if vs > 0.0:
-        vt = vt / vs
-
-    # Coulomb condition
-    ft = wp.min(vs * kf, mu * wp.abs(fn))
-
-    # total force
-    f[tid] = f[tid] - n * fn - vt * ft
-
-
-@wp.kernel
-def eval_particle_contacts(
-    particle_x: wp.array(dtype=wp.vec3),
-    particle_v: wp.array(dtype=wp.vec3),
-    body_q: wp.array(dtype=wp.transform),
-    body_qd: wp.array(dtype=wp.spatial_vector),
-    particle_radius: wp.array(dtype=float),
-    particle_flags: wp.array(dtype=wp.uint32),
-    body_com: wp.array(dtype=wp.vec3),
-    shape_body: wp.array(dtype=int),
-    shape_materials: ModelShapeMaterials,
-    particle_ke: float,
-    particle_kd: float,
-    particle_kf: float,
-    particle_mu: float,
-    particle_ka: float,
-    contact_count: wp.array(dtype=int),
-    contact_particle: wp.array(dtype=int),
-    contact_shape: wp.array(dtype=int),
-    contact_body_pos: wp.array(dtype=wp.vec3),
-    contact_body_vel: wp.array(dtype=wp.vec3),
-    contact_normal: wp.array(dtype=wp.vec3),
-    contact_max: int,
-    body_f_in_world_frame: bool,
-    # outputs
-    particle_f: wp.array(dtype=wp.vec3),
-    body_f: wp.array(dtype=wp.spatial_vector),
-):
-    tid = wp.tid()
-
-    count = min(contact_max, contact_count[0])
-    if tid >= count:
-        return
-
-    shape_index = contact_shape[tid]
-    body_index = shape_body[shape_index]
-    particle_index = contact_particle[tid]
-    if (particle_flags[particle_index] & PARTICLE_FLAG_ACTIVE) == 0:
-        return
-
-    px = particle_x[particle_index]
-    pv = particle_v[particle_index]
-
-    X_wb = wp.transform_identity()
-    X_com = wp.vec3()
-    body_v_s = wp.spatial_vector()
-
-    if body_index >= 0:
-        X_wb = body_q[body_index]
-        X_com = body_com[body_index]
-        body_v_s = body_qd[body_index]
-
-    # body position in world space
-    bx = wp.transform_point(X_wb, contact_body_pos[tid])
-    r = bx - wp.transform_point(X_wb, X_com)
-
-    n = contact_normal[tid]
-    c = wp.dot(n, px - bx) - particle_radius[particle_index]
-
-    if c > particle_ka:
-        return
-
-    # take average material properties of shape and particle parameters
-    ke = 0.5 * (particle_ke + shape_materials.ke[shape_index])
-    kd = 0.5 * (particle_kd + shape_materials.kd[shape_index])
-    kf = 0.5 * (particle_kf + shape_materials.kf[shape_index])
-    mu = 0.5 * (particle_mu + shape_materials.mu[shape_index])
-
-    body_w = wp.spatial_top(body_v_s)
-    body_v = wp.spatial_bottom(body_v_s)
-
-    # compute the body velocity at the particle position
-    bv = body_v + wp.transform_vector(X_wb, contact_body_vel[tid])
-    if body_f_in_world_frame:
-        bv += wp.cross(body_w, bx)
-    else:
-        bv += wp.cross(body_w, r)
-
-    # relative velocity
-    v = pv - bv
-
-    # decompose relative velocity
-    vn = wp.dot(n, v)
-    vt = v - n * vn
-
-    # contact elastic
-    fn = n * c * ke
-
-    # contact damping
-    fd = n * wp.min(vn, 0.0) * kd
-
-    # viscous friction
-    # ft = vt*kf
-
-    # Coulomb friction (box)
-    # lower = mu * c * ke
-    # upper = -lower
-
-    # vx = wp.clamp(wp.dot(wp.vec3(kf, 0.0, 0.0), vt), lower, upper)
-    # vz = wp.clamp(wp.dot(wp.vec3(0.0, 0.0, kf), vt), lower, upper)
-
-    # ft = wp.vec3(vx, 0.0, vz)
-
-    # Coulomb friction (smooth, but gradients are numerically unstable around |vt| = 0)
-    ft = wp.normalize(vt) * wp.min(kf * wp.length(vt), abs(mu * c * ke))
-
-    f_total = fn + (fd + ft)
-
-    wp.atomic_sub(particle_f, particle_index, f_total)
-
-    if body_index >= 0:
-        if body_f_in_world_frame:
-            wp.atomic_sub(body_f, body_index, wp.spatial_vector(wp.cross(bx, f_total), f_total))
-        else:
-            wp.atomic_add(body_f, body_index, wp.spatial_vector(wp.cross(r, f_total), f_total))
-
-
-@wp.kernel
-def eval_rigid_contacts(
-    body_q: wp.array(dtype=wp.transform),
-    body_qd: wp.array(dtype=wp.spatial_vector),
-    body_com: wp.array(dtype=wp.vec3),
-    shape_materials: ModelShapeMaterials,
-    geo: ModelShapeGeometry,
-    shape_body: wp.array(dtype=int),
-    contact_count: wp.array(dtype=int),
-    contact_point0: wp.array(dtype=wp.vec3),
-    contact_point1: wp.array(dtype=wp.vec3),
-    contact_normal: wp.array(dtype=wp.vec3),
-    contact_shape0: wp.array(dtype=int),
-    contact_shape1: wp.array(dtype=int),
-    force_in_world_frame: bool,
-    friction_smoothing: float,
-    # outputs
-    body_f: wp.array(dtype=wp.spatial_vector),
-):
-    tid = wp.tid()
-
-    count = contact_count[0]
-    if tid >= count:
-        return
-
-    # retrieve contact thickness, compute average contact material properties
-    ke = 0.0  # contact normal force stiffness
-    kd = 0.0  # damping coefficient
-    kf = 0.0  # friction force stiffness
-    ka = 0.0  # adhesion distance
-    mu = 0.0  # friction coefficient
-    mat_nonzero = 0
-    thickness_a = 0.0
-    thickness_b = 0.0
-    shape_a = contact_shape0[tid]
-    shape_b = contact_shape1[tid]
-    if shape_a == shape_b:
-        return
-    body_a = -1
-    body_b = -1
-    if shape_a >= 0:
-        mat_nonzero += 1
-        ke += shape_materials.ke[shape_a]
-        kd += shape_materials.kd[shape_a]
-        kf += shape_materials.kf[shape_a]
-        ka += shape_materials.ka[shape_a]
-        mu += shape_materials.mu[shape_a]
-        thickness_a = geo.thickness[shape_a]
-        body_a = shape_body[shape_a]
-    if shape_b >= 0:
-        mat_nonzero += 1
-        ke += shape_materials.ke[shape_b]
-        kd += shape_materials.kd[shape_b]
-        kf += shape_materials.kf[shape_b]
-        ka += shape_materials.ka[shape_b]
-        mu += shape_materials.mu[shape_b]
-        thickness_b = geo.thickness[shape_b]
-        body_b = shape_body[shape_b]
-    if mat_nonzero > 0:
-        ke /= float(mat_nonzero)
-        kd /= float(mat_nonzero)
-        kf /= float(mat_nonzero)
-        ka /= float(mat_nonzero)
-        mu /= float(mat_nonzero)
-
-    # contact normal in world space
-    n = contact_normal[tid]
-    bx_a = contact_point0[tid]
-    bx_b = contact_point1[tid]
-    r_a = wp.vec3(0.0)
-    r_b = wp.vec3(0.0)
-    if body_a >= 0:
-        X_wb_a = body_q[body_a]
-        X_com_a = body_com[body_a]
-        bx_a = wp.transform_point(X_wb_a, bx_a) - thickness_a * n
-        r_a = bx_a - wp.transform_point(X_wb_a, X_com_a)
-
-    if body_b >= 0:
-        X_wb_b = body_q[body_b]
-        X_com_b = body_com[body_b]
-        bx_b = wp.transform_point(X_wb_b, bx_b) + thickness_b * n
-        r_b = bx_b - wp.transform_point(X_wb_b, X_com_b)
-
-    d = wp.dot(n, bx_a - bx_b)
-
-    if d >= ka:
-        return
-
-    # compute contact point velocity
-    bv_a = wp.vec3(0.0)
-    bv_b = wp.vec3(0.0)
-    if body_a >= 0:
-        body_v_s_a = body_qd[body_a]
-        body_w_a = wp.spatial_top(body_v_s_a)
-        body_v_a = wp.spatial_bottom(body_v_s_a)
-        if force_in_world_frame:
-            bv_a = body_v_a + wp.cross(body_w_a, bx_a)
-        else:
-            bv_a = body_v_a + wp.cross(body_w_a, r_a)
-
-    if body_b >= 0:
-        body_v_s_b = body_qd[body_b]
-        body_w_b = wp.spatial_top(body_v_s_b)
-        body_v_b = wp.spatial_bottom(body_v_s_b)
-        if force_in_world_frame:
-            bv_b = body_v_b + wp.cross(body_w_b, bx_b)
-        else:
-            bv_b = body_v_b + wp.cross(body_w_b, r_b)
-
-    # relative velocity
-    v = bv_a - bv_b
-
-    # print(v)
-
-    # decompose relative velocity
-    vn = wp.dot(n, v)
-    vt = v - n * vn
-
-    # contact elastic
-    fn = d * ke
-
-    # contact damping
-    fd = wp.min(vn, 0.0) * kd * wp.step(d)
-
-    # viscous friction
-    # ft = vt*kf
-
-    # Coulomb friction (box)
-    # lower = mu * d * ke
-    # upper = -lower
-
-    # vx = wp.clamp(wp.dot(wp.vec3(kf, 0.0, 0.0), vt), lower, upper)
-    # vz = wp.clamp(wp.dot(wp.vec3(0.0, 0.0, kf), vt), lower, upper)
-
-    # ft = wp.vec3(vx, 0.0, vz)
-
-    # Coulomb friction (smooth, but gradients are numerically unstable around |vt| = 0)
-    ft = wp.vec3(0.0)
-    if d < 0.0:
-        # use a smooth vector norm to avoid gradient instability at/around zero velocity
-        vs = wp.norm_huber(vt, delta=friction_smoothing)
-        if vs > 0.0:
-            fr = vt / vs
-            ft = fr * wp.min(kf * vs, -mu * (fn + fd))
-
-    f_total = n * (fn + fd) + ft
-    # f_total = n * (fn + fd)
-    # f_total = n * fn
-
-    if body_a >= 0:
-        if force_in_world_frame:
-            wp.atomic_add(body_f, body_a, wp.spatial_vector(wp.cross(bx_a, f_total), f_total))
-        else:
-            wp.atomic_sub(body_f, body_a, wp.spatial_vector(wp.cross(r_a, f_total), f_total))
-
-    if body_b >= 0:
-        if force_in_world_frame:
-            wp.atomic_sub(body_f, body_b, wp.spatial_vector(wp.cross(bx_b, f_total), f_total))
-        else:
-            wp.atomic_add(body_f, body_b, wp.spatial_vector(wp.cross(r_b, f_total), f_total))
-
-
-@wp.func
-def eval_joint_force(
-    q: float,
-    qd: float,
-    act: float,
-    target_ke: float,
-    target_kd: float,
-    limit_lower: float,
-    limit_upper: float,
-    limit_ke: float,
-    limit_kd: float,
-    mode: wp.int32,
-) -> float:
-    """Joint force evaluation for a single degree of freedom."""
-
-    limit_f = 0.0
-    damping_f = 0.0
-    target_f = 0.0
-
-    if mode == wp.sim.JOINT_MODE_FORCE:
-        target_f = act
-    elif mode == wp.sim.JOINT_MODE_TARGET_POSITION:
-        target_f = target_ke * (act - q) - target_kd * qd
-    elif mode == wp.sim.JOINT_MODE_TARGET_VELOCITY:
-        target_f = target_ke * (act - qd)
-
-    # compute limit forces, damping only active when limit is violated
-    if q < limit_lower:
-        limit_f = limit_ke * (limit_lower - q)
-        damping_f = -limit_kd * qd
-        if mode == wp.sim.JOINT_MODE_TARGET_VELOCITY:
-            target_f = 0.0  # override target force when limit is violated
-    elif q > limit_upper:
-        limit_f = limit_ke * (limit_upper - q)
-        damping_f = -limit_kd * qd
-        if mode == wp.sim.JOINT_MODE_TARGET_VELOCITY:
-            target_f = 0.0  # override target force when limit is violated
-
-    return limit_f + damping_f + target_f
-
-
-@wp.kernel
-def eval_body_joints(
-    body_q: wp.array(dtype=wp.transform),
-    body_qd: wp.array(dtype=wp.spatial_vector),
-    body_com: wp.array(dtype=wp.vec3),
-    joint_qd_start: wp.array(dtype=int),
-    joint_type: wp.array(dtype=int),
-    joint_enabled: wp.array(dtype=int),
-    joint_child: wp.array(dtype=int),
-    joint_parent: wp.array(dtype=int),
-    joint_X_p: wp.array(dtype=wp.transform),
-    joint_X_c: wp.array(dtype=wp.transform),
-    joint_axis: wp.array(dtype=wp.vec3),
-    joint_axis_start: wp.array(dtype=int),
-    joint_axis_dim: wp.array(dtype=int, ndim=2),
-    joint_axis_mode: wp.array(dtype=int),
-    joint_act: wp.array(dtype=float),
-    joint_target_ke: wp.array(dtype=float),
-    joint_target_kd: wp.array(dtype=float),
-    joint_limit_lower: wp.array(dtype=float),
-    joint_limit_upper: wp.array(dtype=float),
-    joint_limit_ke: wp.array(dtype=float),
-    joint_limit_kd: wp.array(dtype=float),
-    joint_attach_ke: float,
-    joint_attach_kd: float,
-    body_f: wp.array(dtype=wp.spatial_vector),
-):
-    tid = wp.tid()
-    type = joint_type[tid]
-
-    # early out for free joints
-    if joint_enabled[tid] == 0 or type == wp.sim.JOINT_FREE:
-        return
-
-    c_child = joint_child[tid]
-    c_parent = joint_parent[tid]
-
-    X_pj = joint_X_p[tid]
-    X_cj = joint_X_c[tid]
-
-    X_wp = X_pj
-    r_p = wp.vec3()
-    w_p = wp.vec3()
-    v_p = wp.vec3()
-
-    # parent transform and moment arm
-    if c_parent >= 0:
-        X_wp = body_q[c_parent] * X_wp
-        r_p = wp.transform_get_translation(X_wp) - wp.transform_point(body_q[c_parent], body_com[c_parent])
-
-        twist_p = body_qd[c_parent]
-
-        w_p = wp.spatial_top(twist_p)
-        v_p = wp.spatial_bottom(twist_p) + wp.cross(w_p, r_p)
-
-    # child transform and moment arm
-    X_wc = body_q[c_child] * X_cj
-    r_c = wp.transform_get_translation(X_wc) - wp.transform_point(body_q[c_child], body_com[c_child])
-
-    twist_c = body_qd[c_child]
-
-    w_c = wp.spatial_top(twist_c)
-    v_c = wp.spatial_bottom(twist_c) + wp.cross(w_c, r_c)
-
-    # joint properties (for 1D joints)
-    # q_start = joint_q_start[tid]
-    # qd_start = joint_qd_start[tid]
-    axis_start = joint_axis_start[tid]
-
-    lin_axis_count = joint_axis_dim[tid, 0]
-    ang_axis_count = joint_axis_dim[tid, 1]
-
-    x_p = wp.transform_get_translation(X_wp)
-    x_c = wp.transform_get_translation(X_wc)
-
-    q_p = wp.transform_get_rotation(X_wp)
-    q_c = wp.transform_get_rotation(X_wc)
-
-    # translational error
-    x_err = x_c - x_p
-    r_err = wp.quat_inverse(q_p) * q_c
-    v_err = v_c - v_p
-    w_err = w_c - w_p
-
-    # total force/torque on the parent
-    t_total = wp.vec3()
-    f_total = wp.vec3()
-
-    # reduce angular damping stiffness for stability
-    angular_damping_scale = 0.01
-
-    if type == wp.sim.JOINT_FIXED:
-        ang_err = wp.normalize(wp.vec3(r_err[0], r_err[1], r_err[2])) * wp.acos(r_err[3]) * 2.0
-
-        f_total += x_err * joint_attach_ke + v_err * joint_attach_kd
-        t_total += (
-            wp.transform_vector(X_wp, ang_err) * joint_attach_ke + w_err * joint_attach_kd * angular_damping_scale
-        )
-
-    if type == wp.sim.JOINT_PRISMATIC:
-        axis = joint_axis[axis_start]
-
-        # world space joint axis
-        axis_p = wp.transform_vector(X_wp, axis)
-
-        # evaluate joint coordinates
-        q = wp.dot(x_err, axis_p)
-        qd = wp.dot(v_err, axis_p)
-        act = joint_act[axis_start]
-
-        f_total = axis_p * -eval_joint_force(
-            q,
-            qd,
-            act,
-            joint_target_ke[axis_start],
-            joint_target_kd[axis_start],
-            joint_limit_lower[axis_start],
-            joint_limit_upper[axis_start],
-            joint_limit_ke[axis_start],
-            joint_limit_kd[axis_start],
-            joint_axis_mode[axis_start],
-        )
-
-        # attachment dynamics
-        ang_err = wp.normalize(wp.vec3(r_err[0], r_err[1], r_err[2])) * wp.acos(r_err[3]) * 2.0
-
-        # project off any displacement along the joint axis
-        f_total += (x_err - q * axis_p) * joint_attach_ke + (v_err - qd * axis_p) * joint_attach_kd
-        t_total += (
-            wp.transform_vector(X_wp, ang_err) * joint_attach_ke + w_err * joint_attach_kd * angular_damping_scale
-        )
-
-    if type == wp.sim.JOINT_REVOLUTE:
-        axis = joint_axis[axis_start]
-
-        axis_p = wp.transform_vector(X_wp, axis)
-        axis_c = wp.transform_vector(X_wc, axis)
-
-        # swing twist decomposition
-        twist = quat_twist(axis, r_err)
-
-        q = wp.acos(twist[3]) * 2.0 * wp.sign(wp.dot(axis, wp.vec3(twist[0], twist[1], twist[2])))
-        qd = wp.dot(w_err, axis_p)
-        act = joint_act[axis_start]
-
-        t_total = axis_p * -eval_joint_force(
-            q,
-            qd,
-            act,
-            joint_target_ke[axis_start],
-            joint_target_kd[axis_start],
-            joint_limit_lower[axis_start],
-            joint_limit_upper[axis_start],
-            joint_limit_ke[axis_start],
-            joint_limit_kd[axis_start],
-            joint_axis_mode[axis_start],
-        )
-
-        # attachment dynamics
-        swing_err = wp.cross(axis_p, axis_c)
-
-        f_total += x_err * joint_attach_ke + v_err * joint_attach_kd
-        t_total += swing_err * joint_attach_ke + (w_err - qd * axis_p) * joint_attach_kd * angular_damping_scale
-
-    if type == wp.sim.JOINT_BALL:
-        ang_err = wp.normalize(wp.vec3(r_err[0], r_err[1], r_err[2])) * wp.acos(r_err[3]) * 2.0
-
-        # TODO joint limits
-        # TODO expose target_kd or target_ke for ball joints
-        # t_total += target_kd * w_err + target_ke * wp.transform_vector(X_wp, ang_err)
-        f_total += x_err * joint_attach_ke + v_err * joint_attach_kd
-
-    if type == wp.sim.JOINT_COMPOUND:
-        q_pc = wp.quat_inverse(q_p) * q_c
-
-        # decompose to a compound rotation each axis
-        angles = quat_decompose(q_pc)
-
-        # reconstruct rotation axes
-        axis_0 = wp.vec3(1.0, 0.0, 0.0)
-        q_0 = wp.quat_from_axis_angle(axis_0, angles[0])
-
-        axis_1 = wp.quat_rotate(q_0, wp.vec3(0.0, 1.0, 0.0))
-        q_1 = wp.quat_from_axis_angle(axis_1, angles[1])
-
-        axis_2 = wp.quat_rotate(q_1 * q_0, wp.vec3(0.0, 0.0, 1.0))
-        # q_2 = wp.quat_from_axis_angle(axis_2, angles[2])
-
-        # q_w = q_p
-
-        axis_0 = wp.transform_vector(X_wp, axis_0)
-        axis_1 = wp.transform_vector(X_wp, axis_1)
-        axis_2 = wp.transform_vector(X_wp, axis_2)
-
-        # joint dynamics
-        # # TODO remove wp.quat_rotate(q_w, ...)?
-        # t_total += eval_joint_force(angles[0], wp.dot(wp.quat_rotate(q_w, axis_0), w_err), joint_target[axis_start+0], joint_target_ke[axis_start+0],joint_target_kd[axis_start+0], joint_act[axis_start+0], joint_limit_lower[axis_start+0], joint_limit_upper[axis_start+0], joint_limit_ke[axis_start+0], joint_limit_kd[axis_start+0], wp.quat_rotate(q_w, axis_0))
-        # t_total += eval_joint_force(angles[1], wp.dot(wp.quat_rotate(q_w, axis_1), w_err), joint_target[axis_start+1], joint_target_ke[axis_start+1],joint_target_kd[axis_start+1], joint_act[axis_start+1], joint_limit_lower[axis_start+1], joint_limit_upper[axis_start+1], joint_limit_ke[axis_start+1], joint_limit_kd[axis_start+1], wp.quat_rotate(q_w, axis_1))
-        # t_total += eval_joint_force(angles[2], wp.dot(wp.quat_rotate(q_w, axis_2), w_err), joint_target[axis_start+2], joint_target_ke[axis_start+2],joint_target_kd[axis_start+2], joint_act[axis_start+2], joint_limit_lower[axis_start+2], joint_limit_upper[axis_start+2], joint_limit_ke[axis_start+2], joint_limit_kd[axis_start+2], wp.quat_rotate(q_w, axis_2))
-
-        t_total += axis_0 * -eval_joint_force(
-            angles[0],
-            wp.dot(axis_0, w_err),
-            joint_act[axis_start + 0],
-            joint_target_ke[axis_start + 0],
-            joint_target_kd[axis_start + 0],
-            joint_limit_lower[axis_start + 0],
-            joint_limit_upper[axis_start + 0],
-            joint_limit_ke[axis_start + 0],
-            joint_limit_kd[axis_start + 0],
-            joint_axis_mode[axis_start + 0],
-        )
-        t_total += axis_1 * -eval_joint_force(
-            angles[1],
-            wp.dot(axis_1, w_err),
-            joint_act[axis_start + 1],
-            joint_target_ke[axis_start + 1],
-            joint_target_kd[axis_start + 1],
-            joint_limit_lower[axis_start + 1],
-            joint_limit_upper[axis_start + 1],
-            joint_limit_ke[axis_start + 1],
-            joint_limit_kd[axis_start + 1],
-            joint_axis_mode[axis_start + 1],
-        )
-        t_total += axis_2 * -eval_joint_force(
-            angles[2],
-            wp.dot(axis_2, w_err),
-            joint_act[axis_start + 2],
-            joint_target_ke[axis_start + 2],
-            joint_target_kd[axis_start + 2],
-            joint_limit_lower[axis_start + 2],
-            joint_limit_upper[axis_start + 2],
-            joint_limit_ke[axis_start + 2],
-            joint_limit_kd[axis_start + 2],
-            joint_axis_mode[axis_start + 2],
-        )
-
-        f_total += x_err * joint_attach_ke + v_err * joint_attach_kd
-
-    if type == wp.sim.JOINT_UNIVERSAL:
-        q_pc = wp.quat_inverse(q_p) * q_c
-
-        # decompose to a compound rotation each axis
-        angles = quat_decompose(q_pc)
-
-        # reconstruct rotation axes
-        axis_0 = wp.vec3(1.0, 0.0, 0.0)
-        q_0 = wp.quat_from_axis_angle(axis_0, angles[0])
-
-        axis_1 = wp.quat_rotate(q_0, wp.vec3(0.0, 1.0, 0.0))
-        q_1 = wp.quat_from_axis_angle(axis_1, angles[1])
-
-        axis_2 = wp.quat_rotate(q_1 * q_0, wp.vec3(0.0, 0.0, 1.0))
-
-        axis_0 = wp.transform_vector(X_wp, axis_0)
-        axis_1 = wp.transform_vector(X_wp, axis_1)
-        axis_2 = wp.transform_vector(X_wp, axis_2)
-
-        # joint dynamics
-
-        t_total += axis_0 * -eval_joint_force(
-            angles[0],
-            wp.dot(axis_0, w_err),
-            joint_act[axis_start + 0],
-            joint_target_ke[axis_start + 0],
-            joint_target_kd[axis_start + 0],
-            joint_limit_lower[axis_start + 0],
-            joint_limit_upper[axis_start + 0],
-            joint_limit_ke[axis_start + 0],
-            joint_limit_kd[axis_start + 0],
-            joint_axis_mode[axis_start + 0],
-        )
-        t_total += axis_1 * -eval_joint_force(
-            angles[1],
-            wp.dot(axis_1, w_err),
-            joint_act[axis_start + 1],
-            joint_target_ke[axis_start + 1],
-            joint_target_kd[axis_start + 1],
-            joint_limit_lower[axis_start + 1],
-            joint_limit_upper[axis_start + 1],
-            joint_limit_ke[axis_start + 1],
-            joint_limit_kd[axis_start + 1],
-            joint_axis_mode[axis_start + 1],
-        )
-
-        # last axis (fixed)
-        t_total += axis_2 * -eval_joint_force(
-            angles[2],
-            wp.dot(axis_2, w_err),
-            0.0,
-            joint_attach_ke,
-            joint_attach_kd * angular_damping_scale,
-            0.0,
-            0.0,
-            0.0,
-            0.0,
-            wp.sim.JOINT_MODE_FORCE,
-        )
-
-        f_total += x_err * joint_attach_ke + v_err * joint_attach_kd
-
-    if type == wp.sim.JOINT_D6:
-        pos = wp.vec3(0.0)
-        vel = wp.vec3(0.0)
-        if lin_axis_count >= 1:
-            axis_0 = wp.transform_vector(X_wp, joint_axis[axis_start + 0])
-            q0 = wp.dot(x_err, axis_0)
-            qd0 = wp.dot(v_err, axis_0)
-
-            f_total += axis_0 * -eval_joint_force(
-                q0,
-                qd0,
-                joint_act[axis_start + 0],
-                joint_target_ke[axis_start + 0],
-                joint_target_kd[axis_start + 0],
-                joint_limit_lower[axis_start + 0],
-                joint_limit_upper[axis_start + 0],
-                joint_limit_ke[axis_start + 0],
-                joint_limit_kd[axis_start + 0],
-                joint_axis_mode[axis_start + 0],
-            )
-
-            pos += q0 * axis_0
-            vel += qd0 * axis_0
-
-        if lin_axis_count >= 2:
-            axis_1 = wp.transform_vector(X_wp, joint_axis[axis_start + 1])
-            q1 = wp.dot(x_err, axis_1)
-            qd1 = wp.dot(v_err, axis_1)
-
-            f_total += axis_1 * -eval_joint_force(
-                q1,
-                qd1,
-                joint_act[axis_start + 1],
-                joint_target_ke[axis_start + 1],
-                joint_target_kd[axis_start + 1],
-                joint_limit_lower[axis_start + 1],
-                joint_limit_upper[axis_start + 1],
-                joint_limit_ke[axis_start + 1],
-                joint_limit_kd[axis_start + 1],
-                joint_axis_mode[axis_start + 1],
-            )
-
-            pos += q1 * axis_1
-            vel += qd1 * axis_1
-
-        if lin_axis_count == 3:
-            axis_2 = wp.transform_vector(X_wp, joint_axis[axis_start + 2])
-            q2 = wp.dot(x_err, axis_2)
-            qd2 = wp.dot(v_err, axis_2)
-
-            f_total += axis_2 * -eval_joint_force(
-                q2,
-                qd2,
-                joint_act[axis_start + 2],
-                joint_target_ke[axis_start + 2],
-                joint_target_kd[axis_start + 2],
-                joint_limit_lower[axis_start + 2],
-                joint_limit_upper[axis_start + 2],
-                joint_limit_ke[axis_start + 2],
-                joint_limit_kd[axis_start + 2],
-                joint_axis_mode[axis_start + 2],
-            )
-
-            pos += q2 * axis_2
-            vel += qd2 * axis_2
-
-        f_total += (x_err - pos) * joint_attach_ke + (v_err - vel) * joint_attach_kd
-
-        if ang_axis_count == 0:
-            ang_err = wp.normalize(wp.vec3(r_err[0], r_err[1], r_err[2])) * wp.acos(r_err[3]) * 2.0
-            t_total += (
-                wp.transform_vector(X_wp, ang_err) * joint_attach_ke + w_err * joint_attach_kd * angular_damping_scale
-            )
-
-        i_0 = lin_axis_count + axis_start + 0
-        i_1 = lin_axis_count + axis_start + 1
-        i_2 = lin_axis_count + axis_start + 2
-
-        if ang_axis_count == 1:
-            axis = joint_axis[i_0]
-
-            axis_p = wp.transform_vector(X_wp, axis)
-            axis_c = wp.transform_vector(X_wc, axis)
-
-            # swing twist decomposition
-            twist = quat_twist(axis, r_err)
-
-            q = wp.acos(twist[3]) * 2.0 * wp.sign(wp.dot(axis, wp.vec3(twist[0], twist[1], twist[2])))
-            qd = wp.dot(w_err, axis_p)
-
-            t_total = axis_p * -eval_joint_force(
-                q,
-                qd,
-                joint_act[i_0],
-                joint_target_ke[i_0],
-                joint_target_kd[i_0],
-                joint_limit_lower[i_0],
-                joint_limit_upper[i_0],
-                joint_limit_ke[i_0],
-                joint_limit_kd[i_0],
-                joint_axis_mode[i_0],
-            )
-
-            # attachment dynamics
-            swing_err = wp.cross(axis_p, axis_c)
-
-            t_total += swing_err * joint_attach_ke + (w_err - qd * axis_p) * joint_attach_kd * angular_damping_scale
-
-        if ang_axis_count == 2:
-            q_pc = wp.quat_inverse(q_p) * q_c
-
-            # decompose to a compound rotation each axis
-            angles = quat_decompose(q_pc)
-
-            orig_axis_0 = joint_axis[i_0]
-            orig_axis_1 = joint_axis[i_1]
-            orig_axis_2 = wp.cross(orig_axis_0, orig_axis_1)
-
-            # reconstruct rotation axes
-            axis_0 = orig_axis_0
-            q_0 = wp.quat_from_axis_angle(axis_0, angles[0])
-
-            axis_1 = wp.quat_rotate(q_0, orig_axis_1)
-            q_1 = wp.quat_from_axis_angle(axis_1, angles[1])
-
-            axis_2 = wp.quat_rotate(q_1 * q_0, orig_axis_2)
-
-            axis_0 = wp.transform_vector(X_wp, axis_0)
-            axis_1 = wp.transform_vector(X_wp, axis_1)
-            axis_2 = wp.transform_vector(X_wp, axis_2)
-
-            # joint dynamics
-
-            t_total += axis_0 * -eval_joint_force(
-                angles[0],
-                wp.dot(axis_0, w_err),
-                joint_act[i_0],
-                joint_target_ke[i_0],
-                joint_target_kd[i_0],
-                joint_limit_lower[i_0],
-                joint_limit_upper[i_0],
-                joint_limit_ke[i_0],
-                joint_limit_kd[i_0],
-                joint_axis_mode[i_0],
-            )
-            t_total += axis_1 * -eval_joint_force(
-                angles[1],
-                wp.dot(axis_1, w_err),
-                joint_act[i_1],
-                joint_target_ke[i_1],
-                joint_target_kd[i_1],
-                joint_limit_lower[i_1],
-                joint_limit_upper[i_1],
-                joint_limit_ke[i_1],
-                joint_limit_kd[i_1],
-                joint_axis_mode[i_1],
-            )
-
-            # last axis (fixed)
-            t_total += axis_2 * -eval_joint_force(
-                angles[2],
-                wp.dot(axis_2, w_err),
-                0.0,
-                joint_attach_ke,
-                joint_attach_kd * angular_damping_scale,
-                0.0,
-                0.0,
-                0.0,
-                0.0,
-                wp.sim.JOINT_MODE_FORCE,
-            )
-
-        if ang_axis_count == 3:
-            q_pc = wp.quat_inverse(q_p) * q_c
-
-            # decompose to a compound rotation each axis
-            angles = quat_decompose(q_pc)
-
-            orig_axis_0 = joint_axis[i_0]
-            orig_axis_1 = joint_axis[i_1]
-            orig_axis_2 = joint_axis[i_2]
-
-            # reconstruct rotation axes
-            axis_0 = orig_axis_0
-            q_0 = wp.quat_from_axis_angle(axis_0, angles[0])
-
-            axis_1 = wp.quat_rotate(q_0, orig_axis_1)
-            q_1 = wp.quat_from_axis_angle(axis_1, angles[1])
-
-            axis_2 = wp.quat_rotate(q_1 * q_0, orig_axis_2)
-
-            axis_0 = wp.transform_vector(X_wp, axis_0)
-            axis_1 = wp.transform_vector(X_wp, axis_1)
-            axis_2 = wp.transform_vector(X_wp, axis_2)
-
-            t_total += axis_0 * -eval_joint_force(
-                angles[0],
-                wp.dot(axis_0, w_err),
-                joint_act[i_0],
-                joint_target_ke[i_0],
-                joint_target_kd[i_0],
-                joint_limit_lower[i_0],
-                joint_limit_upper[i_0],
-                joint_limit_ke[i_0],
-                joint_limit_kd[i_0],
-                joint_axis_mode[i_0],
-            )
-            t_total += axis_1 * -eval_joint_force(
-                angles[1],
-                wp.dot(axis_1, w_err),
-                joint_act[i_1],
-                joint_target_ke[i_1],
-                joint_target_kd[i_1],
-                joint_limit_lower[i_1],
-                joint_limit_upper[i_1],
-                joint_limit_ke[i_1],
-                joint_limit_kd[i_1],
-                joint_axis_mode[i_1],
-            )
-            t_total += axis_2 * -eval_joint_force(
-                angles[2],
-                wp.dot(axis_2, w_err),
-                joint_act[i_2],
-                joint_target_ke[i_2],
-                joint_target_kd[i_2],
-                joint_limit_lower[i_2],
-                joint_limit_upper[i_2],
-                joint_limit_ke[i_2],
-                joint_limit_kd[i_2],
-                joint_axis_mode[i_2],
-            )
-
-    # write forces
-    if c_parent >= 0:
-        wp.atomic_add(body_f, c_parent, wp.spatial_vector(t_total + wp.cross(r_p, f_total), f_total))
-
-    wp.atomic_sub(body_f, c_child, wp.spatial_vector(t_total + wp.cross(r_c, f_total), f_total))
-
-
-@wp.func
-def compute_muscle_force(
-    i: int,
-    body_X_s: wp.array(dtype=wp.transform),
-    body_v_s: wp.array(dtype=wp.spatial_vector),
-    body_com: wp.array(dtype=wp.vec3),
-    muscle_links: wp.array(dtype=int),
-    muscle_points: wp.array(dtype=wp.vec3),
-    muscle_activation: float,
-    body_f_s: wp.array(dtype=wp.spatial_vector),
-):
-    link_0 = muscle_links[i]
-    link_1 = muscle_links[i + 1]
-
-    if link_0 == link_1:
-        return 0
-
-    r_0 = muscle_points[i]
-    r_1 = muscle_points[i + 1]
-
-    xform_0 = body_X_s[link_0]
-    xform_1 = body_X_s[link_1]
-
-    pos_0 = wp.transform_point(xform_0, r_0 - body_com[link_0])
-    pos_1 = wp.transform_point(xform_1, r_1 - body_com[link_1])
-
-    n = wp.normalize(pos_1 - pos_0)
-
-    # todo: add passive elastic and viscosity terms
-    f = n * muscle_activation
-
-    wp.atomic_sub(body_f_s, link_0, wp.spatial_vector(f, wp.cross(pos_0, f)))
-    wp.atomic_add(body_f_s, link_1, wp.spatial_vector(f, wp.cross(pos_1, f)))
-
-
-@wp.kernel
-def eval_muscles(
-    body_X_s: wp.array(dtype=wp.transform),
-    body_v_s: wp.array(dtype=wp.spatial_vector),
-    body_com: wp.array(dtype=wp.vec3),
-    muscle_start: wp.array(dtype=int),
-    muscle_params: wp.array(dtype=float),
-    muscle_links: wp.array(dtype=int),
-    muscle_points: wp.array(dtype=wp.vec3),
-    muscle_activation: wp.array(dtype=float),
-    # output
-    body_f_s: wp.array(dtype=wp.spatial_vector),
-):
-    tid = wp.tid()
-
-    m_start = muscle_start[tid]
-    m_end = muscle_start[tid + 1] - 1
-
-    activation = muscle_activation[tid]
-
-    for i in range(m_start, m_end):
-        compute_muscle_force(i, body_X_s, body_v_s, body_com, muscle_links, muscle_points, activation, body_f_s)
-
-
-def eval_spring_forces(model: Model, state: State, particle_f: wp.array):
-    if model.spring_count:
-        wp.launch(
-            kernel=eval_springs,
-            dim=model.spring_count,
-            inputs=[
-                state.particle_q,
-                state.particle_qd,
-                model.spring_indices,
-                model.spring_rest_length,
-                model.spring_stiffness,
-                model.spring_damping,
-            ],
-            outputs=[particle_f],
-            device=model.device,
-        )
-
-
-def eval_triangle_forces(model: Model, state: State, control: Control, particle_f: wp.array):
-    if model.tri_count:
-        wp.launch(
-            kernel=eval_triangles,
-            dim=model.tri_count,
-            inputs=[
-                state.particle_q,
-                state.particle_qd,
-                model.tri_indices,
-                model.tri_poses,
-                control.tri_activations,
-                model.tri_materials,
-            ],
-            outputs=[particle_f],
-            device=model.device,
-        )
-
-
-def eval_triangle_contact_forces(model: Model, state: State, particle_f: wp.array):
-    if model.enable_tri_collisions:
-        wp.launch(
-            kernel=eval_triangles_contact,
-            dim=model.tri_count * model.particle_count,
-            inputs=[
-                model.particle_count,
-                state.particle_q,
-                state.particle_qd,
-                model.tri_indices,
-                model.tri_materials,
-                model.particle_radius,
-            ],
-            outputs=[particle_f],
-            device=model.device,
-        )
-
-
-def eval_bending_forces(model: Model, state: State, particle_f: wp.array):
-    if model.edge_count:
-        wp.launch(
-            kernel=eval_bending,
-            dim=model.edge_count,
-            inputs=[
-                state.particle_q,
-                state.particle_qd,
-                model.edge_indices,
-                model.edge_rest_angle,
-                model.edge_bending_properties,
-            ],
-            outputs=[particle_f],
-            device=model.device,
-        )
-
-
-def eval_particle_ground_contact_forces(model: Model, state: State, particle_f: wp.array):
-    if model.ground and model.particle_count:
-        wp.launch(
-            kernel=eval_particle_ground_contacts,
-            dim=model.particle_count,
-            inputs=[
-                state.particle_q,
-                state.particle_qd,
-                model.particle_radius,
-                model.particle_flags,
-                model.soft_contact_ke,
-                model.soft_contact_kd,
-                model.soft_contact_kf,
-                model.soft_contact_mu,
-                model.ground_plane,
-            ],
-            outputs=[particle_f],
-            device=model.device,
-        )
-
-
-def eval_tetrahedral_forces(model: Model, state: State, control: Control, particle_f: wp.array):
-    if model.tet_count:
-        wp.launch(
-            kernel=eval_tetrahedra,
-            dim=model.tet_count,
-            inputs=[
-                state.particle_q,
-                state.particle_qd,
-                model.tet_indices,
-                model.tet_poses,
-                control.tet_activations,
-                model.tet_materials,
-            ],
-            outputs=[particle_f],
-            device=model.device,
-        )
-
-
-def eval_body_contact_forces(model: Model, state: State, particle_f: wp.array, friction_smoothing: float = 1.0):
-    if model.rigid_contact_max and (
-        (model.ground and model.shape_ground_contact_pair_count) or model.shape_contact_pair_count
-    ):
-        wp.launch(
-            kernel=eval_rigid_contacts,
-            dim=model.rigid_contact_max,
-            inputs=[
-                state.body_q,
-                state.body_qd,
-                model.body_com,
-                model.shape_materials,
-                model.shape_geo,
-                model.shape_body,
-                model.rigid_contact_count,
-                model.rigid_contact_point0,
-                model.rigid_contact_point1,
-                model.rigid_contact_normal,
-                model.rigid_contact_shape0,
-                model.rigid_contact_shape1,
-                False,
-                friction_smoothing,
-            ],
-            outputs=[state.body_f],
-            device=model.device,
-        )
-
-
-def eval_body_joint_forces(model: Model, state: State, control: Control, body_f: wp.array):
-    if model.joint_count:
-        wp.launch(
-            kernel=eval_body_joints,
-            dim=model.joint_count,
-            inputs=[
-                state.body_q,
-                state.body_qd,
-                model.body_com,
-                model.joint_qd_start,
-                model.joint_type,
-                model.joint_enabled,
-                model.joint_child,
-                model.joint_parent,
-                model.joint_X_p,
-                model.joint_X_c,
-                model.joint_axis,
-                model.joint_axis_start,
-                model.joint_axis_dim,
-                model.joint_axis_mode,
-                control.joint_act,
-                model.joint_target_ke,
-                model.joint_target_kd,
-                model.joint_limit_lower,
-                model.joint_limit_upper,
-                model.joint_limit_ke,
-                model.joint_limit_kd,
-                model.joint_attach_ke,
-                model.joint_attach_kd,
-            ],
-            outputs=[body_f],
-            device=model.device,
-        )
-
-
-def eval_particle_body_contact_forces(
-    model: Model, state: State, particle_f: wp.array, body_f: wp.array, body_f_in_world_frame: bool = False
-):
-    if model.particle_count and model.shape_count > 1:
-        wp.launch(
-            kernel=eval_particle_contacts,
-            dim=model.soft_contact_max,
-            inputs=[
-                state.particle_q,
-                state.particle_qd,
-                state.body_q,
-                state.body_qd,
-                model.particle_radius,
-                model.particle_flags,
-                model.body_com,
-                model.shape_body,
-                model.shape_materials,
-                model.soft_contact_ke,
-                model.soft_contact_kd,
-                model.soft_contact_kf,
-                model.soft_contact_mu,
-                model.particle_adhesion,
-                model.soft_contact_count,
-                model.soft_contact_particle,
-                model.soft_contact_shape,
-                model.soft_contact_body_pos,
-                model.soft_contact_body_vel,
-                model.soft_contact_normal,
-                model.soft_contact_max,
-                body_f_in_world_frame,
-            ],
-            # outputs
-            outputs=[particle_f, body_f],
-            device=model.device,
-        )
-
-
-def eval_muscle_forces(model: Model, state: State, control: Control, body_f: wp.array):
-    if model.muscle_count:
-        wp.launch(
-            kernel=eval_muscles,
-            dim=model.muscle_count,
-            inputs=[
-                state.body_q,
-                state.body_qd,
-                model.body_com,
-                model.muscle_start,
-                model.muscle_params,
-                model.muscle_bodies,
-                model.muscle_points,
-                control.muscle_activations,
-            ],
-            outputs=[body_f],
-            device=model.device,
-        )
-
-
-def compute_forces(
-    model: Model,
-    state: State,
-    control: Control,
-    particle_f: wp.array,
-    body_f: wp.array,
-    dt: float,
-    friction_smoothing: float = 1.0,
-):
-    # damped springs
-    eval_spring_forces(model, state, particle_f)
-
-    # triangle elastic and lift/drag forces
-    eval_triangle_forces(model, state, control, particle_f)
-
-    # triangle/triangle contacts
-    eval_triangle_contact_forces(model, state, particle_f)
-
-    # triangle bending
-    eval_bending_forces(model, state, particle_f)
-
-    # tetrahedral FEM
-    eval_tetrahedral_forces(model, state, control, particle_f)
-
-    # body joints
-    eval_body_joint_forces(model, state, control, body_f)
-
-    # particle-particle interactions
-    eval_particle_forces(model, state, particle_f)
-
-    # particle ground contacts
-    eval_particle_ground_contact_forces(model, state, particle_f)
-
-    # body contacts
-    eval_body_contact_forces(model, state, particle_f, friction_smoothing=friction_smoothing)
-
-    # particle shape contact
-    eval_particle_body_contact_forces(model, state, particle_f, body_f, body_f_in_world_frame=False)
-
-    # muscles
-    if False:
-        eval_muscle_forces(model, state, control, body_f)
-
-
-class SemiImplicitIntegrator(Integrator):
-    """A semi-implicit integrator using symplectic Euler
-
-    After constructing `Model` and `State` objects this time-integrator
-    may be used to advance the simulation state forward in time.
-
-    Semi-implicit time integration is a variational integrator that
-    preserves energy, however it not unconditionally stable, and requires a time-step
-    small enough to support the required stiffness and damping forces.
-
-    See: https://en.wikipedia.org/wiki/Semi-implicit_Euler_method
-
-    Example
-    -------
-
-    .. code-block:: python
-
-        integrator = wp.SemiImplicitIntegrator()
-
-        # simulation loop
-        for i in range(100):
-            state = integrator.simulate(model, state_in, state_out, dt)
-
-    """
-
-    def __init__(self, angular_damping: float = 0.05, friction_smoothing: float = 1.0):
-        """
-        Args:
-            angular_damping (float, optional): Angular damping factor. Defaults to 0.05.
-            friction_smoothing (float, optional): The delta value for the Huber norm (see :func:`warp.math.norm_huber`) used for the friction velocity normalization. Defaults to 1.0.
-        """
-        self.angular_damping = angular_damping
-        self.friction_smoothing = friction_smoothing
-
-    def simulate(self, model: Model, state_in: State, state_out: State, dt: float, control: Control = None):
-        with wp.ScopedTimer("simulate", False):
-            particle_f = None
-            body_f = None
-
-            if state_in.particle_count:
-                particle_f = state_in.particle_f
-
-            if state_in.body_count:
-                body_f = state_in.body_f
-
-            if control is None:
-                control = model.control(clone_variables=False)
-
-            compute_forces(model, state_in, control, particle_f, body_f, dt, friction_smoothing=self.friction_smoothing)
-
-            self.integrate_bodies(model, state_in, state_out, dt, self.angular_damping)
-
-            self.integrate_particles(model, state_in, state_out, dt)
-
-            return state_out