octree.py

Go to the documentation of this file.

# 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, 0.5]
Green = [0, 1, 0, 0.5]
Blue = [0, 0, 1, 0.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()