diff --git a/test/test_fcl_collision.py b/test/test_fcl_collision.py new file mode 100644 index 0000000000000000000000000000000000000000..b53a0d551a51cc555da52e0da57ae89aa2bed592 --- /dev/null +++ b/test/test_fcl_collision.py @@ -0,0 +1,39 @@ +# Load "env.obj" and "rob.obj" in gepetto-gui + +import numpy as np +import os +from gepetto.corbaserver import Client + +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) + +q2 = (0,0,0,0,0,0,1) +q1=(-1435.35587657243, 2891.398094594479, 1462.830701842904)+( 0.6741912139367736, -0.2384437590607974, 0.6418622372743962, -0.2768097707389008) + +c.gui.applyConfiguration ('env', q1) +c.gui.applyConfiguration ('rob', q2) + +c.gui.refresh () + diff --git a/test/test_fcl_distance_lower_bound.py b/test/test_fcl_distance_lower_bound.py new file mode 100644 index 0000000000000000000000000000000000000000..4eee3c2b61f8a36cb0561263910c424e8479ba9d --- /dev/null +++ b/test/test_fcl_distance_lower_bound.py @@ -0,0 +1,39 @@ +# Load "env.obj" and "rob.obj" in gepetto-gui + +import numpy as np +import os +from gepetto.corbaserver import Client + +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.addToGroup ("env", sceneName) +c.gui.addBox ("box", 500, 200, 150, Blue) +c.gui.addToGroup ("box", sceneName) + +q2 = (0,0,0,0,0,0,1) +q1=(608.56046341359615, 1624.1152798756957, 2661.5910432301462, 0.8083991299501978, 0.25803832576728564, 0.47026407332553366, 0.24240208429437343) + +c.gui.applyConfiguration ('env', q1) +c.gui.applyConfiguration ('rob', q2) + +c.gui.refresh () + diff --git a/test/test_fcl_gjk.py b/test/test_fcl_gjk.py new file mode 100644 index 0000000000000000000000000000000000000000..a8e96435c7d9c5f5d4b926c3f58c9f6e9984cd14 --- /dev/null +++ b/test/test_fcl_gjk.py @@ -0,0 +1,55 @@ +# This script displays two triangles and two spheres in gepetto-gui +# It is useful to debug distance computation between two triangles. +import numpy as np +from gepetto.corbaserver import Client + +Red = [1, 0, 0, .5] +Green = [0, 1, 0, .5] +Blue = [0, 0, 1, .5] + +c = Client () +wid = len (c.gui.getWindowList ()) - 1 + +sceneName = 'scene/triangles' +if sceneName in c.gui.getNodeList (): + c.gui.deleteNode (sceneName, True) + +wid = c.gui.createWindow ('triangles') + +c.gui.createScene (sceneName) +c.gui.addSceneToWindow (sceneName, wid) + +P1 = (-0.6475786872429674, -0.519875255189778, 0.5955961037406681) +P2 = (0.4653088233737781, 0.313127305970121, 0.934810277044219) +P3 = (0.2789166910941325, 0.5194696837661181, -0.8130390456938367) +Q1 = (-0.7301951766620367, 0.04042013969291935, -0.8435357165725602) +Q2 = (-0.8601872044895716, -0.5906898274974384, -0.07715905321629679) +Q3 = (0.6393545603562867, 0.1466372567911807, 0.5111616707924576) +p1 = (0.2238377827027012, 0.2163758468474661, 0.4505435657492071) +p2 = (0.2238377827027012, 0.2163758468474662, 0.450543565749207) +tf2 = (-0.1588705771755817, 0.1959319373363798, 0.04007284069698749) + (0, 0, 0, 1) +normal = (-0.6220181878092853, 0.7671227156255918, 0.1568952300284174) + +c.gui.addTriangleFace ("triangle1", P1, P2, P3, Red) +c.gui.addTriangleFace ("triangle2", Q1, Q2, Q3, Green) +c.gui.addToGroup ('triangle1', sceneName) +c.gui.addToGroup ('triangle2', sceneName) + +c.gui.addSphere ('closest point 1', .02, Red) +c.gui.addSphere ('closest point 2', .02, Green) +c.gui.addToGroup ('closest point 1', sceneName) +c.gui.addToGroup ('closest point 2', sceneName) + +c.gui.addLine ('normal', P1, tuple (np.array (P1) + np.array (normal)), Red) +c.gui.addToGroup ('normal', sceneName) + +q1 = p1 + (1,0,0,0) +q2 = p2 + (1,0,0,0) +tf1 = (0, 0, 0, 0, 0, 0, 1) + +c.gui.applyConfiguration ('closest point 1', q1) +c.gui.applyConfiguration ('closest point 2', q2) +c.gui.applyConfiguration ('triangle1', tf1) +c.gui.applyConfiguration ('triangle2', tf2) +c.gui.refresh () +