From 32a3df5ca72e048a25da335f9a49a8c87a182b56 Mon Sep 17 00:00:00 2001 From: Mikael Dallaire Cote <110583667+0mdc@users.noreply.github.com> Date: Thu, 28 Mar 2024 14:37:31 -0400 Subject: [PATCH 1/3] Place objects back to their original position when placement fails. --- examples/hitl/rearrange_v2/rearrange_v2.py | 24 +++++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/examples/hitl/rearrange_v2/rearrange_v2.py b/examples/hitl/rearrange_v2/rearrange_v2.py index 01015985c6..687a8c6181 100644 --- a/examples/hitl/rearrange_v2/rearrange_v2.py +++ b/examples/hitl/rearrange_v2/rearrange_v2.py @@ -5,7 +5,7 @@ # LICENSE file in the root directory of this source tree. -from typing import Any, Dict, Set +from typing import Any, Dict, Optional, Set import hydra import magnum as mn @@ -58,6 +58,8 @@ def __init__(self, app_service): self._recent_reach_pos = None self._paused = False self._hide_gui_text = False + self._can_place_object = False + self._original_object_position: Optional[mn.Vector3] = None self._camera_helper = CameraHelper( self._app_service.hitl_config, @@ -219,8 +221,15 @@ def _update_grasping_and_set_act_hints(self, user_index): user_index ) # self._gui_agent_controllers.get_base_translation() else: - # GuiPlacementHelper has already placed this object, so nothing to do here - pass + # GuiPlacementHelper has already placed this object. + # Move object back to original position if it could not be placed. + if not self._can_place_object: + query_obj = ( + self.get_sim() + .get_rigid_object_manager() + .get_object_by_id(self._held_obj_id) + ) + query_obj.translation = self._original_object_position self._held_obj_id = None else: query_pos = self._get_gui_agent_translation(user_index) @@ -232,6 +241,12 @@ def _update_grasping_and_set_act_hints(self, user_index): if DO_HUMANOID_GRASP_OBJECTS: grasp_object_id = obj_id self._held_obj_id = obj_id + query_obj = ( + self.get_sim() + .get_rigid_object_manager() + .get_object_by_id(self._held_obj_id) + ) + self._original_object_position = query_obj.translation else: self._has_grasp_preview = True @@ -367,6 +382,9 @@ def _update_held_object_placement(self): if self._placement_helper.update(ray, self._held_obj_id): # sloppy: save another keyframe here since we just moved the held object self.get_sim().gfx_replay_manager.save_keyframe() + self._can_place_object = True + else: + self._can_place_object = False def sim_update(self, dt, post_sim_update_dict): if ( From e5af4235b84529001e365ba725662126d1202994 Mon Sep 17 00:00:00 2001 From: Mikael Dallaire Cote <110583667+0mdc@users.noreply.github.com> Date: Thu, 28 Mar 2024 14:50:49 -0400 Subject: [PATCH 2/3] Cancel placement if not possible. --- examples/hitl/rearrange_v2/rearrange_v2.py | 20 +++----------------- 1 file changed, 3 insertions(+), 17 deletions(-) diff --git a/examples/hitl/rearrange_v2/rearrange_v2.py b/examples/hitl/rearrange_v2/rearrange_v2.py index 687a8c6181..e395b70482 100644 --- a/examples/hitl/rearrange_v2/rearrange_v2.py +++ b/examples/hitl/rearrange_v2/rearrange_v2.py @@ -59,7 +59,6 @@ def __init__(self, app_service): self._paused = False self._hide_gui_text = False self._can_place_object = False - self._original_object_position: Optional[mn.Vector3] = None self._camera_helper = CameraHelper( self._app_service.hitl_config, @@ -213,8 +212,8 @@ def _update_grasping_and_set_act_hints(self, user_index): self._has_grasp_preview = False # todo: implement grasping properly for each user. _held_obj_id, _has_grasp_preview, etc. must be tracked per user. - if self._held_obj_id is not None: - if self._get_user_key_down(user_index, GuiInput.KeyNS.SPACE): + if self._held_obj_id is not None : + if self._get_user_key_down(user_index, GuiInput.KeyNS.SPACE) and self._can_place_object: if DO_HUMANOID_GRASP_OBJECTS: # todo: better drop pos drop_pos = self._get_gui_agent_translation( @@ -222,14 +221,7 @@ def _update_grasping_and_set_act_hints(self, user_index): ) # self._gui_agent_controllers.get_base_translation() else: # GuiPlacementHelper has already placed this object. - # Move object back to original position if it could not be placed. - if not self._can_place_object: - query_obj = ( - self.get_sim() - .get_rigid_object_manager() - .get_object_by_id(self._held_obj_id) - ) - query_obj.translation = self._original_object_position + pass self._held_obj_id = None else: query_pos = self._get_gui_agent_translation(user_index) @@ -241,12 +233,6 @@ def _update_grasping_and_set_act_hints(self, user_index): if DO_HUMANOID_GRASP_OBJECTS: grasp_object_id = obj_id self._held_obj_id = obj_id - query_obj = ( - self.get_sim() - .get_rigid_object_manager() - .get_object_by_id(self._held_obj_id) - ) - self._original_object_position = query_obj.translation else: self._has_grasp_preview = True From ee3e71a6023a7af07030cb150c390b5102c0b79a Mon Sep 17 00:00:00 2001 From: Mikael Dallaire Cote <110583667+0mdc@users.noreply.github.com> Date: Thu, 28 Mar 2024 14:51:19 -0400 Subject: [PATCH 3/3] Formatting fixes. --- examples/hitl/rearrange_v2/rearrange_v2.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/examples/hitl/rearrange_v2/rearrange_v2.py b/examples/hitl/rearrange_v2/rearrange_v2.py index e395b70482..22f6f8ee78 100644 --- a/examples/hitl/rearrange_v2/rearrange_v2.py +++ b/examples/hitl/rearrange_v2/rearrange_v2.py @@ -5,7 +5,7 @@ # LICENSE file in the root directory of this source tree. -from typing import Any, Dict, Optional, Set +from typing import Any, Dict, Set import hydra import magnum as mn @@ -212,8 +212,11 @@ def _update_grasping_and_set_act_hints(self, user_index): self._has_grasp_preview = False # todo: implement grasping properly for each user. _held_obj_id, _has_grasp_preview, etc. must be tracked per user. - if self._held_obj_id is not None : - if self._get_user_key_down(user_index, GuiInput.KeyNS.SPACE) and self._can_place_object: + if self._held_obj_id is not None: + if ( + self._get_user_key_down(user_index, GuiInput.KeyNS.SPACE) + and self._can_place_object + ): if DO_HUMANOID_GRASP_OBJECTS: # todo: better drop pos drop_pos = self._get_gui_agent_translation(