display_tools.py 1.74 KB
Newer Older
1
2
import time

3

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
def createSphere(name, r, size=0.01, color=[0, 0, 0, 1]):
    r.client.gui.addSphere(name, size, color)
    r.client.gui.addToGroup(name, r.sceneName)
    r.client.gui.setVisibility(name, 'ALWAYS_ON_TOP')
    r.client.gui.refresh()


def moveSphere(name, r, pos):
    if len(pos) == 3:
        q = pos + [1, 0, 0, 0]
    elif len(pos) == 7:
        q = pos
    else:
        raise ValueError("pos should be of size 3 or 7 ")
    r.client.gui.applyConfiguration(name, q)
    r.client.gui.refresh()


def displayContactSequence(r, configs, pause=1.):
    for i in range(0, len(configs)):
        r(configs[i])
        time.sleep(pause)


def moveObject(viewer, pos, rotation=[1, 0, 0, 0]):
    viewer.client.gui.applyConfiguration(name, pos + rotation)  # noqa TODO
    viewer.client.gui.refresh()


def addVector(viewer, rbprmBuilder, color, v, name=None):
    gui = viewer.client.gui
    if name is None:
        i = 0
        name = 'vector_' + str(i)
        while name in gui.getNodeList():
            i = i + 1
            name = 'sphere_' + str(i)
    quat = rbprmBuilder.quaternionFromVector(v[3:6])
    v[3:7] = quat[::]
    gui.addArrow(name, 0.02, 1, color)
    gui.addToGroup(name, viewer.sceneName)
    gui.setVisibility(name, "ON")
    gui.applyConfiguration(name, v)
    gui.refresh()


def displaySurfaceFromPoints(viewer, p_list, color=[0, 0, 0, 1], name=None):
51
52
    if len(p_list) < 2:
        return
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
53
    gui = viewer.client.gui
54
55
    node_list = gui.getNodeList()
    if name is None and node_list is not None:
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
56
57
        i = 0
        name = 'surface_' + str(i)
58
        while name in node_list:
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
59
60
61
62
63
            i = i + 1
            name = 'surface_' + str(i)
    gui.addCurve(name, p_list, color)
    gui.addToGroup(name, viewer.sceneName)
    gui.refresh()