createVisualShapeArray and clone_body (focus on collision-only now)

- clear INFO_FROM_DATA when disconnect or reset_simulation
- test fixture separated

CHANGELOG.rst

@@ -17,6 +17,7 @@ Unreleased
 - 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 `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`.
 
 **Changed**
 - Apply `HideOutput` to pybullet IK error printouts in `inverse_kinematics_helper`
@@ -30,6 +31,7 @@ Unreleased
 - Fixed `get_extend_fn(q1,q2)` to include `q1` in its output path. This bug was causing incorrect planning result in birrt, because the two trees will have a gap in the middle that is unchecked.
 - Fixed a minor mesh vertex scaling bug in `vertices_from_data`
 - catch `get_num_joints(body) == 0` when a rigid body without joint and link are passed to `clone_body`
+- clear `INFO_FROM_DATA` when `disconnect` or `reset_simulation`
 
 0.5.1
 ----------

pytest.ini

@@ -13,3 +13,9 @@ markers =
     motion_planning_2D
     flying_body
     se3_motion_plan
+    clone_body_single_obj
+    clone_body_urdf
+    cart_plan
+    collision_fn
+    floatting_collision_fn
+    convex_collision

src/pybullet_planning/interfaces/env_manager/shape_creation.py

@@ -377,13 +377,13 @@ def vertices_from_data(data, body=None):
                 mio_mesh = meshio.read(filename)
                 vertices = [np.array(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 as e:
-            raise RuntimeError('Unknown file from data {}'.format(data))
+            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 as e:
+                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)]
@@ -394,30 +394,59 @@ def vertices_from_data(data, body=None):
 
 #####################################
 
-def visual_shape_from_data(data, client=None):
+def visual_shape_from_data(data_list, client=None):
     client = get_client(client)
-    file_name = get_data_filename(data)
-    if (get_data_type(data) == p.GEOM_MESH) and (file_name == UNKNOWN_FILE):
-        LOGGER.warning('Visual shape creation from data fails due to no filename data stored in {}'.format(data))
-        return NULL_ID
 
-    # visualFramePosition: translational offset of the visual shape with respect to the link
-    # visualFrameOrientation: rotational offset (quaternion x,y,z,w) of the visual shape with respect to the link frame
-    #inertial_pose = get_joint_inertial_pose(data.objectUniqueId, data.linkIndex)
-    #point, quat = multiply(invert(inertial_pose), pose)
-    point, quat = get_data_pose(data)
-    return p.createVisualShape(shapeType=data.visualGeometryType,
-                               radius=get_data_radius(data),
-                               halfExtents=np.array(get_data_extents(data))/2,
-                               length=get_data_height(data),  # TODO: pybullet bug
-                               fileName=file_name,
-                               meshScale=get_data_scale(data),
-                               planeNormal=get_data_normal(data),
-                               rgbaColor=data.rgbaColor,
-                               # specularColor=,
-                               visualFramePosition=point,
-                               visualFrameOrientation=quat,
-                               physicsClientId=client)
+    if len(data_list) == 1:
+        data = data_list[0]
+        # ! recover filename and height from data, but load from INFO_FROM_BODY when filename == UNKNOWN_FILE
+        file_name, length = get_data_filename_and_height(data)
+        if (get_data_type(data) == p.GEOM_MESH) and (file_name == UNKNOWN_FILE):
+            LOGGER.warning('Visual shape creation from data fails due to no filename data stored in {}'.format(data))
+            return NULL_ID
+
+        # visualFramePosition: translational offset of the visual shape with respect to the link
+        # visualFrameOrientation: rotational offset (quaternion x,y,z,w) of the visual shape with respect to the link frame
+        #inertial_pose = get_joint_inertial_pose(data.objectUniqueId, data.linkIndex)
+        #point, quat = multiply(invert(inertial_pose), pose)
+        point, quat = get_data_pose(data)
+        return p.createVisualShape(shapeType=data.visualGeometryType,
+                                   radius=get_data_radius(data),
+                                   halfExtents=np.array(get_data_extents(data))/2,
+                                   length=length,  # TODO: pybullet bug
+                                   fileName=file_name,
+                                   meshScale=get_data_scale(data),
+                                   planeNormal=get_data_normal(data),
+                                   rgbaColor=data.rgbaColor,
+                                   # specularColor=,
+                                   visualFramePosition=point,
+                                   visualFrameOrientation=quat,
+                                   physicsClientId=client)
+    else:
+        file_names = []
+        poses = []
+        half_extents = [[], []]
+        for data in data_list:
+            file_name = get_data_filename(data)
+            if (get_data_type(data) == p.GEOM_MESH) and (file_name == UNKNOWN_FILE):
+                LOGGER.warning('Visual shape creation from data fails due to no filename data stored in {}'.format(data))
+                return NULL_ID
+            file_names.append(file_name)
+            poses.append(get_data_pose(data))
+            data_half_extent = np.array(get_data_extents(data)) / 2
+            half_extents[0].append(data_half_extent[0])
+            half_extents[1].append(data_half_extent[1])
+        return p.createVisualShapeArray(shapeTypes=[data.visualGeometryType for data in data_list],
+                                        radii=[get_data_radius(data) for data in data_list],
+                                        halfExtents=half_extents,
+                                        lengths=[get_data_height(data) for data in data_list],
+                                        fileNames=file_names,
+                                        meshScales=[get_data_scale(data) for data in data_list],
+                                        planeNormals=[get_data_normal(data) for data in data_list],
+                                        flags=[p.GEOM_FORCE_CONCAVE_TRIMESH for _ in data_list],
+                                        visualFramePositions=[dpose[0] for dpose in poses],
+                                        visualFrameOrientations=[dpose[1] for dpose in poses],
+                                        physicsClientId=client)
 
 
 def get_visual_data(body, link=BASE_LINK):
@@ -432,8 +461,8 @@ def clone_visual_shape(body, link, client=None):
     visual_data = get_visual_data(body, link)
     if not visual_data:
         return NULL_ID
-    assert (len(visual_data) == 1)
-    return visual_shape_from_data(visual_data[0], client)
+    # assert (len(visual_data) == 1)
+    return visual_shape_from_data(visual_data, client)
 
 #####################################
 
@@ -444,7 +473,8 @@ def collision_shape_from_data(data_list, body, link, client=None):
     client = get_client(client)
     if len(data_list) == 1:
         data = data_list[0]
-        file_name = get_data_filename2(data)
+        # ! 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)
         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))
@@ -457,7 +487,7 @@ def collision_shape_from_data(data_list, body, link, client=None):
                                       radius=get_data_radius(data),
                                       # halfExtents=get_data_extents(data.geometry_type, data.dimensions),
                                       halfExtents=np.array(get_data_extents(data)) / 2,
-                                      height=get_data_height(data),
+                                      height=height,
                                       fileName=file_name,
                                       meshScale=get_data_scale(data),
                                       planeNormal=get_data_normal(data),
@@ -527,20 +557,14 @@ def get_data_filename(data):
     return (data.filename if isinstance(data, CollisionShapeData)
             else data.meshAssetFileName).decode(encoding='UTF-8')
 
-def get_data_filename2(data, body=None):
-    # ! when used together with `get_data_scale`, you should use `get_data_filename` to make sure it's loading
-    # the right scale. When loading nonconvex object, PyBullet creates a temporary obj files for VHAD and then
-    # throw them away. For example, if you load a conconvex mesh with scale=1e-3 and then use this function to
-    # get its filename for getting its vertices, you will load the original mesh, but `get_data_scale` will
-    # give you (1.0, 1.0, 1.0), so scaling of the loaded vertices will be incorrect.
-    # The correct mesh data is only stored in memory and scale unified to one.
-    # See `vertices_from_data` and `pybullet.getMeshData` for more information.
+def get_data_filename_and_height(data, body=None):
+    """load filename and scale from data, if filename is UNKNOWNFILE, we load filename and scale from the cached INFO_FROM_BODY"""
     filename = get_data_filename(data)
     if (get_data_type(data) == p.GEOM_MESH) and (filename == UNKNOWN_FILE):
         info = get_model_info(body or data.objectUniqueId)
         if info is not None and os.path.exists(info.path):
-            filename = info.path
-    return filename
+            return info.path, np.ones(3)*info.scale
+    return filename, get_data_height(data)
 
 def get_data_pose(data):
     """Get the local frame pose of the original shape.

src/pybullet_planning/interfaces/env_manager/simulation.py

@@ -128,6 +128,9 @@ def disconnect():
     # TODO: change CLIENT?
     if CLIENT in CLIENTS:
         del CLIENTS[CLIENT]
+    for k in list(INFO_FROM_BODY.keys()):
+        if k[0] == CLIENT:
+            del INFO_FROM_BODY[k]
     with HideOutput():
         return p.disconnect(physicsClientId=CLIENT)
 
@@ -185,6 +188,9 @@ def reset_simulation():
     """resetSimulation will remove all objects from the world and reset the world to initial conditions.
     """
     p.resetSimulation(physicsClientId=CLIENT)
+    for k in list(INFO_FROM_BODY.keys()):
+        if k[0] == CLIENT:
+            del INFO_FROM_BODY[k]
 
 #####################################
 

src/pybullet_planning/interfaces/robots/body.py

@@ -7,7 +7,7 @@
 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 get_collision_data, clone_collision_shape, clone_visual_shape, get_model_info
+from pybullet_planning.interfaces.env_manager.shape_creation import create_obj, get_collision_data, clone_collision_shape, clone_visual_shape, get_model_info
 
 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, \
@@ -147,6 +147,13 @@ def clone_body(body, links=None, collision=True, visual=True, client=None):
     # localVisualFrame orientation: orientation of local visual frame relative to link/joint frame
     # parentFramePos: joint position in parent frame
     # parentFrameOrn: joint orientation in parent frame
+    # ! the following get around a bug when cloning from an object created from obj file with a non-one scale
+    model_info = get_model_info(body)
+    if model_info is not None and model_info.path.endswith('.obj'):
+        new_body = create_obj(model_info.path, scale=model_info.scale, collision=collision)
+        INFO_FROM_BODY[CLIENT, new_body] = copy(model_info)
+        return new_body
+
     client = get_client(client) # client is the new client for the body
     if links is None or get_num_joints(body) == 0:
         links = get_links(body)
@@ -274,7 +281,7 @@ def get_body_collision_vertices(body):
         # ! 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=body == body_clone)
+        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)
 

tests/conftest.py

@@ -1,8 +1,14 @@
 import pytest
+from fixtures import *
 
 def pytest_addoption(parser):
     parser.addoption('--viewer', action='store_true', help='Enables the pybullet viewer')
+    parser.addoption('--print_debug', action='store_true', help='print out info')
 
 @pytest.fixture
 def viewer(request):
     return request.config.getoption("--viewer")
+
+@pytest.fixture
+def print_debug(request):
+    return request.config.getoption("--print_debug")

tests/fixtures.py

@@ -0,0 +1,23 @@
+import os, pytest
+
+@pytest.fixture
+def robot_path():
+    here = os.path.dirname(__file__)
+    return os.path.join(here, 'test_data', 'universal_robot', 'ur_description', 'urdf', 'ur5.urdf')
+
+@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')
+
+@pytest.fixture
+def clamp_urdf_path():
+    here = os.path.dirname(__file__)
+    return os.path.join(here, 'test_data', 'c1', 'urdf', 'c1.urdf')
+
+@pytest.fixture
+def ee_path():
+    here = os.path.dirname(__file__)
+    return os.path.join(here, 'test_data', 'dms_bar_gripper.obj')
+
+