Add implementation of mjd_ellipsoidFluid derivative #897
Conversation
Implements derivative of ellipsoid-based fluid forces with respect to velocities. Includes all component derivatives: - Added mass forces - Viscous torque and drag - Kutta lift - Magnus force The implementation adds the fluid force derivatives to qDeriv, following the pattern from the reference C implementation.
mujoco_warp/_src/derivative.py
Outdated
|
|
||
|
|
||
| @wp.kernel | ||
| def _qderiv_ellipsoid_fluid( |
There was a problem hiding this comment.
lets rename to _deriv_ellipsoid_fluid
mujoco_warp/_src/derivative.py
Outdated
|
|
||
|
|
||
| @wp.kernel | ||
| def _qderiv_ellipsoid_fluid( |
There was a problem hiding this comment.
lets make this a wp.func instead of a wp.kernel. additionally, it should have worldid and bodyid inputs.
mujoco_warp/_src/derivative.py
Outdated
| continue | ||
|
|
||
| size = geom_size[worldid % geom_size.shape[0], geomid] | ||
| semiaxes = _geom_semiaxes_deriv(size, geom_type[geomid]) |
There was a problem hiding this comment.
lets import _geom_semiaxes from passive.py. additionally let's change the name (in passive.py) to geom_semiaxes
mujoco_warp/_src/derivative.py
Outdated
| slender_drag_coef = geom_fluid[geomid, 2] | ||
| ang_drag_coef = geom_fluid[geomid, 3] | ||
|
|
||
| if density > 0.0 and magnus_coef != 0.0: |
There was a problem hiding this comment.
lets structure this with one if density > 0.0 check and checks for magnus_coef, kutta_coef and viscosity in the density check scope.
mujoco_warp/_src/derivative.py
Outdated
|
|
||
|
|
||
| @wp.func | ||
| def _pow2_deriv(val: float) -> float: |
There was a problem hiding this comment.
this function is unused, lets remove it
mujoco_warp/_src/derivative.py
Outdated
| lin_vel = wp.spatial_bottom(local_vels) | ||
| ang_vel = wp.spatial_top(local_vels) | ||
|
|
||
| virtual_lin_mom = wp.vec3( |
There was a problem hiding this comment.
fluid_density * wp.cw_mul(virtual_mass, lin_vel)
mujoco_warp/_src/derivative.py
Outdated
| fluid_density * virtual_mass[1] * lin_vel[1], | ||
| fluid_density * virtual_mass[2] * lin_vel[2] | ||
| ) | ||
| virtual_ang_mom = wp.vec3( |
There was a problem hiding this comment.
fluid_density * wp.cw_mul(virtual_inertia, ang_vel)
mujoco_warp/_src/derivative.py
Outdated
| ang_drag_coef * II[2] + slender_drag_coef * (I_max - II[2]) | ||
| ) | ||
|
|
||
| mom_visc = wp.vec3(x * mom_coef[0], y * mom_coef[1], z * mom_coef[2]) |
mujoco_warp/_src/derivative.py
Outdated
|
|
||
| mom_visc = wp.vec3(x * mom_coef[0], y * mom_coef[1], z * mom_coef[2]) | ||
| norm = wp.length(mom_visc) | ||
| density = fluid_density / wp.max(wp.static(1e-10), norm) |
There was a problem hiding this comment.
lets utilize MJ_MINVAL instead of wp.static(1e-10)
mujoco_warp/_src/derivative.py
Outdated
| norm = wp.length(mom_visc) | ||
| density = fluid_density / wp.max(wp.static(1e-10), norm) | ||
|
|
||
| mom_sq = wp.vec3( |
There was a problem hiding this comment.
-density * wp.cw_mul(wp.cw_mul(ang, mom_coef), mom_coef)
mujoco_warp/_src/derivative.py
Outdated
|
|
||
| proj_denom = aa * xx + bb * yy + cc * zz | ||
| proj_num = a * xx + b * yy + c * zz | ||
| dA_coef = wp.pi / wp.max(wp.static(1e-10), wp.sqrt(proj_num * proj_num * proj_num * proj_denom)) |
mujoco_warp/_src/derivative.py
Outdated
| proj_num = a * xx + b * yy + c * zz | ||
| dA_coef = wp.pi / wp.max(wp.static(1e-10), wp.sqrt(proj_num * proj_num * proj_num * proj_denom)) | ||
|
|
||
| A_proj = wp.pi * wp.sqrt(proj_denom / wp.max(wp.static(1e-10), proj_num)) |
mujoco_warp/_src/derivative.py
Outdated
| A_proj = wp.pi * wp.sqrt(proj_denom / wp.max(wp.static(1e-10), proj_num)) | ||
|
|
||
| norm = wp.sqrt(xx + yy + zz) | ||
| inv_norm = 1.0 / wp.max(wp.static(1e-10), norm) |
mujoco_warp/_src/derivative.py
Outdated
| xz, yz, zz + inner | ||
| ) | ||
|
|
||
| D = D * (-quad_coef * inv_norm) |
mujoco_warp/_src/derivative.py
Outdated
| proj_num = a * xx + b * yy + c * zz | ||
| norm2 = xx + yy + zz | ||
| df_denom = wp.pi * kutta_lift_coef * fluid_density / wp.max( | ||
| wp.static(1e-10), wp.sqrt(proj_denom * proj_num * norm2) |
mujoco_warp/_src/derivative.py
Outdated
| 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(wp.static(1e-10), proj_denom) |
mujoco_warp/_src/derivative.py
Outdated
| dfy_coef = xx * (b - a) + zz * (b - c) | ||
| dfz_coef = xx * (c - a) + yy * (c - b) | ||
| proj_term = proj_num / wp.max(wp.static(1e-10), proj_denom) | ||
| cos_term = proj_num / wp.max(wp.static(1e-10), norm2) |
mujoco_warp/_src/derivative.py
Outdated
| proj_term = proj_num / wp.max(wp.static(1e-10), proj_denom) | ||
| cos_term = proj_num / wp.max(wp.static(1e-10), norm2) | ||
|
|
||
| D = wp.mat33( |
There was a problem hiding this comment.
a - a, b - b and c - c can be replaced with 0.0
mujoco_warp/_src/derivative.py
Outdated
| a - b, b - b, c - b, | ||
| a - c, b - c, c - c | ||
| ) | ||
| D = D * (wp.static(2.0) * proj_num) |
There was a problem hiding this comment.
*= and we can remove the parentheses
mujoco_warp/_src/derivative.py
Outdated
| ) | ||
| D = D * (wp.static(2.0) * proj_num) | ||
|
|
||
| inner_term = wp.vec3( |
There was a problem hiding this comment.
we can utilize wp.cw_mul here
mujoco_warp/_src/derivative.py
Outdated
| lin_vel_scaled = lin_vel * magnus_coef | ||
| ang_vel_scaled = ang_vel * magnus_coef | ||
|
|
||
| D_ang = _cross_deriv_a(ang_vel_scaled, lin_vel) |
There was a problem hiding this comment.
should lin_vel -> lin_vel_scaled?
mujoco_warp/_src/derivative.py
Outdated
| ang_vel_scaled = ang_vel * magnus_coef | ||
|
|
||
| D_ang = _cross_deriv_a(ang_vel_scaled, lin_vel) | ||
| D_lin = _cross_deriv_b(ang_vel, lin_vel_scaled) |
There was a problem hiding this comment.
should ang_vel -> ang_vel_scaled?
mujoco_warp/_src/derivative.py
Outdated
|
|
||
|
|
||
| @wp.func | ||
| def _cross_deriv_a(a: wp.vec3, b: wp.vec3) -> wp.mat33: |
There was a problem hiding this comment.
lets consider combining _cross_deriv_a and _cross_deriv_b into one function that returns 2 wp.mat33 since it looks like we always compute derivatives wrt to both cross product inputs
thowell
left a comment
thowell
left a comment
There was a problem hiding this comment.
@Aaryan-549 thank you for contributing this pr!
in addition to the comments, lets add a test. please let us know if you have any questions. thanks!
…J_MINVAL, restructure density checks, combine cross derivatives
|
Hey @thowell, really sorry for the delay. I have implemented all fixes that you asked for. Let me know If there are any other changes needed. |
2 participants
Fixes #750
Implements derivative of ellipsoid-based fluid forces with respect to velocities. Includes all component derivatives:
The implementation adds the fluid force derivatives to qDeriv, following the pattern from the reference C implementation.