[generic object] for perceiving in real world and spawn/describe obbjects from type unkown

src/pycram/object_descriptors/generic.py

@@ -0,0 +1,146 @@
+from typing_extensions import List, Any, Union, Dict
+
+from geometry_msgs.msg import Point
+
+from ..datastructures.dataclasses import VisualShape, BoxVisualShape, Color
+from ..datastructures.enums import JointType
+from ..datastructures.pose import Pose
+from ..description import JointDescription as AbstractJointDescription, LinkDescription as AbstractLinkDescription, \
+    ObjectDescription as AbstractObjectDescription
+
+
+class LinkDescription(AbstractLinkDescription):
+
+    def __init__(self, name: str, visual_frame_position: List[float], half_extents: List[float], color: Color = Color()):
+        self.parsed_description: BoxVisualShape = BoxVisualShape(color, visual_frame_position, half_extents)
+        self._name: str = name
+
+    @property
+    def geometry(self) -> Union[VisualShape, None]:
+        return self.parsed_description
+
+    @property
+    def origin(self) -> Pose:
+        return Pose(self.parsed_description.visual_frame_position)
+
+    @property
+    def name(self) -> str:
+        return self._name
+
+    @property
+    def color(self) -> Color:
+        return self.parsed_description.rgba_color
+
+
+class JointDescription(AbstractJointDescription):
+
+    @property
+    def type(self) -> JointType:
+        return JointType.UNKNOWN
+
+    @property
+    def axis(self) -> Point:
+        return Point(0, 0, 0)
+
+    @property
+    def has_limits(self) -> bool:
+        return False
+
+    @property
+    def lower_limit(self) -> Union[float, None]:
+        return 0
+
+    @property
+    def upper_limit(self) -> Union[float, None]:
+        return 0
+
+    @property
+    def parent_link_name(self) -> str:
+        raise NotImplementedError
+
+    @property
+    def child_link_name(self) -> str:
+        raise NotImplementedError
+
+    @property
+    def origin(self) -> Pose:
+        raise NotImplementedError
+
+    @property
+    def name(self) -> str:
+        raise NotImplementedError
+
+
+class ObjectDescription(AbstractObjectDescription):
+    """
+    A generic description of an object in the environment. This description can be applied to any object.
+    The current use case involves perceiving objects using RoboKudo and spawning them with specified size and color.
+    """
+
+    class Link(AbstractObjectDescription.Link, LinkDescription):
+        ...
+
+    class RootLink(AbstractObjectDescription.RootLink, Link):
+        ...
+
+    class Joint(AbstractObjectDescription.Joint, JointDescription):
+        ...
+
+    def __init__(self, *args, **kwargs):
+        self._links = [LinkDescription(*args, **kwargs)]
+
+    def load_description(self, path: str) -> Any:
+        ...
+
+    @classmethod
+    def generate_from_mesh_file(cls, path: str, name: str) -> str:
+        raise NotImplementedError
+
+    @classmethod
+    def generate_from_description_file(cls, path: str) -> str:
+        raise NotImplementedError
+
+    @classmethod
+    def generate_from_parameter_server(cls, name: str) -> str:
+        raise NotImplementedError
+
+    @property
+    def shape_data(self) -> List[float]:
+        return self._links[0].geometry.shape_data()['halfExtents']
+
+    @property
+    def color(self) -> Color:
+        return self._links[0].color
+
+    @property
+    def links(self) -> List[LinkDescription]:
+        return self._links
+
+    def get_link_by_name(self, link_name: str) -> LinkDescription:
+        if link_name == self._links[0].name:
+            return self._links[0]
+
+    @property
+    def joints(self) -> List[JointDescription]:
+        return []
+
+    def get_joint_by_name(self, joint_name: str) -> JointDescription:
+        ...
+
+    def get_root(self) -> str:
+        return self._links[0].name
+
+    def get_chain(self, start_link_name: str, end_link_name: str) -> List[str]:
+        raise NotImplementedError("Do Not Do This on generic objects as they have no chains")
+
+    @staticmethod
+    def get_file_extension() -> str:
+        raise NotImplementedError("Do Not Do This on generic objects as they have no extensions")
+
+    @property
+    def origin(self) -> Pose:
+        return self._links[0].origin
+
+    @property
+    def name(self) -> str:
+        return self._links[0].name

src/pycram/world_concepts/world_object.py

@@ -10,6 +10,7 @@
 
 from ..description import ObjectDescription, LinkDescription, Joint
 from ..object_descriptors.urdf import ObjectDescription as URDFObject
+from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
 from ..robot_descriptions import robot_description
 from ..datastructures.world import WorldEntity, World
 from ..world_concepts.constraints import Attachment
@@ -121,6 +122,9 @@ def _load_object_and_get_id(self, path: Optional[str] = None,
         :param ignore_cached_files: Whether to ignore files in the cache directory.
         :return: The unique id of the object and the path of the file that was loaded.
         """
+        if isinstance(self.description, GenericObjectDescription):
+            return self.world.load_generic_object_and_get_id(self.description), path
+
         if path is not None:
             try:
                 path = self.world.update_cache_dir_with_object(path, ignore_cached_files, self)

src/pycram/worlds/bullet_world.py

@@ -11,12 +11,13 @@
 from geometry_msgs.msg import Point
 from typing_extensions import List, Optional, Dict
 
+from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape
 from ..datastructures.enums import ObjectType, WorldMode, JointType
 from ..datastructures.pose import Pose
-from ..object_descriptors.urdf import ObjectDescription
 from ..datastructures.world import World
+from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
+from ..object_descriptors.urdf import ObjectDescription
 from ..world_concepts.constraints import Constraint
-from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape
 from ..world_concepts.world_object import Object
 
 Link = ObjectDescription.Link
@@ -68,6 +69,25 @@ def _init_world(self, mode: WorldMode):
         self._gui_thread.start()
         time.sleep(0.1)
 
+    def load_generic_object_and_get_id(self, description: GenericObjectDescription) -> int:
+        """
+        Creates a visual and collision box in the simulation.
+        """
+        # Create visual shape
+        vis_shape = p.createVisualShape(p.GEOM_BOX, halfExtents=description.shape_data,
+                                        rgbaColor=description.color.get_rgba())
+
+        # Create collision shape
+        col_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=description.shape_data)
+
+        # Create MultiBody with both visual and collision shapes
+        obj_id = p.createMultiBody(baseMass=1.0, baseCollisionShapeIndex=col_shape, baseVisualShapeIndex=vis_shape,
+                                   basePosition=description.origin.position_as_list(),
+                                   baseOrientation=description.origin.orientation_as_list())
+
+        # Assuming you have a list to keep track of created objects
+        return obj_id
+
     def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int:
         if pose is None:
             pose = Pose()

test/test_object.py

@@ -6,6 +6,7 @@
 from pycram.datastructures.pose import Pose
 from pycram.datastructures.dataclasses import Color
 from pycram.world_concepts.world_object import Object
+from pycram.object_descriptors.generic import ObjectDescription as GenericObjectDescription
 
 from geometry_msgs.msg import Point, Quaternion
 import pathlib
@@ -160,3 +161,11 @@ def test_object_equal(self):
         self.assertEqual(self.milk, self.milk)
         self.assertNotEqual(self.milk, self.cereal)
         self.assertNotEqual(self.milk, self.world)
+
+
+class GenericObjectTestCase(BulletWorldTestCase):
+
+    def test_init_generic_object(self):
+        gen_obj_desc = lambda: GenericObjectDescription("robokudo_object", [0,0,0], [0.1, 0.1, 0.1])
+        obj = Object("robokudo_object", ObjectType.MILK, None, gen_obj_desc)
+        self.assertTrue(True)