diff --git a/AUTHORS.rst b/AUTHORS.rst index 0232ac9..31fbace 100644 --- a/AUTHORS.rst +++ b/AUTHORS.rst @@ -4,3 +4,13 @@ Authors * Caelan Garrett `@caelan `_ * Yijiang Huang `@yijiangh `_ + +Acknowledgements +================ + +We thank the authors for the following models that we use in the unit tests. + +- The clamp `c1` URDF model is designed by `@yck011522 `_. +- The dms gripper mesh model is designed by `@stefanaparascho `_. +- The `kuka_kr6_r900` URDF model is from `@ros-industrial/kuka_experimental `_. +- The `universal_robot` URDF model is from `@ros-industrial/universal_robot `_. diff --git a/CHANGELOG.rst b/CHANGELOG.rst index ca685c0..6c30145 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -19,11 +19,11 @@ Unreleased - Added `distance_threshold` to `pairwise_link_collision_info` and `pairwise_link_collision` to allow collision checking given a penetration threshold - Added `sweep_collision_fn` to `birrt` to allow sweep collision check in edge expansion - 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_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`. +- Added `get_body_collision_vertices` for getting body collision vertices in its current configuration. **Changed** - Apply `HideOutput` to pybullet IK error printouts in `inverse_kinematics_helper` diff --git a/src/pybullet_planning/interfaces/debug_utils/debug_utils.py b/src/pybullet_planning/interfaces/debug_utils/debug_utils.py index 6994b45..8366b5c 100644 --- a/src/pybullet_planning/interfaces/debug_utils/debug_utils.py +++ b/src/pybullet_planning/interfaces/debug_utils/debug_utils.py @@ -171,7 +171,7 @@ def draw_collision_diagnosis(pb_closest_pt_output, viz_last_duration=-1, point_c from pybullet_planning.interfaces.env_manager.simulation import has_gui from pybullet_planning.interfaces.env_manager.user_io import wait_for_user, wait_for_duration from pybullet_planning.interfaces.robots.link import get_link_name - from pybullet_planning.interfaces.robots.body import set_color, get_name, clone_body, remove_body + from pybullet_planning.interfaces.robots.body import set_color, get_name, clone_body, remove_body, set_pose if not pb_closest_pt_output: return @@ -276,6 +276,7 @@ def draw_ray_result_diagnosis(ray, ray_result, sweep_body=None, sweep_link=None, set_color(cloned_sweep_body, apply_alpha(RED, 0.2)) handles.append(add_body_name(sweep_body, name=sweep_body_name)) handles.append(draw_link_name(sweep_body, sweep_link, name=sweep_link_name)) + handles.append(add_body_name(b2, name=b2_name)) handles.append(draw_link_name(b2, l2, name=l2_name)) @@ -293,12 +294,12 @@ def draw_ray_result_diagnosis(ray, ray_result, sweep_body=None, sweep_link=None, # * restore lines and colors remove_handles(handles) if sweep_body is not None and sweep_link is not None: - # set_color(sweep_body, apply_alpha(WHITE, 0.5), link=sweep_link) remove_body(cloned_sweep_body) + set_color(sweep_body, apply_alpha(GREY, 1), link=sweep_link) remove_body(cloned_b2) - - # TODO cannot retrieve original color yet + # # TODO cannot retrieve original color yet set_color(b2, apply_alpha(GREY, 1), link=l2) + # else: # wait_for_user('Ray collision diagnosis. Press Enter to continue.') diff --git a/src/pybullet_planning/interfaces/robots/body.py b/src/pybullet_planning/interfaces/robots/body.py index 9cc22bb..38490f8 100644 --- a/src/pybullet_planning/interfaces/robots/body.py +++ b/src/pybullet_planning/interfaces/robots/body.py @@ -156,6 +156,7 @@ def clone_body(body, links=None, collision=True, visual=True, client=None): 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) + set_pose(new_body, get_pose(body)) INFO_FROM_BODY[CLIENT, new_body] = copy(model_info) return new_body diff --git a/src/pybullet_planning/motion_planners/__init__.py b/src/pybullet_planning/motion_planners/__init__.py index 00b10ff..2fc24c3 100644 --- a/src/pybullet_planning/motion_planners/__init__.py +++ b/src/pybullet_planning/motion_planners/__init__.py @@ -10,7 +10,7 @@ to be flexible and independent of simulation platforms. `pybullet_planning` includes this package as a built-in component as it is frequently used. -We are now up-to-date with `commit e6f23053e`_ +We are now up-to-date with commit `e6f23053e `_ of ``motion-planners``. Sampling-based: