Merge branch 'future' of github.com:petercorke/robotics-toolbox-python into future

petercorke · petercorke · commit 1c4bdf364fca · 2023-04-30T12:46:13.000+10:00
diff --git a/roboticstoolbox/backends/PyPlot/EllipsePlot.py b/roboticstoolbox/backends/PyPlot/EllipsePlot.py
@@ -12,7 +12,6 @@
 
 class ShapePlot:
     def __init__(self, shape, wireframe=True, **kwargs):
-
         self.shape = shape  # reference to the spatialgeom shape
         self.wireframe = wireframe
         self.args = kwargs
@@ -54,7 +53,6 @@ def make(self):
 
 class EllipsePlot:
     def __init__(self, robot, q, etype, opt="trans", centre=[0, 0, 0], scale=1.0):
-
         super(EllipsePlot, self).__init__()
 
         try:
@@ -65,7 +63,7 @@ def __init__(self, robot, q, etype, opt="trans", centre=[0, 0, 0], scale=1.0):
         except TypeError:
             if centre != "ee":
                 raise ValueError(
-                    "Centre must be a three vector or 'ee' meaning" "end-effector"
+                    "Centre must be a three vector or 'ee' meaningend-effector"
                 )
 
         self.ell = None
@@ -91,7 +89,11 @@ def draw(self):
         self.make_ellipsoid()
 
         if self.ell is not None:
-            self.ax.collections.remove(self.ell)
+            self.ell.remove()
+            # print(type(self.ell))
+            # print(type(self.ax))
+            # assert True == False
+            # self.ax.collections.remove(self.ell)
 
         self.ell = self.ax.plot_wireframe(
             self.x, self.y, self.z, rstride=6, cstride=6, color="#2980b9", alpha=0.2
@@ -181,7 +183,7 @@ def make_ellipsoid2(self):
             A = J @ J.T
         elif self.opt == "rot":
             raise ValueError(
-                "Can not do rotational ellipse for a 2d robot plot." " Set opt='trans'"
+                "Can not do rotational ellipse for a 2d robot plot. Set opt='trans'"
             )
 
         # if not self.vell:
diff --git a/roboticstoolbox/robot/BaseRobot.py b/roboticstoolbox/robot/BaseRobot.py
@@ -2336,13 +2336,13 @@ def plot(
         env.add(self, readonly=True, **kwargs)
 
         if vellipse:
-            vell = self.vellipse(q, centre="ee")
+            vell = self.vellipse(q[0], centre="ee")
             env.add(vell)
         else:
             vell = None
 
         if fellipse:
-            fell = self.fellipse(q, centre="ee")
+            fell = self.fellipse(q[0], centre="ee")
             env.add(fell)
         else:
             fell = None
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
