Interbotix URDF models (wx200 etc.): gripper counted as arm joints, ambiguous end-effector, qr outside joint limits #729

The Interbotix URDF models (rtb.models.URDF.wx200 and its siblings) are not set up like the other arm models: the gripper joints are counted as arm joints, there are four candidate end-effectors with a poor default, and the named qr configuration looks copied from the Panda and is outside the joint limits. Found while answering discussion #445 (a user servoing a wx200 whose loop never terminated).

RTB 1.4.4, rtb.models.URDF.wx200().

1. The gripper is part of the "arm"

ERobot: wx200 (by Interbotix), 8 joints (RRRRRRPP), 4 branches, dynamics, geometry, collision
...
│    5 │ /gripper_link       │     4 │ /wrist_link       │ SE3(0.065, 0, 0; -180°, -0°, 0°) ⊕ Rx(q4) │
│    6 │ /ee_arm_link        │       │ /gripper_link     │ SE3(0.043, 0, 0)                          │
│    7 │ @/gripper_prop_link │     5 │ /ee_arm_link      │ SE3(0.0055, 0, 0) ⊕ Rx(q5)                │
│    8 │ /gripper_bar_link   │       │ /ee_arm_link      │ SE3()                                     │
│    9 │ /fingers_link       │       │ /gripper_bar_link │ SE3(0.023, 0, 0)                          │
│   10 │ @/left_finger_link  │     6 │ /fingers_link     │ SE3() ⊕ ty(q6)                            │
│   11 │ @/right_finger_link │     7 │ /fingers_link     │ SE3() ⊕ ty(q7)                            │
│   12 │ @/ee_gripper_link   │       │ /fingers_link     │ SE3(0.02757, 0, 0)                        │

The wx200 is a 5-DoF arm (q0 to q4). The model has n = 8: q5 is the gripper's rotary "prop" joint (Rx, limits ±π), and q6 and q7 are the two prismatic fingers. robot.q, qr, qlim and the Jacobian all mix arm and gripper coordinates. Compare rtb.models.URDF.Panda, where the hand is a Gripper and n = 7.

2. Four end-effectors, default is not the tool point

>>> r.ee_links
['/gripper_prop_link', '/left_finger_link', '/right_finger_link', '/ee_gripper_link']
>>> r.grippers
[]
>>> r.fkine(r.qr)    # prints: multiple end-effectors present, ambiguous, using self.ee_links[0]

• fkine and jacob0 default to gripper_prop_link, which is a rotary joint link, not the tool point. At qr its position differs from /ee_gripper_link by about 4.5 cm: (0.143, 0, 0.268) vs (0.128, 0, 0.225).
• Because gripper_prop_link carries the q5 joint, the default Jacobian is 6 x 6. It reports a 6-DoF arm, but the sixth "DoF" is the gripper roll.
• grippers is empty even though there are finger joints.

Related: #725 (default end-effector frame of the Panda matches no named link) and #705 (payload() has no answer for multiple end-effectors).

3. qr and qz are not valid configurations

qr  = [0, -0.3, 0, -2.2, 0, 2.0, 0.785, 0]
qz  = [0, 0, 0, 0, 0, 0, 0, 0]
qlim (rows are joints):
  q0 [-3.142, 3.142]   q1 [-1.885, 1.972]   q2 [-1.623, 1.885]   q3 [-2.147, 1.745]
  q4 [-3.142, 3.142]   q5 [-3.142, 3.142]   q6 [ 0.015, 0.037]   q7 [-0.037, -0.015]

• qr is exactly the Panda's qr plus a trailing zero. Here that sets the finger joint q6 to 0.785 m (the finger travel is 0.015 to 0.037 m), the wrist-angle joint q3 to -2.2 (limit -2.147), and q7 to 0, outside its range.
• qz has the fingers at 0, outside their limits (0.015 to 0.037 m).

4. Same pattern on the other Interbotix models

Checked on 1.4.4. All have 4 ee_links with gripper_prop_link first, and the same copied qr:

Effect on users

Someone servoing the default end-effector to a full 6-DoF pose with a pseudo-inverse loop never converges, because the arm has 5 DoF plus the gripper roll. A full-pose goal from fkine converges (about 110 steps), but an arbitrary position and orientation does not (still 5.5 cm away after 1500 steps), while servoing on position only arrives in about 170 steps.

Suggested fix

• Model the gripper as a Gripper (like the Panda), so n is the number of arm joints, with the fingers and prop as gripper joints.
• Set the default end-effector to /ee_gripper_link (the URDF's own tool point) and make ee_links unambiguous.
• Replace qr with a real, in-limit "ready" configuration for each model, and make qz in-limit or document it.
• Add a test per model: qr and qz within qlim, one end-effector, expected n.