Issues #101: SpatialInertia.__add__ method fix and test (#109)

Author: jcao-bdai

spatialmath/spatialvector.py

@@ -543,7 +543,7 @@ def __init__(self, m=None, r=None, I=None):
         :param I: inertia about the centre of mass, axes aligned with link frame
         :type I: numpy.array, shape=(6,6)
 
-        - ``SpatialInertia(m, r I)`` is a spatial inertia object for a rigid-body
+        - ``SpatialInertia(m, r, I)`` is a spatial inertia object for a rigid-body
           with mass ``m``, centre of mass at ``r`` relative to the link frame, and an
           inertia matrix ``I`` (3x3) about the centre of mass.
 
@@ -588,8 +588,9 @@ def isvalid(self, x, check):
         :return: True if the matrix has shape (6,6).
         :rtype: bool
         """
-        return self.shape == SpatialVector.shape
+        return self.shape == x.shape
 
+    @property
     def shape(self):
         """
         Shape of the object's interal matrix representation
@@ -603,7 +604,6 @@ def __getitem__(self, i):
         return SpatialInertia(self.data[i])
 
     def __repr__(self):
-
         """
         Convert to string
 
@@ -634,7 +634,7 @@ def __add__(
         """
         if not isinstance(right, SpatialInertia):
             raise TypeError("can only add spatial inertia to spatial inertia")
-        return SpatialInertia(left.I + left.I)
+        return SpatialInertia(left.A + right.A)
 
     def __mul__(
         left, right
@@ -682,7 +682,6 @@ def __rmul__(
 
 
 if __name__ == "__main__":
-
     import numpy.testing as nt
     import pathlib
 

tests/test_spatialvector.py

@@ -1,4 +1,3 @@
-
 import unittest
 import numpy.testing as nt
 import numpy as np
@@ -55,8 +54,8 @@ def test_velocity(self):
 
         s = str(a)
         self.assertIsInstance(s, str)
-        self.assertEqual(s.count('\n'), 0)
-        self.assertTrue(s.startswith('SpatialVelocity'))
+        self.assertEqual(s.count("\n"), 0)
+        self.assertTrue(s.startswith("SpatialVelocity"))
 
         r = np.random.rand(6, 10)
         a = SpatialVelocity(r)
@@ -70,11 +69,11 @@ def test_velocity(self):
         self.assertIsInstance(b, SpatialVector)
         self.assertIsInstance(b, SpatialM6)
         self.assertEqual(len(b), 1)
-        self.assertTrue(all(b.A == r[:,3]))
+        self.assertTrue(all(b.A == r[:, 3]))
 
         s = str(a)
         self.assertIsInstance(s, str)
-        self.assertEqual(s.count('\n'), 9)
+        self.assertEqual(s.count("\n"), 9)
 
     def test_acceleration(self):
         a = SpatialAcceleration([1, 2, 3, 4, 5, 6])
@@ -93,8 +92,8 @@ def test_acceleration(self):
 
         s = str(a)
         self.assertIsInstance(s, str)
-        self.assertEqual(s.count('\n'), 0)
-        self.assertTrue(s.startswith('SpatialAcceleration'))
+        self.assertEqual(s.count("\n"), 0)
+        self.assertTrue(s.startswith("SpatialAcceleration"))
 
         r = np.random.rand(6, 10)
         a = SpatialAcceleration(r)
@@ -108,14 +107,12 @@ def test_acceleration(self):
         self.assertIsInstance(b, SpatialVector)
         self.assertIsInstance(b, SpatialM6)
         self.assertEqual(len(b), 1)
-        self.assertTrue(all(b.A == r[:,3]))
+        self.assertTrue(all(b.A == r[:, 3]))
 
         s = str(a)
         self.assertIsInstance(s, str)
 
-
     def test_force(self):
-
         a = SpatialForce([1, 2, 3, 4, 5, 6])
         self.assertIsInstance(a, SpatialForce)
         self.assertIsInstance(a, SpatialVector)
@@ -132,8 +129,8 @@ def test_force(self):
 
         s = str(a)
         self.assertIsInstance(s, str)
-        self.assertEqual(s.count('\n'), 0)
-        self.assertTrue(s.startswith('SpatialForce'))
+        self.assertEqual(s.count("\n"), 0)
+        self.assertTrue(s.startswith("SpatialForce"))
 
         r = np.random.rand(6, 10)
         a = SpatialForce(r)
@@ -153,7 +150,6 @@ def test_force(self):
         self.assertIsInstance(s, str)
 
     def test_momentum(self):
-
         a = SpatialMomentum([1, 2, 3, 4, 5, 6])
         self.assertIsInstance(a, SpatialMomentum)
         self.assertIsInstance(a, SpatialVector)
@@ -170,8 +166,8 @@ def test_momentum(self):
 
         s = str(a)
         self.assertIsInstance(s, str)
-        self.assertEqual(s.count('\n'), 0)
-        self.assertTrue(s.startswith('SpatialMomentum'))
+        self.assertEqual(s.count("\n"), 0)
+        self.assertTrue(s.startswith("SpatialMomentum"))
 
         r = np.random.rand(6, 10)
         a = SpatialMomentum(r)
@@ -190,9 +186,7 @@ def test_momentum(self):
         s = str(a)
         self.assertIsInstance(s, str)
 
-
     def test_arith(self):
-
         # just test SpatialVelocity since all types derive from same superclass
 
         r1 = np.r_[1, 2, 3, 4, 5, 6]
@@ -206,8 +200,26 @@ def test_arith(self):
 
     def test_inertia(self):
         # constructor
+        i0 = SpatialInertia()
+        nt.assert_equal(i0.A, np.zeros((6, 6)))
+
+        i1 = SpatialInertia(np.eye(6, 6))
+        nt.assert_equal(i1.A, np.eye(6, 6))
+
+        i2 = SpatialInertia(m=1, r=(1, 2, 3))
+        nt.assert_almost_equal(i2.A, i2.A.T)
+
+        i3 = SpatialInertia(m=1, r=(1, 2, 3), I=np.ones((3, 3)))
+        nt.assert_almost_equal(i3.A, i3.A.T)
+
         # addition
-        pass
+        m_a, m_b = 1.1, 2.2
+        r = (1, 2, 3)
+        i4a, i4b = SpatialInertia(m=m_a, r=r), SpatialInertia(m=m_b, r=r)
+        nt.assert_almost_equal((i4a + i4b).A, SpatialInertia(m=m_a + m_b, r=r).A)
+
+        # isvalid - note this method is very barebone, to be improved
+        self.assertTrue(SpatialInertia().isvalid(np.ones((6, 6)), check=False))
 
     def test_products(self):
         # v x v = a  *, v x F6 = a
@@ -218,6 +230,5 @@ def test_products(self):
 
 
 # ---------------------------------------------------------------------------------------#
-if __name__ == '__main__':
-
-    unittest.main()
+if __name__ == "__main__":
+    unittest.main()