Merge pull request #87 from adamheins/master

Raise error on invalid rotation matrix in UnitQuaternion constructor.

Author: myeatman-bdai

spatialmath/quaternion.py

@@ -989,9 +989,12 @@ def __init__(
                 #  a quaternion as a 1D array
                 #  an array of quaternions as an nx4 array
 
-                if smb.isrot(s, check=check):
-                    # UnitQuaternion(R) R is 3x3 rotation matrix
-                    self.data = [smb.r2q(s)]
+                if s.shape == (3, 3):
+                    if smb.isrot(s, check=check):
+                        # UnitQuaternion(R) R is 3x3 rotation matrix
+                        self.data = [smb.r2q(s)]
+                    else:
+                        raise ValueError("invalid rotation matrix provided to UnitQuaternion constructor")
                 elif s.shape == (4,):
                     # passed a 4-vector
                     if norm:
@@ -1004,6 +1007,8 @@ def __init__(
                     else:
                         # self.data = [smb.qpositive(x) for x in s]
                         self.data = [x for x in s]
+                else:
+                    raise ValueError("array could not be interpreted as UnitQuaternion")
 
             elif isinstance(s, SO3):
                 # UnitQuaternion(x) x is SO3 or SE3 (since SE3 is subclass of SO3)

tests/test_quaternion.py

@@ -151,6 +151,19 @@ def test_constructor(self):
         q = UnitQuaternion(rotx(0.3))
         qcompare(UnitQuaternion(q), q)
 
+        # fail when invalid arrays are provided
+        # invalid rotation matrix
+        R = 1.1 * np.eye(3)
+        with self.assertRaises(ValueError):
+            UnitQuaternion(R, check=True)
+
+        # wrong shape to be anything
+        R = np.zeros((5, 5))
+        with self.assertRaises(ValueError):
+            UnitQuaternion(R, check=True)
+        with self.assertRaises(ValueError):
+            UnitQuaternion(R, check=False)
+
     def test_concat(self):
         u = UnitQuaternion()
         uu = UnitQuaternion([u, u, u, u])