add check for SE3 size

jhavl · jhavl · commit 1ca9d9c5bac9 · 2023-04-30T10:54:29.000+10:00
diff --git a/roboticstoolbox/robot/IK.py b/roboticstoolbox/robot/IK.py
@@ -241,6 +241,9 @@ def solve(
         if isinstance(Tep, SE3):
             Tep: np.ndarray = Tep.A
 
+        if Tep.shape != (4, 4):
+            raise ValueError("Tep must be a 4x4 SE3 matrix")
+
         # Iteration count
         i = 0
         total_i = 0
