From 96b3bd0ea514b48cd3f9ea81a7df6f934dc1a626 Mon Sep 17 00:00:00 2001
From: Gavin Suddrey <--get>
Date: Fri, 22 Jan 2021 14:34:37 +1000
Subject: [PATCH] Setting end effector link for UR models; Added unit test to
 verify URDF models fkine calculation

---
 roboticstoolbox/models/URDF/UR10.py | 9 +++++----
 roboticstoolbox/models/URDF/UR3.py  | 9 +++++----
 roboticstoolbox/models/URDF/UR5.py  | 9 +++++----
 tests/test_models.py                | 6 ++++++
 4 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/roboticstoolbox/models/URDF/UR10.py b/roboticstoolbox/models/URDF/UR10.py
index 92d0a75a4..c4a7e9b1f 100644
--- a/roboticstoolbox/models/URDF/UR10.py
+++ b/roboticstoolbox/models/URDF/UR10.py
@@ -29,14 +29,15 @@ class UR10(ERobot):
 
     def __init__(self):
 
-        args = super().urdf_to_ets_args(
+        elinks, name = super().urdf_to_ets_args(
             "ur_description/urdf/ur10_joint_limited_robot.urdf.xacro"
         )
 
         super().__init__(
-                args[0],
-                name=args[1],
-                manufacturer='Universal Robotics'
+                elinks,
+                name=name,
+                manufacturer='Universal Robotics',
+                gripper_links=elinks[7]
             )
 
         self.addconfiguration(
diff --git a/roboticstoolbox/models/URDF/UR3.py b/roboticstoolbox/models/URDF/UR3.py
index 4e8950b24..521f09e5c 100644
--- a/roboticstoolbox/models/URDF/UR3.py
+++ b/roboticstoolbox/models/URDF/UR3.py
@@ -28,13 +28,14 @@ class UR3(ERobot):
     """
     def __init__(self):
 
-        args = self.urdf_to_ets_args(
+        elinks, name = self.urdf_to_ets_args(
             "ur_description/urdf/ur3_joint_limited_robot.urdf.xacro")
 
         super().__init__(
-                args[0],
-                name=args[1],
-                manufacturer='Universal Robotics'
+                elinks,
+                name=name,
+                manufacturer='Universal Robotics',
+                gripper_links=elinks[7]
             )
 
         self.addconfiguration("qz", np.array([0, 0, 0, 0, 0, 0]))
diff --git a/roboticstoolbox/models/URDF/UR5.py b/roboticstoolbox/models/URDF/UR5.py
index ad8fcfda7..a5420430b 100644
--- a/roboticstoolbox/models/URDF/UR5.py
+++ b/roboticstoolbox/models/URDF/UR5.py
@@ -29,13 +29,14 @@ class UR5(ERobot):
 
     def __init__(self):
 
-        args = super().urdf_to_ets_args(
+        elinks, name = super().urdf_to_ets_args(
             "ur_description/urdf/ur5_joint_limited_robot.urdf.xacro")
 
         super().__init__(
-                args[0],
-                name=args[1],
-                manufacturer='Universal Robotics'
+                elinks,
+                name=name,
+                manufacturer='Universal Robotics',
+                gripper_links=elinks[7]
             )
 
         self.addconfiguration("qz", np.array([0, 0, 0, 0, 0, 0]))
diff --git a/tests/test_models.py b/tests/test_models.py
index 80d9ddc43..6ed115086 100644
--- a/tests/test_models.py
+++ b/tests/test_models.py
@@ -166,6 +166,12 @@ def test_ikine_a_puma(self):
         nt.assert_array_almost_equal(sol0.q, qr0, decimal=4)
         nt.assert_array_almost_equal(sol1.q, qr1, decimal=4)
 
+    def test_fkine_urdf(self):
+      for model_name in rp.models.URDF.__all__:
+        m = getattr(rp.models.URDF, model_name)
+        r = m()
+        r.fkine(r.q)
+
 if __name__ == '__main__':  # pragma nocover
     unittest.main()
     # pytest.main(['tests/test_SerialLink.py'])
\ No newline at end of file