diff --git a/contrib/kernel_analyzer/kernel_analyzer/cli.py b/contrib/kernel_analyzer/kernel_analyzer/cli.py index 151d93e77..239dd82a5 100644 --- a/contrib/kernel_analyzer/kernel_analyzer/cli.py +++ b/contrib/kernel_analyzer/kernel_analyzer/cli.py @@ -47,7 +47,7 @@ def err_github(iss): err = {"console": err_console, "github": err_github}[_OUTPUT.value] if not filepath.is_file() or filepath.suffix != ".py": - err("Skipping non-Python file") + logging.warning(f"Skipping non-Python file: {filepath}") continue if _TYPES_PATH.value: diff --git a/mujoco_warp/_src/derivative.py b/mujoco_warp/_src/derivative.py index 87df3a8b7..420e7f03d 100644 --- a/mujoco_warp/_src/derivative.py +++ b/mujoco_warp/_src/derivative.py @@ -15,6 +15,9 @@ import warp as wp +from . import support +from .passive import geom_semiaxes +from .types import MJ_MINVAL from .types import BiasType from .types import Data from .types import DisableBit @@ -22,6 +25,7 @@ from .types import GainType from .types import Model from .types import TileSet +from .types import mat66 from .types import vec10f from .warp_util import cache_kernel from .warp_util import event_scope @@ -210,6 +214,570 @@ def _qderiv_tendon_damping( qDeriv_out[worldid, dofjid, dofiid] -= qderiv +@wp.func +def _ellipsoid_max_moment_deriv(size: wp.vec3, dir: int) -> float: # kernel_analyzer: ignore + d0 = size[dir] + d1 = size[(dir + 1) % 3] + d2 = size[(dir + 2) % 3] + d_max = wp.max(d1, d2) + return wp.static(8.0 / 15.0 * wp.pi) * d0 * d_max * d_max * d_max * d_max + + +@wp.func +def _add_to_quadrant(B: mat66, D: wp.mat33, col_quad: int, row_quad: int) -> mat66: + """Add 3x3 matrix D^T to a quadrant of 6x6 matrix B. + + Args: + B: The 6x6 matrix to modify + D: The 3x3 matrix to add (will be transposed) + col_quad: Column quadrant (0 = cols 0-2, 1 = cols 3-5) + row_quad: Row quadrant (0 = rows 0-2, 1 = rows 3-5) + + This matches MuJoCo's addToQuadrant(B, D, col_quad, row_quad) convention, + which adds D^T to the specified quadrant due to column-major storage. + """ + r = 3 * row_quad + c = 3 * col_quad + for i in range(3): + for j in range(3): + B[r + i, c + j] += D[j, i] # Note: D[j,i] transposes D + return B + + +@wp.func +def _cross_deriv(a: wp.vec3, b: wp.vec3) -> tuple[wp.mat33, wp.mat33]: # kernel_analyzer: ignore + """Returns derivatives of cross product a x b w.r.t. both inputs. + + Returns: + (deriv_a, deriv_b): Derivatives w.r.t. a and b respectively + """ + deriv_a = wp.mat33(0.0, b[2], -b[1], -b[2], 0.0, b[0], b[1], -b[0], 0.0) + deriv_b = wp.mat33(0.0, -a[2], a[1], a[2], 0.0, -a[0], -a[1], a[0], 0.0) + return deriv_a, deriv_b + + +@wp.func +def _added_mass_forces_deriv( # kernel_analyzer: ignore + local_vels: wp.spatial_vector, fluid_density: float, virtual_mass: wp.vec3, virtual_inertia: wp.vec3 +) -> mat66: + lin_vel = wp.spatial_bottom(local_vels) + ang_vel = wp.spatial_top(local_vels) + + virtual_lin_mom = fluid_density * wp.cw_mul(virtual_mass, lin_vel) + virtual_ang_mom = fluid_density * wp.cw_mul(virtual_inertia, ang_vel) + + B = mat66(0.0) + + Da, Db = _cross_deriv(virtual_ang_mom, ang_vel) + B = _add_to_quadrant(B, Db, 0, 0) + for i in range(3): + for j in range(3): + Da[i, j] *= fluid_density * virtual_inertia[j] + B = _add_to_quadrant(B, Da, 0, 0) + + Da, Db = _cross_deriv(virtual_lin_mom, lin_vel) + B = _add_to_quadrant(B, Db, 0, 1) + for i in range(3): + for j in range(3): + Da[i, j] *= fluid_density * virtual_mass[j] + B = _add_to_quadrant(B, Da, 0, 1) + + Da, Db = _cross_deriv(virtual_lin_mom, ang_vel) + B = _add_to_quadrant(B, Db, 1, 0) + for i in range(3): + for j in range(3): + Da[i, j] *= fluid_density * virtual_mass[j] + B = _add_to_quadrant(B, Da, 1, 1) + + return B + + +@wp.func +def _viscous_torque_deriv( # kernel_analyzer: ignore + lvel: wp.spatial_vector, + fluid_density: float, + fluid_viscosity: float, + size: wp.vec3, + slender_drag_coef: float, + ang_drag_coef: float, +) -> wp.mat33: + d_max = wp.max(wp.max(size[0], size[1]), size[2]) + d_min = wp.min(wp.min(size[0], size[1]), size[2]) + d_mid = size[0] + size[1] + size[2] - d_max - d_min + + eq_sphere_D = wp.static(2.0 / 3.0) * (size[0] + size[1] + size[2]) + lin_visc_torq_coef = wp.pi * eq_sphere_D * eq_sphere_D * eq_sphere_D + + I_max = wp.static(8.0 / 15.0 * wp.pi) * d_mid * d_max * d_max * d_max * d_max + II = wp.vec3(_ellipsoid_max_moment_deriv(size, 0), _ellipsoid_max_moment_deriv(size, 1), _ellipsoid_max_moment_deriv(size, 2)) + + ang = wp.spatial_top(lvel) + x = ang[0] + y = ang[1] + z = ang[2] + + mom_coef = wp.vec3( + ang_drag_coef * II[0] + slender_drag_coef * (I_max - II[0]), + ang_drag_coef * II[1] + slender_drag_coef * (I_max - II[1]), + ang_drag_coef * II[2] + slender_drag_coef * (I_max - II[2]), + ) + + mom_visc = wp.cw_mul(ang, mom_coef) + norm = wp.length(mom_visc) + density = fluid_density / wp.max(MJ_MINVAL, norm) + + mom_sq = -density * wp.cw_mul(wp.cw_mul(ang, mom_coef), mom_coef) + + lin_coef = fluid_viscosity * lin_visc_torq_coef + diag_val = x * mom_sq[0] + y * mom_sq[1] + z * mom_sq[2] - lin_coef + + D = wp.mat33( + diag_val + mom_sq[0] * x, + mom_sq[1] * x, + mom_sq[2] * x, + mom_sq[0] * y, + diag_val + mom_sq[1] * y, + mom_sq[2] * y, + mom_sq[0] * z, + mom_sq[1] * z, + diag_val + mom_sq[2] * z, + ) + + return D + + +@wp.func +def _viscous_drag_deriv( # kernel_analyzer: ignore + lvel: wp.spatial_vector, + fluid_density: float, + fluid_viscosity: float, + size: wp.vec3, + blunt_drag_coef: float, + slender_drag_coef: float, +) -> wp.mat33: + d_max = wp.max(wp.max(size[0], size[1]), size[2]) + d_min = wp.min(wp.min(size[0], size[1]), size[2]) + d_mid = size[0] + size[1] + size[2] - d_max - d_min + + eq_sphere_D = wp.static(2.0 / 3.0) * (size[0] + size[1] + size[2]) + A_max = wp.pi * d_max * d_mid + + a = (size[1] * size[2]) * (size[1] * size[2]) + b = (size[2] * size[0]) * (size[2] * size[0]) + c = (size[0] * size[1]) * (size[0] * size[1]) + aa = a * a + bb = b * b + cc = c * c + + lin = wp.spatial_bottom(lvel) + x = lin[0] + y = lin[1] + z = lin[2] + xx = x * x + yy = y * y + zz = z * z + xy = x * y + yz = y * z + xz = x * z + + proj_denom = aa * xx + bb * yy + cc * zz + proj_num = a * xx + b * yy + c * zz + dA_coef = wp.pi / wp.max(MJ_MINVAL, wp.sqrt(proj_num * proj_num * proj_num * proj_denom)) + + A_proj = wp.pi * wp.sqrt(proj_denom / wp.max(MJ_MINVAL, proj_num)) + + norm = wp.sqrt(xx + yy + zz) + inv_norm = 1.0 / wp.max(MJ_MINVAL, norm) + + lin_coef = fluid_viscosity * wp.static(3.0 * wp.pi) * eq_sphere_D + quad_coef = fluid_density * (A_proj * blunt_drag_coef + slender_drag_coef * (A_max - A_proj)) + Aproj_coef = fluid_density * norm * (blunt_drag_coef - slender_drag_coef) + + dAproj_dv = wp.vec3( + Aproj_coef * dA_coef * a * x * (b * yy * (a - b) + c * zz * (a - c)), + Aproj_coef * dA_coef * b * y * (a * xx * (b - a) + c * zz * (b - c)), + Aproj_coef * dA_coef * c * z * (a * xx * (c - a) + b * yy * (c - b)), + ) + + inner = xx + yy + zz + D = wp.mat33(xx + inner, xy, xz, xy, yy + inner, yz, xz, yz, zz + inner) + + D *= -quad_coef * inv_norm + + for i in range(3): + D[0, i] -= x * dAproj_dv[i] + D[1, i] -= y * dAproj_dv[i] + D[2, i] -= z * dAproj_dv[i] + + D[0, 0] -= lin_coef + D[1, 1] -= lin_coef + D[2, 2] -= lin_coef + + return D + + +@wp.func +def _kutta_lift_deriv(lvel: wp.spatial_vector, fluid_density: float, size: wp.vec3, kutta_lift_coef: float) -> wp.mat33: # kernel_analyzer: ignore + a = (size[1] * size[2]) * (size[1] * size[2]) + b = (size[2] * size[0]) * (size[2] * size[0]) + c = (size[0] * size[1]) * (size[0] * size[1]) + aa = a * a + bb = b * b + cc = c * c + + lin = wp.spatial_bottom(lvel) + x = lin[0] + y = lin[1] + z = lin[2] + xx = x * x + yy = y * y + zz = z * z + xy = x * y + yz = y * z + xz = x * z + + proj_denom = aa * xx + bb * yy + cc * zz + proj_num = a * xx + b * yy + c * zz + norm2 = xx + yy + zz + df_denom = wp.pi * kutta_lift_coef * fluid_density / wp.max(MJ_MINVAL, wp.sqrt(proj_denom * proj_num * norm2)) + + dfx_coef = yy * (a - b) + zz * (a - c) + dfy_coef = xx * (b - a) + zz * (b - c) + dfz_coef = xx * (c - a) + yy * (c - b) + proj_term = proj_num / wp.max(MJ_MINVAL, proj_denom) + cos_term = proj_num / wp.max(MJ_MINVAL, norm2) + + D = wp.mat33(0.0, b - a, c - a, a - b, 0.0, c - b, a - c, b - c, 0.0) + D *= wp.static(2.0) * proj_num + + inner_term = ( + wp.cw_mul(wp.vec3(aa, bb, cc), wp.vec3(proj_term, proj_term, proj_term)) + - wp.vec3(a, b, c) + + wp.vec3(cos_term, cos_term, cos_term) + ) + + for i in range(3): + D[0, i] += inner_term[i] * dfx_coef + D[1, i] += inner_term[i] * dfy_coef + D[2, i] += inner_term[i] * dfz_coef + + D[0, 0] *= xx + D[0, 1] *= xy + D[0, 2] *= xz + D[1, 0] *= xy + D[1, 1] *= yy + D[1, 2] *= yz + D[2, 0] *= xz + D[2, 1] *= yz + D[2, 2] *= zz + + D[0, 0] -= dfx_coef * proj_num + D[1, 1] -= dfy_coef * proj_num + D[2, 2] -= dfz_coef * proj_num + + return D * df_denom + + +@wp.func +def _magnus_force_deriv(lvel: wp.spatial_vector, fluid_density: float, size: wp.vec3, magnus_lift_coef: float) -> mat66: # kernel_analyzer: ignore + volume = wp.static(4.0 / 3.0 * wp.pi) * size[0] * size[1] * size[2] + magnus_coef = magnus_lift_coef * fluid_density * volume + + lin_vel = wp.spatial_bottom(lvel) + ang_vel = wp.spatial_top(lvel) + + lin_vel_scaled = lin_vel * magnus_coef + ang_vel_scaled = ang_vel * magnus_coef + + D_ang, _ = _cross_deriv(ang_vel_scaled, lin_vel_scaled) + _, D_lin = _cross_deriv(ang_vel_scaled, lin_vel_scaled) + + B = mat66(0.0) + B = _add_to_quadrant(B, D_ang, 1, 0) + B = _add_to_quadrant(B, D_lin, 1, 1) + + return B + + +@wp.func +def _ellipsoid_fluid_qderiv_contrib( + # Model: + opt_density: float, + opt_viscosity: float, + opt_wind: wp.vec3, + opt_integrator: int, + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_geomnum: wp.array(dtype=int), + body_geomadr: wp.array(dtype=int), + geom_type: wp.array(dtype=int), + geom_size: wp.array2d(dtype=wp.vec3), + geom_fluid: wp.array2d(dtype=float), + dof_bodyid: wp.array(dtype=int), + # Data in: + xipos: wp.vec3, + ximat: wp.mat33, + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + cvel: wp.spatial_vector, + # In: + bodyid: int, + dofiid: int, + dofjid: int, + worldid: int, +) -> float: + """Compute qDeriv contribution for a single DOF pair from ellipsoid fluid forces.""" + wind = opt_wind + density = opt_density + viscosity = opt_viscosity + + rotT = wp.transpose(ximat) + ang_global = wp.spatial_top(cvel) + lin_global = wp.spatial_bottom(cvel) + subtree_root = subtree_com_in[worldid, body_rootid[bodyid]] + lin_com = lin_global - wp.cross(xipos - subtree_root, ang_global) + + qderiv_contrib = float(0.0) + + start = body_geomadr[bodyid] + count = body_geomnum[bodyid] + + for i in range(count): + geomid = start + i + coef = geom_fluid[geomid, 0] + if coef <= 0.0: + continue + + size = geom_size[worldid % geom_size.shape[0], geomid] + semiaxes = geom_semiaxes(size, geom_type[geomid]) + geom_rot = geom_xmat_in[worldid, geomid] + geom_rotT = wp.transpose(geom_rot) + geom_pos = geom_xpos_in[worldid, geomid] + + lin_point = lin_com + wp.cross(ang_global, geom_pos - xipos) + + l_ang = geom_rotT @ ang_global + l_lin = geom_rotT @ lin_point + + if wind[0] != 0.0 or wind[1] != 0.0 or wind[2] != 0.0: + l_lin -= geom_rotT @ wind + + lvel = wp.spatial_vector(l_ang, l_lin) + + B = mat66(0.0) + + magnus_coef = geom_fluid[geomid, 5] + kutta_coef = geom_fluid[geomid, 4] + blunt_drag_coef = geom_fluid[geomid, 1] + slender_drag_coef = geom_fluid[geomid, 2] + ang_drag_coef = geom_fluid[geomid, 3] + + if density > 0.0: + virtual_mass = wp.vec3(geom_fluid[geomid, 6], geom_fluid[geomid, 7], geom_fluid[geomid, 8]) + virtual_inertia = wp.vec3(geom_fluid[geomid, 9], geom_fluid[geomid, 10], geom_fluid[geomid, 11]) + B += _added_mass_forces_deriv(lvel, density, virtual_mass, virtual_inertia) + + if magnus_coef != 0.0: + B += _magnus_force_deriv(lvel, density, semiaxes, magnus_coef) + + if kutta_coef != 0.0: + D_kutta = _kutta_lift_deriv(lvel, density, semiaxes, kutta_coef) + B = _add_to_quadrant(B, D_kutta, 1, 1) + + if viscosity > 0.0: + D_drag = _viscous_drag_deriv(lvel, density, viscosity, semiaxes, blunt_drag_coef, slender_drag_coef) + B = _add_to_quadrant(B, D_drag, 1, 1) + + D_torque = _viscous_torque_deriv(lvel, density, viscosity, semiaxes, slender_drag_coef, ang_drag_coef) + B = _add_to_quadrant(B, D_torque, 0, 0) + + B = B * coef + + # Symmetrize B for implicitfast integrator (matches MuJoCo behavior) + # Note: MuJoCo only symmetrizes for IMPLICITFAST, not IMPLICIT + if opt_integrator == 2: # mjINT_IMPLICITFAST + for row in range(6): + for col in range(row + 1, 6): + avg = (B[row, col] + B[col, row]) * 0.5 + B[row, col] = avg + B[col, row] = avg + + # Compute Jacobian at geometry position in local frame + jacp_i, jacr_i = support.jac( + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, + geom_pos, bodyid, dofiid, worldid + ) + jacp_i_local = geom_rotT @ jacp_i + jacr_i_local = geom_rotT @ jacr_i + + jacp_j, jacr_j = support.jac( + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, + geom_pos, bodyid, dofjid, worldid + ) + jacp_j_local = geom_rotT @ jacp_j + jacr_j_local = geom_rotT @ jacr_j + + # B matrix is in (angular, linear) order: + # B[0:3, 0:3] = angular-angular, B[0:3, 3:6] = angular-linear + # B[3:6, 0:3] = linear-angular, B[3:6, 3:6] = linear-linear + # Jacobian vectors: jacr = angular, jacp = linear + # We compute J^T * B * J where J = [jacr; jacp] (angular first, linear second) + cdof_i_local = wp.spatial_vector(jacr_i_local, jacp_i_local) + cdof_j_local = wp.spatial_vector(jacr_j_local, jacp_j_local) + + for k in range(6): + for j in range(6): + if k < 3: + cdof_i_k = wp.spatial_top(cdof_i_local)[k] + else: + cdof_i_k = wp.spatial_bottom(cdof_i_local)[k - 3] + if j < 3: + cdof_j_j = wp.spatial_top(cdof_j_local)[j] + else: + cdof_j_j = wp.spatial_bottom(cdof_j_local)[j - 3] + qderiv_contrib += cdof_i_k * B[k, j] * cdof_j_j + + return qderiv_contrib + + +@wp.kernel +def _deriv_ellipsoid_fluid_dense( + # Model: + nv: int, + opt_density: wp.array(dtype=float), + opt_viscosity: wp.array(dtype=float), + opt_wind: wp.array(dtype=wp.vec3), + opt_timestep: wp.array(dtype=float), + opt_integrator: int, + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_geomnum: wp.array(dtype=int), + body_geomadr: wp.array(dtype=int), + body_fluid_ellipsoid: wp.array(dtype=bool), + geom_type: wp.array(dtype=int), + geom_size: wp.array2d(dtype=wp.vec3), + geom_fluid: wp.array2d(dtype=float), + dof_bodyid: wp.array(dtype=int), + # Data in: + xipos_in: wp.array2d(dtype=wp.vec3), + ximat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cvel_in: wp.array2d(dtype=wp.spatial_vector), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + # Out: + qDeriv_out: wp.array3d(dtype=float), +): + """Dense version: iterates over all nv x nv DOF pairs.""" + worldid, dofiid, dofjid = wp.tid() + + # Only process lower triangle (i >= j) to avoid duplicate work + if dofiid < dofjid: + return + + bodyid_i = dof_bodyid[dofiid] + bodyid_j = dof_bodyid[dofjid] + + if bodyid_i == 0 or not body_fluid_ellipsoid[bodyid_i]: + return + + if bodyid_i != bodyid_j: + return + + bodyid = bodyid_i + density = opt_density[worldid % opt_density.shape[0]] + viscosity = opt_viscosity[worldid % opt_viscosity.shape[0]] + wind = opt_wind[worldid % opt_wind.shape[0]] + timestep = opt_timestep[worldid % opt_timestep.shape[0]] + xipos = xipos_in[worldid, bodyid] + ximat = ximat_in[worldid, bodyid] + cvel = cvel_in[worldid, bodyid] + + qderiv_contrib = _ellipsoid_fluid_qderiv_contrib( + density, viscosity, wind, opt_integrator, + body_parentid, body_rootid, body_geomnum, body_geomadr, + geom_type, geom_size, geom_fluid, dof_bodyid, + xipos, ximat, geom_xpos_in, geom_xmat_in, subtree_com_in, cdof_in, cvel, + bodyid, dofiid, dofjid, worldid + ) + + qderiv_contrib *= timestep + + wp.atomic_sub(qDeriv_out, worldid, dofiid, dofjid, qderiv_contrib) + if dofiid != dofjid: + wp.atomic_sub(qDeriv_out, worldid, dofjid, dofiid, qderiv_contrib) + + +@wp.kernel +def _deriv_ellipsoid_fluid_sparse( + # Model: + opt_density: wp.array(dtype=float), + opt_viscosity: wp.array(dtype=float), + opt_wind: wp.array(dtype=wp.vec3), + opt_timestep: wp.array(dtype=float), + opt_integrator: int, + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_geomnum: wp.array(dtype=int), + body_geomadr: wp.array(dtype=int), + body_fluid_ellipsoid: wp.array(dtype=bool), + geom_type: wp.array(dtype=int), + geom_size: wp.array2d(dtype=wp.vec3), + geom_fluid: wp.array2d(dtype=float), + dof_bodyid: wp.array(dtype=int), + # Data in: + xipos_in: wp.array2d(dtype=wp.vec3), + ximat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cvel_in: wp.array2d(dtype=wp.spatial_vector), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + # In: + qMi: wp.array(dtype=int), + qMj: wp.array(dtype=int), + # Out: + qDeriv_out: wp.array3d(dtype=float), +): + """Sparse version: iterates over sparse matrix elements.""" + worldid, elemid = wp.tid() + dofiid = qMi[elemid] + dofjid = qMj[elemid] + + bodyid_i = dof_bodyid[dofiid] + bodyid_j = dof_bodyid[dofjid] + + if bodyid_i == 0 or not body_fluid_ellipsoid[bodyid_i]: + return + + if bodyid_i != bodyid_j: + return + + bodyid = bodyid_i + density = opt_density[worldid % opt_density.shape[0]] + viscosity = opt_viscosity[worldid % opt_viscosity.shape[0]] + wind = opt_wind[worldid % opt_wind.shape[0]] + timestep = opt_timestep[worldid % opt_timestep.shape[0]] + xipos = xipos_in[worldid, bodyid] + ximat = ximat_in[worldid, bodyid] + cvel = cvel_in[worldid, bodyid] + + qderiv_contrib = _ellipsoid_fluid_qderiv_contrib( + density, viscosity, wind, opt_integrator, + body_parentid, body_rootid, body_geomnum, body_geomadr, + geom_type, geom_size, geom_fluid, dof_bodyid, + xipos, ximat, geom_xpos_in, geom_xmat_in, subtree_com_in, cdof_in, cvel, + bodyid, dofiid, dofjid, worldid + ) + + qderiv_contrib *= timestep + + wp.atomic_sub(qDeriv_out, worldid, 0, elemid, qderiv_contrib) + + @event_scope def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d(dtype=float)): """Analytical derivative of smooth forces w.r.t. velocities. @@ -289,4 +857,68 @@ def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d(dtype=float)): outputs=[out], ) + if m.opt.has_fluid and not m.opt.disableflags & DisableBit.DAMPER: + if m.opt.is_sparse: + wp.launch( + _deriv_ellipsoid_fluid_sparse, + dim=(d.nworld, qMi.size), + inputs=[ + m.opt.density, + m.opt.viscosity, + m.opt.wind, + m.opt.timestep, + m.opt.integrator, + m.body_parentid, + m.body_rootid, + m.body_geomnum, + m.body_geomadr, + m.body_fluid_ellipsoid, + m.geom_type, + m.geom_size, + m.geom_fluid, + m.dof_bodyid, + d.xipos, + d.ximat, + d.geom_xpos, + d.geom_xmat, + d.subtree_com, + d.cvel, + d.cdof, + qMi, + qMj, + ], + outputs=[out], + ) + else: + # Dense mode: iterate over all nv x nv DOF pairs + wp.launch( + _deriv_ellipsoid_fluid_dense, + dim=(d.nworld, m.nv, m.nv), + inputs=[ + m.nv, + m.opt.density, + m.opt.viscosity, + m.opt.wind, + m.opt.timestep, + m.opt.integrator, + m.body_parentid, + m.body_rootid, + m.body_geomnum, + m.body_geomadr, + m.body_fluid_ellipsoid, + m.geom_type, + m.geom_size, + m.geom_fluid, + m.dof_bodyid, + d.xipos, + d.ximat, + d.geom_xpos, + d.geom_xmat, + d.subtree_com, + d.cvel, + d.cdof, + ], + outputs=[out], + ) + # TODO(team): rne derivative diff --git a/mujoco_warp/_src/derivative_test.py b/mujoco_warp/_src/derivative_test.py index 6ce0e5c7b..82fd95c3d 100644 --- a/mujoco_warp/_src/derivative_test.py +++ b/mujoco_warp/_src/derivative_test.py @@ -118,6 +118,54 @@ def test_smooth_vel(self, jacobian): _assert_eq(mjw_out, mj_out, "qM - dt * qDeriv") + @parameterized.parameters(mujoco.mjtJacobian.mjJAC_DENSE, mujoco.mjtJacobian.mjJAC_SPARSE) + def test_smooth_vel_fluid(self, jacobian): + """Tests qDeriv with ellipsoid fluid forces.""" + mjm, mjd, m, d = test_data.fixture( + xml=""" + + + """, + keyframe=0, + overrides={"opt.jacobian": jacobian}, + ) + + mujoco.mj_step(mjm, mjd) # step w/ implicitfast calls mjd_smooth_vel to compute qDeriv + + if jacobian == mujoco.mjtJacobian.mjJAC_SPARSE: + out_smooth_vel = wp.zeros((1, 1, m.nM), dtype=float) + else: + out_smooth_vel = wp.zeros((1, m.nv, m.nv), dtype=float) + + mjw.deriv_smooth_vel(m, d, out_smooth_vel) + + if jacobian == mujoco.mjtJacobian.mjJAC_SPARSE: + mjw_out = np.zeros((m.nv, m.nv)) + for elem, (i, j) in enumerate(zip(m.qM_fullm_i.numpy(), m.qM_fullm_j.numpy())): + mjw_out[i, j] = out_smooth_vel.numpy()[0, 0, elem] + else: + mjw_out = out_smooth_vel.numpy()[0] + + mj_qDeriv = np.zeros((mjm.nv, mjm.nv)) + mujoco.mju_sparse2dense(mj_qDeriv, mjd.qDeriv, mjm.D_rownnz, mjm.D_rowadr, mjm.D_colind) + + mj_qM = np.zeros((m.nv, m.nv)) + mujoco.mj_fullM(mjm, mj_qM, mjd.qM) + mj_out = mj_qM - mjm.opt.timestep * mj_qDeriv + + _assert_eq(mjw_out, mj_out, "qM - dt * qDeriv (fluid)") + if __name__ == "__main__": wp.init() diff --git a/mujoco_warp/_src/io.py b/mujoco_warp/_src/io.py index ed5463d32..e1e20d59e 100644 --- a/mujoco_warp/_src/io.py +++ b/mujoco_warp/_src/io.py @@ -112,12 +112,6 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: if mjm.opt.noslip_iterations > 0: raise NotImplementedError(f"noslip solver not implemented.") - if (mjm.opt.viscosity > 0 or mjm.opt.density > 0) and mjm.opt.integrator in ( - mujoco.mjtIntegrator.mjINT_IMPLICITFAST, - mujoco.mjtIntegrator.mjINT_IMPLICIT, - ): - raise NotImplementedError(f"Implicit integrators and fluid model not implemented.") - if (mjm.body_plugin != -1).any(): raise NotImplementedError("Body plugins not supported.") diff --git a/mujoco_warp/_src/io_test.py b/mujoco_warp/_src/io_test.py index 29c975d86..1c7536b55 100644 --- a/mujoco_warp/_src/io_test.py +++ b/mujoco_warp/_src/io_test.py @@ -464,23 +464,6 @@ def test_collision_sensor_box_box(self): """ ) - def test_implicit_integrator_fluid_model(self): - """Tests for implicit integrator with fluid model.""" - with self.assertRaises(NotImplementedError): - test_data.fixture( - xml=""" - - - """ - ) - def test_plugin(self): with self.assertRaises(NotImplementedError): test_data.fixture( diff --git a/mujoco_warp/_src/passive.py b/mujoco_warp/_src/passive.py index 66d6317b9..706563616 100644 --- a/mujoco_warp/_src/passive.py +++ b/mujoco_warp/_src/passive.py @@ -40,7 +40,7 @@ def _pow4(val: float) -> float: @wp.func -def _geom_semiaxes(size: wp.vec3, geom_type: int) -> wp.vec3: # kernel_analyzer: ignore +def geom_semiaxes(size: wp.vec3, geom_type: int) -> wp.vec3: # kernel_analyzer: ignore if geom_type == GeomType.SPHERE: r = size[0] return wp.vec3(r, r, r) @@ -324,7 +324,7 @@ def _fluid_force( continue size = geom_size[worldid % geom_size.shape[0], geomid] - semiaxes = _geom_semiaxes(size, geom_type[geomid]) + semiaxes = geom_semiaxes(size, geom_type[geomid]) geom_rot = geom_xmat_in[worldid, geomid] geom_rotT = wp.transpose(geom_rot) geom_pos = geom_xpos_in[worldid, geomid] diff --git a/mujoco_warp/_src/types.py b/mujoco_warp/_src/types.py index 9b484c60e..103df4ccb 100644 --- a/mujoco_warp/_src/types.py +++ b/mujoco_warp/_src/types.py @@ -611,6 +611,10 @@ class mat63f(wp.types.matrix(shape=(6, 3), dtype=float)): pass +class mat66f(wp.types.matrix(shape=(6, 6), dtype=float)): + pass + + vec5 = vec5f vec6 = vec6f vec8 = vec8f @@ -619,6 +623,7 @@ class mat63f(wp.types.matrix(shape=(6, 3), dtype=float)): mat23 = mat23f mat43 = mat43f mat63 = mat63f +mat66 = mat66f def array(*args) -> wp.array: