From c41e0d870c92d796ce7f812bebb01c2ae2dc1fb7 Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Thu, 26 Aug 2021 13:01:18 +0200 Subject: [PATCH 1/8] [world/plugin_pybullet] decoupling removing and attaching in world.py and instead doing it in plugin_pybullet.py --- src/giskardpy/plugin_pybullet.py | 6 +++++- src/giskardpy/world.py | 1 - 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/giskardpy/plugin_pybullet.py b/src/giskardpy/plugin_pybullet.py index 013b6694e6..f98f4dbb84 100644 --- a/src/giskardpy/plugin_pybullet.py +++ b/src/giskardpy/plugin_pybullet.py @@ -340,7 +340,7 @@ def attach_object(self, req): p.pose = self.unsafe_get_world().get_object(req.body.name).base_pose p = transform_pose(req.pose.header.frame_id, p) world_object = self.unsafe_get_world().get_object(req.body.name) - self.unsafe_get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose) + self.attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose) m = world_object.as_marker_msg() m.header.frame_id = p.header.frame_id m.pose = p.pose @@ -359,6 +359,10 @@ def attach_object(self, req): except: pass + def attach_existing_obj_to_robot(self, name, link, pose): + self.unsafe_get_world().attach_existing_obj_to_robot(name, link, pose) + self.remove_object(name) + def remove_object(self, name): # assumes that parent has god map lock try: diff --git a/src/giskardpy/world.py b/src/giskardpy/world.py index 9044c0f5f1..32ea5513d7 100644 --- a/src/giskardpy/world.py +++ b/src/giskardpy/world.py @@ -161,7 +161,6 @@ def attach_existing_obj_to_robot(self, name, link, pose): """ # TODO this should know the object pose and not require it as input self._robot.attach_urdf_object(self.get_object(name), link, pose) - self.remove_object(name) logging.loginfo(u'--> attached object {} on link {}'.format(name, link)) def detach(self, joint_name, from_obj=None): From b1a5b88d7f1e55a2d1702bc526a2bbf1948307a6 Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Thu, 26 Aug 2021 13:02:02 +0200 Subject: [PATCH 2/8] [test:world.py] fixing tests for attach_existing_obj_to_robot --- test/test_world.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/test_world.py b/test/test_world.py index 237b396a8b..bf7302510d 100644 --- a/test/test_world.py +++ b/test/test_world.py @@ -343,6 +343,7 @@ def test_attach_existing_obj_to_robot1(self, function_setup): p = Pose() p.orientation.w = 1 world_with_pr2.attach_existing_obj_to_robot(u'box', u'l_gripper_tool_frame', p) + world_with_pr2.remove_object(u'box') assert u'box' not in world_with_pr2.get_object_names() assert set(world_with_pr2.robot.get_link_names()).difference(links_before) == {u'box'} assert set(world_with_pr2.robot.get_joint_names()).difference(joints_before) == {u'box'} @@ -371,6 +372,7 @@ def test_attach_detach_existing_obj_to_robot1(self, function_setup): p = Pose() p.orientation.w = 1 world_with_pr2.attach_existing_obj_to_robot(obj_name, u'l_gripper_tool_frame', p) + world_with_pr2.remove_object(obj_name) assert obj_name not in world_with_pr2.get_object_names() assert set(world_with_pr2.robot.get_link_names()).difference(links_before) == {obj_name} assert set(world_with_pr2.robot.get_joint_names()).difference(joints_before) == {obj_name} From 7e7115b8b5ffb06c3c2f755e2d15b7fade1efcf8 Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Thu, 26 Aug 2021 13:03:52 +0200 Subject: [PATCH 3/8] [plugin_world_vis] checking if objects were removed in has_env_changed and simplifying marker publishing of "simple objects" e.g. trays, cups and so on --- src/giskardpy/plugin_world_visualization.py | 35 +++++++++++---------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/giskardpy/plugin_world_visualization.py b/src/giskardpy/plugin_world_visualization.py index 92bbc1021a..d12e229a6d 100644 --- a/src/giskardpy/plugin_world_visualization.py +++ b/src/giskardpy/plugin_world_visualization.py @@ -46,13 +46,12 @@ def get_id_str(self, object_name, link_name): def has_environment_changed(self): """ - Checks if new objects in the world were added and if so it returns True. Otherwise, False. + Checks if objects in the world were added or removed and if so it returns True. Otherwise, False. """ objects_dict = self.get_world().get_objects() - for object_name, object in objects_dict.items(): - if object_name not in self.currently_publishing_objects: - return True - return False + object_names = [object_name for object_name, _ in objects_dict.items()] + curr_publishing_object_names = [object_name for object_name, _ in self.currently_publishing_objects.items()] + return object_names != curr_publishing_object_names def update(self): markers = [] @@ -67,19 +66,23 @@ def update(self): for object_name, object in objects_dict.items(): for link_name in object.get_link_names(): if object.has_link_visuals(link_name): - marker = object.link_as_marker(link_name) - if marker is None: + # Simple objects (containing only one link) are published here: + if link_name == object_name and len(object.get_link_names()) == 1: + marker = object.as_marker_msg() + markers.append(marker) continue - marker.header.frame_id = self.map_frame - id_str = self.get_id_str(object_name, link_name) - marker.id = int(hashlib.md5(id_str).hexdigest()[:6], - 16) # FIXME find a better way to give the same link the same id - self.ids.add(marker.id) - marker.ns = self.marker_namespace - marker.header.stamp = time_stamp - if link_name == object_name: - marker.pose = object.base_pose + # More complex objects will be published here: else: + marker = object.link_as_marker(link_name) + if marker is None: + continue + marker.header.frame_id = self.map_frame + id_str = self.get_id_str(object_name, link_name) + marker.id = int(hashlib.md5(id_str).hexdigest()[:6], + 16) # FIXME find a better way to give the same link the same id + self.ids.add(marker.id) + marker.ns = self.marker_namespace + marker.header.stamp = time_stamp try: full_link_name = self.links_full_frame_name[link_name] except KeyError: From 16b1eb15531077df439fe063129a0fe3287fe72f Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Fri, 10 Sep 2021 13:28:15 +0200 Subject: [PATCH 4/8] [test] reverting remove fun call --- test/test_world.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/test/test_world.py b/test/test_world.py index bf7302510d..237b396a8b 100644 --- a/test/test_world.py +++ b/test/test_world.py @@ -343,7 +343,6 @@ def test_attach_existing_obj_to_robot1(self, function_setup): p = Pose() p.orientation.w = 1 world_with_pr2.attach_existing_obj_to_robot(u'box', u'l_gripper_tool_frame', p) - world_with_pr2.remove_object(u'box') assert u'box' not in world_with_pr2.get_object_names() assert set(world_with_pr2.robot.get_link_names()).difference(links_before) == {u'box'} assert set(world_with_pr2.robot.get_joint_names()).difference(joints_before) == {u'box'} @@ -372,7 +371,6 @@ def test_attach_detach_existing_obj_to_robot1(self, function_setup): p = Pose() p.orientation.w = 1 world_with_pr2.attach_existing_obj_to_robot(obj_name, u'l_gripper_tool_frame', p) - world_with_pr2.remove_object(obj_name) assert obj_name not in world_with_pr2.get_object_names() assert set(world_with_pr2.robot.get_link_names()).difference(links_before) == {obj_name} assert set(world_with_pr2.robot.get_joint_names()).difference(joints_before) == {obj_name} From 7e9d290a564bbb7bb61592898cc2d3f17ba60370 Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Fri, 10 Sep 2021 13:28:43 +0200 Subject: [PATCH 5/8] [world] reverting changes --- src/giskardpy/world.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/giskardpy/world.py b/src/giskardpy/world.py index 32ea5513d7..9044c0f5f1 100644 --- a/src/giskardpy/world.py +++ b/src/giskardpy/world.py @@ -161,6 +161,7 @@ def attach_existing_obj_to_robot(self, name, link, pose): """ # TODO this should know the object pose and not require it as input self._robot.attach_urdf_object(self.get_object(name), link, pose) + self.remove_object(name) logging.loginfo(u'--> attached object {} on link {}'.format(name, link)) def detach(self, joint_name, from_obj=None): From eb3430461c6b735786590745a70757a2bec3aae9 Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Fri, 10 Sep 2021 13:29:17 +0200 Subject: [PATCH 6/8] [plugin_world_vis] ignoring simple (one link) objects --- src/giskardpy/plugin_world_visualization.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/giskardpy/plugin_world_visualization.py b/src/giskardpy/plugin_world_visualization.py index d12e229a6d..8986085b52 100644 --- a/src/giskardpy/plugin_world_visualization.py +++ b/src/giskardpy/plugin_world_visualization.py @@ -48,8 +48,7 @@ def has_environment_changed(self): """ Checks if objects in the world were added or removed and if so it returns True. Otherwise, False. """ - objects_dict = self.get_world().get_objects() - object_names = [object_name for object_name, _ in objects_dict.items()] + object_names = self.get_world().get_object_names() curr_publishing_object_names = [object_name for object_name, _ in self.currently_publishing_objects.items()] return object_names != curr_publishing_object_names @@ -66,10 +65,9 @@ def update(self): for object_name, object in objects_dict.items(): for link_name in object.get_link_names(): if object.has_link_visuals(link_name): - # Simple objects (containing only one link) are published here: + # Simple objects (containing only one link) are skipped, since they are already managed + # in plugin_pybullet.py and as marker encoded with the function as_marker_msg from urdf_object.py if link_name == object_name and len(object.get_link_names()) == 1: - marker = object.as_marker_msg() - markers.append(marker) continue # More complex objects will be published here: else: From f277d91bf1e1670aae96ec75825addfe57250772 Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Fri, 10 Sep 2021 13:29:55 +0200 Subject: [PATCH 7/8] [plugin_pybullet] removing marker if object attached somewhere --- src/giskardpy/plugin_pybullet.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/giskardpy/plugin_pybullet.py b/src/giskardpy/plugin_pybullet.py index f98f4dbb84..0944aa8ec1 100644 --- a/src/giskardpy/plugin_pybullet.py +++ b/src/giskardpy/plugin_pybullet.py @@ -340,7 +340,8 @@ def attach_object(self, req): p.pose = self.unsafe_get_world().get_object(req.body.name).base_pose p = transform_pose(req.pose.header.frame_id, p) world_object = self.unsafe_get_world().get_object(req.body.name) - self.attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose) + self.unsafe_get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose) + self.delete_object_marker(req.body.name) m = world_object.as_marker_msg() m.header.frame_id = p.header.frame_id m.pose = p.pose @@ -359,18 +360,17 @@ def attach_object(self, req): except: pass - def attach_existing_obj_to_robot(self, name, link, pose): - self.unsafe_get_world().attach_existing_obj_to_robot(name, link, pose) - self.remove_object(name) - - def remove_object(self, name): - # assumes that parent has god map lock + def delete_object_marker(self, name): try: m = self.unsafe_get_world().get_object(name).as_marker_msg() m.action = m.DELETE self.publish_object_as_marker(m) except: pass + + def remove_object(self, name): + # assumes that parent has god map lock + self.remove_object_marker(name) self.unsafe_get_world().remove_object(name) if name in self.object_js_subs: self.object_js_subs[name].unregister() From 8b26c91707818ac34ec5795963b2439469e290a3 Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Thu, 16 Sep 2021 13:04:31 +0200 Subject: [PATCH 8/8] [plugin_pybullet] refactor stuff fixed --- src/giskardpy/plugin_pybullet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/giskardpy/plugin_pybullet.py b/src/giskardpy/plugin_pybullet.py index 0944aa8ec1..41415c6b1a 100644 --- a/src/giskardpy/plugin_pybullet.py +++ b/src/giskardpy/plugin_pybullet.py @@ -370,7 +370,7 @@ def delete_object_marker(self, name): def remove_object(self, name): # assumes that parent has god map lock - self.remove_object_marker(name) + self.delete_object_marker(name) self.unsafe_get_world().remove_object(name) if name in self.object_js_subs: self.object_js_subs[name].unregister()