From 527a7e8b725d19976dd68cafedb77ade7655c67a Mon Sep 17 00:00:00 2001 From: Lidang-Jiang Date: Sat, 28 Mar 2026 22:34:47 +0800 Subject: [PATCH] Fix duplicate variable in null-space condition of _calc_qnull MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The condition `if λΣ > 0 or λΣ > 0:` duplicates the λΣ check, which should be `if λΣ > 0 or λm > 0:`. This caused null-space motion to be skipped entirely when only km (manipulability maximisation) was enabled without kq (joint limit avoidance). Fixes #499 Signed-off-by: Lidang-Jiang --- roboticstoolbox/robot/IK.py | 2 +- tests/test_IK.py | 70 +++++++++++++++++++++++++++++++++++++ 2 files changed, 71 insertions(+), 1 deletion(-) diff --git a/roboticstoolbox/robot/IK.py b/roboticstoolbox/robot/IK.py index f784158b6..5d350f5f9 100644 --- a/roboticstoolbox/robot/IK.py +++ b/roboticstoolbox/robot/IK.py @@ -569,7 +569,7 @@ def _calc_qnull( qnull_grad += (1.0 / λm * Jm).flatten() # Calculate the null-space motion - if λΣ > 0 or λΣ > 0: + if λΣ > 0 or λm > 0: null_space = np.eye(ets.n) - np.linalg.pinv(J) @ J qnull = null_space @ qnull_grad diff --git a/tests/test_IK.py b/tests/test_IK.py index aacfe1029..e1f428725 100644 --- a/tests/test_IK.py +++ b/tests/test_IK.py @@ -841,6 +841,76 @@ def test_iter_iksol(self): self.assertEqual(f, "") +class TestCalcQnull(unittest.TestCase): + """Tests for _calc_qnull to verify null-space motion activation logic. + + Regression tests for https://github.com/petercorke/robotics-toolbox-python/issues/499 + The bug was a typo: `if λΣ > 0 or λΣ > 0:` instead of `if λΣ > 0 or λm > 0:`. + This caused null-space motion to be skipped when only km (manipulability + maximisation gain) was set and kq (joint limit avoidance gain) was zero. + """ + + def setUp(self): + from roboticstoolbox.robot.IK import _calc_qnull + + self._calc_qnull = _calc_qnull + self.panda = rtb.models.Panda().ets() + self.q = np.array([0.5, -1.0, 0.3, -1.5, 0.2, 1.5, 0.5]) + self.J = self.panda.jacob0(self.q) + + def test_qnull_both_gains_zero(self): + """When both kq and km are zero, null-space motion should be zero.""" + result = self._calc_qnull( + ets=self.panda, q=self.q, J=self.J, + λΣ=0.0, λm=0.0, ps=0.05, pi=0.3, + ) + nt.assert_array_equal(result, np.zeros(self.panda.n)) + + def test_qnull_km_only(self): + """When only km > 0 (manipulability maximisation), null-space motion + should be non-zero. This is the regression test for issue #499.""" + result = self._calc_qnull( + ets=self.panda, q=self.q, J=self.J, + λΣ=0.0, λm=1.0, ps=0.05, pi=0.3, + ) + self.assertFalse( + np.allclose(result, 0), + "Null-space motion should be non-zero when km > 0, " + "even if kq == 0 (issue #499)", + ) + + def test_qnull_kq_only(self): + """When only kq > 0 (joint limit avoidance), _calc_qnull should run + without error and return a result with the correct shape.""" + result = self._calc_qnull( + ets=self.panda, q=self.q, J=self.J, + λΣ=1.0, λm=0.0, ps=0.05, pi=0.3, + ) + self.assertEqual(result.shape, (self.panda.n,)) + + def test_qnull_both_gains_positive(self): + """When both gains are positive, null-space motion should combine + joint limit avoidance and manipulability maximisation.""" + result = self._calc_qnull( + ets=self.panda, q=self.q, J=self.J, + λΣ=1.0, λm=1.0, ps=0.05, pi=0.3, + ) + self.assertEqual(result.shape, (self.panda.n,)) + + def test_qnull_km_only_matches_both_when_kq_inactive(self): + """When joint positions are far from limits (so joint limit gradient + is zero), km-only result should match the both-gains result.""" + result_km = self._calc_qnull( + ets=self.panda, q=self.q, J=self.J, + λΣ=0.0, λm=1.0, ps=0.05, pi=0.3, + ) + result_both = self._calc_qnull( + ets=self.panda, q=self.q, J=self.J, + λΣ=1.0, λm=1.0, ps=0.05, pi=0.3, + ) + nt.assert_array_almost_equal(result_km, result_both, decimal=10) + + if __name__ == "__main__": unittest.main()