From 806af03e28f960859c693692d3bd894188f5d5f6 Mon Sep 17 00:00:00 2001 From: Yijiang Huang Date: Tue, 18 May 2021 17:19:02 -0400 Subject: [PATCH] add tests for visual/collision_data --- .github/workflows/integration.yml | 1 + CHANGELOG.rst | 1 + pytest.ini | 9 +- requirements-dev.txt | 2 +- setup.py | 1 + .../interfaces/env_manager/shape_creation.py | 69 +++- .../interfaces/geometry/mesh.py | 4 + .../interfaces/robots/body.py | 43 ++- tests/test_body.py | 265 ++++++++++++++- .../c1/meshes/collision/L0_collision_0.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_1.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_10.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_11.stl | Bin 0 -> 884 bytes .../c1/meshes/collision/L0_collision_12.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_13.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_14.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_15.stl | Bin 0 -> 1084 bytes .../c1/meshes/collision/L0_collision_2.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_3.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_4.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_5.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_6.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L0_collision_7.stl | Bin 0 -> 484 bytes .../c1/meshes/collision/L0_collision_8.stl | Bin 0 -> 484 bytes .../c1/meshes/collision/L0_collision_9.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L1_collision_0.stl | Bin 0 -> 884 bytes .../c1/meshes/collision/L1_collision_1.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L1_collision_2.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L2_collision_0.stl | Bin 0 -> 884 bytes .../c1/meshes/collision/L2_collision_1.stl | Bin 0 -> 684 bytes .../c1/meshes/collision/L2_collision_2.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_0.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_1.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_10.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_11.stl | Bin 0 -> 884 bytes .../c1/meshes/visual/L0_visual_12.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_13.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_14.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_15.stl | Bin 0 -> 1084 bytes .../c1/meshes/visual/L0_visual_2.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_3.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_4.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_5.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_6.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L0_visual_7.stl | Bin 0 -> 484 bytes .../c1/meshes/visual/L0_visual_8.stl | Bin 0 -> 484 bytes .../c1/meshes/visual/L0_visual_9.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L1_visual_0.stl | Bin 0 -> 884 bytes .../c1/meshes/visual/L1_visual_1.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L1_visual_2.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L2_visual_0.stl | Bin 0 -> 884 bytes .../c1/meshes/visual/L2_visual_1.stl | Bin 0 -> 684 bytes .../c1/meshes/visual/L2_visual_2.stl | Bin 0 -> 684 bytes tests/test_data/c1/urdf/c1.urdf | 309 ++++++++++++++++++ 54 files changed, 658 insertions(+), 46 deletions(-) create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_0.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_1.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_10.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_11.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_12.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_13.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_14.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_15.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_2.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_3.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_4.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_5.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_6.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_7.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_8.stl create mode 100644 tests/test_data/c1/meshes/collision/L0_collision_9.stl create mode 100644 tests/test_data/c1/meshes/collision/L1_collision_0.stl create mode 100644 tests/test_data/c1/meshes/collision/L1_collision_1.stl create mode 100644 tests/test_data/c1/meshes/collision/L1_collision_2.stl create mode 100644 tests/test_data/c1/meshes/collision/L2_collision_0.stl create mode 100644 tests/test_data/c1/meshes/collision/L2_collision_1.stl create mode 100644 tests/test_data/c1/meshes/collision/L2_collision_2.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_0.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_1.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_10.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_11.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_12.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_13.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_14.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_15.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_2.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_3.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_4.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_5.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_6.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_7.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_8.stl create mode 100644 tests/test_data/c1/meshes/visual/L0_visual_9.stl create mode 100644 tests/test_data/c1/meshes/visual/L1_visual_0.stl create mode 100644 tests/test_data/c1/meshes/visual/L1_visual_1.stl create mode 100644 tests/test_data/c1/meshes/visual/L1_visual_2.stl create mode 100644 tests/test_data/c1/meshes/visual/L2_visual_0.stl create mode 100644 tests/test_data/c1/meshes/visual/L2_visual_1.stl create mode 100644 tests/test_data/c1/meshes/visual/L2_visual_2.stl create mode 100644 tests/test_data/c1/urdf/c1.urdf diff --git a/.github/workflows/integration.yml b/.github/workflows/integration.yml index 7977a07..04f203a 100644 --- a/.github/workflows/integration.yml +++ b/.github/workflows/integration.yml @@ -25,6 +25,7 @@ jobs: - name: Install run: | python -m pip install --no-cache-dir -r requirements-dev.txt + python -m pip install ikfast_pybind - name: Run integration tests run: | pytest --doctest-modules diff --git a/CHANGELOG.rst b/CHANGELOG.rst index a4e37f3..d9503b3 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -16,6 +16,7 @@ Unreleased **Changed** - Apply `HideOutput` to pybullet IK error printouts in `inverse_kinematics_helper` - ``motion_planners`` module up-to-date with `commit e6f23053e`_. +- Changed the mesh reading procedure in `vertices_from_data` from `pp.read_obj` to `meshio.read`. This fixes #9. **Fixed** - Fixed `read_obj` returns empty dict if obj file does not start with objects (``o object_name``) diff --git a/pytest.ini b/pytest.ini index 0ab0b66..7281ebd 100644 --- a/pytest.ini +++ b/pytest.ini @@ -1,8 +1,7 @@ [pytest] -; testpaths = tests testpaths = tests src -addopts = --doctest-modules -p no:warnings +addopts = --doctest-modules doctest_optionflags= NORMALIZE_WHITESPACE IGNORE_EXCEPTION_DETAIL ALLOW_UNICODE ALLOW_BYTES -markers = - wip - clone +; markers = +; wip +; clone diff --git a/requirements-dev.txt b/requirements-dev.txt index 64f5ca7..fc7d272 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -11,5 +11,5 @@ pytest-cov python-coveralls isort twine -termcolor # well, just to protect my eyes from digging info from the terminal +# ikfast_pybind # used by tests -e . diff --git a/setup.py b/setup.py index 86ca80a..84a6810 100644 --- a/setup.py +++ b/setup.py @@ -28,6 +28,7 @@ def read(*names, **kwargs): 'recordclass', 'psutil', 'meshio>=4.3.11', + 'termcolor', ] keywords_list = ['robotic planning', 'pybullet'] diff --git a/src/pybullet_planning/interfaces/env_manager/shape_creation.py b/src/pybullet_planning/interfaces/env_manager/shape_creation.py index 26d3ae9..2230545 100644 --- a/src/pybullet_planning/interfaces/env_manager/shape_creation.py +++ b/src/pybullet_planning/interfaces/env_manager/shape_creation.py @@ -25,8 +25,10 @@ # p.GEOM_FORCE_CONCAVE_TRIMESH } -# object_unique_id and linkIndex seem to be noise -CollisionShapeData = namedtuple('CollisionShapeData', ['object_unique_id', 'linkIndex', +# In most cases, objectUniqueId and linkIndex correspond to the parent body index and corresponding link index +# objectUniqueId and linkIndex seem to be noise when multiple meshes are attached to the same link in an URDF +# See ``test_test_geom_data_index`` in ``test_body.py`` for details. +CollisionShapeData = namedtuple('CollisionShapeData', ['objectUniqueId', 'linkIndex', 'geometry_type', 'dimensions', 'filename', 'local_frame_pos', 'local_frame_orn']) @@ -244,6 +246,26 @@ def create_plane(normal=[0, 0, 1], mass=STATIC_MASS, color=BLACK): def create_obj(path, scale=1., mass=STATIC_MASS, collision=True, color=GREY): + """Create a body from a given mesh file. Only `.obj` and `.stl` formats are supported. + + Parameters + ---------- + path : str + absolute file path. + scale : float, optional + mesh scale, by default 1. + mass : [type], optional + [description], by default STATIC_MASS + collision : bool, optional + [description], by default True + color : [type], optional + [description], by default GREY + + Returns + ------- + int + body index + """ collision_id, visual_id = create_shape(get_mesh_geometry(path, scale=scale), collision=collision, color=color) body = create_body(collision_id, visual_id, mass=mass) fixed_base = (mass == STATIC_MASS) @@ -293,6 +315,24 @@ def create_flying_body(group, collision_id=NULL_ID, visual_id=NULL_ID, mass=STAT ##################################### def vertices_from_data(data): + """Get vertices in an object's local coordinate from its geometric data. + + Parameters + ---------- + data : CollisionShapeData or VisualShapeData + geometric data, see ``get_collision_data`` and ``get_visual_data`` + + Returns + ------- + list of vertices + + Raises + ------ + RuntimeError + if an unknown mesh format is encountered, we only support ``.obj`` and ``.stl`` now. + NotImplementedError + if an unknown pybullet geometric type is encountered. See ``SHAPE_TYPES``. + """ from pybullet_planning.interfaces.env_manager.pose_transformation import apply_affine from pybullet_planning.interfaces.env_manager.shape_creation import get_data_type, get_data_extents, get_data_radius, get_data_height, \ get_data_filename, get_data_scale, get_collision_data, get_data_pose @@ -318,22 +358,17 @@ def vertices_from_data(data): aabb = AABB(-half_extents, +half_extents) vertices = get_aabb_vertices(aabb) elif geometry_type == p.GEOM_MESH: - from pybullet_planning.interfaces.geometry.mesh import Mesh, read_obj - import meshio - filename, scale = get_data_filename(data), get_data_scale(data) - if filename != UNKNOWN_FILE: - mio_mesh = meshio.read(filename) - mesh = Mesh(list(mio_mesh.points), list([list(cell_group.data) for cell_group in mio_mesh.cells])) - # mesh = read_obj(filename, decompose=False) - vertices = [scale*np.array(vertex) for vertex in mesh.vertices] - else: - raise RuntimeError('Unknown file from data {}'.format(data)) + # from pybullet_planning.interfaces.geometry.mesh import Mesh #, read_obj + # import meshio + # filename, scale = get_data_filename(data), get_data_scale(data) + # if filename != UNKNOWN_FILE: + # mio_mesh = meshio.read(filename) + # vertices = [scale*np.array(vertex) for vertex in mio_mesh.points] + # else: + # raise RuntimeError('Unknown file from data {}'.format(data)) + mesh_data = p.getMeshData(data.objectUniqueId, data.linkIndex, collisionShapeIndex=data.objectUniqueId, + flags=p.MESH_DATA_SIMULATION_MESH) # TODO: could compute AABB here for improved speed at the cost of being conservative - - # mesh_data = p.getMeshData(data.object_unique_id, data.linkIndex, collisionShapeIndex=data.object_unique_id, - # flags=p.MESH_DATA_SIMULATION_MESH) - # vertices = mesh_data[1] - #elif geometry_type == p.GEOM_PLANE: # parameters = [get_data_extents(data)] else: diff --git a/src/pybullet_planning/interfaces/geometry/mesh.py b/src/pybullet_planning/interfaces/geometry/mesh.py index 2cfdb3d..a9b01aa 100644 --- a/src/pybullet_planning/interfaces/geometry/mesh.py +++ b/src/pybullet_planning/interfaces/geometry/mesh.py @@ -68,6 +68,10 @@ def get_connected_components(vertices, edges): def read_obj(path, decompose=True): """Read meshes from an obj file. + Note: We read `o group_name` for a new group in the mesh. + This differs from `meshio`, which uses `g group_name` for new group lines. + https://github.com/nschloe/meshio/blob/main/src/meshio/obj/_obj.py#L59-L60 + Parameters ---------- path : string diff --git a/src/pybullet_planning/interfaces/robots/body.py b/src/pybullet_planning/interfaces/robots/body.py index db294b4..146fbe3 100644 --- a/src/pybullet_planning/interfaces/robots/body.py +++ b/src/pybullet_planning/interfaces/robots/body.py @@ -263,8 +263,33 @@ def vertices_from_link(body, link, collision=True): vertices.extend(vertices_from_data(data)) return vertices +def vertices_from_link2(body, link=BASE_LINK): + """Experimental: get body link vertices using pybullet's ``getMeshData``. + (``getMeshData`` is an experimental undocumented pybullet API to return mesh information (vertices, indices) of triangle meshes.) + + Parameters + ---------- + body : int + link : int, optional + by default BASE_LINK + + Returns + ------- + list of vertices + """ + from pybullet_planning.interfaces.env_manager.shape_creation import get_data_pose + vertices = [] + for data in get_collision_data(body, link): + # this will return the convexified objects + mesh_data = p.getMeshData(body, link, collisionShapeIndex=data.objectUniqueId, + flags=p.MESH_DATA_SIMULATION_MESH) + # print('data: ', data) + # print('meshdata: ', mesh_data) + vertices.extend(apply_affine(get_data_pose(data), mesh_data[1])) + return vertices + def vertices_from_rigid(body, link=BASE_LINK): - """get all vertices of given body in its local frame. + """get all vertices of given body (collision body) in its local frame. Parameters ---------- @@ -278,20 +303,16 @@ def vertices_from_rigid(body, link=BASE_LINK): list of three-float lists body vertices """ - import os - from pybullet_planning.interfaces.env_manager.pose_transformation import get_pose - from pybullet_planning.interfaces.env_manager import get_model_info - from pybullet_planning.interfaces.geometry.mesh import read_obj - from pybullet_planning.interfaces.robots.link import get_num_links + # import os + # from pybullet_planning.interfaces.env_manager.pose_transformation import get_pose + # from pybullet_planning.interfaces.env_manager import get_model_info + # from pybullet_planning.interfaces.geometry.mesh import read_obj + # from pybullet_planning.interfaces.robots.link import get_num_links # assert implies(link == BASE_LINK, get_num_links(body) == 0), 'body {} has links {}'.format(body, get_all_links(body)) try: vertices = vertices_from_link(body, link) except RuntimeError: - vertices = [] - for data in get_collision_data(body, link): - # this will return the convexified objects - mesh_data = p.getMeshData(body, link, collisionShapeIndex=data.object_unique_id, flags=p.MESH_DATA_SIMULATION_MESH) - vertices.extend(mesh_data[1]) + vertices = vertices_from_link2(body, link) # ! archived method for getting vertices from the mesh_cache # info = get_model_info(body) # assert info is not None diff --git a/tests/test_body.py b/tests/test_body.py index da071c2..7d638d5 100644 --- a/tests/test_body.py +++ b/tests/test_body.py @@ -1,17 +1,19 @@ import os import sys +import random import pytest -import pybullet +import pybullet as pb import warnings import pybullet_planning as pp from pybullet_planning import load_pybullet, connect, wait_for_user, LockRenderer, has_gui, WorldSaver, HideOutput, \ - reset_simulation, disconnect, set_camera_pose, has_gui, wait_if_gui + reset_simulation, disconnect, set_camera_pose, has_gui, wait_if_gui, apply_alpha from pybullet_planning import create_obj, create_attachment, Attachment from pybullet_planning import link_from_name, get_link_pose, get_moving_links, get_link_name from pybullet_planning import get_num_joints, get_joint_names, get_movable_joints from pybullet_planning import dump_world, set_pose -from pybullet_planning import clone_body, create_box +from pybullet_planning import clone_body, create_box, read_obj from pybullet_planning import Pose +from termcolor import cprint @pytest.fixture @@ -119,7 +121,7 @@ def test_clone_body(viewer, workspace_path, ee_path): set_pose(c_ee_v, move_pose) print('clone collision ee from obj') - with pytest.raises(pybullet.error): + with pytest.raises(pb.error): warnings.warn('Currently, we do not support clone bodies that are created from an obj file.') c_ee_c = clone_body(ee_body, visual=False, collision=True) set_pose(c_ee_c, move_pose) @@ -128,15 +130,17 @@ def test_clone_body(viewer, workspace_path, ee_path): @pytest.mark.create_body @pytest.mark.parametrize("file_format",[ - ('obj'), - ('stl'), - # ('dae'), - # ('ply'), - ] -) + 'obj', + 'stl', + # 'dae', + # 'ply', + ]) def test_create_body(viewer, file_format): here = os.path.dirname(__file__) - path = os.path.join(here, 'test_data', 'link_4.' + file_format) + if file_format == 'dae': + path = os.path.join(here, 'test_data', 'kuka_kr6_r900/meshes/kr6_agilus/visual/base_link.dae', ) + else: + path = os.path.join(here, 'test_data', 'link_4.' + file_format) connect(use_gui=viewer) try: with HideOutput(): @@ -147,7 +151,7 @@ def test_create_body(viewer, file_format): @pytest.mark.read_obj -def test_create_body(viewer): +def test_read_obj(viewer): here = os.path.dirname(__file__) path = os.path.join(here, 'test_data', 'box_obstacle.obj') connect(use_gui=viewer) @@ -160,3 +164,240 @@ def test_create_body(viewer): assert(len(meshes[None].faces)==6) finally: disconnect() + + +@pytest.mark.vertices_from_link +def test_vertices_from_link(viewer, workspace_path): + here = os.path.dirname(__file__) + backwall_path = os.path.join(here, 'test_data', 'mit_3-412_workspace', 'meshes', 'mit_3-412_workspace', \ + 'collision', 'MIT_3-412_right_corner_piece.stl') + connect(use_gui=viewer) + with HideOutput(): + body = load_pybullet(workspace_path, fixed_base=False) + link_body = create_obj(backwall_path) + link = link_from_name(body, 'MIT_3412_right_corner_piece') + + # ! before setting pose + body_vertices = pp.vertices_from_rigid(body, link) + body_vertices1 = pp.vertices_from_link2(body, link) + + link_body_vertices = pp.vertices_from_rigid(link_body) + link_body_vertices1 = pp.vertices_from_link2(link_body) + + for v1, v2 in zip(body_vertices, link_body_vertices): + # assert pp.get_distance(v1, v2) < 1e-6 + # print('{} | {}'.format(v1, v2)) + pp.draw_point(v1, color=pp.BLUE, size=0.1) + pp.draw_point(v2, color=pp.RED, size=0.1) + + wait_if_gui() + pp.remove_all_debug() + + for v1, v2 in zip(body_vertices1, link_body_vertices1): + assert pp.get_distance(v1, v2) < 1e-6 + pp.draw_point(v1, color=pp.GREEN, size=0.1) + pp.draw_point(v2, color=pp.BLACK, size=0.1) + + wait_if_gui() + + print('====') + set_pose(body, Pose(point=(0, 3, 0))) + pp.step_simulation() + + # # # ! after setting pose + # new_body_vertices = pp.vertices_from_rigid(body, link) + # new_body_vertices1 = pp.vertices_from_link2(body, link) + + # new_link_body_vertices = pp.vertices_from_rigid(link_body) + # new_link_body_vertices1 = pp.vertices_from_link2(link_body) + + # for v1, v2 in zip(body_vertices, link_body_vertices): + # assert pp.get_distance(v1, v2) < 1e-6 + # pp.draw_point(v1, color=pp.BLUE, size=0.1) + + # for v1, v2 in zip(body_vertices1, link_body_vertices1): + # assert pp.get_distance(v1, v2) < 1e-6 + # pp.draw_point(v1, color=pp.GREEN, size=0.1) + + # for v1, v2 in zip(vertices, backwall_mesh.points): + # # assert pp.get_distance(v1, v2) < 1e-6 + # print('{} | {}'.format(v1, v2)) + + wait_if_gui() + + +@pytest.mark.vertices_from_link_geom +def test_vertices_from_link_geometry(viewer): + here = os.path.dirname(__file__) + mesh_path = os.path.join(here, 'test_data', 'duck.obj') + connect(use_gui=viewer) + with HideOutput(): + bodies = [] + types = [] + bodies.append(pp.create_box(1,1,1, color=apply_alpha(pp.RED, 0.2))) + types.append(pb.GEOM_BOX) + bodies.append(pp.create_cylinder(0.5, 3, color=apply_alpha(pp.GREEN, 0.2))) + types.append(pb.GEOM_CYLINDER) + bodies.append(pp.create_capsule(0.5, 3, color=apply_alpha(pp.BLUE, 0.2))) + types.append(pb.GEOM_CAPSULE) + bodies.append(pp.create_sphere(0.5, color=apply_alpha(pp.TAN, 0.2))) + types.append(pb.GEOM_SPHERE) + bodies.append(pp.create_obj(mesh_path, color=apply_alpha(pp.TAN, 0.2), scale=0.01)) + types.append(pb.GEOM_MESH) + + for body, geom_type in zip(bodies, types): + cprint(pp.SHAPE_TYPES[geom_type], 'cyan') + body_vertices1 = pp.vertices_from_link(body, pp.BASE_LINK) + body_vertices2 = pp.vertices_from_link2(body, pp.BASE_LINK) + if geom_type == pb.GEOM_MESH: + assert len(body_vertices2) > 0 and len(body_vertices1) > 0 + # pybullet uses a convexified mesh, which should have less number of vertices compared to the original mesh + assert len(body_vertices2) <= len(body_vertices1) + else: + # pb.getMeshData will return an empty mesh if the geometric type is not pb.GEOM_MESH + assert len(body_vertices2) == 0 + with LockRenderer(): + for v1 in body_vertices1: + pp.draw_point(v1, color=pp.BLUE, size=0.1) + for v1 in body_vertices2: + pp.draw_point(v1, color=pp.BLACK, size=0.1) + wait_if_gui() + pp.remove_all_debug() + + +@pytest.mark.geom_data_index +def test_geom_data_index(viewer): + verbose = True + here = os.path.dirname(__file__) + c1_path = os.path.join(here, 'test_data', 'c1', 'urdf', 'c1.urdf') + kuka_path = os.path.join(here, 'test_data', 'kuka_kr6_r900', 'urdf', 'kuka_kr6_r900.urdf') + mesh_path = os.path.join(here, 'test_data', 'duck.obj') + + connect(use_gui=viewer) + with HideOutput(): + # just to shift body indices + for _ in range(random.choice(range(10))): + pp.create_box(0.1,0.1,0.1) + + bodies = [] + geom_types = [] + body_dimensions = [] + + dimensions = [1.2,2.1,13.1] + bodies.append(pp.create_box(*dimensions, color=apply_alpha(pp.RED, 0.2))) + geom_types.append(pb.GEOM_BOX) + body_dimensions.append(dimensions) + + # radius, height + dimensions = [0.6,3] + bodies.append(pp.create_cylinder(*dimensions, color=apply_alpha(pp.GREEN, 0.2))) + geom_types.append(pb.GEOM_CYLINDER) + # pb collision/visual data returns [height, radius, 0] + body_dimensions.append(dimensions[::-1] + [0]) + + # radius, height + dimensions = [0.7,4] + bodies.append(pp.create_capsule(*dimensions, color=apply_alpha(pp.BLUE, 0.2))) + geom_types.append(pb.GEOM_CAPSULE) + # pb collision/visual data returns [height, radius, 0] + body_dimensions.append(dimensions[::-1] + [0]) + + # radius + dimensions = [0.8] + bodies.append(pp.create_sphere(*dimensions, color=apply_alpha(pp.TAN, 0.2))) + geom_types.append(pb.GEOM_SPHERE) + body_dimensions.append(dimensions*3) + + dimensions = [0.01] + bodies.append(pp.create_obj(mesh_path, color=apply_alpha(pp.TAN, 0.2), scale=dimensions[0])) + geom_types.append(pb.GEOM_MESH) + body_dimensions.append(dimensions*3) + + robot_scale = 0.01 + robot = load_pybullet(kuka_path, fixed_base=True, scale=robot_scale) + c1_scale = 0.001 + c1 = load_pybullet(c1_path, fixed_base=True, scale=c1_scale) + + # * robot + cprint('kuka robot', 'cyan') + for link in pp.get_all_links(robot): + link_name = get_link_name(robot, link) + print('====') + cprint(link_name, 'cyan') + cdatas = pp.get_collision_data(robot, link) + vdatas = pp.get_visual_data(robot, link) + if 'robot_link' in link_name or 'robot_base_link' == link_name: + assert len(cdatas) == 1 and len(vdatas) == 1 + else: + assert len(cdatas) == 0 and len(vdatas) == 0 + for cdata in cdatas: + if verbose: print(cdata) + assert(cdata.objectUniqueId == robot) + assert(cdata.linkIndex == link) + assert(cdata.geometry_type == pb.GEOM_MESH) + assert(cdata.dimensions == tuple([robot_scale]*3)) + for vdata in vdatas: + if verbose: print(vdata) + assert(vdata.objectUniqueId == robot) + assert(vdata.linkIndex == link) + assert(vdata.visualGeometryType == pb.GEOM_MESH) + assert(vdata.dimensions == tuple([robot_scale]*3)) + + # * c1 + cprint('C1', 'cyan') + num_meshes_from_link_name = { + 'gripper_base' : 16, + 'gripper_jaw' : 3, + 'clamp_jaw' : 3, + } + for link in pp.get_all_links(c1): + link_name = get_link_name(c1, link) + print('====') + cprint(link_name, 'cyan') + cdatas = pp.get_collision_data(c1, link) + vdatas = pp.get_visual_data(c1, link) + assert len(cdatas) == num_meshes_from_link_name[link_name] + assert len(vdatas) == num_meshes_from_link_name[link_name] + for cdata in cdatas: + if verbose: print(cdata) + # ! in this case the objectUniqueId and linkIndex can be random... + # assert(cdata.objectUniqueId == c1) + # assert(cdata.linkIndex == link) + assert(cdata.geometry_type == pb.GEOM_MESH) + assert(cdata.dimensions == tuple([1e-3*c1_scale]*3)) + for vdata in vdatas: + if verbose: print(vdata) + assert(vdata.objectUniqueId == c1) + assert(vdata.linkIndex == link) + assert(vdata.visualGeometryType == pb.GEOM_MESH) + assert(vdata.dimensions == tuple([1e-3*c1_scale]*3)) + + # primitive geometries + for body, geom_type, b_dims in zip(bodies, geom_types, body_dimensions): + cprint(pp.SHAPE_TYPES[geom_type], 'cyan') + cdatas = pp.get_collision_data(body) + assert len(cdatas) == 1 + cdata = cdatas[0] + if verbose: print(cdata) + # print(pb.getCollisionShapeData(body, pp.BASE_LINK, physicsClientId=pp.CLIENT)) + assert(cdata.objectUniqueId == body) + assert(cdata.linkIndex == pp.BASE_LINK) + assert(cdata.geometry_type == geom_type) + if geom_type == pb.GEOM_MESH: + # ! it seems that pybullet does v-hacd inside and the dimensions will always be (1,1,1) + assert(cdata.dimensions == (1.0, 1.0, 1.0)) + else: + assert(cdata.dimensions == tuple(b_dims)) + + vdatas = pp.get_visual_data(body) + assert len(vdatas) == 1 + vdata = vdatas[0] + if verbose: print(vdata) + assert(vdata.objectUniqueId == body) + assert(vdata.linkIndex == pp.BASE_LINK) + assert(vdata.visualGeometryType == geom_type) + if geom_type == pb.GEOM_SPHERE: + assert(vdata.dimensions == (b_dims[0], 0.0, 0.0)) + else: + assert(vdata.dimensions == tuple(b_dims)) + diff --git a/tests/test_data/c1/meshes/collision/L0_collision_0.stl b/tests/test_data/c1/meshes/collision/L0_collision_0.stl new file mode 100644 index 0000000000000000000000000000000000000000..836bb845a24a6843fe4c0387ba6d566cbcd38c67 GIT binary patch literal 684 zcmZQzpf2D68TX09I3&r)n1P{T|K8(ePR|XP9hskhH9hvT+>!17LPz@pX{OBn;!YA9 zU7e;Mw=!d3sQ-V(XztQ7BajYcRmi$$Se$Y!XAE}$>1JS9X<4$VTPSNYP=`HC)x|S6 z9AV~O012Y$-3fUwEhUwk0_VZmC>_K{AG9Z7z>;SnP#s=AkssmXD zrW@G~kVy;-T3+7!pHG(t+X2%7G8g7TkZ)mZh(C~ZAj`mXgIoe*LqfN|J9hsXSBCu{ zf53Er%!TjJtG}!;x;OfNbFMg;Cd@q2tG&3#rM2eJ&zT#zbclNcCwxiai;?~b(xg%C^zgG^#zcs@PPK6KK~eIPqvIzZ;aTnGv)7#rdbWF5#dFx?=R zz}S$`J?hF}-`E|yA7lqi2gqEQ&ym>>JCJq2RDon*x{=u+-3$z0r%Ugb_wokY0n-7J XLG~?-4Y31R2eJ%IH%JwX4Y30N_}DBZ literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L0_collision_10.stl b/tests/test_data/c1/meshes/collision/L0_collision_10.stl new file mode 100644 index 0000000000000000000000000000000000000000..6fd92f2525f268e717de1bd12a1f1b51c073f585 GIT binary patch literal 684 zcmZQzpf2D68kZ!~vhPSp`F@be5y!lo45v07YB;Urba`5aqjXo?p-09O;Ow+C2`7Y( zGy67dhN%L{aHJbJdCAuux^U)(BeF?g9d_K+8{F~tweztnfKHeoy_aVCkSrxKwR2|4N z$ZkhAiGhJHZ27(khd0@}y(rj^>~mxnBHPEnzzfvT57mJzgKRFcNem2!fpL*8(_#k- zE0_!@Okj2(vmx<;tOKSBBm>ip%!Y)n&2KsTwbwNF)kwS8Bl{LP6ksNy>Zk?kK$d~o L0aAr*5(5JOLP+{o literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L0_collision_11.stl b/tests/test_data/c1/meshes/collision/L0_collision_11.stl new file mode 100644 index 0000000000000000000000000000000000000000..5ed5f7dc875bedafc38a4455eaeaf59cad58efa5 GIT binary patch literal 884 zcmZQzpe_&q8ka27vhPSp`F@bep@mF`#B_8HWu!?s1*$e3vZ>=dl$Is}XP-H9!;yg@ z0jgvDshP$c=>|?-@^yzU0A(&u%W#zLiUaD9a6&eTfgus9PV4k zu{#`6Zoe?$@@B1@HxER5ggJp+RU{&N$oN}@6G(RiSqt>_f; zO4$kIk~!zPom4|voIox_HVN!1u#UH?z9z`#BCA5y&A`AHw%l&Q;Z1wpUKH3P%OKl_ z9108!yg(iOP#wrJ$Ua9liGe|1U&&;?lkR2)h6a0NbCF$z8de}33!plXWsvPdHi?0u zwD|R&3+=zH|2?X=M>ZEZL}Bp+3Eg6#j?+*b$TGA+abynECH*<9q1 zhM5FOS0Ei3Kpmizh%5tRgIt1a5(C2}ksjk~OQ&uEhSh##bCE+7Iiyi_Aj`n)0J#L& HBnAcmBV$Dd literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L0_collision_12.stl b/tests/test_data/c1/meshes/collision/L0_collision_12.stl new file mode 100644 index 0000000000000000000000000000000000000000..a7d684de140817f4d6eb926d8ceb555bffd56197 GIT binary patch literal 684 zcmb`FyAA#J)Tfcm_&PYJ%E5Npt#Ro1?Nhjuc8UEb-f@k(N3?|% z!&^w-Jd;Zk&6F803Tr~t((x90aVDZI6IGAE4EByDq7_eF=FWP}ghVfPh#UD5E9FSc wU_CV9`?luj#c6TVzXWEm9-3ZV=@ov(qcS!0;qd4hst#lsWVa)m#J~UwrwpLW zfV2s+xyb%Nm;?$dm=2I_$TA3a2%68oykIt!ea*9lfuUhPvbo5#E$Pm$V+Y%a2IkwXEYV@AE@R%97u`;bjyU;qFFwDj=+ literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L0_collision_14.stl b/tests/test_data/c1/meshes/collision/L0_collision_14.stl new file mode 100644 index 0000000000000000000000000000000000000000..867bbd022e511f086bb1649282ecf4601c2454b5 GIT binary patch literal 684 zcmZQzpf2D68W$_mvhPSp`TkhRDVswInGT8R=p6EreB|`or}xnF4(3B)VnWV=s!fM% z>NpRjrHME(FeF2DNY76+LY4`I>dr`$a6&eTfgw(&#SUnP{W;~8#!5GD9*Fb^a{}2> zC?b2v_*;b&$lPPe#)mpy*ExagV_-;u>X4nExCvPmvTiM?Ng(?e7)py@+njCxz5Cyz z`u)f{kX0eOkb$8XsN)<|2eJ&ZpO8&rU^oo)Lpsp+r;LI(Bb$rtDpa2%>p+%4wh!4P z1_t}za`qdpY3{3(cCkk`7dd>8;|8Il3aA5F2H8GjlNcBj{*>((c-w53x*}pPvbo5< YMGggs4t}7H7@!Vh8D#sAO=4gG0IZzw&j0`b literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L0_collision_15.stl b/tests/test_data/c1/meshes/collision/L0_collision_15.stl new file mode 100644 index 0000000000000000000000000000000000000000..066bc9b0f35a9ae631c2b3c88a724aa5e6eaf615 GIT binary patch literal 1084 zcmb`FO(;ZB7=}Mg$O=gk3&M_tOKCFqaxF*}eufx@QZpr+$?t{{6NaR$P)d?4MoE6d zOiuoig_M*H8Cm#QD5O~Uu9>O#&e~a>&Uv5jJ>UBsqW@h9TrTaWTZwob^3*NYn`V7E z;&Q7`&6U2?i&jaDKliJ(iSZ@h6%*oNG(u>5KM~GEdkBH5dUuP+GfftK?l@GUboK~i z!p?-{!6`N)qlI>ZDx`^y%st+Q6sO!VmbsR-aUw?Crw4Zp_v%~Hx=><`#a3&ts-2JW zxwiKc7A9h1T)CmDAYHjXBP@>{>Md6y>gq9h&u!)M?nH*N)H*2Q%wvC@qYAzK#2FKN zOjt{0VR#x)jn{D{NFRHqgl(RLc23mE9zg(VA2fqqC61&OwgWC>G571Bh~l~LaG03C>F9tdyuHmxS&(8~n#ajZAP>O1`Z|LKV{JN0eP%WtyIA-YZ^octi6E{yzPy nG-4(|;0<<;m55&K?){a5?ZSmfhk`T5IqUmC0&lPrp)e>MHF!PSY=U;GeIe8rc43mrfH zEI(*}AkCD4VdsUnM(m6KZUpH-mYHF3%CVd=+ySHt*(3&r0JRTBzy1eq2I{bf$y_{h z!x5$%#D>{-0mw%7WrIDkDrDU-`#`!G7=Axz+jsOxtvyIDvMLxG`uyv3>HYFv-uppzAp0D~ z2Du&CJ_ZJ49mp~;HYh}4x*`795naCD{YA4K$PQ%R!q^}?U^*aSg{%Wv2F3=N3)2m; F0|0rF5|sb| literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L0_collision_3.stl b/tests/test_data/c1/meshes/collision/L0_collision_3.stl new file mode 100644 index 0000000000000000000000000000000000000000..11691c7d9dcb9cb93055e4cfa4dc1dbf31be2127 GIT binary patch literal 684 zcmZQzpf2D68kZ!~vhPSp`TlKhc{elNYj$$=lR3O;%}b}4r9FohUwMDXb-I*u8QBnE~AnHD>s9rndHEsO)r*E_xR znRWN;YF>?&k`pxOb_u^OraSq9lYWRn;eY_HVYB`@pP`_Zf19@$)E-y*vZq9YZm z16c;yK4g;^7#{P*+IOkv>=Rz{bRV+0$f1ksb7URJGRXEJn}nQ(8ulZbi|kwEP(amz MEQ4$xvPldK0Jc6PuK)l5 literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L0_collision_4.stl b/tests/test_data/c1/meshes/collision/L0_collision_4.stl new file mode 100644 index 0000000000000000000000000000000000000000..a7fbeb22422be1f28caad92ae566630d2e8b4892 GIT binary patch literal 684 zcmZQzpf2D68kZu|vhPSp`F@beF-v<6Exz*pkn40Q=Q6&}hYnR;IW$YM4$i)C=7u8! zLn=_mVW5tc?^kX@R)wrP7HSemHv>bmOp6`R4ttOtk~5qRv2jU*?NHx({7}c569+-& z!q^ZUKs)S!cGv^$FhZ7*hUx~XLN_c`5%!SByK>YC)sAC;e z2h0wT46?b%CNVI8!YLl;GLS!z&4sZ+u7a5a@dvUFWEmJ6WFJg7#8n`(A)vt?CId1H zIYf~|0j3&6H`pV~!0Z6&Mm7oT5Bo%5EG`n7Z;Wg%vTuCpryT7Mq?t1Ni#thdbak43+{*0O&vHk$ z{|mw53=BCtpBSB#5C-c&R)wsa%VEMH?&`TtAd?svEOu3FVl>O#479@@rt0FE8;&q@ zL2Q^wmw;>r28~mXHo4vs1nWSSftd@^fou{3gO-=K{qyP4`#~XuY%YwA?k7|oFgrlL zg|R^{f$0X>$G}kVqIut@=yI?f$hO1SAUj|>AU;Rdfh+@KWBP+lQFi}JDKD@c$i9WK zLFOXc$H0KB16c;f2H64A4e`g*7tMByqs#32A&Pknf1|H6YHHrwl~hwdHl zJ7j+#%@nL-Gf>B7pbmSOj;m*GIKot21hQeeLE;PyU-OsQt&u3&4{|-SDi|ANE=&i= zK2#mZGB7r}eGClpF--gS2F2Ke>_B!EjE(6JWF5#Zfw4jUfa!+#V?uYV{c2YRupP)Q Zfw4h$z;rKk>FQvTfL3&}T(Cxsj16c;98`(aPNem1k?_BqXMl08uoKJOgO|{J=e+pK$__ci&F>78N&~-1VkkLN!{&=^#~5_9Np7KplxtJMQ&y7$e((O*dSJ9ne+w`!X(XdK#_h z6!J>h2_&O+^X7p_k1(eUpdIp1HpqowSAlhK$g*rcUwOnKt=Pi}v)dr-Wi`y6Hm*taM#h^zx<2eK-VOJKUe zE@ZIzEoZ;@n&!S*X%~B#3`hrZ+#sg~1{57AGDt2#Hi>~D>^$3kO%?$=r-fnm$mSx4 YC~`3Vs|8@+`b_t$VlR2xzo}73a5-T9;cT~3l2VDTH%xiWWRy3Q&vhl zBXmr?z0L?(=4JC=r_Hv;PAvbQIw6|`*0B$0$Nt|bbB#izW1T|33WMz^S$FkdV0X3? z$lO98I{?TA*@w`f@A=XYSrxKwcF7M;?QL@%LH02)fXq(<`bC#1V>7bbk}&00LYCxBvhE literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L2_collision_0.stl b/tests/test_data/c1/meshes/collision/L2_collision_0.stl new file mode 100644 index 0000000000000000000000000000000000000000..0a74a0918c85332929e7f23e7e3cc3181cb4712e GIT binary patch literal 884 zcmZQzpe_&q8h0q9e1D-#i`~+;Vq+G)y=ILWmteSMBT6YTb-%7P2a2-3$!gx3l(Ly&k=vZ`!MU$U2bygz66;ppGk0 z9mq1sb|9O?z>o@b5zsIDMMRp6kbR3B2gvrJ>Ohu3HW%3>*D7oHilLAe)P95(5JO@?d6` literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L2_collision_1.stl b/tests/test_data/c1/meshes/collision/L2_collision_1.stl new file mode 100644 index 0000000000000000000000000000000000000000..04b728f9e40ac1b913ff582d906e9ea65ab3e441 GIT binary patch literal 684 zcmZQzpf2D68h0$De1C{ciyg@1qR>Bw{LF3~x^U)(V__@f;g=yFQP}TRRXQ;+90BS` z1nQ6|7B+#Y0?Dk&dw=M_^W}%q(kh&gO=4g;98zwdBh#`EWXGY|OAe_mc@H+%IB@l$ zghZCZAl)!F$ZoJ5_6a~8&l5OJU@{<8$h!Y-nSN+`=zg$$3=FR9yw)MiEH*p;*4rby z3RyR99mq1s{y;W~fguIxB9QMv{(#AV+zYb5~dDh8JHa)RmdhWF!VEq+kJQPwC6k{vLD&E$f1C29|Oa5s16RG4rCc*bCFGA GU;qI4cqg*} literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/collision/L2_collision_2.stl b/tests/test_data/c1/meshes/collision/L2_collision_2.stl new file mode 100644 index 0000000000000000000000000000000000000000..c00ae9a7f7b616a84b7619e34756ede92737784a GIT binary patch literal 684 zcmZQzpf2D68h1FPe1ED;i{0-ZH#e`zdw=M_^W}%q(kh(J=I;?7#JonhVT2~}hW*IqB8N0`h@k2~mO-`;*(3%A@7q~+SFcCg^G$nY dhiopgZ;?ZRfx!o;;|f#!17LPz@pX{OBn;!YA9 zU7e;Mw=!d3sQ-V(XztQ7BajYcRmi$$Se$Y!XAE}$>1JS9X<4$VTPSNYP=`HC)x|S6 z9AV~O012Y$-3fUwEhUwk0_VZmC>_K{AG9Z7z>;SnP#s=AkssmXD zrW@G~kVy;-T3+7!pHG(t+X2%7G8g7TkZ)mZh(C~ZAj`mXgIoe*LqfN|J9hsXSBCu{ zf53Er%!TjJtG}!;x;OfNbFMg;Cd@q2tG&3#rM2eJ&zT#zbclNcCwxiai;?~b(xg%C^zgG^#zcs@PPK6KK~eIPqvIzZ;aTnGv)7#rdbWF5#dFx?=R zz}S$`J?hF}-`E|yA7lqi2gqEQ&ym>>JCJq2RDon*x{=u+-3$z0r%Ugb_wokY0n-7J XLG~?-4Y31R2eJ%IH%JwX4Y30N_}DBZ literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L0_visual_10.stl b/tests/test_data/c1/meshes/visual/L0_visual_10.stl new file mode 100644 index 0000000000000000000000000000000000000000..6fd92f2525f268e717de1bd12a1f1b51c073f585 GIT binary patch literal 684 zcmZQzpf2D68kZ!~vhPSp`F@be5y!lo45v07YB;Urba`5aqjXo?p-09O;Ow+C2`7Y( zGy67dhN%L{aHJbJdCAuux^U)(BeF?g9d_K+8{F~tweztnfKHeoy_aVCkSrxKwR2|4N z$ZkhAiGhJHZ27(khd0@}y(rj^>~mxnBHPEnzzfvT57mJzgKRFcNem2!fpL*8(_#k- zE0_!@Okj2(vmx<;tOKSBBm>ip%!Y)n&2KsTwbwNF)kwS8Bl{LP6ksNy>Zk?kK$d~o L0aAr*5(5JOLP+{o literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L0_visual_11.stl b/tests/test_data/c1/meshes/visual/L0_visual_11.stl new file mode 100644 index 0000000000000000000000000000000000000000..5ed5f7dc875bedafc38a4455eaeaf59cad58efa5 GIT binary patch literal 884 zcmZQzpe_&q8ka27vhPSp`F@bep@mF`#B_8HWu!?s1*$e3vZ>=dl$Is}XP-H9!;yg@ z0jgvDshP$c=>|?-@^yzU0A(&u%W#zLiUaD9a6&eTfgus9PV4k zu{#`6Zoe?$@@B1@HxER5ggJp+RU{&N$oN}@6G(RiSqt>_f; zO4$kIk~!zPom4|voIox_HVN!1u#UH?z9z`#BCA5y&A`AHw%l&Q;Z1wpUKH3P%OKl_ z9108!yg(iOP#wrJ$Ua9liGe|1U&&;?lkR2)h6a0NbCF$z8de}33!plXWsvPdHi?0u zwD|R&3+=zH|2?X=M>ZEZL}Bp+3Eg6#j?+*b$TGA+abynECH*<9q1 zhM5FOS0Ei3Kpmizh%5tRgIt1a5(C2}ksjk~OQ&uEhSh##bCE+7Iiyi_Aj`n)0J#L& HBnAcmBV$Dd literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L0_visual_12.stl b/tests/test_data/c1/meshes/visual/L0_visual_12.stl new file mode 100644 index 0000000000000000000000000000000000000000..a7d684de140817f4d6eb926d8ceb555bffd56197 GIT binary patch literal 684 zcmb`FyAA#J)Tfcm_&PYJ%E5Npt#Ro1?Nhjuc8UEb-f@k(N3?|% z!&^w-Jd;Zk&6F803Tr~t((x90aVDZI6IGAE4EByDq7_eF=FWP}ghVfPh#UD5E9FSc wU_CV9`?luj#c6TVzXWEm9-3ZV=@ov(qcS!0;qd4hst#lsWVa)m#J~UwrwpLW zfV2s+xyb%Nm;?$dm=2I_$TA3a2%68oykIt!ea*9lfuUhPvbo5#E$Pm$V+Y%a2IkwXEYV@AE@R%97u`;bjyU;qFFwDj=+ literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L0_visual_14.stl b/tests/test_data/c1/meshes/visual/L0_visual_14.stl new file mode 100644 index 0000000000000000000000000000000000000000..867bbd022e511f086bb1649282ecf4601c2454b5 GIT binary patch literal 684 zcmZQzpf2D68W$_mvhPSp`TkhRDVswInGT8R=p6EreB|`or}xnF4(3B)VnWV=s!fM% z>NpRjrHME(FeF2DNY76+LY4`I>dr`$a6&eTfgw(&#SUnP{W;~8#!5GD9*Fb^a{}2> zC?b2v_*;b&$lPPe#)mpy*ExagV_-;u>X4nExCvPmvTiM?Ng(?e7)py@+njCxz5Cyz z`u)f{kX0eOkb$8XsN)<|2eJ&ZpO8&rU^oo)Lpsp+r;LI(Bb$rtDpa2%>p+%4wh!4P z1_t}za`qdpY3{3(cCkk`7dd>8;|8Il3aA5F2H8GjlNcBj{*>((c-w53x*}pPvbo5< YMGggs4t}7H7@!Vh8D#sAO=4gG0IZzw&j0`b literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L0_visual_15.stl b/tests/test_data/c1/meshes/visual/L0_visual_15.stl new file mode 100644 index 0000000000000000000000000000000000000000..066bc9b0f35a9ae631c2b3c88a724aa5e6eaf615 GIT binary patch literal 1084 zcmb`FO(;ZB7=}Mg$O=gk3&M_tOKCFqaxF*}eufx@QZpr+$?t{{6NaR$P)d?4MoE6d zOiuoig_M*H8Cm#QD5O~Uu9>O#&e~a>&Uv5jJ>UBsqW@h9TrTaWTZwob^3*NYn`V7E z;&Q7`&6U2?i&jaDKliJ(iSZ@h6%*oNG(u>5KM~GEdkBH5dUuP+GfftK?l@GUboK~i z!p?-{!6`N)qlI>ZDx`^y%st+Q6sO!VmbsR-aUw?Crw4Zp_v%~Hx=><`#a3&ts-2JW zxwiKc7A9h1T)CmDAYHjXBP@>{>Md6y>gq9h&u!)M?nH*N)H*2Q%wvC@qYAzK#2FKN zOjt{0VR#x)jn{D{NFRHqgl(RLc23mE9zg(VA2fqqC61&OwgWC>G571Bh~l~LaG03C>F9tdyuHmxS&(8~n#ajZAP>O1`Z|LKV{JN0eP%WtyIA-YZ^octi6E{yzPy nG-4(|;0<<;m55&K?){a5?ZSmfhk`T5IqUmC0&lPrp)e>MHF!PSY=U;GeIe8rc43mrfH zEI(*}AkCD4VdsUnM(m6KZUpH-mYHF3%CVd=+ySHt*(3&r0JRTBzy1eq2I{bf$y_{h z!x5$%#D>{-0mw%7WrIDkDrDU-`#`!G7=Axz+jsOxtvyIDvMLxG`uyv3>HYFv-uppzAp0D~ z2Du&CJ_ZJ49mp~;HYh}4x*`795naCD{YA4K$PQ%R!q^}?U^*aSg{%Wv2F3=N3)2m; F0|0rF5|sb| literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L0_visual_3.stl b/tests/test_data/c1/meshes/visual/L0_visual_3.stl new file mode 100644 index 0000000000000000000000000000000000000000..11691c7d9dcb9cb93055e4cfa4dc1dbf31be2127 GIT binary patch literal 684 zcmZQzpf2D68kZ!~vhPSp`TlKhc{elNYj$$=lR3O;%}b}4r9FohUwMDXb-I*u8QBnE~AnHD>s9rndHEsO)r*E_xR znRWN;YF>?&k`pxOb_u^OraSq9lYWRn;eY_HVYB`@pP`_Zf19@$)E-y*vZq9YZm z16c;yK4g;^7#{P*+IOkv>=Rz{bRV+0$f1ksb7URJGRXEJn}nQ(8ulZbi|kwEP(amz MEQ4$xvPldK0Jc6PuK)l5 literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L0_visual_4.stl b/tests/test_data/c1/meshes/visual/L0_visual_4.stl new file mode 100644 index 0000000000000000000000000000000000000000..a7fbeb22422be1f28caad92ae566630d2e8b4892 GIT binary patch literal 684 zcmZQzpf2D68kZu|vhPSp`F@beF-v<6Exz*pkn40Q=Q6&}hYnR;IW$YM4$i)C=7u8! zLn=_mVW5tc?^kX@R)wrP7HSemHv>bmOp6`R4ttOtk~5qRv2jU*?NHx({7}c569+-& z!q^ZUKs)S!cGv^$FhZ7*hUx~XLN_c`5%!SByK>YC)sAC;e z2h0wT46?b%CNVI8!YLl;GLS!z&4sZ+u7a5a@dvUFWEmJ6WFJg7#8n`(A)vt?CId1H zIYf~|0j3&6H`pV~!0Z6&Mm7oT5Bo%5EG`n7Z;Wg%vTuCpryT7Mq?t1Ni#thdbak43+{*0O&vHk$ z{|mw53=BCtpBSB#5C-c&R)wsa%VEMH?&`TtAd?svEOu3FVl>O#479@@rt0FE8;&q@ zL2Q^wmw;>r28~mXHo4vs1nWSSftd@^fou{3gO-=K{qyP4`#~XuY%YwA?k7|oFgrlL zg|R^{f$0X>$G}kVqIut@=yI?f$hO1SAUj|>AU;Rdfh+@KWBP+lQFi}JDKD@c$i9WK zLFOXc$H0KB16c;f2H64A4e`g*7tMByqs#32A&Pknf1|H6YHHrwl~hwdHl zJ7j+#%@nL-Gf>B7pbmSOj;m*GIKot21hQeeLE;PyU-OsQt&u3&4{|-SDi|ANE=&i= zK2#mZGB7r}eGClpF--gS2F2Ke>_B!EjE(6JWF5#Zfw4jUfa!+#V?uYV{c2YRupP)Q Zfw4h$z;rKk>FQvTfL3&}T(Cxsj16c;98`(aPNem1k?_BqXMl08uoKJOgO|{J=e+pK$__ci&F>78N&~-1VkkLN!{&=^#~5_9Np7KplxtJMQ&y7$e((O*dSJ9ne+w`!X(XdK#_h z6!J>h2_&O+^X7p_k1(eUpdIp1HpqowSAlhK$g*rcUwOnKt=Pi}v)dr-Wi`y6Hm*taM#h^zx<2eK-VOJKUe zE@ZIzEoZ;@n&!S*X%~B#3`hrZ+#sg~1{57AGDt2#Hi>~D>^$3kO%?$=r-fnm$mSx4 YC~`3Vs|8@+`b_t$VlR2xzo}73a5-T9;cT~3l2VDTH%xiWWRy3Q&vhl zBXmr?z0L?(=4JC=r_Hv;PAvbQIw6|`*0B$0$Nt|bbB#izW1T|33WMz^S$FkdV0X3? z$lO98I{?TA*@w`f@A=XYSrxKwcF7M;?QL@%LH02)fXq(<`bC#1V>7bbk}&00LYCxBvhE literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L2_visual_0.stl b/tests/test_data/c1/meshes/visual/L2_visual_0.stl new file mode 100644 index 0000000000000000000000000000000000000000..0a74a0918c85332929e7f23e7e3cc3181cb4712e GIT binary patch literal 884 zcmZQzpe_&q8h0q9e1D-#i`~+;Vq+G)y=ILWmteSMBT6YTb-%7P2a2-3$!gx3l(Ly&k=vZ`!MU$U2bygz66;ppGk0 z9mq1sb|9O?z>o@b5zsIDMMRp6kbR3B2gvrJ>Ohu3HW%3>*D7oHilLAe)P95(5JO@?d6` literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L2_visual_1.stl b/tests/test_data/c1/meshes/visual/L2_visual_1.stl new file mode 100644 index 0000000000000000000000000000000000000000..04b728f9e40ac1b913ff582d906e9ea65ab3e441 GIT binary patch literal 684 zcmZQzpf2D68h0$De1C{ciyg@1qR>Bw{LF3~x^U)(V__@f;g=yFQP}TRRXQ;+90BS` z1nQ6|7B+#Y0?Dk&dw=M_^W}%q(kh&gO=4g;98zwdBh#`EWXGY|OAe_mc@H+%IB@l$ zghZCZAl)!F$ZoJ5_6a~8&l5OJU@{<8$h!Y-nSN+`=zg$$3=FR9yw)MiEH*p;*4rby z3RyR99mq1s{y;W~fguIxB9QMv{(#AV+zYb5~dDh8JHa)RmdhWF!VEq+kJQPwC6k{vLD&E$f1C29|Oa5s16RG4rCc*bCFGA GU;qI4cqg*} literal 0 HcmV?d00001 diff --git a/tests/test_data/c1/meshes/visual/L2_visual_2.stl b/tests/test_data/c1/meshes/visual/L2_visual_2.stl new file mode 100644 index 0000000000000000000000000000000000000000..c00ae9a7f7b616a84b7619e34756ede92737784a GIT binary patch literal 684 zcmZQzpf2D68h1FPe1ED;i{0-ZH#e`zdw=M_^W}%q(kh(J=I;?7#JonhVT2~}hW*IqB8N0`h@k2~mO-`;*(3%A@7q~+SFcCg^G$nY dhiopgZ;?ZRfx!o;;|f# + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +