set_pose fix when cloning obj-generated body

AUTHORS.rst

@@ -4,3 +4,13 @@ Authors
 
 * Caelan Garrett <[email protected]> `@caelan <https://github.com/caelan>`_
 * Yijiang Huang <[email protected]> `@yijiangh <https://github.com/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 <https://github.com/yck011522>`_.
+- The dms gripper mesh model is designed by `@stefanaparascho <https://github.com/stefanaparascho>`_.
+- The `kuka_kr6_r900` URDF model is from `@ros-industrial/kuka_experimental <https://github.com/ros-industrial/kuka_experimental>`_.
+- The `universal_robot` URDF model is from `@ros-industrial/universal_robot <https://github.com/ros-industrial/universal_robot>`_.

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`

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.')
 

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
 

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<https://github.com/caelan/motion-planners/commit/e6f23053e441af091b898b7f56c6fee48223be48>`_
+We are now up-to-date with commit `e6f23053e <https://github.com/caelan/motion-planners/commit/e6f23053e441af091b898b7f56c6fee48223be48>`_
 of ``motion-planners``.
 
 Sampling-based: