Making the mask compatible with robots with more than 6 joints

robopy/base/serial_link.py

@@ -142,7 +142,7 @@ class SerialLink:
         """
         assert type(T) is np.matrix and T.shape == (4, 4)
         assert mask.shape == (6,), 'The mask must have 6 elements'
-        assert mask.sum() == self.length, 'The mask must have equal amount of 1s as the degrees of freedom of the kinematic chain'
+        assert mask.sum() == min(self.length,6), 'The mask must have equal amount of 1s as the degrees of freedom of the kinematic chain'
         bounds = [(link.qlim[0], link.qlim[1]) for link in self]
 
         if q0 is None: