Fix configure_origin crash on xyz-rpy 6-vector input (#784)

The (6,) branch overwrote the input with np.eye(4) before reading it,
so any 6-vector origin raised a broadcast ValueError.  Build the matrix
with xyzrpy2matrix instead, and cover the path with unit tests.

Author: iory

skrobot/utils/urdf.py

@@ -29,6 +29,7 @@
 from skrobot.coordinates import normalize_vector
 from skrobot.coordinates.math import matrix2ypr
 from skrobot.coordinates.math import rpy2matrix
+from skrobot.coordinates.math import xyzrpy2matrix
 from skrobot.pycompat import lru_cache
 
 
@@ -708,10 +709,7 @@ def configure_origin(value):
     elif isinstance(value, (list, tuple, np.ndarray)):
         value = np.asanyarray(value).astype(np.float64)
         if value.shape == (6,):
-            value = np.eye(4).astype(np.float64)
-            value[:3, 3] = value[:3]
-            roll, pitch, yaw = value[3:]
-            value[:3, :3] = rpy2matrix(roll, pitch, yaw)
+            value = xyzrpy2matrix(value[:3], value[3:])
         elif value.shape != (4, 4):
             raise ValueError('Origin must be specified as a 4x4 '
                              'homogenous transformation matrix')

tests/skrobot_tests/utils_tests/test_urdf.py

@@ -362,3 +362,32 @@ def test_get_path_with_cache_falls_back_to_env(self):
              mock.patch.object(urdf_utils, '_try_rospkg', return_value=None):
             assert os.path.samefile(
                 urdf_utils.get_path_with_cache('my_robot'), share)
+
+
+class TestConfigureOrigin(unittest.TestCase):
+
+    def test_xyzrpy_6vector(self):
+        # xyz + rpy 6-vector -> 4x4 (this path used to overwrite the input
+        # with np.eye(4) before reading it and crashed on unpacking)
+        from skrobot.coordinates.math import rpy2matrix
+
+        xyz = [1.0, 2.0, 3.0]
+        rpy = [0.1, 0.2, 0.3]
+        matrix = urdf_utils.configure_origin(list(xyz) + list(rpy))
+        self.assertEqual(matrix.shape, (4, 4))
+        np.testing.assert_allclose(matrix[:3, 3], xyz)
+        np.testing.assert_allclose(matrix[:3, :3], rpy2matrix(*rpy))
+        np.testing.assert_allclose(matrix[3], [0, 0, 0, 1])
+
+    def test_none_and_4x4_passthrough(self):
+        np.testing.assert_allclose(
+            urdf_utils.configure_origin(None), np.eye(4))
+        m = np.eye(4)
+        m[:3, 3] = [4.0, 5.0, 6.0]
+        np.testing.assert_allclose(urdf_utils.configure_origin(m), m)
+
+    def test_invalid_shape_raises(self):
+        with pytest.raises(ValueError):
+            urdf_utils.configure_origin(np.zeros(5))
+        with pytest.raises(TypeError):
+            urdf_utils.configure_origin('not-a-matrix')