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 0000000..836bb84 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_0.stl differ diff --git a/tests/test_data/c1/meshes/collision/L0_collision_1.stl b/tests/test_data/c1/meshes/collision/L0_collision_1.stl new file mode 100644 index 0000000..3b6694d Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_1.stl differ 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 0000000..6fd92f2 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_10.stl differ 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 0000000..5ed5f7d Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_11.stl differ 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 0000000..a7d684d Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_12.stl differ diff --git a/tests/test_data/c1/meshes/collision/L0_collision_13.stl b/tests/test_data/c1/meshes/collision/L0_collision_13.stl new file mode 100644 index 0000000..df89f0e Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_13.stl differ 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 0000000..867bbd0 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_14.stl differ 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 0000000..066bc9b Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_15.stl differ diff --git a/tests/test_data/c1/meshes/collision/L0_collision_2.stl b/tests/test_data/c1/meshes/collision/L0_collision_2.stl new file mode 100644 index 0000000..bda1785 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_2.stl differ 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 0000000..11691c7 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_3.stl differ 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 0000000..a7fbeb2 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_4.stl differ diff --git a/tests/test_data/c1/meshes/collision/L0_collision_5.stl b/tests/test_data/c1/meshes/collision/L0_collision_5.stl new file mode 100644 index 0000000..b4f4711 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_5.stl differ diff --git a/tests/test_data/c1/meshes/collision/L0_collision_6.stl b/tests/test_data/c1/meshes/collision/L0_collision_6.stl new file mode 100644 index 0000000..b42feec Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_6.stl differ diff --git a/tests/test_data/c1/meshes/collision/L0_collision_7.stl b/tests/test_data/c1/meshes/collision/L0_collision_7.stl new file mode 100644 index 0000000..c0a098a Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_7.stl differ diff --git a/tests/test_data/c1/meshes/collision/L0_collision_8.stl b/tests/test_data/c1/meshes/collision/L0_collision_8.stl new file mode 100644 index 0000000..c1d1e53 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_8.stl differ diff --git a/tests/test_data/c1/meshes/collision/L0_collision_9.stl b/tests/test_data/c1/meshes/collision/L0_collision_9.stl new file mode 100644 index 0000000..28d4179 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L0_collision_9.stl differ diff --git a/tests/test_data/c1/meshes/collision/L1_collision_0.stl b/tests/test_data/c1/meshes/collision/L1_collision_0.stl new file mode 100644 index 0000000..47ed555 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L1_collision_0.stl differ diff --git a/tests/test_data/c1/meshes/collision/L1_collision_1.stl b/tests/test_data/c1/meshes/collision/L1_collision_1.stl new file mode 100644 index 0000000..ac8ec84 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L1_collision_1.stl differ diff --git a/tests/test_data/c1/meshes/collision/L1_collision_2.stl b/tests/test_data/c1/meshes/collision/L1_collision_2.stl new file mode 100644 index 0000000..7080e5e Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L1_collision_2.stl differ 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 0000000..0a74a09 Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L2_collision_0.stl differ 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 0000000..04b728f Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L2_collision_1.stl differ 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 0000000..c00ae9a Binary files /dev/null and b/tests/test_data/c1/meshes/collision/L2_collision_2.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_0.stl b/tests/test_data/c1/meshes/visual/L0_visual_0.stl new file mode 100644 index 0000000..836bb84 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_0.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_1.stl b/tests/test_data/c1/meshes/visual/L0_visual_1.stl new file mode 100644 index 0000000..3b6694d Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_1.stl differ 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 0000000..6fd92f2 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_10.stl differ 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 0000000..5ed5f7d Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_11.stl differ 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 0000000..a7d684d Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_12.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_13.stl b/tests/test_data/c1/meshes/visual/L0_visual_13.stl new file mode 100644 index 0000000..df89f0e Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_13.stl differ 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 0000000..867bbd0 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_14.stl differ 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 0000000..066bc9b Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_15.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_2.stl b/tests/test_data/c1/meshes/visual/L0_visual_2.stl new file mode 100644 index 0000000..bda1785 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_2.stl differ 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 0000000..11691c7 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_3.stl differ 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 0000000..a7fbeb2 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_4.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_5.stl b/tests/test_data/c1/meshes/visual/L0_visual_5.stl new file mode 100644 index 0000000..b4f4711 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_5.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_6.stl b/tests/test_data/c1/meshes/visual/L0_visual_6.stl new file mode 100644 index 0000000..b42feec Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_6.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_7.stl b/tests/test_data/c1/meshes/visual/L0_visual_7.stl new file mode 100644 index 0000000..c0a098a Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_7.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_8.stl b/tests/test_data/c1/meshes/visual/L0_visual_8.stl new file mode 100644 index 0000000..c1d1e53 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_8.stl differ diff --git a/tests/test_data/c1/meshes/visual/L0_visual_9.stl b/tests/test_data/c1/meshes/visual/L0_visual_9.stl new file mode 100644 index 0000000..28d4179 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L0_visual_9.stl differ diff --git a/tests/test_data/c1/meshes/visual/L1_visual_0.stl b/tests/test_data/c1/meshes/visual/L1_visual_0.stl new file mode 100644 index 0000000..47ed555 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L1_visual_0.stl differ diff --git a/tests/test_data/c1/meshes/visual/L1_visual_1.stl b/tests/test_data/c1/meshes/visual/L1_visual_1.stl new file mode 100644 index 0000000..ac8ec84 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L1_visual_1.stl differ diff --git a/tests/test_data/c1/meshes/visual/L1_visual_2.stl b/tests/test_data/c1/meshes/visual/L1_visual_2.stl new file mode 100644 index 0000000..7080e5e Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L1_visual_2.stl differ 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 0000000..0a74a09 Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L2_visual_0.stl differ 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 0000000..04b728f Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L2_visual_1.stl differ 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 0000000..c00ae9a Binary files /dev/null and b/tests/test_data/c1/meshes/visual/L2_visual_2.stl differ diff --git a/tests/test_data/c1/urdf/c1.urdf b/tests/test_data/c1/urdf/c1.urdf new file mode 100644 index 0000000..0284b79 --- /dev/null +++ b/tests/test_data/c1/urdf/c1.urdf @@ -0,0 +1,309 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +