From 7561b23b8dc24815719bc06feea7401ad9dcee4a Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Mon, 12 Oct 2026 14:26:34 +0800 Subject: [PATCH 1/2] fix(models): separate wx200 arm and gripper joints --- src/roboticstoolbox/models/URDF/wx200.py | 15 ++++++++++++--- tests/test_models.py | 15 +++++++++++++-- 2 files changed, 25 insertions(+), 5 deletions(-) mode change 100755 => 100644 src/roboticstoolbox/models/URDF/wx200.py diff --git a/src/roboticstoolbox/models/URDF/wx200.py b/src/roboticstoolbox/models/URDF/wx200.py old mode 100755 new mode 100644 index 1e3670a15..9ae4690a1 --- a/src/roboticstoolbox/models/URDF/wx200.py +++ b/src/roboticstoolbox/models/URDF/wx200.py @@ -1,6 +1,7 @@ #!/usr/bin/env python import numpy as np +from spatialmath import SE3 from roboticstoolbox.models.URDF.URDFRobot import URDFRobot @@ -33,11 +34,19 @@ class wx200(URDFRobot): def __init__(self): 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) diff --git a/tests/test_models.py b/tests/test_models.py index cf7a52335..e345fc7e8 100644 --- a/tests/test_models.py +++ b/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,18 @@ 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) + self.assertEqual(r.ee_links[0].name, "/ee_arm_link") + nt.assert_allclose(r.grippers[0].tool.t, [0.050575, 0, 0]) + + 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() From d0d2f21c0fef3491ea5db1ae2633f987a11e9a09 Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Mon, 12 Oct 2026 14:31:02 +0800 Subject: [PATCH 2/2] test(models): verify WX200 gripper-root and tool frames --- tests/test_models.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tests/test_models.py b/tests/test_models.py index e345fc7e8..c6f33ba8c 100644 --- a/tests/test_models.py +++ b/tests/test_models.py @@ -111,8 +111,13 @@ def test_wx200(self): self.assertEqual(len(r.grippers), 1) self.assertEqual(r.grippers[0].n, 3) self.assertEqual(len(r.ee_links), 1) - self.assertEqual(r.ee_links[0].name, "/ee_arm_link") + # 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,))