From 99490163df46f65a0cabcf8efef61b3164faa620 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 15 Apr 2025 08:33:00 -0700 Subject: [PATCH] Prevent NaN in cylinder SDF. Fixes #2573. The correction matches the one used in MJX (see _cylinder_grad in collision_sdf.py). PiperOrigin-RevId: 747887406 Change-Id: I1a4f1ac769adb96c98665eace7ce54fb09a7d3a2 --- src/engine/engine_collision_sdf.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index c6fe8eba..f58f414a 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -158,7 +158,9 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, e = mju_abs(x[2]); a[0] = c - size[0]; a[1] = e - size[1]; - mjtNum grada[3] = {x[0] / c, x[1] / c, x[2] / e}; + mjtNum grada[3] = {x[0] / mju_max(c, 1. / mjMAXVAL), + x[1] / mju_max(c, 1. / mjMAXVAL), + x[2] / mju_max(e, 1. / mjMAXVAL)}; int j = a[0] > a[1] ? 0 : 1; if (a[j] < 0) { gradient[0] = j == 0 ? grada[0] : 0; @@ -167,7 +169,7 @@ static void geomGradient(mjtNum gradient[3], const mjModel* m, const mjData* d, } else { b[0] = mju_max(a[0], 0); b[1] = mju_max(a[1], 0); - mjtNum bnorm = mju_norm(b, 2); + mjtNum bnorm = mju_max(mju_norm(b, 2), 1./mjMAXVAL); gradient[0] = grada[0] * b[0] / bnorm; gradient[1] = grada[1] * b[0] / bnorm; gradient[2] = grada[2] * b[1] / bnorm;