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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
|
import platform
import pytest
import dartpy as dart
def collision_groups_tester(cd):
size = [1, 1, 1]
pos1 = [0, 0, 0]
pos2 = [0.5, 0, 0]
simple_frame1 = dart.dynamics.SimpleFrame()
simple_frame2 = dart.dynamics.SimpleFrame()
sphere1 = dart.dynamics.SphereShape(1)
sphere2 = dart.dynamics.SphereShape(1)
simple_frame1.setShape(sphere1)
simple_frame2.setShape(sphere2)
cd = dart.collision.FCLCollisionDetector()
group = cd.createCollisionGroup()
group.addShapeFrame(simple_frame1)
group.addShapeFrame(simple_frame2)
#
# ( s1,s2 ) collision!
# ---+---|---+---+---+---+--->
# -1 0 +1 +2 +3 +4
#
assert group.collide()
#
# ( s1 ) ( s2 ) no collision
# ---+---|---+---+---+---+--->
# -1 0 +1 +2 +3 +4
#
simple_frame2.setTranslation([3, 0, 0])
assert not group.collide()
option = dart.collision.CollisionOption()
result = dart.collision.CollisionResult()
group.collide(option, result)
assert not result.isCollision()
assert result.getNumContacts() is 0
option.enableContact = True
simple_frame2.setTranslation([1.99, 0, 0])
group.collide(option, result)
assert result.isCollision()
assert result.getNumContacts() is not 0
def test_collision_groups():
cd = dart.collision.FCLCollisionDetector()
collision_groups_tester(cd)
cd = dart.collision.DARTCollisionDetector()
collision_groups_tester(cd)
if hasattr(dart.collision, "BulletCollisionDetector"):
cd = dart.collision.BulletCollisionDetector()
collision_groups_tester(cd)
if hasattr(dart.collision, "OdeCollisionDetector"):
cd = dart.collision.OdeCollisionDetector()
collision_groups_tester(cd)
if __name__ == "__main__":
pytest.main()
|
