add tests for visual/collision_data

.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

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<https://github.com/caelan/motion-planners/commit/e6f23053e441af091b898b7f56c6fee48223be48>`_.
+- 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``)

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

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 .

setup.py

@@ -28,6 +28,7 @@ def read(*names, **kwargs):
     'recordclass',
     'psutil',
     'meshio>=4.3.11',
+    'termcolor',
 ]
 keywords_list = ['robotic planning', 'pybullet']
 

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:

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

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