diff --git a/README.rst b/README.rst index 7c10b23..7dd25df 100644 --- a/README.rst +++ b/README.rst @@ -23,6 +23,10 @@ pybullet_planning :target: https://github.com/yijiangh/pybullet_planning/blob/dev/LICENSE :alt: License MIT +.. image:: https://img.shields.io/badge/pybullet->=3.1.7-ff69b4 + :target: https://github.com/bulletphysics/bullet3 + :alt: pybullet version + .. end-badges .. Write project description diff --git a/setup.py b/setup.py index 84a6810..a112233 100644 --- a/setup.py +++ b/setup.py @@ -22,7 +22,7 @@ def read(*names, **kwargs): requirements = [ 'numpy', 'scipy', - 'pybullet>=3.1.0', + 'pybullet>=3.1.7', 'imageio', 'ghalton', 'recordclass', diff --git a/src/pybullet_planning/interfaces/env_manager/shape_creation.py b/src/pybullet_planning/interfaces/env_manager/shape_creation.py index 1f496d5..a0a582a 100644 --- a/src/pybullet_planning/interfaces/env_manager/shape_creation.py +++ b/src/pybullet_planning/interfaces/env_manager/shape_creation.py @@ -358,17 +358,24 @@ 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) - # 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) - vertices = mesh_data[1] + import meshio + from pybullet_planning.interfaces.geometry.mesh import read_obj + filename, scale = get_data_filename(data), get_data_scale(data) + if filename != UNKNOWN_FILE: + if filename.endswith('.obj'): + mesh = read_obj(filename, decompose=False) + vertices = [[v[i]*scale for i in range(3)] for v in mesh.vertices] + else: + mio_mesh = meshio.read(filename) + vertices = [scale*np.array(vertex) for vertex in mio_mesh.points] + else: + try: + # ! this fails randomly if multiple meshes are attached to the same link + mesh_data = p.getMeshData(data.objectUniqueId, data.linkIndex, collisionShapeIndex=data.objectUniqueId, + flags=p.MESH_DATA_SIMULATION_MESH) + vertices = mesh_data[1] + except p.error: + raise RuntimeError('Unknown file from data {}'.format(data)) # TODO: could compute AABB here for improved speed at the cost of being conservative #elif geometry_type == p.GEOM_PLANE: # parameters = [get_data_extents(data)] diff --git a/src/pybullet_planning/interfaces/robots/body.py b/src/pybullet_planning/interfaces/robots/body.py index c50f82b..fe9c1cd 100644 --- a/src/pybullet_planning/interfaces/robots/body.py +++ b/src/pybullet_planning/interfaces/robots/body.py @@ -263,32 +263,6 @@ 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])) - vertices.extend(mesh_data[1]) - return vertices - def vertices_from_rigid(body, link=BASE_LINK): """get all vertices of given body (collision body) in its local frame. @@ -304,27 +278,24 @@ 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 + import os + from pybullet_planning.interfaces.geometry.mesh import read_obj + from pybullet_planning.interfaces.env_manager import get_model_info # 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 = vertices_from_link2(body, link) - # ! archived method for getting vertices from the mesh_cache - # info = get_model_info(body) - # assert info is not None - # _, ext = os.path.splitext(info.path) - # if ext == '.obj': - # if info.path not in OBJ_MESH_CACHE: - # OBJ_MESH_CACHE[info.path] = read_obj(info.path, decompose=False) - # mesh = OBJ_MESH_CACHE[info.path] - # vertices = [[v[i]*info.scale for i in range(3)] for v in mesh.vertices] - # else: - # raise NotImplementedError(ext) + info = get_model_info(body) + assert info is not None + _, ext = os.path.splitext(info.path) + if ext == '.obj': + if info.path not in OBJ_MESH_CACHE: + OBJ_MESH_CACHE[info.path] = read_obj(info.path, decompose=False) + mesh = OBJ_MESH_CACHE[info.path] + vertices = [[v[i]*info.scale for i in range(3)] for v in mesh.vertices] + else: + raise NotImplementedError(ext) return vertices def approximate_as_prism(body, body_pose=unit_pose(), **kwargs): diff --git a/tests/test_body.py b/tests/test_body.py index 7d638d5..f962657 100644 --- a/tests/test_body.py +++ b/tests/test_body.py @@ -3,6 +3,7 @@ import random import pytest import pybullet as pb +import numpy as np import warnings import pybullet_planning as pp from pybullet_planning import load_pybullet, connect, wait_for_user, LockRenderer, has_gui, WorldSaver, HideOutput, \ @@ -154,76 +155,92 @@ def test_create_body(viewer, file_format): def test_read_obj(viewer): here = os.path.dirname(__file__) path = os.path.join(here, 'test_data', 'box_obstacle.obj') + path2 = os.path.join(here, 'test_data', 'link_4.obj') connect(use_gui=viewer) try: + # obj without `o group_name` mesh = pp.read_obj(path, decompose=False) assert(len(mesh.vertices)==48) assert(len(mesh.faces)==6) meshes = pp.read_obj(path, decompose=True) assert(len(meshes[None].vertices)==48) assert(len(meshes[None].faces)==6) + + # obj with multiple `o group_name` + meshes2 = pp.read_obj(path2, decompose=True) + assert(len(meshes2)==5) + assert(len(meshes2['convex_0'].vertices)==60) + assert(len(meshes2['convex_1'].faces)==124) + assert(len(meshes2['convex_4'].vertices)==27) finally: disconnect() +@pytest.mark.parametrize("urdf_path",[ + os.path.join(os.path.dirname(__file__), 'test_data', 'mit_3-412_workspace', 'urdf', 'mit_3-412_workspace.urdf'), + os.path.join(os.path.dirname(__file__), 'test_data', 'c1', 'urdf', 'c1.urdf'), + os.path.join(os.path.dirname(__file__), 'test_data', 'link_4.obj'), + os.path.join(os.path.dirname(__file__), 'test_data', 'link_4.stl'), +]) @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') +def test_vertices_from_link(viewer, urdf_path): + eps = 1e-6 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) - + if urdf_path.endswith('.urdf'): + body = load_pybullet(urdf_path, fixed_base=False) + else: + body = create_obj(urdf_path) 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) + _, body_links = pp.expand_links(body) + body_name = pp.get_body_name(body) + + vertices_from_link = {} + for attempt in range(3): + for body_link in body_links: + local_from_vertices = pp.vertices_from_rigid(body, body_link) + if attempt == 0: + vertices_from_link[body_link] = local_from_vertices + cprint('#V {} at {} link {}'.format(len(local_from_vertices), body_name, pp.get_link_name(body, body_link)), 'cyan') + + # ! `pybullet.getMeshData` fails randomly if multiple meshes are attached to the same link + # so we can't guarantee it returns the same number of vertices in that case + # if body_name != 'c1': + assert len(vertices_from_link[body_link]) == len(local_from_vertices), \ + 'unequal num of vertics at link {}'.format(pp.get_link_name(body, body_link)) + for v1, v2 in zip(local_from_vertices, vertices_from_link[body_link]): + assert pp.get_distance(v1, v2) < eps + + with LockRenderer(): + for v in local_from_vertices: + pp.draw_point(v, size=0.05) + print('====') + wait_if_gui() + pp.remove_all_debug() - # for v1, v2 in zip(vertices, backwall_mesh.points): - # # assert pp.get_distance(v1, v2) < 1e-6 - # print('{} | {}'.format(v1, v2)) + for attempt in range(3): + set_pose(body, Pose(point=np.random.random(3)*3.0)) + pp.step_simulation() + for body_link in body_links: + local_from_vertices = pp.vertices_from_rigid(body, body_link) + cprint('#V {} at {} link {}'.format(len(local_from_vertices), body_name, + pp.get_link_name(body, body_link)), 'cyan') + + # if body_name != 'c1': + assert len(vertices_from_link[body_link]) == len(local_from_vertices), \ + 'unequal num of vertics at link {}'.format(pp.get_link_name(body, body_link)) + for v1, v2 in zip(local_from_vertices, vertices_from_link[body_link]): + assert pp.get_distance(v1, v2) < eps + + with LockRenderer(): + for v in local_from_vertices: + pp.draw_point(v, size=0.05) + print('====') + wait_if_gui() + pp.remove_all_debug() - wait_if_gui() + pp.disconnect() @pytest.mark.vertices_from_link_geom @@ -247,20 +264,11 @@ def test_vertices_from_link_geometry(viewer): 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 + body_vertices = pp.vertices_from_link(body, pp.BASE_LINK) + assert len(body_vertices) > 0 with LockRenderer(): - for v1 in body_vertices1: + for v1 in body_vertices: 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()