get_body_collision_vertices tested

CHANGELOG.rst

@@ -7,6 +7,11 @@ All notable changes to this project will be documented in this file.
 The format is based on `Keep a Changelog <https://keepachangelog.com/en/1.0.0/>`_
 and this project adheres to `Semantic Versioning <https://semver.org/spec/v2.0.0.html>`_.
 
+Known Unresolved Issues
+-----------------------
+- `get_body_collision_vertices` does not support cloned body now. We don't have a good way to do catch-throw for this now.
+- Cloning body from an already cloned body introduces unexpected result (visual or collision data erased etc).
+
 Unreleased
 ----------
 
@@ -16,15 +21,17 @@ Unreleased
 - Added `coarse_waypoints` to the `smooth_path` function to give options for use refined shortcut to ensure collision-free results.
 - Added `get_body_collision_vertices` for getting body collision vertices in its current configuration.
 - `LOGGER` introduced to replace `print`
-- Added `get_data_filename2` to include cached mesh filename from `get_model_info`
+- Added `get_data_filename_and_height` to include cached mesh filename and scale from `get_model_info`
 - Added `CreateVisualArray` and `CreateCollisionArray` for `visual_shape_from_data` and `visual_shape_from_data`. We use the new `get_data_filename_and_height` when file_name is `UNKNOWN_FILE`.
+- Added `load_pybullet` support for `stl` files by `create_obj`.
 
 **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.
 - `smooth_path`'s `max_iterations` argument changed to `max_smooth_iterations`
 - 'set_color' defaulted to set the color of all the links of body.
+- `vertices_from_data` get filename and **scale** from cached `INFO_FROM_BODY` (through `get_data_filename_and_height`) if the data's filename does not end with `.urdf`
 
 **Fixed**
 - Fixed `read_obj` returns empty dict if obj file does not start with objects (``o object_name``)

src/pybullet_planning/interfaces/env_manager/shape_creation.py

@@ -365,25 +365,32 @@ def vertices_from_data(data, body=None):
         from pybullet_planning.interfaces.geometry.mesh import read_obj
         body_index = body if body is not None else data.objectUniqueId
         filename, scale = get_data_filename(data), get_data_scale(data)
+        # * load scale from cache if exists
+        model_info = get_model_info(body_index)
+        if model_info and not model_info.path.endswith('.urdf'):
+            # ! exception handling when the data is not a link of a body parsed from a URDF file
+            filename = model_info.path
+            scale = model_info.scale
         if filename != UNKNOWN_FILE:
-            # * load scale from cache if exists
-            model_info = get_model_info(body_index)
-            if model_info:
-                scale = np.ones(3) * model_info.scale
+            # LOGGER.debug(f'{filename}')
+            # LOGGER.debug(f'data scale {scale} info_from_body scale {model_info.scale}')
             if filename.endswith('.obj'):
                 mesh = read_obj(filename, decompose=False)
                 vertices = [np.array(scale)*np.array(vertex) for vertex in mesh.vertices]
+                # LOGGER.debug(f'cloned from obj inside vertices_from_data | #verts {len(vertices)}')
             else:
                 mio_mesh = meshio.read(filename)
                 vertices = [np.array(scale)*np.array(vertex) for vertex in mio_mesh.points]
+                # LOGGER.debug(f'cloned from stl inside vertices_from_data | #verts {len(vertices)}')
         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]
+                # LOGGER.debug('cloned from p.getMeshData')
             except p.error as e:
-                raise RuntimeError('Unknown file from data {}'.format(data))
+                raise RuntimeError('Unknown file from data {} | pybullet error {}'.format(data, e))
         # 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)]
@@ -475,6 +482,7 @@ def collision_shape_from_data(data_list, body, link, client=None):
         data = data_list[0]
         # ! recover filename and height from data, but load from INFO_FROM_BODY when filename == UNKNOWN_FILE
         file_name, height = get_data_filename_and_height(data)
+        LOGGER.debug(f'{file_name}, {height}, {get_data_type(data)}')
         if (get_data_type(data) == p.GEOM_MESH) and (file_name == UNKNOWN_FILE):
             LOGGER.warning('Collision shape creation from body #{} fails due to no filename data stored in {}'.format(
                 body, link, data))

src/pybullet_planning/interfaces/env_manager/simulation.py

@@ -209,7 +209,7 @@ def load_pybullet(filename, fixed_base=False, scale=1., **kwargs):
             body = p.loadMJCF(filename, physicsClientId=CLIENT)
         elif filename.endswith('.bullet'):
             body = p.loadBullet(filename, physicsClientId=CLIENT)
-        elif filename.endswith('.obj'):
+        elif filename.endswith('.obj') or filename.endswith('.stl'):
             # TODO: fixed_base => mass = 0?
             body = create_obj(filename, scale=scale, **kwargs)
         else:

src/pybullet_planning/interfaces/robots/body.py

@@ -3,11 +3,16 @@
 from collections import namedtuple
 import pybullet as p
 
+from pybullet_planning.interfaces.debug_utils.debug_utils import draw_pose, draw_point
+from pybullet_planning.interfaces.env_manager.simulation import LockRenderer
+from pybullet_planning.interfaces.env_manager.user_io import wait_for_user
+from pybullet_planning.utils import BLUE
+
 from pybullet_planning.utils import CLIENT, INFO_FROM_BODY, STATIC_MASS, BASE_LINK, NULL_ID, LOGGER, OBJ_MESH_CACHE
 from pybullet_planning.interfaces.env_manager.pose_transformation import Pose, Point, Euler
 from pybullet_planning.interfaces.env_manager.pose_transformation import euler_from_quat, base_values_from_pose, \
-    quat_from_euler, z_rotation, get_pose, set_pose, apply_affine, unit_pose
-from pybullet_planning.interfaces.env_manager.shape_creation import create_obj, get_collision_data, clone_collision_shape, clone_visual_shape, get_model_info
+    quat_from_euler, z_rotation, get_pose, set_pose, apply_affine, unit_pose, multiply, invert
+from pybullet_planning.interfaces.env_manager.shape_creation import create_obj, get_collision_data, clone_collision_shape, clone_visual_shape, get_model_info, get_data_pose
 
 from pybullet_planning.interfaces.robots.dynamics import get_mass, get_dynamics_info, get_local_link_pose
 from pybullet_planning.interfaces.robots.joint import JOINT_TYPES, get_joint_name, get_joint_type, get_num_joints, is_circular, get_joint_limits, is_fixed, \
@@ -267,26 +272,23 @@ def get_body_collision_vertices(body):
     dict
         {link_id : list[point 3d position]}
     """
-    from .collision import expand_links
-    body_vertices_from_link = {}
-    joints = get_movable_joints(body)
-    conf = get_joint_positions(body, joints)
-
-    # cloned body links' positions work more as expected
-    body_clone = clone_body(body, visual=False, collision=True)
-    _, body_links = expand_links(body_clone)
-    set_joint_positions(body_clone, get_movable_joints(body_clone), conf)
+    from .link import get_com_pose
 
+    body_vertices_from_link = {}
+    # joints = get_movable_joints(body)
+    # conf = get_joint_positions(body, joints)
+    # set_joint_positions(body, get_movable_joints(body), conf)
+    body_links = get_links(body)
+    # * get links except for BASE_LINK
     for body_link in body_links:
-        # ! pybullet performs VHACD for stl meshes and delete those temporary meshes,
-        # which left the CollisionShapeData.filename = UNKNOWN_FILENAME
-        # cloned body only has collision shapes, and saves them as visual shapes
-        local_from_vertices = vertices_from_rigid(body_clone, body_link, collision=True) #body == body_clone)
-        world_from_current_pose = get_link_pose(body_clone, body_link)
-        body_vertices_from_link[body_link] = apply_affine(world_from_current_pose, local_from_vertices)
-
-    if body_clone != body:
-        remove_body(body_clone)
+        local_from_vertices = vertices_from_rigid(body, body_link, collision=True)
+        world_from_com = get_com_pose(body, body_link)
+        body_vertices_from_link[body_link] = apply_affine(world_from_com, local_from_vertices)
+    # * base link handling
+    base_local_from_vertices = vertices_from_rigid(body, BASE_LINK, collision=True)
+    world_from_current_pose = get_link_pose(body, BASE_LINK)
+    body_vertices_from_link[BASE_LINK] = apply_affine(world_from_current_pose, base_local_from_vertices)
+
     return body_vertices_from_link
 
 def vertices_from_link(body, link, collision=True):
@@ -336,21 +338,22 @@ def vertices_from_rigid(body, link=BASE_LINK, collision=True):
     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:
-        # ! sometimes pybullet attach random collision shape to the obj-loaded bodies
-        vertices = vertices_from_link(body, link, collision=collision)
-    except RuntimeError as e:
-        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 e
-    return vertices
+    # try:
+    return vertices_from_link(body, link, collision=collision)
+    # except RuntimeError as e:
+    #     # ! this
+    #     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]
+    #         # LOGGER.debug('cloned from obj in vertices_from_rigid')
+    #     else:
+    #         raise e
+    # return vertices
 
 def approximate_as_prism(body, body_pose=unit_pose(), link=BASE_LINK, **kwargs):
     """get the AABB bounding box of a body

src/pybullet_planning/utils/debug_utils.py

@@ -128,7 +128,7 @@ def get_logger(name):
     handler = logging.StreamHandler()
     handler.setFormatter(formatter)
     logger.addHandler(handler)
-    logger.setLevel(logging.DEBUG)
+    logger.setLevel(logging.INFO)
 
     return logger
 

tests/conftest.py

@@ -1,4 +1,8 @@
+import logging
 import pytest
+from pybullet_planning import LOGGER
+LOGGER.setLevel(logging.DEBUG)
+
 from fixtures import *
 
 def pytest_addoption(parser):

tests/fixtures.py

@@ -1,23 +1,57 @@
 import os, pytest
+import pybullet_planning as pp
+from pybullet_planning import apply_alpha
+
+here = os.path.dirname(__file__)
+FILE_FROM_NAME = {
+    'robot' : os.path.join(here, 'test_data', 'universal_robot', 'ur_description', 'urdf', 'ur5.urdf'),
+    'workspace' : os.path.join(here, 'test_data', 'mit_3-412_workspace', 'urdf', 'mit_3-412_workspace.urdf'),
+    'clamp' : os.path.join(here, 'test_data', 'c1', 'urdf', 'c1.urdf'),
+    'gripper' : os.path.join(here, 'test_data', 'dms_bar_gripper.obj'),
+    'duck' : os.path.join(here, 'test_data', 'duck.obj'),
+    'link4_stl' : os.path.join(here, 'test_data', 'link_4.obj'),
+}
 
 @pytest.fixture
 def robot_path():
-    here = os.path.dirname(__file__)
-    return os.path.join(here, 'test_data', 'universal_robot', 'ur_description', 'urdf', 'ur5.urdf')
+    return FILE_FROM_NAME['robot']
 
 @pytest.fixture
 def workspace_path():
-    here = os.path.dirname(__file__)
-    return os.path.join(here, 'test_data', 'mit_3-412_workspace', 'urdf', 'mit_3-412_workspace.urdf')
+    return FILE_FROM_NAME['workspace']
 
 @pytest.fixture
 def clamp_urdf_path():
-    here = os.path.dirname(__file__)
-    return os.path.join(here, 'test_data', 'c1', 'urdf', 'c1.urdf')
+    return FILE_FROM_NAME['clamp']
 
 @pytest.fixture
 def ee_path():
-    here = os.path.dirname(__file__)
-    return os.path.join(here, 'test_data', 'dms_bar_gripper.obj')
+    return FILE_FROM_NAME['gripper']
+
+@pytest.fixture
+def duck_obj_path():
+    return FILE_FROM_NAME['duck']
+
+# @pytest.fixture
+# def abb_path():
+#     return FILE_FROM_NAME['abb']
+
+@pytest.fixture
+def link4_stl_path():
+    return FILE_FROM_NAME['link4_stl']
+
+################################
 
+def load_body_lib():
+    body_lib = {}
+    body_lib['box'] = pp.create_box(1,1,1, color=apply_alpha(pp.RED, 0.2))
+    body_lib['cylinder'] = pp.create_cylinder(0.5, 3, color=apply_alpha(pp.GREEN, 0.2))
+    body_lib['capsule'] = pp.create_capsule(0.5, 3, color=apply_alpha(pp.BLUE, 0.2))
+    body_lib['sphere'] = pp.create_sphere(0.5, color=apply_alpha(pp.TAN, 0.2))
+    body_lib['duck'] = pp.create_obj(FILE_FROM_NAME['duck'], color=apply_alpha(pp.TAN, 0.2), scale=5e-3)
+    body_lib['robot'] = pp.load_pybullet(FILE_FROM_NAME['robot'], fixed_base=True, scale=1.2)
+    body_lib['clamp'] = pp.load_pybullet(FILE_FROM_NAME['clamp'], fixed_base=True, scale=1.5)
+    body_lib['link4_stl'] = pp.load_pybullet(FILE_FROM_NAME['link4_stl'], fixed_base=True, scale=0.8)
+    return body_lib
 
+    # body_lib['abb'] = pp.load_pybullet(FILE_FROM_NAME['abb'], fixed_base=True, scale=1.2)