catch vertice_from_rigid, final fix (I hope)...

Update pybullet to 3.1.7

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

setup.py

@@ -22,7 +22,7 @@ def read(*names, **kwargs):
 requirements = [
     'numpy',
     'scipy',
-    'pybullet>=3.1.0',
+    'pybullet>=3.1.7',
     'imageio',
     'ghalton',
     'recordclass',

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)]

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):

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()