← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/visualiser/mayaviviz.py

  • Committer: Vikas Dhiman
  • Date: 2012-05-14 16:28:10 UTC
  • Revision ID: wecacuee@gmail.com-20120514162810-gqqevqvisr4qyi5r
Backtracing must be working fine now.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/visualiser/mayaviviz.py
1
 
from mayaviclient import RemoteMlab
2
 
import numpy as np
3
 
 
4
 
class Visualiser:
5
 
    def __init__(self, npts=100000, Rotate=False, title=''):
6
 
        self.mlab = RemoteMlab()
7
 
        z = np.zeros(1)
8
 
        self.mlab.clf()
9
 
        self.mlab.points3d(
10
 
            z,
11
 
            z,
12
 
            z,
13
 
            z,
14
 
            scale_mode='none',
15
 
            mode='cube'
16
 
        )
17
 
        self.p3d = self.mlab.getreturnvar()
18
 
        self.lasttrajectorypoint = None
19
 
        self.trajectory_point = np.eye(4)
20
 
 
21
 
    def addmappts(self, xyzs, colour=None):
22
 
        """Display xyzs"""
23
 
        self.mlab.ex("""{0}.mlab_source.x = a0""".format(self.p3d), xyzs[:, 0],
24
 
                    numpyargs=1)
25
 
        self.mlab.ex("""{0}.mlab_source.y = a0""".format(self.p3d), xyzs[:, 1],
26
 
                    numpyargs=1)
27
 
        self.mlab.ex("""{0}.mlab_source.z = a0""".format(self.p3d), xyzs[:, 2],
28
 
                    numpyargs=1)
29
 
        self.mlab.ex("""{0}.mlab_source.t = a0""".format(self.p3d), xyzs[:, 2],
30
 
                    numpyargs=1)
31
 
 
32
 
    def setrobotpose(self, transform_mat):
33
 
        """Display robot position with given pose"""
34
 
        self.trajectory_point[:3, :3] = transform_mat[:3, :3]
35
 
        rotv = np.dot(np.array([0, 0, 1]), transform_mat[:3, :3])
36
 
        if self.lasttrajectorypoint:
37
 
            self.mlab.ex("""{0}.mlab_source.u = a0""".format(
38
 
                self.lasttrajectorypoint), rotv[0])
39
 
            self.mlab.ex("""{0}.mlab_source.v = a0""".format(
40
 
                self.lasttrajectorypoint), rotv[1])
41
 
            self.mlab.ex("""{0}.mlab_source.w = a0""".format(
42
 
                self.lasttrajectorypoint), rotv[2])
43
 
 
44
 
    def addtrajectorypoint(self, xyz):
45
 
        """Display robot position at given coordinate"""
46
 
        self.trajectory_point[:3, 3] = np.array(xyz)
47
 
        u = np.zeros(1)
48
 
        v = np.zeros(1)
49
 
        w = np.ones(1)
50
 
        self.mlab.quiver3d(
51
 
            self.trajectory_point[0, 3],
52
 
            self.trajectory_point[1, 3],
53
 
            self.trajectory_point[2, 3],
54
 
            u, v, w, numpyargs=6)
55
 
        self.lasttrajectorypoint = self.mlab.getreturnvar()
56
 
 
57
 
    def getkeypress(self):
58
 
        pass
59
 
    
60
 
    def setminmax(self, colourmin, colourmax):
61
 
        pass
62
 
    
63
 
    def setautominmax(self, Bool=True):
64
 
        pass
65
 
 
66
 
    def set_orthographic(self, Bool=False):
67
 
        pass
68
 
 
69
 
    def setleftpts(self, xyzs, M=None):
70
 
        pass
71
 
 
72
 
    def setrightpts(self, xyzs, M=None):
73
 
        pass
74
 
 
75
 
    def setspinpts(self, xyzs, M=None):
76
 
        pass
77
 
 
78
 
    def clear(self):
79
 
        pass
80
 
 
81
 
    def setinfostr(self, labeltext):
82
 
        pass
83