fix(models): separate WX200 arm and gripper joints

src/roboticstoolbox/models/URDF/wx200.py

@@ -3,6 +3,7 @@
 
 #!/usr/bin/env python
 import numpy as np
+from spatialmath import SE3
 
 from roboticstoolbox.models.URDF.URDFRobot import URDFRobot
 
@@ -36,11 +37,19 @@ class wx200(URDFRobot):
     def __init__(self) -> None:
 
         super().__init__(
-            "trossen_descriptions/urdf/wx200.urdf.xacro", manufacturer="Interbotix"
+            "trossen_descriptions/urdf/wx200.urdf.xacro",
+            manufacturer="Interbotix",
+            gripper_link_name="/ee_arm_link",
         )
 
-        self.qr = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4, 0])
-        self.qz = np.zeros(8)
+        # ee_gripper_link is reached from ee_arm_link by two fixed x-translations
+        # (0.023 m to fingers_link and 0.027575 m to ee_gripper_link).
+        self.grippers[0].tool = SE3.Tx(0.050575)
+
+        # Only the five arm joints belong to Robot.q; the three gripper joints
+        # are managed by the Gripper object. Keep named arm poses within limits.
+        self.qr = np.array([0, -0.3, 0, -2.0, 0])
+        self.qz = np.zeros(5)
 
         self.addconfiguration("qr", self.qr)
         self.addconfiguration("qz", self.qz)

tests/test_models.py

@@ -5,6 +5,7 @@
 
 import roboticstoolbox as rp
 import unittest
+import numpy as np
 import numpy.testing as nt
 
 
@@ -105,8 +106,23 @@ def test_vx300s(self):
 
     def test_wx200(self):
         r = rp.models.wx200()
-        r.qr
-        r.qz
+        # The three gripper joints must not inflate the five-DOF arm Jacobian.
+        self.assertEqual(r.n, 5)
+        self.assertEqual(len(r.grippers), 1)
+        self.assertEqual(r.grippers[0].n, 3)
+        self.assertEqual(len(r.ee_links), 1)
+        # Robot.ee_links names the parent of the gripper root, not the root.
+        self.assertEqual(r.ee_links[0].name, "/gripper_link")
+        nt.assert_allclose(r.grippers[0].tool.t, [0.050575, 0, 0])
+        # The default tool pose must match the URDF's actual fixed tool link.
+        nt.assert_allclose(
+            r.fkine(r.qz).A, r.fkine(r.qz, end="/ee_gripper_link").A, atol=1e-10
+        )
+
+        for q in (r.qr, r.qz):
+            self.assertEqual(q.shape, (r.n,))
+            self.assertTrue(np.all(q >= r.qlim[0]))
+            self.assertTrue(np.all(q <= r.qlim[1]))
 
     def test_wx250(self):
         r = rp.models.wx250()