fix(retargeters): clear Se3Rel smoothing state when grip pose is invalid

Re-arming _first_frame alone drops only the rebaseline sample; the
smoothed-delta EMA buffers still hold pre-loss motion at full strength
(they never decay while the invalid branch returns early), so the second
valid frame after recovery blends stale motion into the output. Zero
both buffers in the invalid-grip branch, matching the reset handler's
convention for re-arming the first-frame baseline.

Addresses CodeRabbit review feedback on #743.

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Signed-off-by: Hougant Chen <hougantc@nvidia.com>

Author: hougantc-nvda

src/core/retargeting_engine_tests/python/test_se3_retargeter_pose_validity.py

@@ -150,3 +150,31 @@ def test_recovery_rebaselines_without_jump(self, retargeter):
 
         delta = np.from_dlpack(outputs["ee_delta"][0])
         npt.assert_array_almost_equal(delta, np.zeros(6))
+
+    def test_invalid_grip_clears_smoothing_state(self, retargeter):
+        """Pre-loss motion must not bleed into the output after recovery.
+
+        The smoothed-delta EMA buffers must be cleared when the grip pose
+        goes invalid; otherwise the second valid frame after recovery blends
+        against stale pre-loss motion.
+        """
+        inputs, outputs = _build_io(retargeter)
+
+        # Baseline, then a large motion so the smoothing buffers are nonzero.
+        _fill_controller(inputs[_DEVICE], grip_valid=True)
+        retargeter.compute(inputs, outputs, _make_context())
+        _fill_controller(inputs[_DEVICE], grip_valid=True, position=(0.2, 0.2, 0.2))
+        retargeter.compute(inputs, outputs, _make_context())
+
+        _fill_controller(inputs[_DEVICE], grip_valid=False)
+        retargeter.compute(inputs, outputs, _make_context())
+
+        # Recovery: first valid frame rebaselines, second is stationary and
+        # must emit zero — any residual comes from stale smoothing state.
+        _fill_controller(inputs[_DEVICE], grip_valid=True, position=(0.5, 0.5, 0.5))
+        retargeter.compute(inputs, outputs, _make_context())
+        _fill_controller(inputs[_DEVICE], grip_valid=True, position=(0.5, 0.5, 0.5))
+        retargeter.compute(inputs, outputs, _make_context())
+
+        delta = np.from_dlpack(outputs["ee_delta"][0])
+        npt.assert_array_almost_equal(delta, np.zeros(6))

src/retargeters/se3_retargeter.py

@@ -393,10 +393,14 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N
         else:
             # A connected controller can report an invalid grip pose (e.g. at
             # rest with pose tracking lost) whose zero-norm orientation
-            # Rotation.from_quat rejects. Emit a zero delta and re-arm the
-            # first-frame baseline so the next valid frame does not jump.
+            # Rotation.from_quat rejects. Emit a zero delta and, as in the
+            # reset handler above, re-arm the first-frame baseline and clear
+            # the smoothing state so the next valid frame neither jumps nor
+            # blends with pre-loss motion.
             if not bool(inp[ControllerInputIndex.GRIP_IS_VALID]):
                 self._first_frame = True
+                self._smoothed_delta_pos = np.zeros(3)
+                self._smoothed_delta_rot = np.zeros(3)
                 ee_delta[0] = np.zeros(6, dtype=np.float32)
                 return
             grip_pos = np.from_dlpack(inp[ControllerInputIndex.GRIP_POSITION])