Debian Codegraph @

python/tests/unit/collision/test_collision.py

raw
1 import platform
2 import pytest
3 import dartpy as dart
4 import numpy as np
5
6
7 def collision_groups_tester(cd):
8 size = [1, 1, 1]
9 pos1 = [0, 0, 0]
10 pos2 = [0.5, 0, 0]
11
12 simple_frame1 = dart.dynamics.SimpleFrame()
13 simple_frame2 = dart.dynamics.SimpleFrame()
14
15 sphere1 = dart.dynamics.SphereShape(1)
16 sphere2 = dart.dynamics.SphereShape(1)
17
18 simple_frame1.setShape(sphere1)
19 simple_frame2.setShape(sphere2)
20
21 group = cd.createCollisionGroup()
22 group.addShapeFrame(simple_frame1)
23 group.addShapeFrame(simple_frame2)
24 assert group.getNumShapeFrames() is 2
25
26 #
27 # ( s1,s2 ) collision!
28 # ---+---|---+---+---+---+--->
29 # -1 0 +1 +2 +3 +4
30 #
31 assert group.collide()
32
33 #
34 # ( s1 ) ( s2 ) no collision
35 # ---+---|---+---+---+---+--->
36 # -1 0 +1 +2 +3 +4
37 #
38 simple_frame2.setTranslation([3, 0, 0])
39 assert not group.collide()
40
41 option = dart.collision.CollisionOption()
42 result = dart.collision.CollisionResult()
43
44 group.collide(option, result)
45 assert not result.isCollision()
46 assert result.getNumContacts() is 0
47
48 option.enableContact = True
49 simple_frame2.setTranslation([1.99, 0, 0])
50
51 group.collide(option, result)
52 assert result.isCollision()
53 assert result.getNumContacts() is not 0
54
55 # Repeat the same test with BodyNodes instead of SimpleFrames
56
57 group.removeAllShapeFrames()
58 assert group.getNumShapeFrames() is 0
59
60 skel1 = dart.dynamics.Skeleton()
61 skel2 = dart.dynamics.Skeleton()
62
63 [joint1, body1] = skel1.createFreeJointAndBodyNodePair(None)
64 [joint2, body2] = skel2.createFreeJointAndBodyNodePair(None)
65
66 shape_node1 = body1.createShapeNode(sphere1)
67 shape_node1.createVisualAspect()
68 shape_node1.createCollisionAspect()
69
70 shape_node2 = body2.createShapeNode(sphere2)
71 shape_node2.createVisualAspect()
72 shape_node2.createCollisionAspect()
73
74 group.addShapeFramesOf(body1)
75 group.addShapeFramesOf(body2)
76
77 assert group.getNumShapeFrames() is 2
78
79 assert group.collide()
80
81 joint2.setPosition(3, 3)
82 assert not group.collide()
83
84 # Repeat the same test with BodyNodes and two groups
85
86 joint2.setPosition(3, 0)
87
88 group.removeAllShapeFrames()
89 assert group.getNumShapeFrames() is 0
90 group2 = cd.createCollisionGroup()
91
92 group.addShapeFramesOf(body1)
93 group2.addShapeFramesOf(body2)
94
95 assert group.getNumShapeFrames() is 1
96 assert group2.getNumShapeFrames() is 1
97
98 assert group.collide(group2)
99
100 joint2.setPosition(3, 3)
101 assert not group.collide(group2)
102
103
104 def test_collision_groups():
105 cd = dart.collision.FCLCollisionDetector()
106 collision_groups_tester(cd)
107
108 cd = dart.collision.DARTCollisionDetector()
109 collision_groups_tester(cd)
110
111 if hasattr(dart.collision, "BulletCollisionDetector"):
112 cd = dart.collision.BulletCollisionDetector()
113 collision_groups_tester(cd)
114
115 if hasattr(dart.collision, "OdeCollisionDetector"):
116 cd = dart.collision.OdeCollisionDetector()
117 collision_groups_tester(cd)
118
119
120 # TODO: Add more collision detectors
121 @pytest.mark.parametrize("cd", [dart.collision.FCLCollisionDetector()])
122 def test_filter(cd):
123 # Create two bodies skeleton. The two bodies are placed at the same position
124 # with the same size shape so that they collide by default.
125 skel = dart.dynamics.Skeleton()
126
127 shape = dart.dynamics.BoxShape(np.ones(3))
128
129 _, body0 = skel.createRevoluteJointAndBodyNodePair()
130 shape_node0 = body0.createShapeNode(shape)
131 shape_node0.createVisualAspect()
132 shape_node0.createCollisionAspect()
133
134 _, body1 = skel.createRevoluteJointAndBodyNodePair(body0)
135 shape_node1 = body1.createShapeNode(shape)
136 shape_node1.createVisualAspect()
137 shape_node1.createCollisionAspect()
138
139 # Create a world and add the created skeleton
140 world = dart.simulation.World()
141 world.addSkeleton(skel)
142
143 # Set a new collision detector
144 constraint_solver = world.getConstraintSolver()
145 constraint_solver.setCollisionDetector(cd)
146
147 # Get the collision group from the constraint solver
148 group = constraint_solver.getCollisionGroup()
149 assert group.getNumShapeFrames() == 2
150
151 # Create BodyNodeCollisionFilter
152 option = constraint_solver.getCollisionOption()
153 body_node_filter = dart.collision.BodyNodeCollisionFilter()
154 option.collisionFilter = body_node_filter
155
156 skel.enableSelfCollisionCheck()
157 skel.enableAdjacentBodyCheck()
158 assert skel.isEnabledSelfCollisionCheck()
159 assert skel.isEnabledAdjacentBodyCheck()
160 assert group.collide()
161 assert group.collide(option)
162
163 skel.enableSelfCollisionCheck()
164 skel.disableAdjacentBodyCheck()
165 assert skel.isEnabledSelfCollisionCheck()
166 assert not skel.isEnabledAdjacentBodyCheck()
167 assert group.collide()
168 assert not group.collide(option)
169
170 skel.disableSelfCollisionCheck()
171 skel.enableAdjacentBodyCheck()
172 assert not skel.isEnabledSelfCollisionCheck()
173 assert skel.isEnabledAdjacentBodyCheck()
174 assert group.collide()
175 assert not group.collide(option)
176
177 skel.disableSelfCollisionCheck()
178 skel.disableAdjacentBodyCheck()
179 assert not skel.isEnabledSelfCollisionCheck()
180 assert not skel.isEnabledAdjacentBodyCheck()
181 assert group.collide()
182 assert not group.collide(option)
183
184 # Test collision body filtering
185 skel.enableSelfCollisionCheck()
186 skel.enableAdjacentBodyCheck()
187 body_node_filter.addBodyNodePairToBlackList(body0, body1)
188 assert not group.collide(option)
189 body_node_filter.removeBodyNodePairFromBlackList(body0, body1)
190 assert group.collide(option)
191 body_node_filter.addBodyNodePairToBlackList(body0, body1)
192 assert not group.collide(option)
193 body_node_filter.removeAllBodyNodePairsFromBlackList()
194 assert group.collide(option)
195
196
197 def test_raycast():
198 cd = dart.collision.BulletCollisionDetector()
199
200 simple_frame = dart.dynamics.SimpleFrame()
201 sphere = dart.dynamics.SphereShape(1)
202 simple_frame.setShape(sphere)
203
204 group = cd.createCollisionGroup()
205 group.addShapeFrame(simple_frame)
206 assert group.getNumShapeFrames() == 1
207
208 option = dart.collision.RaycastOption()
209 option.mEnableAllHits = False
210
211 result = dart.collision.RaycastResult()
212 assert not result.hasHit()
213
214 ray_hit = dart.collision.RayHit()
215
216 result.clear()
217 simple_frame.setTranslation(np.zeros(3))
218 assert group.raycast([-2, 0, 0], [2, 0, 0], option, result)
219 assert result.hasHit()
220 assert len(result.mRayHits) == 1
221 ray_hit = result.mRayHits[0]
222 assert np.isclose(ray_hit.mPoint, [-1, 0, 0]).all()
223 assert np.isclose(ray_hit.mNormal, [-1, 0, 0]).all()
224 assert ray_hit.mFraction == pytest.approx(0.25)
225
226 result.clear()
227 simple_frame.setTranslation(np.zeros(3))
228 assert group.raycast([2, 0, 0], [-2, 0, 0], option, result)
229 assert result.hasHit()
230 assert len(result.mRayHits) == 1
231 ray_hit = result.mRayHits[0]
232 assert np.isclose(ray_hit.mPoint, [1, 0, 0]).all()
233 assert np.isclose(ray_hit.mNormal, [1, 0, 0]).all()
234 assert ray_hit.mFraction == pytest.approx(0.25)
235
236 result.clear()
237 simple_frame.setTranslation([1, 0, 0])
238 assert group.raycast([-2, 0, 0], [2, 0, 0], option, result)
239 assert result.hasHit()
240 assert len(result.mRayHits) == 1
241 ray_hit = result.mRayHits[0]
242 assert np.isclose(ray_hit.mPoint, [0, 0, 0]).all()
243 assert np.isclose(ray_hit.mNormal, [-1, 0, 0]).all()
244 assert ray_hit.mFraction == pytest.approx(0.5)
245
246
247 if __name__ == "__main__":
248 pytest.main()