(self, original_position, position, original_quaternion,
quaternion, expected_quaternion, freejoint)
| 368 | freejoint=[False, True], |
| 369 | )) |
| 370 | def testShiftPose(self, original_position, position, original_quaternion, |
| 371 | quaternion, expected_quaternion, freejoint): |
| 372 | # Setup entity. |
| 373 | test_arena = arena.Arena() |
| 374 | subentity = TestEntity(name='subentity') |
| 375 | frame = test_arena.attach(subentity) |
| 376 | if freejoint: |
| 377 | frame.add('freejoint') |
| 378 | |
| 379 | physics = mjcf.Physics.from_mjcf_model(test_arena.mjcf_model) |
| 380 | |
| 381 | # Set the original position |
| 382 | subentity.set_pose( |
| 383 | physics, position=original_position, quaternion=original_quaternion) |
| 384 | |
| 385 | if position is None: |
| 386 | ground_truth_pos = original_position |
| 387 | else: |
| 388 | ground_truth_pos = original_position + np.array(position) |
| 389 | subentity.shift_pose(physics, position=position, quaternion=quaternion) |
| 390 | np.testing.assert_array_equal(physics.bind(frame).xpos, ground_truth_pos) |
| 391 | |
| 392 | updated_quat = physics.bind(frame).xquat |
| 393 | np.testing.assert_array_almost_equal(updated_quat, expected_quaternion, |
| 394 | 1e-4) |
| 395 | |
| 396 | @parameterized.parameters(False, True) |
| 397 | def testShiftPoseWithVelocity(self, rotate_velocity): |
nothing calls this directly
no test coverage detected