11
11
12
12
13
13
model = pin .buildSampleModelHumanoid ()
14
+ geometry = pin .buildSampleGeometryModelHumanoid (model )
14
15
rdata : pin .Data = model .createData ()
15
16
SEED = 0
16
17
np .random .seed (SEED )
@@ -193,7 +194,7 @@ def test_fly_high():
193
194
194
195
195
196
def test_frame_collision ():
196
- import hppfcl
197
+ import coal
197
198
198
199
fr_name1 = "larm_shoulder2_body"
199
200
fr_id1 = model .getFrameId (fr_name1 )
@@ -210,16 +211,15 @@ def test_frame_collision():
210
211
gamma = np .random .rand ()
211
212
delta = np .random .rand ()
212
213
213
- geometry = pin .GeometryModel ()
214
214
ig_frame = geometry .addGeometryObject (
215
215
pin .GeometryObject (
216
- "frame" , fr_id1 , joint_id , hppfcl .Capsule (alpha , gamma ), frame_SE3
216
+ "frame" , fr_id1 , joint_id , coal .Capsule (alpha , gamma ), frame_SE3
217
217
)
218
218
)
219
219
220
220
ig_frame2 = geometry .addGeometryObject (
221
221
pin .GeometryObject (
222
- "frame2" , fr_id2 , joint_id2 , hppfcl .Capsule (beta , delta ), frame_SE3_bis
222
+ "frame2" , fr_id2 , joint_id2 , coal .Capsule (beta , delta ), frame_SE3_bis
223
223
)
224
224
)
225
225
geometry .addCollisionPair (pin .CollisionPair (ig_frame , ig_frame2 ))
@@ -265,47 +265,18 @@ def test_frame_collision():
265
265
assert np .allclose (fdata .Jx , fdata2 .Jx , atol = ATOL )
266
266
267
267
268
- def test_FrameCollisionResidual_no_collision_pairs ():
269
- import hppfcl
270
-
271
- fr_name1 = "larm_shoulder2_body"
272
- fr_id1 = model .getFrameId (fr_name1 )
273
- joint_id = model .frames [fr_id1 ].parentJoint
274
-
275
- fr_name2 = "rleg_elbow_body"
276
- fr_id2 = model .getFrameId (fr_name2 )
277
- joint_id2 = model .frames [fr_id2 ].parentJoint
278
-
279
- frame_SE3 = pin .SE3 .Random ()
280
- frame_SE3_bis = pin .SE3 .Random ()
281
- alpha = np .random .rand ()
282
- beta = np .random .rand ()
283
- gamma = np .random .rand ()
284
- delta = np .random .rand ()
285
-
286
- geometry = pin .GeometryModel ()
287
- ig_frame = geometry .addGeometryObject (
288
- pin .GeometryObject (
289
- "frame" , fr_id1 , joint_id , hppfcl .Capsule (alpha , gamma ), frame_SE3
290
- )
291
- )
292
-
293
- ig_frame2 = geometry .addGeometryObject (
294
- pin .GeometryObject (
295
- "frame2" , fr_id2 , joint_id2 , hppfcl .Capsule (beta , delta ), frame_SE3_bis
296
- )
297
- )
298
-
268
+ def test_frame_collision_no_collision_pairs ():
299
269
space = manifolds .MultibodyConfiguration (model )
300
270
ndx = space .ndx
301
271
x0 = space .neutral ()
302
272
d = np .random .randn (space .ndx ) * 0.1
303
273
d [6 :] = 0.0
304
274
x0 = space .integrate (x0 , d )
305
- u0 = np .zeros (nu )
306
- q0 = x0 [:nq ]
307
275
308
276
fun = aligator .FrameCollisionResidual (ndx , nu , model , geometry , 0 )
277
+ data = fun .createData ()
278
+ fun .evaluate (x0 , data )
279
+ fun .computeJacobians (x0 , data )
309
280
310
281
311
282
if __name__ == "__main__" :
0 commit comments