Skip to content

Commit b2bd50e

Browse files
mjd3mjd3
authored
Enable double precision libccd builds for windows/linux (#52)
* add debug failure case * fix: use double precision ccd, add tests * bump version for new wheel
1 parent 8ffedeb commit b2bd50e

File tree

4 files changed

+210
-4
lines changed

4 files changed

+210
-4
lines changed

‎build_dependencies/install_linux.sh‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ tar -zxf eigen-3.3.9.tar.gz
1010
rm -rf libccd
1111
git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd.git
1212

13-
rm -rf octomap
13+
rm -rf octomap
1414
git clone --depth 1 --branch v1.9.8 https://github.com/OctoMap/octomap.git
1515

1616
rm -rf fcl
@@ -22,13 +22,13 @@ cmake --install build
2222

2323
# Build and install libccd
2424
cd libccd
25-
cmake .
25+
cmake . -D ENABLE_DOUBLE_PRECISION=ON
2626
make -j4
2727
make install
2828
cd ..
2929

3030
# Build and install octomap
31-
cd octomap
31+
cd octomap
3232
cmake . -D CMAKE_BUILD_TYPE=Release -D BUILD_OCTOVIS_SUBPROJECT=OFF -D BUILD_DYNAMICETD3D_SUBPROJECT=OFF
3333
make -j4
3434
make install

‎build_dependencies/install_windows.ps1‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ cmake -B build `
5252
-D CMAKE_BUILD_TYPE=Release `
5353
-G $generator `
5454
-D BUILD_SHARED_LIBS=ON `
55+
-D ENABLE_DOUBLE_PRECISION=ON `
5556
-D CMAKE_INSTALL_PREFIX=$install_dir
5657
cmake --build build --config Release --target install
5758

‎src/fcl/version.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
__version__ = "0.7.0"
1+
__version__ = "0.7.0.1"

‎tests/test_precision.py‎

Lines changed: 205 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,205 @@
1+
import unittest
2+
3+
import numpy as np
4+
5+
import fcl
6+
7+
8+
# These test cases were added because there was a very sneaky crash that would occur
9+
# in the signed distance functions if all libs were not compiled with double precision
10+
class TestPrecision(unittest.TestCase):
11+
def setUp(self):
12+
self.act_dist = -0.00735518
13+
14+
# Set up first mesh
15+
f1 = np.array(
16+
[
17+
[0, 1, 2],
18+
[0, 2, 3],
19+
[3, 4, 5],
20+
[3, 5, 0],
21+
[2, 6, 4],
22+
[2, 4, 3],
23+
[1, 7, 6],
24+
[1, 6, 2],
25+
[7, 5, 4],
26+
[7, 4, 6],
27+
[0, 5, 7],
28+
[0, 7, 1],
29+
]
30+
)
31+
tris1 = np.concatenate(
32+
(3 * np.ones((len(f1), 1), dtype=np.int64), f1), axis=1
33+
).flatten()
34+
v1 = np.array(
35+
[
36+
[-1.5596524477005005, -1.4861732721328735, 0.0],
37+
[-1.55965256690979, -1.3408161401748657, 0.0],
38+
[0.8114118576049805, -1.3408160209655762, 0.0],
39+
[0.8114122152328491, -1.4861732721328735, 0.0],
40+
[0.8114122152328491, -1.4861732721328735, 2.566922187805176],
41+
[-1.559652328491211, -1.4861699342727661, 2.566922187805176],
42+
[0.8114122152328491, -1.3408160209655762, 2.566922187805176],
43+
[-1.5596498250961304, -1.3408160209655762, 2.566922187805176],
44+
]
45+
)
46+
t1 = np.zeros(3)
47+
r1 = np.eye(3)
48+
49+
# Set up second mesh
50+
f2 = np.array(
51+
[
52+
[0, 1, 2],
53+
[0, 2, 3],
54+
[3, 2, 4],
55+
[3, 4, 5],
56+
[5, 4, 6],
57+
[5, 6, 7],
58+
[7, 6, 1],
59+
[7, 1, 0],
60+
[3, 5, 7],
61+
[3, 7, 0],
62+
[4, 2, 1],
63+
[4, 1, 6],
64+
]
65+
)
66+
tris2 = np.concatenate(
67+
(3 * np.ones((len(f2), 1), dtype=np.int64), f2), axis=1
68+
).flatten()
69+
v2 = np.array(
70+
[
71+
[-0.035486817359924316, -0.15411892533302307, 0.08323988318443298],
72+
[-0.035486817359924316, -0.034327179193496704, 0.13931824266910553],
73+
[-0.035486817359924316, -0.004236131906509399, 0.07503926753997803],
74+
[-0.035486817359924316, -0.12402787804603577, 0.018960915505886078],
75+
[0.035486817359924316, -0.004236131906509399, 0.07503926753997803],
76+
[0.035486817359924316, -0.12402787804603577, 0.018960915505886078],
77+
[0.035486817359924316, -0.034327179193496704, 0.13931824266910553],
78+
[0.035486817359924316, -0.15411892533302307, 0.08323988318443298],
79+
]
80+
)
81+
t2 = np.array([-0.17816561357553456, -1.1899346069644454, 1.1061233975645952])
82+
r2 = np.array(
83+
[
84+
[-0.7731251991739131, 0.3586043541062879, -0.5231446679631834],
85+
[0.4540350514442685, 0.8888443213778688, -0.06170854409493361],
86+
[0.4428652147801832, -0.28523444667558645, -0.8500068893646534],
87+
]
88+
)
89+
90+
# Wrap meshes in FCL Convex objects
91+
c1 = fcl.Convex(v1, len(f1), tris1)
92+
c2 = fcl.Convex(v2, len(f2), tris2)
93+
94+
# Wrap in CollisionObjects
95+
self.o1 = fcl.CollisionObject(c1, fcl.Transform(r1, t1))
96+
self.o2 = fcl.CollisionObject(c2, fcl.Transform(r2, t2))
97+
98+
# Create managers
99+
self.mgr1 = fcl.DynamicAABBTreeCollisionManager()
100+
self.mgr2 = fcl.DynamicAABBTreeCollisionManager()
101+
102+
self.mgr1.registerObjects([self.o1])
103+
self.mgr2.registerObjects([self.o2])
104+
105+
self.mgr1.setup()
106+
self.mgr2.setup()
107+
108+
def test_obj_obj_coll(self):
109+
request = fcl.CollisionRequest()
110+
result = fcl.CollisionResult()
111+
ret = fcl.collide(self.o1, self.o2, request, result)
112+
assert ret == 1 # Objects are in collision
113+
114+
request = fcl.CollisionRequest()
115+
result = fcl.CollisionResult()
116+
ret = fcl.collide(self.o2, self.o1, request, result)
117+
assert ret == 1 # Objects are in collision
118+
119+
def test_obj_obj_simple_distance(self):
120+
request = fcl.DistanceRequest()
121+
result = fcl.DistanceResult()
122+
ret = fcl.distance(self.o1, self.o2, request, result)
123+
assert ret == -1
124+
125+
request = fcl.DistanceRequest()
126+
result = fcl.DistanceResult()
127+
ret = fcl.distance(self.o2, self.o1, request, result)
128+
assert ret == -1
129+
130+
def test_obj_obj_signed_distance(self):
131+
132+
request = fcl.DistanceRequest(
133+
enable_nearest_points=True, enable_signed_distance=True
134+
)
135+
result = fcl.DistanceResult()
136+
ret = fcl.distance(self.o1, self.o2, request, result)
137+
assert np.isclose(ret, self.act_dist)
138+
assert np.isclose(np.linalg.norm(np.subtract(*result.nearest_points)), abs(ret))
139+
140+
request = fcl.DistanceRequest(
141+
enable_nearest_points=True, enable_signed_distance=True
142+
)
143+
result = fcl.DistanceResult()
144+
ret = fcl.distance(self.o2, self.o1, request, result)
145+
assert np.isclose(ret, self.act_dist)
146+
assert np.isclose(np.linalg.norm(np.subtract(*result.nearest_points)), abs(ret))
147+
148+
def test_mgr_obj_coll(self):
149+
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
150+
rdata = fcl.CollisionData(request=req)
151+
self.mgr1.collide(self.o2, rdata, fcl.defaultCollisionCallback)
152+
assert rdata.result.is_collision
153+
154+
rdata = fcl.CollisionData(request=req)
155+
self.mgr2.collide(self.o1, rdata, fcl.defaultCollisionCallback)
156+
assert rdata.result.is_collision
157+
158+
def test_mgr_mgr_coll(self):
159+
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
160+
rdata = fcl.CollisionData(request=req)
161+
self.mgr1.collide(self.mgr2, rdata, fcl.defaultCollisionCallback)
162+
assert rdata.result.is_collision
163+
164+
rdata = fcl.CollisionData(request=req)
165+
self.mgr2.collide(self.mgr1, rdata, fcl.defaultCollisionCallback)
166+
assert rdata.result.is_collision
167+
168+
def test_mgr_obj_signed_distance(self):
169+
req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True)
170+
ddata = fcl.DistanceData(req)
171+
self.mgr1.distance(self.o2, ddata, fcl.defaultDistanceCallback)
172+
assert np.isclose(ddata.result.min_distance, self.act_dist)
173+
assert np.isclose(
174+
np.linalg.norm(np.subtract(*ddata.result.nearest_points)),
175+
abs(ddata.result.min_distance),
176+
)
177+
178+
req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True)
179+
ddata = fcl.DistanceData(req)
180+
self.mgr2.distance(self.o1, ddata, fcl.defaultDistanceCallback)
181+
assert np.isclose(ddata.result.min_distance, self.act_dist)
182+
assert np.isclose(
183+
np.linalg.norm(np.subtract(*ddata.result.nearest_points)),
184+
abs(ddata.result.min_distance),
185+
)
186+
187+
def test_mgr_mgr_signed_distance(self):
188+
189+
req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True)
190+
ddata = fcl.DistanceData(req)
191+
self.mgr1.distance(self.mgr2, ddata, fcl.defaultDistanceCallback)
192+
assert np.isclose(ddata.result.min_distance, self.act_dist)
193+
assert np.isclose(
194+
np.linalg.norm(np.subtract(*ddata.result.nearest_points)),
195+
abs(ddata.result.min_distance),
196+
)
197+
198+
req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True)
199+
ddata = fcl.DistanceData(req)
200+
self.mgr2.distance(self.mgr1, ddata, fcl.defaultDistanceCallback)
201+
assert np.isclose(ddata.result.min_distance, self.act_dist)
202+
assert np.isclose(
203+
np.linalg.norm(np.subtract(*ddata.result.nearest_points)),
204+
abs(ddata.result.min_distance),
205+
)

0 commit comments

Comments
 (0)