diff --git a/mujoco_warp/_src/derivative.py b/mujoco_warp/_src/derivative.py index 71bd7c64f..327d6f790 100644 --- a/mujoco_warp/_src/derivative.py +++ b/mujoco_warp/_src/derivative.py @@ -15,11 +15,15 @@ import warp as wp +from mujoco_warp._src import support +from mujoco_warp._src.types import MJ_MINVAL from mujoco_warp._src.types import BiasType from mujoco_warp._src.types import Data from mujoco_warp._src.types import DisableBit from mujoco_warp._src.types import DynType from mujoco_warp._src.types import GainType +from mujoco_warp._src.types import GeomType +from mujoco_warp._src.types import IntegratorType from mujoco_warp._src.types import Model from mujoco_warp._src.types import TileSet from mujoco_warp._src.types import vec10f @@ -210,6 +214,538 @@ def _qderiv_tendon_damping( qDeriv_out[worldid, dofjid, dofiid] -= qderiv +@wp.func +def _ellipsoid_max_moment(size: wp.vec3, dir: int) -> float: + d0 = size[dir] + d1 = size[(dir + 1) % 3] + d2 = size[(dir + 2) % 3] + max_d1_d2 = wp.max(d1, d2) + return 8.0 / 15.0 * wp.PI * d0 * wp.pow(max_d1_d2, 4.0) + + +@wp.func +def _add_to_quadrant(B: wp.spatial_matrix, D: wp.mat33, col_quad: int, row_quad: int) -> wp.spatial_matrix: + r = 3 * row_quad + c = 3 * col_quad + for i in range(3): + for j in range(3): + B[r + i, c + j] += D[i, j] + return B + + +@wp.func +def _derivative_cross(vec_a: wp.vec3, vec_b: wp.vec3): + Da = wp.skew(vec_b) + Db = -wp.skew(vec_a) + return Da, Db + + +@wp.func +def _fluid_added_mass_forces( + # In: + B: wp.spatial_matrix, + lvel: wp.spatial_vector, + fluid_density: float, + virtual_mass: wp.vec3, + virtual_inertia: wp.vec3, +) -> wp.spatial_matrix: + lin_vel = lvel[3:6] + ang_vel = lvel[0:3] + + virtual_lin_mom = fluid_density * wp.cw_mul(virtual_mass, lin_vel) + virtual_ang_mom = fluid_density * wp.cw_mul(virtual_inertia, ang_vel) + + Da, Db = _derivative_cross(virtual_ang_mom, ang_vel) + scale = fluid_density * virtual_inertia + Da_scaled = wp.transpose( + wp.mat33( + Da[0] * scale[0], + Da[1] * scale[1], + Da[2] * scale[2], + ) + ) + B = _add_to_quadrant(B, Da_scaled, 0, 0) + B = _add_to_quadrant(B, Db, 0, 0) + + Da, Db = _derivative_cross(virtual_lin_mom, lin_vel) + scale = fluid_density * virtual_mass + Da_scale = wp.transpose( + wp.mat33( + Da[0] * scale[0], + Da[1] * scale[1], + Da[2] * scale[2], + ) + ) + B = _add_to_quadrant(B, Da_scale, 0, 1) + B = _add_to_quadrant(B, Db, 0, 1) + + Da, Db = _derivative_cross(virtual_lin_mom, ang_vel) + scale = fluid_density * virtual_mass + Da_scale = wp.transpose( + wp.mat33( + Da[0] * scale[0], + Da[1] * scale[1], + Da[2] * scale[2], + ) + ) + B = _add_to_quadrant(B, Da_scale, 1, 1) + B = _add_to_quadrant(B, Db, 1, 0) + + return B + + +@wp.func +def _fluid_viscous_torque( + # In: + B: wp.spatial_matrix, + lvel: wp.spatial_vector, + fluid_density: float, + fluid_viscosity: float, + size: wp.vec3, + slender_drag_coef: float, + ang_drag_coef: float, +) -> wp.spatial_matrix: + 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 = 2.0 / 3.0 * (size[0] + size[1] + size[2]) + lin_visc_torq_coef = wp.PI * wp.pow(eq_sphere_D, 3.0) + + I_max = 8.0 / 15.0 * wp.PI * d_mid * wp.pow(d_max, 4.0) + II = wp.vec3(_ellipsoid_max_moment(size, 0), _ellipsoid_max_moment(size, 1), _ellipsoid_max_moment(size, 2)) + + ang_vel = lvel[0:3] + + mom_coef = ang_drag_coef * II + slender_drag_coef * (wp.vec3(I_max) - II) + + mom_visc = wp.cw_mul(ang_vel, mom_coef) + density = fluid_density / wp.max(MJ_MINVAL, wp.length(mom_visc)) + + mom_sq = -density * wp.cw_mul(ang_vel, wp.cw_mul(mom_coef, mom_coef)) + lin_coef = fluid_viscosity * lin_visc_torq_coef + + sum_val = wp.dot(ang_vel, mom_sq) - lin_coef + D = wp.diag(wp.vec3(sum_val, sum_val, sum_val)) + D += wp.outer(mom_sq, ang_vel) + + B = _add_to_quadrant(B, D, 0, 0) + return B + + +@wp.func +def _fluid_viscous_drag( + # In: + B: wp.spatial_matrix, + lvel: wp.spatial_vector, + fluid_density: float, + fluid_viscosity: float, + size: wp.vec3, + blunt_drag_coef: float, + slender_drag_coef: float, +) -> wp.spatial_matrix: + 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 = 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 + + x = lvel[3] + y = lvel[4] + z = lvel[5] + xx = x * x + yy = y * y + zz = z * 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 * 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_sum = xx + yy + zz + # D = diag(inner_sum) + outer(v, v) + D = wp.diag(wp.vec3(inner_sum, inner_sum, inner_sum)) + wp.outer(wp.vec3(x, y, z), wp.vec3(x, y, z)) + + scale = -quad_coef * inv_norm + D *= scale + + D += wp.outer(dAproj_dv, -wp.vec3(x, y, z)) + D -= wp.diag(wp.vec3(lin_coef, lin_coef, lin_coef)) + + B = _add_to_quadrant(B, D, 1, 1) + return B + + +@wp.func +def _fluid_kutta_lift( + # In: + B: wp.spatial_matrix, + lvel: wp.spatial_vector, + fluid_density: float, + size: wp.vec3, + kutta_lift_coef: float, +) -> wp.spatial_matrix: + a = wp.pow(size[1] * size[2], 2.0) + b = wp.pow(size[2] * size[0], 2.0) + c = wp.pow(size[0] * size[1], 2.0) + aa = a * a + bb = b * b + cc = c * c + + x = lvel[3] + y = lvel[4] + z = lvel[5] + xx = x * x + yy = y * y + zz = z * 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) + + skew_vec = wp.vec3(c - b, a - c, b - a) + D = wp.skew(skew_vec) * (2.0 * proj_num) + + inner_term = wp.vec3(aa * proj_term - a + cos_term, bb * proj_term - b + cos_term, cc * proj_term - c + cos_term) + + df_coef = wp.vec3(dfx_coef, dfy_coef, dfz_coef) + D += wp.outer(inner_term, df_coef) + + # Element-wise multiply by outer(v, v) + v = wp.vec3(x, y, z) + D = wp.cw_mul(D, wp.outer(v, v)) + + # Subtract diagonal terms: df_coef * proj_num + D -= wp.diag(df_coef * proj_num) + + D *= df_denom + + B = _add_to_quadrant(B, D, 1, 1) + return B + + +@wp.func +def _fluid_magnus_force( + # In: + B: wp.spatial_matrix, + lvel: wp.spatial_vector, + fluid_density: float, + size: wp.vec3, + magnus_lift_coef: float, +) -> wp.spatial_matrix: + volume = 4.0 / 3.0 * wp.PI * size[0] * size[1] * size[2] + magnus_coef = magnus_lift_coef * fluid_density * volume + + lin_vel = lvel[3:] + ang_vel = lvel[:3] + lin_vel *= magnus_coef + ang_vel *= magnus_coef + + D_ang, D_lin = _derivative_cross(ang_vel, lin_vel) + + B = _add_to_quadrant(B, D_ang, 1, 0) + B = _add_to_quadrant(B, D_lin, 1, 1) + return B + + +@wp.func +def _fluid_inertia_box_forces( + # In: + B: wp.spatial_matrix, + lvel: wp.spatial_vector, + fluid_density: float, + fluid_viscosity: float, + mass: float, + inertia: wp.vec3, +) -> wp.spatial_matrix: + # Equivalent inertia box + box = wp.vec3( + wp.sqrt(wp.max(MJ_MINVAL, inertia[1] + inertia[2] - inertia[0]) / mass * 6.0), + wp.sqrt(wp.max(MJ_MINVAL, inertia[0] + inertia[2] - inertia[1]) / mass * 6.0), + wp.sqrt(wp.max(MJ_MINVAL, inertia[0] + inertia[1] - inertia[2]) / mass * 6.0), + ) + + # Viscous force and torque + if fluid_viscosity > 0.0: + diam = (box[0] + box[1] + box[2]) / 3.0 + + # Rotational viscosity + visc_rot = -wp.PI * diam * diam * diam * fluid_viscosity + B[0, 0] += visc_rot + B[1, 1] += visc_rot + B[2, 2] += visc_rot + + # Translational viscosity + visc_lin = -3.0 * wp.PI * diam * fluid_viscosity + B[3, 3] += visc_lin + B[4, 4] += visc_lin + B[5, 5] += visc_lin + + # Lift and drag force and torque + if fluid_density > 0.0: + term0 = box[1] * box[1] * box[1] * box[1] + box[2] * box[2] * box[2] * box[2] + term1 = box[0] * box[0] * box[0] * box[0] + box[2] * box[2] * box[2] * box[2] + term2 = box[0] * box[0] * box[0] * box[0] + box[1] * box[1] * box[1] * box[1] + + B[0, 0] -= fluid_density * box[0] * term0 * wp.abs(lvel[0]) / 32.0 + B[1, 1] -= fluid_density * box[1] * term1 * wp.abs(lvel[1]) / 32.0 + B[2, 2] -= fluid_density * box[2] * term2 * wp.abs(lvel[2]) / 32.0 + + B[3, 3] -= fluid_density * box[1] * box[2] * wp.abs(lvel[3]) + B[4, 4] -= fluid_density * box[0] * box[2] * wp.abs(lvel[4]) + B[5, 5] -= fluid_density * box[0] * box[1] * wp.abs(lvel[5]) + + return B + + +@wp.func +def _get_geom_semiaxes(type: int, size: wp.vec3) -> wp.vec3: + if type == GeomType.SPHERE: + return wp.vec3(size[0], size[0], size[0]) + elif type == GeomType.CAPSULE: + return wp.vec3(size[0], size[0], size[0] + size[1]) + elif type == GeomType.ELLIPSOID: + return size + elif type == GeomType.CYLINDER: + return wp.vec3(size[0], size[0], size[1]) + elif type == GeomType.BOX: + return size + else: + return wp.vec3(0.0, 0.0, 0.0) + + +@wp.func +def _is_ancestor(body_parentid: wp.array(dtype=int), ancestor: int, child: int) -> bool: + if child == ancestor: + return True + + curr = child + while curr != 0: + curr = body_parentid[curr] + if curr == ancestor: + return True + + return False + + +@wp.func +def _get_jac_column_local( + # Model: + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + dof_bodyid: wp.array(dtype=int), + # Data in: + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + # In: + point_global: wp.vec3, + bodyid: int, + dofid: int, + worldid: int, + b_imat: wp.mat33, +) -> wp.spatial_vector: + jacp, jacr = support.jac( + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, point_global, bodyid, dofid, worldid + ) + jacp_loc = wp.transpose(b_imat) @ jacp + jacr_loc = wp.transpose(b_imat) @ jacr + return wp.spatial_vector(jacr_loc, jacp_loc) + + +@wp.kernel +def _qderiv_fluid( + # Model: + nbody: int, + opt_timestep: wp.array(dtype=float), + opt_density: wp.array(dtype=float), + opt_viscosity: wp.array(dtype=float), + opt_wind: wp.array(dtype=wp.vec3), + opt_integrator: int, + opt_is_sparse: bool, + 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_mass: wp.array2d(dtype=float), + body_inertia: wp.array2d(dtype=wp.vec3), + dof_bodyid: wp.array(dtype=int), + geom_type: wp.array(dtype=int), + geom_size: wp.array2d(dtype=wp.vec3), + geom_fluid: wp.array2d(dtype=float), + body_fluid_ellipsoid: wp.array(dtype=bool), + # 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), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + cvel_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), +): + worldid, elemid = wp.tid() + dofi = qMi[elemid] + dofj = qMj[elemid] + + bodyi = dof_bodyid[dofi] + bodyj = dof_bodyid[dofj] + + start_body = wp.max(bodyi, bodyj) + + density = opt_density[worldid] + viscosity = opt_viscosity[worldid] + wind = opt_wind[worldid] + dt = opt_timestep[worldid] + is_implicit = opt_integrator == IntegratorType.IMPLICITFAST + + accum = float(0.0) + + for b in range(start_body, nbody): + if not _is_ancestor(body_parentid, start_body, b) or body_mass[worldid, b] < MJ_MINVAL: + continue + + if body_fluid_ellipsoid[b]: + start_geom = body_geomadr[b] + num_geom = body_geomnum[b] + + for g in range(start_geom, start_geom + num_geom): + # geom_interaction_coef is at index 0 + if geom_fluid[g, 0] == 0.0: + continue + + subtree_root = subtree_com_in[worldid, body_rootid[b]] + xipos_b = xipos_in[worldid, b] + g_pos = geom_xpos_in[worldid, g] + + vel_body = cvel_in[worldid, b] + w_body = wp.vec3(vel_body[0], vel_body[1], vel_body[2]) + v_body_lin = wp.vec3(vel_body[3], vel_body[4], vel_body[5]) + + lin_com = v_body_lin - wp.cross(xipos_b - subtree_root, w_body) + v_geom_lin = lin_com + wp.cross(w_body, g_pos - xipos_b) + + g_mat = geom_xmat_in[worldid, g] + w_local = wp.transpose(g_mat) @ w_body + v_local_linear = wp.transpose(g_mat) @ v_geom_lin + wind_local = wp.transpose(g_mat) @ wind + + lvel = wp.spatial_vector(w_local, v_local_linear - wind_local) + + # Initialize B as zero spatial matrix + B = wp.spatial_matrix(0.0) + + # Handle potentially batched geom_size (safe broadcasting) + geom_size_id = worldid % geom_size.shape[0] + semiaxes = _get_geom_semiaxes(geom_type[g], geom_size[geom_size_id, g]) + + # Unpack params + blunt = geom_fluid[g, 1] + slender = geom_fluid[g, 2] + ang_drag = geom_fluid[g, 3] + kutta = geom_fluid[g, 4] + magnus = geom_fluid[g, 5] + vm = wp.vec3(geom_fluid[g, 6], geom_fluid[g, 7], geom_fluid[g, 8]) + vi = wp.vec3(geom_fluid[g, 9], geom_fluid[g, 10], geom_fluid[g, 11]) + + B = _fluid_magnus_force(B, lvel, density, semiaxes, magnus) + B = _fluid_kutta_lift(B, lvel, density, semiaxes, kutta) + B = _fluid_viscous_drag(B, lvel, density, viscosity, semiaxes, blunt, slender) + B = _fluid_viscous_torque(B, lvel, density, viscosity, semiaxes, slender, ang_drag) + B = _fluid_added_mass_forces(B, lvel, density, vm, vi) + if is_implicit: + B = 0.5 * (B + wp.transpose(B)) + # Get Jacobian columns + J_i = _get_jac_column_local( + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, g_pos, b, dofi, worldid, g_mat + ) + J_j = _get_jac_column_local( + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, g_pos, b, dofj, worldid, g_mat + ) + + accum += wp.dot(J_i, B @ J_j) + else: + # Inertia box approximation + mass = body_mass[worldid, b] + inertia = body_inertia[worldid, b] + + subtree_root = subtree_com_in[worldid, body_rootid[b]] + b_ipos = xipos_in[worldid, b] + + # Body velocity in local frame + vel_subtree = cvel_in[worldid, b] + v_subtree_ang = wp.vec3(vel_subtree[0], vel_subtree[1], vel_subtree[2]) + v_subtree_lin = wp.vec3(vel_subtree[3], vel_subtree[4], vel_subtree[5]) + + lin_com = v_subtree_lin - wp.cross(b_ipos - subtree_root, v_subtree_ang) + + b_imat = ximat_in[worldid, b] + v_local_ang = wp.transpose(b_imat) @ v_subtree_ang + v_local_lin = wp.transpose(b_imat) @ lin_com + + # Wind in local frame + wind_local = wp.transpose(b_imat) @ wind + + lvel = wp.spatial_vector(v_local_ang, v_local_lin - wind_local) + B = wp.spatial_matrix(0.0) + B = _fluid_inertia_box_forces(B, lvel, density, viscosity, mass, inertia) + + if is_implicit: + B = 0.5 * (B + wp.transpose(B)) + + # Jacobian at body frame + J_i = _get_jac_column_local( + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, b_ipos, b, dofi, worldid, b_imat + ) + J_j = _get_jac_column_local( + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, b_ipos, b, dofj, worldid, b_imat + ) + + accum += wp.dot(J_i, B @ J_j) + + val = accum * dt + + if opt_is_sparse: + qDeriv_out[worldid, 0, elemid] -= val + else: + qDeriv_out[worldid, dofi, dofj] -= val + if dofi != dofj: + qDeriv_out[worldid, dofj, dofi] -= val + + @event_scope def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d(dtype=float)): """Analytical derivative of smooth forces w.r.t. velocities. @@ -288,5 +824,40 @@ def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d(dtype=float)): inputs=[m.ntendon, m.opt.timestep, m.opt.is_sparse, m.tendon_damping, d.ten_J, qMi, qMj], outputs=[out], ) + if m.opt.has_fluid: + wp.launch( + _qderiv_fluid, + dim=(d.nworld, m.qM_fullm_i.size), + inputs=[ + m.nbody, + m.opt.timestep, + m.opt.density, + m.opt.viscosity, + m.opt.wind, + m.opt.integrator, + m.opt.is_sparse, + m.body_parentid, + m.body_rootid, + m.body_geomnum, + m.body_geomadr, + m.body_mass, + m.body_inertia, + m.dof_bodyid, + m.geom_type, + m.geom_size, + m.geom_fluid, + m.body_fluid_ellipsoid, + d.xipos, + d.ximat, + d.geom_xpos, + d.geom_xmat, + d.subtree_com, + d.cdof, + d.cvel, + m.qM_fullm_i, + m.qM_fullm_j, + ], + outputs=[out], + ) # TODO(team): rne derivative diff --git a/mujoco_warp/_src/derivative_test.py b/mujoco_warp/_src/derivative_test.py index b8b64208f..bd73ed970 100644 --- a/mujoco_warp/_src/derivative_test.py +++ b/mujoco_warp/_src/derivative_test.py @@ -26,11 +26,11 @@ # tolerance for difference between MuJoCo and mjwarp smooth calculations - mostly # due to float precision -_TOLERANCE = 5e-5 +_TOLERANCE = 1e-6 def _assert_eq(a, b, name): - tol = _TOLERANCE * 10 # avoid test noise + tol = _TOLERANCE err_msg = f"mismatch: {name}" np.testing.assert_allclose(a, b, err_msg=err_msg, atol=tol, rtol=tol) @@ -118,6 +118,62 @@ def test_smooth_vel(self, jacobian): _assert_eq(mjw_out, mj_out, "qM - dt * qDeriv") + @parameterized.product( + jacobian=(mujoco.mjtJacobian.mjJAC_DENSE, mujoco.mjtJacobian.mjJAC_SPARSE), + fluidshape=("none", "ellipsoid"), + ) + def test_smooth_vel_fluid(self, jacobian, fluidshape): + """Tests fluid forces qDeriv.""" + mjm, mjd, m, d = test_data.fixture( + xml=f""" + + + """, + 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] + mjw_out = mjw_out + mjw_out.T - np.diag(np.diag(mjw_out)) + 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") + if __name__ == "__main__": wp.init() diff --git a/mujoco_warp/_src/forward_test.py b/mujoco_warp/_src/forward_test.py index 1880e2433..e72d549cd 100644 --- a/mujoco_warp/_src/forward_test.py +++ b/mujoco_warp/_src/forward_test.py @@ -251,6 +251,43 @@ def test_implicit_position(self): _assert_eq(d.qpos.numpy()[0], mjd.qpos, "qpos") _assert_eq(d.qvel.numpy()[0], mjd.qvel, "qvel") + @parameterized.product( + jacobian=(mujoco.mjtJacobian.mjJAC_DENSE, mujoco.mjtJacobian.mjJAC_SPARSE), + fluidshape=("none", "ellipsoid"), + ) + def test_implicit_fluid(self, jacobian, fluidshape): + mjm, mjd, m, d = test_data.fixture( + xml=f""" + + + """, + keyframe=0, + overrides={"opt.jacobian": jacobian}, + ) + + mujoco.mj_implicit(mjm, mjd) + + mjw.solve(m, d) # compute efc.Ma + mjw.implicit(m, d) + + _assert_eq(d.qpos.numpy()[0], mjd.qpos, "qpos") + _assert_eq(d.qvel.numpy()[0], mjd.qvel, "qvel") + def test_implicit_tendon_damping(self): mjm, mjd, m, d = test_data.fixture( "tendon/damping.xml", diff --git a/mujoco_warp/_src/io.py b/mujoco_warp/_src/io.py index 5df2ed382..217b21a2c 100644 --- a/mujoco_warp/_src/io.py +++ b/mujoco_warp/_src/io.py @@ -132,12 +132,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 91f81611e..27be7cfe3 100644 --- a/mujoco_warp/_src/io_test.py +++ b/mujoco_warp/_src/io_test.py @@ -479,23 +479,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_test.py b/mujoco_warp/_src/passive_test.py index 8d13aad41..9e6ea36dc 100644 --- a/mujoco_warp/_src/passive_test.py +++ b/mujoco_warp/_src/passive_test.py @@ -95,11 +95,15 @@ def test_fluid(self, density, viscosity, wind0, wind1, wind2): _assert_eq(d.qfrc_passive.numpy()[0], mjd.qfrc_passive, "qfrc_passive") _assert_eq(d.qfrc_fluid.numpy()[0], mjd.qfrc_fluid, "qfrc_fluid") - def test_ellipsoid_fluid(self): + @parameterized.parameters( + "Euler", + "implicitfast", + ) + def test_ellipsoid_fluid(self, integrator): mjm, mjd, m, d = test_data.fixture( - xml=""" + xml=f""" -