Added trplot() which displays a rotation (quat or matrix) as a 3D

petercorke · petercorke · commit 8ca7ded1b9df · 2008-03-29T11:31:03.000Z
graphical frame.

Python 3D plotting exists and is reasonably powerful, not sure it will
do animations...
diff --git a/robotics-toolbox-python/robot/__init__.py b/robotics-toolbox-python/robot/__init__.py
@@ -22,6 +22,8 @@
 # Import transformations section
 from transform import *
 
+from trplot import *
+
 from jacobian import *
 
 # import kinematics section
diff --git a/robotics-toolbox-python/robot/trplot.py b/robotics-toolbox-python/robot/trplot.py
@@ -1,13 +1,32 @@
-function trplot(T, name, color)
-
-	if nargin == 1,
-		fmt = '%c';
-	else
-		fmt = sprintf('%%c%s', name);
-	end
-	if nargin < 3,
-		color = 'b';
-	end
-
-	q = quaternion(T);
-	plot(q, T(1:3,4), fmt, color);
+import matplotlib.axes3d as p3
+from numpy import * # for outer and arange
+import pylab as p   # for figure
+from robot.Quaternion import *
+
+def trplot(r, name=''):
+    '''
+    Plot a rotation as a set of axes aligned with the x, y and z
+    directions of a frame rotated by C{r}.
+    '''
+
+    if type(r) is matrix:
+        q = quaternion(r);
+    elif isinstance(r, quaternion):
+        q = r;
+    else:
+        raise ValueError;
+
+    x = q * mat([1,0,0]);
+    y = q * mat([0,1,0]);
+    z = q * mat([0,0,1]);
+
+    fig=p.figure()
+    ax=p3.Axes3D(fig)
+    ax.plot3d([0,x[0,0]], [0,x[0,1]], [0,x[0,2]], color='red')
+    ax.plot3d([0,y[0,0]], [0,y[0,1]], [0,y[0,2]], color='green')
+    ax.plot3d([0,z[0,0]], [0,z[0,1]], [0,z[0,2]], color='blue')
+    p.show()
+
+if __name__ == "__main__":
+    from robot.transform import *
+    trplot( rotx(0.2) );
