Newer
Older
1
2
3
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
51
52
53
54
# Load "env.obj" and "rob.obj" in gepetto-gui
import numpy as np
import csv
import os
from gepetto.corbaserver import Client
pos = list ()
with open ("/home/florent/devel/hpp/src/hpp-fcl/build-rel/test/rob.octree", "r") as f:
r = csv.reader (f, delimiter = ',')
for line in r:
pos.append (map (float, line))
path = None
devel_hpp_dir = os.getenv ('DEVEL_HPP_DIR')
if devel_hpp_dir:
path = devel_hpp_dir + '/src/hpp-fcl/test/fcl_resources'
else:
path = os.getenv ('PWD') + '/fcl_resources'
Red = [1, 0, 0, .5]
Green = [0, 1, 0, .5]
Blue = [0, 0, 1, .5]
c = Client ()
wid = 0
sceneName = 'scene'
wid = c.gui.createWindow ('test-fcl')
c.gui.createScene (sceneName)
c.gui.addSceneToWindow (sceneName, wid)
c.gui.addMesh ("env", path + "/env.obj")
c.gui.addMesh ("rob", path + "/rob.obj")
c.gui.addToGroup ("env", sceneName)
c.gui.addToGroup ("rob", sceneName)
# closest points
c.gui.addSphere ("p1", 10, Red)
c.gui.addToGroup ("p1", sceneName)
q1=(-1373.283643275499, -396.2224237620831, 259.5808934420347)+(0.5956794918568784, -0.2147188057951074, 0.257436335996247, 0.7299234962157893)
q2=(0,0,0,0,0,0,1)
c.gui.applyConfiguration ('rob', q1)
c.gui.applyConfiguration ('env', q2)
p1 = (-1990.2983245164919,-105.42114741459312,359.74684390031132,0,0,0,1)
c.gui.applyConfiguration ("p1", p1)
c.gui.refresh ()