22 lines
772 B
Python
22 lines
772 B
Python
import xml.etree.ElementTree as ET
|
|
|
|
import numpy as np
|
|
import pytest
|
|
import trimesh
|
|
|
|
from iiwa_genesis.config import DESCRIPTION
|
|
|
|
|
|
@pytest.mark.parametrize("name", ["base_link"] + [f"link{i}" for i in range(1, 8)])
|
|
def test_visual_matches_collision_in_metres(name):
|
|
path = DESCRIPTION / "urdf/iiwa_with_gripper.urdf"
|
|
link = ET.parse(path).find(f"link[@name='{name}']")
|
|
sizes = []
|
|
for tag in ("visual", "collision"):
|
|
node = link.find(f"{tag}/geometry/mesh")
|
|
mesh = trimesh.load(path.parent / node.get("filename"), force="scene")
|
|
scale = np.fromstring(node.get("scale", "1 1 1"), sep=" ")
|
|
sizes.append(mesh.extents * scale)
|
|
np.testing.assert_allclose(sizes[0], sizes[1], rtol=.05, atol=.003)
|
|
assert max(sizes[0]) < .6
|