Basic Coordinate System Plotting

csystem.py

import matplotlib.pyplot as plt
from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d import proj3d
from transformations import *


class Arrow3D(FancyArrowPatch):
    def __init__(self, x, y, z, **kwargs):
        FancyArrowPatch.__init__(
                self, (0, 0), (0, 0), arrowstyle='-|>', mutation_scale=5, **kwargs)
        self.vertex = (x, y, z)

    def draw(self, renderer):
        (xs3d, ys3d, zs3d) = self.vertex

        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))
        FancyArrowPatch.draw(self, renderer)


class CoordinateFrame(object):
    def __init__(self, center, rpy, t, label):
        self.label = label
        self.center = center
        self.rpy = rpy
        self.t = t

    def add_to_plot(self, ax):
        matrix = euler_matrix(*self.rpy)[:3, :3]

        target_x = [1, 0, 0]
        target_y = [0, 1, 0]
        target_z = [0, 0, 1]

        ax.add_artist(self.create_arrow(matrix, target_x, self.center, 'b'))
        ax.add_artist(self.create_arrow(matrix, target_y, self.center, 'g'))
        ax.add_artist(self.create_arrow(matrix, target_z, self.center, 'r'))
        ax.text(*self.center, self.label)

    def create_arrow(self, M, target, center, color):
        result = M.dot(target)
        axes = []
        for (i, pose) in enumerate(center):
            axes.append([pose, result[i]])

        return Arrow3D(*axes, color=color)



fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')

ax.set_xlim(-5, +5)
ax.set_ylim(-5, +5)
ax.set_zlim(-5, +5)

CoordinateFrame([0, 0, 0], [0, 45, 90], [0, 0, 0], 'Sensor').add_to_plot(ax)

plt.draw()
plt.show()