From 4fc320022f0b0586c91579bb89dd4da1f9c39f22 Mon Sep 17 00:00:00 2001 From: Yijiang Huang Date: Mon, 7 Mar 2022 16:34:23 -0500 Subject: [PATCH] createVisualShapeArray and clone_body (focus on collision-only now) - clear INFO_FROM_DATA when disconnect or reset_simulation - test fixture separated --- CHANGELOG.rst | 2 + pytest.ini | 6 + .../interfaces/env_manager/shape_creation.py | 110 ++++--- .../interfaces/env_manager/simulation.py | 6 + .../interfaces/robots/body.py | 11 +- tests/conftest.py | 6 + tests/fixtures.py | 23 ++ tests/test_body.py | 268 ------------------ tests/test_clone_body.py | 153 ++++++++++ tests/test_vertices.py | 123 ++++++++ 10 files changed, 395 insertions(+), 313 deletions(-) create mode 100644 tests/fixtures.py create mode 100644 tests/test_clone_body.py create mode 100644 tests/test_vertices.py diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 5dc3d14..d592971 100644 --- a/CHANGELOG.rst +++ b/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 ---------- diff --git a/pytest.ini b/pytest.ini index a09f19e..38b6e8d 100644 --- a/pytest.ini +++ b/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 diff --git a/src/pybullet_planning/interfaces/env_manager/shape_creation.py b/src/pybullet_planning/interfaces/env_manager/shape_creation.py index 4edb795..128174a 100644 --- a/src/pybullet_planning/interfaces/env_manager/shape_creation.py +++ b/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. diff --git a/src/pybullet_planning/interfaces/env_manager/simulation.py b/src/pybullet_planning/interfaces/env_manager/simulation.py index d5228fd..1a997c0 100644 --- a/src/pybullet_planning/interfaces/env_manager/simulation.py +++ b/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] ##################################### diff --git a/src/pybullet_planning/interfaces/robots/body.py b/src/pybullet_planning/interfaces/robots/body.py index 1dbab96..d917c63 100644 --- a/src/pybullet_planning/interfaces/robots/body.py +++ b/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) diff --git a/tests/conftest.py b/tests/conftest.py index 2680e62..0a1acfd 100644 --- a/tests/conftest.py +++ b/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") diff --git a/tests/fixtures.py b/tests/fixtures.py new file mode 100644 index 0000000..4b8602e --- /dev/null +++ b/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') + + diff --git a/tests/test_body.py b/tests/test_body.py index 59a5997..5863e29 100644 --- a/tests/test_body.py +++ b/tests/test_body.py @@ -4,7 +4,6 @@ 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, \ reset_simulation, disconnect, set_camera_pose, has_gui, wait_if_gui, apply_alpha @@ -17,27 +16,6 @@ from termcolor import cprint from pybullet_planning import LOGGER - -@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') - def test_create_ee_link(viewer, robot_path, ee_path): connect(use_gui=viewer) with HideOutput(): @@ -96,136 +74,6 @@ def test_moving_links_joints(viewer, robot_path, workspace_path): if has_gui() : wait_for_user() -def inspect_data(body, original_aabb=None, collision_empty=False, visual_empty=False): - links = pp.get_all_links(body) - for i, link in enumerate(links): - collision_data = pp.get_collision_data(body, link) - visual_data = pp.get_visual_data(body, link) - LOGGER.debug(f'Link {i}') - LOGGER.debug('collsion: {}'.format(collision_data)) - LOGGER.debug('visual: {}'.format(visual_data)) - if collision_empty: - assert len(collision_data) == 0 - if visual_empty: - assert len(visual_data) == 0 - wait_if_gui() - if original_aabb: - orig_extent = pp.get_aabb_extent(original_aabb) - new_extent = pp.get_aabb_extent(pp.get_aabb(body)) - pp.draw_aabb(original_aabb) - pp.draw_aabb(pp.get_aabb(body)) - # ! pybullet's AABB is a bit noisy sometimes, with a safety margin added - assert np.allclose(new_extent, orig_extent, atol=np.max(orig_extent)/10) - -@pytest.mark.clone_body_single_obj -@pytest.mark.parametrize("body_name",[ - 'obj', - 'box', - ]) -def test_clone_body_single_obj(viewer, body_name, ee_path): - connect(use_gui=viewer) - with HideOutput(): - if body_name == 'obj': - body = create_obj(ee_path, scale=5.0) - elif body_name == 'box': - body = create_box(0.2, 0.2, 0.2) - set_color(body, pp.GREY) - - LOGGER.info('original body') - original_aabb = pp.get_aabb(body) - inspect_data(body, original_aabb) - - spacing = 0.5 - cloned_body = clone_body(body) - set_color(cloned_body, pp.GREEN) - set_pose(cloned_body, Pose(point=[0, 0, spacing])) - LOGGER.info('both cloned body') - inspect_data(cloned_body, original_aabb) - - collision_body = clone_body(body, visual=False) - set_color(collision_body, pp.RED) - set_pose(collision_body, Pose(point=[0, 1*spacing, 0])) - LOGGER.info('collision cloned body') - inspect_data(collision_body, original_aabb) - - visual_body = clone_body(body, collision=False) - set_pose(visual_body, Pose(point=[0, 2*spacing, 0])) - set_color(visual_body, pp.BLUE) - LOGGER.info('visual cloned body') - inspect_data(visual_body) - - double_collision_body = clone_body(collision_body) - set_pose(double_collision_body, Pose(point=[1*spacing, 1*spacing, 0])) - set_color(double_collision_body, pp.YELLOW) - LOGGER.info('double collision cloned body') - inspect_data(double_collision_body, original_aabb) - - double_visual_body = clone_body(visual_body) - set_pose(double_visual_body, Pose(point=[1*spacing, 2*spacing, 0])) - set_color(double_visual_body, pp.BROWN) - LOGGER.info('double visual cloned body') - inspect_data(double_visual_body) - - pp.disconnect() - -@pytest.mark.clone_body_urdf -@pytest.mark.parametrize("body_name",[ - 'workspace', - 'robot', - # 'urdf_link_multi_mesh', - ]) -def test_clone_body_urdf(viewer, body_name, robot_path, clamp_urdf_path, workspace_path): - connect(use_gui=viewer) - with HideOutput(): - if body_name == 'workspace': - body = load_pybullet(workspace_path, fixed_base=True, scale=0.5) - elif body_name == 'robot': - body = load_pybullet(robot_path, fixed_base=True) - elif body_name == 'urdf_link_multi_mesh': - body = load_pybullet(clamp_urdf_path, fixed_base=True) - set_color(body, pp.GREY) - - # ! PyBullet does not return any VisualShapeData at all - LOGGER.info('original body') - original_aabb = pp.get_aabb(body) - inspect_data(body, original_aabb, visual_empty=True) - - spacing = 1.5 - # CollisionShapeData's mesh filename will be turned into `unknown file` - # with VisualShapeData pointing to collision mesh filename. - cloned_body = clone_body(body) - set_color(cloned_body, pp.GREEN) - set_pose(cloned_body, Pose(point=[0, 0, spacing])) - LOGGER.info('both cloned body') - inspect_data(cloned_body, original_aabb) - - # ! both collision and visual data will be cloned, behavior same as with above, - # when `collision=True, visual=True` - collision_body = clone_body(body, visual=False) - set_color(collision_body, pp.RED) - set_pose(collision_body, Pose(point=[0, 1*spacing, 0])) - LOGGER.info('collision cloned body') - inspect_data(collision_body, original_aabb) - - # ! both collison and visual data are empty here - visual_body = clone_body(body, collision=False) - set_pose(visual_body, Pose(point=[0, 2*spacing, 0])) - set_color(visual_body, pp.BLUE) - LOGGER.info('visual cloned body') - inspect_data(visual_body, visual_empty=True, collision_empty=True) - - # ! Double cloning will fail because CollisionShapeData's filename is erased to `unknown file` - with pytest.raises(pb.error) as excinfo: - double_collision_body = clone_body(collision_body) - assert 'createCollisionShape failed' in str(excinfo) - - # ! both collison and visual data are empty here - double_visual_body = clone_body(visual_body) - inspect_data(double_visual_body, visual_empty=True, collision_empty=True) - - pp.disconnect() - - @pytest.mark.create_body @pytest.mark.parametrize("file_format",[ 'obj', @@ -272,122 +120,6 @@ def test_read_obj(viewer): 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_rigid -def test_vertices_from_link(viewer, urdf_path): - eps = 1e-6 - connect(use_gui=viewer) - with HideOutput(): - if urdf_path.endswith('.urdf'): - body = load_pybullet(urdf_path, fixed_base=False) - else: - body = create_obj(urdf_path) - # wait_if_gui() - - # _, body_links = pp.expand_links(body) - body_links = pp.get_all_links(body) - LOGGER.info(f'body links {body_links}') - body_name = pp.get_body_name(body) - - vertices_from_links = {} - for attempt in range(2): - # LOGGER.debug(f'-- attempt {attempt}') - for body_link in body_links: - # LOGGER.debug(f'\tlink {body_link}') - local_from_vertices = pp.vertices_from_rigid(body, body_link) - # assert len(local_from_vertices) > 0 - if attempt == 0: - vertices_from_links[body_link] = local_from_vertices - # LOGGER.debug('#V {} at {} link {}'.format(len(local_from_vertices), body_name, pp.get_link_name(body, body_link))) - - assert len(vertices_from_links[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_links[body_link]): - assert pp.get_distance(v1, v2) < eps - - with LockRenderer(): - for v in local_from_vertices: - pp.draw_point(v, size=0.05) - # wait_if_gui() - pp.remove_all_debug() - - for attempt in range(3): - # LOGGER.debug(f'-- attempt {attempt}') - cloned_body = pp.clone_body(body) - cloned_body_links = pp.get_all_links(cloned_body) - set_pose(cloned_body, Pose(point=np.random.random(3)*3.0)) - pp.set_color(cloned_body, pp.YELLOW) - wait_if_gui('Cloned body') - for body_link in cloned_body_links: - # LOGGER.debug(f'\tlink {body_link}') - local_from_vertices = pp.vertices_from_rigid(cloned_body, body_link) - LOGGER.debug('#V {} at {} link {}'.format(len(local_from_vertices), body_name, pp.get_link_name(cloned_body, body_link))) - - assert len(vertices_from_links[body_link]) == len(local_from_vertices), \ - 'unequal num of vertics at link {}'.format(pp.get_link_name(cloned_body, body_link)) - for v1, v2 in zip(local_from_vertices, vertices_from_links[body_link]): - assert pp.get_distance(v1, v2) < eps - - with LockRenderer(): - for v in local_from_vertices: - pp.draw_point(v, size=0.05) - wait_if_gui() - pp.remove_all_debug() - - pp.disconnect() - - -@pytest.mark.vertices_from_link_geom -def test_vertices_from_link_geometry(viewer, robot_path, clamp_urdf_path): - here = os.path.dirname(__file__) - mesh_path = os.path.join(here, 'test_data', 'duck.obj') - connect(use_gui=viewer) - with HideOutput(): - bodies = {} - bodies['box'] = pp.create_box(1,1,1, color=apply_alpha(pp.RED, 0.2)) - bodies['cylinder'] = pp.create_cylinder(0.5, 3, color=apply_alpha(pp.GREEN, 0.2)) - bodies['capsule'] = pp.create_capsule(0.5, 3, color=apply_alpha(pp.BLUE, 0.2)) - bodies['sphere'] = pp.create_sphere(0.5, color=apply_alpha(pp.TAN, 0.2)) - bodies['duck'] = pp.create_obj(mesh_path, color=apply_alpha(pp.TAN, 0.2), scale=5e-3) - bodies['robot'] = load_pybullet(robot_path, fixed_base=True, scale=1.2) - bodies['clamp'] = load_pybullet(clamp_urdf_path, fixed_base=True, scale=1.5) - - count = 0 - for name, orig_body in bodies.items(): - LOGGER.debug(name) - count += 1 - for i, body in enumerate([orig_body, clone_body(orig_body)]): - set_pose(body, Pose(point=[i*2,count*2,0])) - body_vertices_from_link = pp.get_body_collision_vertices(body) - body_aabb = pp.get_aabb(body) - pp.draw_aabb(body_aabb) - if name not in ['robot', 'clamp']: - assert len(body_vertices_from_link) > 0 - with LockRenderer(): - for link, verts in body_vertices_from_link.items(): - for v in verts: - # * make sure mesh vertices are loaded in the right scale - assert pp.aabb_contains_point(v, body_aabb) - pp.draw_point(v, color=pp.BLUE, size=0.1) - if verts: - orig_extent = pp.get_aabb_extent(body_aabb) - point_aabb = pp.aabb_from_points(verts) - new_extent = pp.get_aabb_extent(point_aabb) - pp.draw_aabb(point_aabb) - # ! pybullet's AABB is a bit noisy sometimes, with a safety margin added - print(np.abs(new_extent-orig_extent)) - # if name != 'capsule': - # # ! capsule's vertices do not contain the cap part - # assert np.allclose(new_extent, orig_extent, atol=np.max(orig_extent)/10) - wait_if_gui() - pp.remove_all_debug() - @pytest.mark.geom_data_index def test_geom_data_index(viewer): verbose = True diff --git a/tests/test_clone_body.py b/tests/test_clone_body.py new file mode 100644 index 0000000..491d1f2 --- /dev/null +++ b/tests/test_clone_body.py @@ -0,0 +1,153 @@ +import os, pytest +import numpy as np +import pybullet_planning as pp +from pybullet_planning import LOGGER, wait_if_gui +from pybullet_planning import load_pybullet, connect, LockRenderer, has_gui, WorldSaver, HideOutput, \ + reset_simulation, disconnect, set_camera_pose, has_gui, wait_if_gui, apply_alpha +from pybullet_planning import create_obj, create_attachment, Attachment +from pybullet_planning import link_from_name, get_link_pose, get_moving_links, get_link_name +from pybullet_planning import get_num_joints, get_joint_names, get_movable_joints +from pybullet_planning import dump_world, set_pose, set_color +from pybullet_planning import clone_body, create_box, read_obj +from pybullet_planning import Pose + +def inspect_data(body, original_aabb=None, collision_empty=False, visual_empty=False, verbose=False): + links = pp.get_all_links(body) + for i, link in enumerate(links): + collision_data = pp.get_collision_data(body, link) + visual_data = pp.get_visual_data(body, link) + if verbose: + LOGGER.debug(f'Link {i}') + LOGGER.debug('collsion: {}'.format(collision_data)) + LOGGER.debug('visual: {}'.format(visual_data)) + if collision_empty: + assert len(collision_data) == 0 + if visual_empty: + assert len(visual_data) == 0 + if original_aabb: + orig_extent = pp.get_aabb_extent(original_aabb) + new_extent = pp.get_aabb_extent(pp.get_aabb(body)) + pp.draw_aabb(original_aabb) + pp.draw_aabb(pp.get_aabb(body)) + wait_if_gui() + # ! pybullet's AABB is a bit noisy sometimes, with a safety margin added + assert np.allclose(new_extent, orig_extent, atol=np.max(orig_extent)/10) + +@pytest.mark.clone_body_single_obj +@pytest.mark.parametrize("body_name",[ + 'obj', + 'box', + ]) +def test_clone_body_single_obj(viewer, body_name, ee_path, print_debug): + connect(use_gui=viewer) + with HideOutput(): + if body_name == 'obj': + body = create_obj(ee_path, scale=5.0) + elif body_name == 'box': + body = create_box(0.2, 0.2, 0.2) + set_color(body, pp.GREY) + + LOGGER.info('original body') + original_aabb = pp.get_aabb(body) + inspect_data(body, original_aabb) + + spacing = 0.5 + cloned_body = clone_body(body) + set_color(cloned_body, pp.apply_alpha(pp.GREEN, 0.2)) + set_pose(cloned_body, Pose(point=[0, 0, spacing])) + LOGGER.info('both cloned body') + inspect_data(cloned_body, original_aabb, verbose=print_debug) + + collision_body = clone_body(body, visual=False) + set_color(collision_body, pp.RED) + set_pose(collision_body, Pose(point=[0, 1*spacing, 0])) + LOGGER.info('collision cloned body') + inspect_data(collision_body, original_aabb, verbose=print_debug) + + # TODO fix these + # visual_body = clone_body(body, collision=False) + # set_pose(visual_body, Pose(point=[0, 2*spacing, 0])) + # set_color(visual_body, pp.BLUE) + # LOGGER.info('visual cloned body') + # inspect_data(visual_body) + + # double_collision_body = clone_body(collision_body) + # set_pose(double_collision_body, Pose(point=[1*spacing, 1*spacing, 0])) + # set_color(double_collision_body, pp.YELLOW) + # LOGGER.info('double collision cloned body') + # inspect_data(double_collision_body, original_aabb) + + # double_visual_body = clone_body(visual_body) + # set_pose(double_visual_body, Pose(point=[1*spacing, 2*spacing, 0])) + # set_color(double_visual_body, pp.BROWN) + # LOGGER.info('double visual cloned body') + # inspect_data(double_visual_body) + + pp.reset_simulation() + pp.disconnect() + +@pytest.mark.clone_body_urdf +@pytest.mark.parametrize("body_name",[ + 'workspace', + 'robot', + 'urdf_link_multi_mesh', + ]) +def test_clone_body_urdf(viewer, body_name, robot_path, clamp_urdf_path, workspace_path, print_debug): + connect(use_gui=viewer) + with HideOutput(): + if body_name == 'workspace': + body = load_pybullet(workspace_path, fixed_base=True, scale=0.1) + elif body_name == 'robot': + body = load_pybullet(robot_path, fixed_base=True, scale=0.2) + elif body_name == 'urdf_link_multi_mesh': + body = load_pybullet(clamp_urdf_path, fixed_base=True) + set_color(body, pp.GREY) + + # TODO PyBullet does not return any VisualShapeData at all when viewer is on + LOGGER.info('original body') + original_aabb = pp.get_aabb(body) + inspect_data(body, original_aabb, verbose=print_debug) + + spacing = 1.5 + # CollisionShapeData's mesh filename will be turned into `unknown file` + # with VisualShapeData pointing to collision mesh filename. + LOGGER.info('both cloned body') + cloned_body = clone_body(body) + set_color(cloned_body, pp.GREEN) + set_pose(cloned_body, Pose(point=[0, 0, spacing])) + inspect_data(cloned_body, original_aabb, verbose=print_debug) + + # ! both collision and visual data will be cloned, behavior same as with above, + # when `collision=True, visual=True` + LOGGER.info('collision cloned body') + collision_body = clone_body(body, visual=False) + set_color(collision_body, pp.RED) + set_pose(collision_body, Pose(point=[0, 1*spacing, 0])) + inspect_data(collision_body, original_aabb, verbose=print_debug) + + # TODO fix these + # # ! both collison and visual data are empty here + # LOGGER.info('visual cloned body') + # visual_body = clone_body(body, collision=False) + # set_pose(visual_body, Pose(point=[0, 2*spacing, 0])) + # set_color(visual_body, pp.BLUE) + # inspect_data(visual_body, visual_empty=has_gui(), collision_empty=True) + + # # ! Double cloning will fail because CollisionShapeData's filename is erased to `unknown file` + # LOGGER.info('double collision cloned body') + # # with pytest.raises(pb.error) as excinfo: + # double_collision_body = clone_body(collision_body) + # # assert 'createCollisionShape failed' in str(excinfo) + # set_pose(double_collision_body, Pose(point=[1*spacing, 1*spacing, 0])) + # set_color(double_collision_body, pp.YELLOW) + # inspect_data(double_collision_body, collision_empty=pp.has_gui()) + + # # ! both collison and visual data are empty here + # LOGGER.info('double visual cloned body') + # double_visual_body = clone_body(visual_body) + # set_pose(double_visual_body, Pose(point=[1*spacing, 2*spacing, 0])) + # set_color(double_visual_body, pp.BROWN) + # inspect_data(double_visual_body, visual_empty=has_gui(), collision_empty=True) + + pp.reset_simulation() + pp.disconnect() diff --git a/tests/test_vertices.py b/tests/test_vertices.py new file mode 100644 index 0000000..4296cbb --- /dev/null +++ b/tests/test_vertices.py @@ -0,0 +1,123 @@ +import os, pytest +import numpy as np +import pybullet_planning as pp +from pybullet_planning import LOGGER, wait_if_gui +from pybullet_planning import load_pybullet, connect, LockRenderer, has_gui, WorldSaver, HideOutput, \ + reset_simulation, disconnect, set_camera_pose, has_gui, wait_if_gui, apply_alpha +from pybullet_planning import create_obj, create_attachment, Attachment +from pybullet_planning import link_from_name, get_link_pose, get_moving_links, get_link_name +from pybullet_planning import get_num_joints, get_joint_names, get_movable_joints +from pybullet_planning import dump_world, set_pose, set_color +from pybullet_planning import clone_body, create_box, read_obj +from pybullet_planning import Pose + +@pytest.mark.vertices_from_link_geom +def test_vertices_from_link_geometry(viewer, robot_path, clamp_urdf_path): + here = os.path.dirname(__file__) + mesh_path = os.path.join(here, 'test_data', 'duck.obj') + connect(use_gui=viewer) + with HideOutput(): + bodies = {} + # bodies['box'] = pp.create_box(1,1,1, color=apply_alpha(pp.RED, 0.2)) + # bodies['cylinder'] = pp.create_cylinder(0.5, 3, color=apply_alpha(pp.GREEN, 0.2)) + # bodies['capsule'] = pp.create_capsule(0.5, 3, color=apply_alpha(pp.BLUE, 0.2)) + # bodies['sphere'] = pp.create_sphere(0.5, color=apply_alpha(pp.TAN, 0.2)) + bodies['duck'] = pp.create_obj(mesh_path, color=apply_alpha(pp.TAN, 0.2), scale=5e-3) + bodies['robot'] = load_pybullet(robot_path, fixed_base=True, scale=1.2) + bodies['clamp'] = load_pybullet(clamp_urdf_path, fixed_base=True, scale=1.5) + + count = 0 + for name, orig_body in bodies.items(): + LOGGER.debug(name) + count += 1 + for i, body in enumerate([orig_body, clone_body(orig_body)]): + set_pose(body, Pose(point=[i*2,count*2,0])) + body_vertices_from_link = pp.get_body_collision_vertices(body) + body_aabb = pp.get_aabb(body) + pp.draw_aabb(body_aabb) + if name not in ['robot', 'clamp']: + assert len(body_vertices_from_link) > 0 + with LockRenderer(): + for link, verts in body_vertices_from_link.items(): + for v in verts: + # * make sure mesh vertices are loaded in the right scale + # assert pp.aabb_contains_point(v, body_aabb) + pp.draw_point(v, color=pp.BLUE, size=0.1) + if verts: + orig_extent = pp.get_aabb_extent(body_aabb) + point_aabb = pp.aabb_from_points(verts) + new_extent = pp.get_aabb_extent(point_aabb) + pp.draw_aabb(point_aabb) + # ! pybullet's AABB is a bit noisy sometimes, with a safety margin added + print(np.abs(new_extent-orig_extent)) + if name != 'capsule': + # ! capsule's vertices do not contain the cap part + assert np.allclose(new_extent, orig_extent, atol=np.max(orig_extent)/10) + wait_if_gui() + pp.remove_all_debug() + +@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_rigid +def test_vertices_from_link(viewer, urdf_path): + eps = 1e-6 + connect(use_gui=viewer) + with HideOutput(): + body = load_pybullet(urdf_path, fixed_base=False) + # wait_if_gui() + + # _, body_links = pp.expand_links(body) + body_links = pp.get_all_links(body) + LOGGER.info(f'body links {body_links}') + body_name = pp.get_body_name(body) + + vertices_from_links = {} + for attempt in range(2): + # LOGGER.debug(f'-- attempt {attempt}') + for body_link in body_links: + # LOGGER.debug(f'\tlink {body_link}') + local_from_vertices = pp.vertices_from_rigid(body, body_link) + # assert len(local_from_vertices) > 0 + if attempt == 0: + vertices_from_links[body_link] = local_from_vertices + # LOGGER.debug('#V {} at {} link {}'.format(len(local_from_vertices), body_name, pp.get_link_name(body, body_link))) + + assert len(vertices_from_links[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_links[body_link]): + assert pp.get_distance(v1, v2) < eps + + with LockRenderer(): + for v in local_from_vertices: + pp.draw_point(v, size=0.05) + # wait_if_gui() + pp.remove_all_debug() + + for attempt in range(3): + # LOGGER.debug(f'-- attempt {attempt}') + cloned_body = pp.clone_body(body) + cloned_body_links = pp.get_all_links(cloned_body) + set_pose(cloned_body, Pose(point=np.random.random(3)*3.0)) + pp.set_color(cloned_body, pp.YELLOW) + wait_if_gui('Cloned body') + for body_link in cloned_body_links: + # LOGGER.debug(f'\tlink {body_link}') + local_from_vertices = pp.vertices_from_rigid(cloned_body, body_link) + LOGGER.debug('#V {} at {} link {}'.format(len(local_from_vertices), body_name, pp.get_link_name(cloned_body, body_link))) + + assert len(vertices_from_links[body_link]) == len(local_from_vertices), \ + 'unequal num of vertics at link {}'.format(pp.get_link_name(cloned_body, body_link)) + for v1, v2 in zip(local_from_vertices, vertices_from_links[body_link]): + assert pp.get_distance(v1, v2) < eps + + with LockRenderer(): + for v in local_from_vertices: + pp.draw_point(v, size=0.05) + wait_if_gui() + pp.remove_all_debug() + + pp.disconnect()