2D ellipse plots
@@ -15,6 +15,9 @@ class EllipsePlot(object):
try:
centre = getvector(centre, 3)
+ except ValueError:
+ centre = getvector(centre, 2)
+ centre = np.array([centre[0], centre[1], 0])
except TypeError:
if centre != 'ee':
raise ValueError(
@@ -45,9 +48,18 @@ class EllipsePlot(object):
self.x, self.y, self.z, rstride=6,
cstride=6, color='#2980b9', alpha=0.2)
+ def draw2(self):
+ self.make_ellipsoid2()
+
+ if self.ell is not None:
+ self.ell[0].set_data(self.x, self.y)
+ else:
+ self.ell = self.ax.plot(
+ self.x, self.y, color='#2980b9', alpha=0.2)
+
def make_ellipsoid(self):
"""
- Plot the 3-d Ellipsoid ell on the Axes3D ax.
+ Plot the 3d Ellipsoid
"""
@@ -87,3 +99,45 @@ class EllipsePlot(object):
self.x = x * self.scale + centre[0]
self.y = y * self.scale + centre[1]
self.z = z * self.scale + centre[2]
+
+ def make_ellipsoid2(self):
+ """
+ Plot the 2d Ellipsoid
+
+ """
+
+ if self.opt == 'trans':
+ J = self.robot.jacobe()[:2, :]
+ A = J @ J.T
+ elif self.opt == 'rot':
+ raise ValueError(
+ "Can not do rotational ellipse for a 2d robot plot."
+ " Set opt='trans'")
+
+ if not self.vell:
+ # Do the extra step for the force ellipse
+ A = np.linalg.inv(A)
+
+ if isinstance(self.centre, str) and self.centre == 'ee':
+ centre = self.robot.fkine().t
+ else:
+ centre = self.centre
+
+ # find the rotation matrix and radii of the axes
+ U, s, rotation = np.linalg.svd(A)
+ radii = 1.0 / np.sqrt(s)
+
+ # points on unit sphere
+ u = np.linspace(0.0, 2.0 * np.pi, 50)
+ v = np.linspace(0.0, np.pi, 50)
+ x = radii[0] * np.outer(np.cos(u), np.sin(v))
+ y = radii[1] * np.outer(np.sin(u), np.sin(v))
+
+ # transform points to ellipsoid
+ for i in range(len(x)):
+ for j in range(len(x)):
+ [x[i, j], y[i, j]] = \
+ np.dot([x[i, j], y[i, j]], rotation)
+
+ self.x = x * self.scale + centre[0]
+ self.y = y * self.scale + centre[1]
