diff --git a/config/world_conf.py b/config/world_conf.py
index 2c0eacf21..75f335d07 100644
--- a/config/world_conf.py
+++ b/config/world_conf.py
@@ -70,7 +70,7 @@ class WorldConfig:
      and the world synchronization.
     """
 
-    position_tolerance: float = 1e-2
+    position_tolerance: float = 1e-3
     orientation_tolerance: float = 10 * math.pi / 180
     prismatic_joint_position_tolerance: float = 1e-2
     revolute_joint_position_tolerance: float = 5 * math.pi / 180
diff --git a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py
index 67dbf38da..7269caea3 100644
--- a/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py
+++ b/demos/pycram_multiverse_demo/demo_euROBIN_industrial_robotics.py
@@ -1,7 +1,6 @@
 import pycrap
-from pycram.datastructures.enums import GripperState, Arms
-from pycram.datastructures.world import UseProspectionWorld
-from pycram.process_module import simulated_robot, real_robot
+from pycram.datastructures.enums import GripperState
+from pycram.process_module import real_robot
 from pycram.world_concepts.world_object import Object
 from pycram.datastructures.pose import Pose
 from pycram.worlds.multiverse import Multiverse
@@ -30,5 +29,4 @@
     with real_robot:
         SetGripperAction(robot_arms, [GripperState.CLOSE]).resolve().perform()
 
-
     world.exit()
diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md
index c57fefe53..6d8f32d20 100644
--- a/examples/cram_plan_tutorial.md
+++ b/examples/cram_plan_tutorial.md
@@ -88,6 +88,7 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True):
 ```
 
 ```python
+import pycrap
 from tf.transformations import quaternion_from_euler
 import pycrap
 from pycram.costmaps import SemanticCostmap
diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py
index e083f42bf..9d5578c51 100644
--- a/src/pycram/datastructures/dataclasses.py
+++ b/src/pycram/datastructures/dataclasses.py
@@ -17,6 +17,7 @@
     from ..description import Link
     from ..world_concepts.world_object import Object
     from ..world_concepts.constraints import Attachment
+    from .world_entity import PhysicalBody
 
 
 def get_point_as_list(point: Point) -> List[float]:
@@ -186,7 +187,7 @@ def from_origin_and_half_extents(cls, origin: Point, half_extents: Point):
         return cls.from_min_max(min_point, max_point)
 
     @classmethod
-    def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> 'AxisAlignedBoundingBox':
+    def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBox]) -> AxisAlignedBoundingBox:
         """
         Set the axis-aligned bounding box from multiple axis-aligned bounding boxes.
 
@@ -200,7 +201,7 @@ def from_multiple_bounding_boxes(cls, bounding_boxes: List[AxisAlignedBoundingBo
         max_z = max([box.max_z for box in bounding_boxes])
         return cls(min_x, min_y, min_z, max_x, max_y, max_z)
 
-    def get_transformed_box(self, transform: Transform) -> 'AxisAlignedBoundingBox':
+    def get_transformed_box(self, transform: Transform) -> AxisAlignedBoundingBox:
         """
         Apply a transformation to the axis-aligned bounding box and return the transformed axis-aligned bounding box.
 
@@ -249,7 +250,7 @@ def from_min_max(cls, min_point: Sequence[float], max_point: Sequence[float], tr
 
     @classmethod
     def from_axis_aligned_bounding_box(cls, axis_aligned_bounding_box: AxisAlignedBoundingBox,
-                                       transform: Transform) -> 'RotatedBoundingBox':
+                                       transform: Transform) -> RotatedBoundingBox:
         """
         Set the rotated bounding box from an axis-aligned bounding box and a transformation.
 
@@ -478,17 +479,80 @@ class State(ABC):
     pass
 
 
+@dataclass
+class PhysicalBodyState(State):
+    """
+    Dataclass for storing the state of a physical body.
+    """
+    pose: Pose
+    is_translating: bool
+    is_rotating: bool
+    velocity: List[float]
+    acceptable_pose_error: Tuple[float, float] = (0.001, 0.001)
+    acceptable_velocity_error: Tuple[float, float] = (0.001, 0.001)
+
+    def __eq__(self, other: PhysicalBodyState):
+        return (self.pose_is_almost_equal(other)
+                and self.is_translating == other.is_translating
+                and self.is_rotating == other.is_rotating
+                and self.velocity_is_almost_equal(other)
+                )
+
+    def pose_is_almost_equal(self, other: PhysicalBodyState) -> bool:
+        """
+        Check if the pose of the object is almost equal to the pose of another object.
+
+        :param other: The state of the other object.
+        :return: True if the poses are almost equal, False otherwise.
+        """
+        return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1])
+
+    def velocity_is_almost_equal(self, other: PhysicalBodyState) -> bool:
+        """
+        Check if the velocity of the object is almost equal to the velocity of another object.
+
+        :param other: The state of the other object.
+        :return: True if the velocities are almost equal, False otherwise.
+        """
+        if self.velocity is None or other.velocity is None:
+            return self.velocity == other.velocity
+        return (self.vector_is_almost_equal(self.velocity[:3], other.velocity[:3], self.acceptable_velocity_error[0])
+                and self.vector_is_almost_equal(self.velocity[3:], other.velocity[3:], self.acceptable_velocity_error[1]))
+
+    @staticmethod
+    def vector_is_almost_equal(vector1: List[float], vector2: List[float], acceptable_error: float) -> bool:
+        """
+        Check if the vector is almost equal to another vector.
+
+        :param vector1: The first vector.
+        :param vector2: The second vector.
+        :param acceptable_error: The acceptable error.
+        :return: True if the vectors are almost equal, False otherwise.
+        """
+        return np.all(np.array(vector1) - np.array(vector2) <= acceptable_error)
+
+    def __copy__(self):
+        return PhysicalBodyState(pose=self.pose.copy(),
+                                 is_translating=self.is_translating, is_rotating=self.is_rotating,
+                                 velocity=self.velocity.copy(),
+                                 acceptable_pose_error=deepcopy(self.acceptable_pose_error),
+                                 acceptable_velocity_error=deepcopy(self.acceptable_velocity_error))
+
+
 @dataclass
 class LinkState(State):
     """
     Dataclass for storing the state of a link.
     """
+    body_state: PhysicalBodyState
     constraint_ids: Dict[Link, int]
 
-    def __eq__(self, other: 'LinkState'):
-        return self.all_constraints_exist(other) and self.all_constraints_are_equal(other)
+    def __eq__(self, other: LinkState):
+        return (self.body_state == other.body_state
+                and self.all_constraints_exist(other)
+                and self.all_constraints_are_equal(other))
 
-    def all_constraints_exist(self, other: 'LinkState') -> bool:
+    def all_constraints_exist(self, other: LinkState) -> bool:
         """
         Check if all constraints exist in the other link state.
 
@@ -498,7 +562,7 @@ def all_constraints_exist(self, other: 'LinkState') -> bool:
         return (all([cid_k in other.constraint_ids.keys() for cid_k in self.constraint_ids.keys()])
                 and len(self.constraint_ids.keys()) == len(other.constraint_ids.keys()))
 
-    def all_constraints_are_equal(self, other: 'LinkState') -> bool:
+    def all_constraints_are_equal(self, other: LinkState) -> bool:
         """
         Check if all constraints are equal to the ones in the other link state.
 
@@ -509,7 +573,7 @@ def all_constraints_are_equal(self, other: 'LinkState') -> bool:
                                                                other.constraint_ids.values())])
 
     def __copy__(self):
-        return LinkState(constraint_ids=copy(self.constraint_ids))
+        return LinkState(copy(self.body_state), constraint_ids=copy(self.constraint_ids))
 
 
 @dataclass
@@ -520,7 +584,7 @@ class JointState(State):
     position: float
     acceptable_error: float
 
-    def __eq__(self, other: 'JointState'):
+    def __eq__(self, other: JointState):
         error = calculate_joint_position_error(self.position, other.position)
         return is_error_acceptable(error, other.acceptable_error)
 
@@ -533,28 +597,22 @@ class ObjectState(State):
     """
     Dataclass for storing the state of an object.
     """
-    pose: Pose
+    body_state: PhysicalBodyState
     attachments: Dict[Object, Attachment]
     link_states: Dict[int, LinkState]
     joint_states: Dict[int, JointState]
-    acceptable_pose_error: Tuple[float, float]
 
-    def __eq__(self, other: 'ObjectState'):
-        return (self.pose_is_almost_equal(other)
+    def __eq__(self, other: ObjectState):
+        return (self.body_state == other.body_state
                 and self.all_attachments_exist(other) and self.all_attachments_are_equal(other)
                 and self.link_states == other.link_states
                 and self.joint_states == other.joint_states)
 
-    def pose_is_almost_equal(self, other: 'ObjectState') -> bool:
-        """
-        Check if the pose of the object is almost equal to the pose of another object.
-
-        :param other: The state of the other object.
-        :return: True if the poses are almost equal, False otherwise.
-        """
-        return self.pose.almost_equal(other.pose, other.acceptable_pose_error[0], other.acceptable_pose_error[1])
+    @property
+    def pose(self) -> Pose:
+        return self.body_state.pose
 
-    def all_attachments_exist(self, other: 'ObjectState') -> bool:
+    def all_attachments_exist(self, other: ObjectState) -> bool:
         """
         Check if all attachments exist in the other object state.
 
@@ -564,7 +622,7 @@ def all_attachments_exist(self, other: 'ObjectState') -> bool:
         return (all([att_k in other.attachments.keys() for att_k in self.attachments.keys()])
                 and len(self.attachments.keys()) == len(other.attachments.keys()))
 
-    def all_attachments_are_equal(self, other: 'ObjectState') -> bool:
+    def all_attachments_are_equal(self, other: ObjectState) -> bool:
         """
         Check if all attachments are equal to the ones in the other object state.
 
@@ -574,10 +632,9 @@ def all_attachments_are_equal(self, other: 'ObjectState') -> bool:
         return all([att == other_att for att, other_att in zip(self.attachments.values(), other.attachments.values())])
 
     def __copy__(self):
-        return ObjectState(pose=self.pose.copy(), attachments=copy(self.attachments),
+        return ObjectState(body_state=self.body_state.copy(), attachments=copy(self.attachments),
                            link_states=copy(self.link_states),
-                           joint_states=copy(self.joint_states),
-                           acceptable_pose_error=deepcopy(self.acceptable_pose_error))
+                           joint_states=copy(self.joint_states))
 
 
 @dataclass
@@ -588,11 +645,11 @@ class WorldState(State):
     object_states: Dict[str, ObjectState]
     simulator_state_id: Optional[int] = None
 
-    def __eq__(self, other: 'WorldState'):
+    def __eq__(self, other: WorldState):
         return (self.simulator_state_is_equal(other) and self.all_objects_exist(other)
                 and self.all_objects_states_are_equal(other))
 
-    def simulator_state_is_equal(self, other: 'WorldState') -> bool:
+    def simulator_state_is_equal(self, other: WorldState) -> bool:
         """
         Check if the simulator state is equal to the simulator state of another world state.
 
@@ -601,7 +658,7 @@ def simulator_state_is_equal(self, other: 'WorldState') -> bool:
         """
         return self.simulator_state_id == other.simulator_state_id
 
-    def all_objects_exist(self, other: 'WorldState') -> bool:
+    def all_objects_exist(self, other: WorldState) -> bool:
         """
         Check if all objects exist in the other world state.
 
@@ -611,7 +668,7 @@ def all_objects_exist(self, other: 'WorldState') -> bool:
         return (all([obj_name in other.object_states.keys() for obj_name in self.object_states.keys()])
                 and len(self.object_states.keys()) == len(other.object_states.keys()))
 
-    def all_objects_states_are_equal(self, other: 'WorldState') -> bool:
+    def all_objects_states_are_equal(self, other: WorldState) -> bool:
         """
         Check if all object states are equal to the ones in the other world state.
 
@@ -640,20 +697,24 @@ class LateralFriction:
 @dataclass
 class ContactPoint:
     """
-    Dataclass for storing the information of a contact point between two objects.
+    Dataclass for storing the information of a contact point between two bodies.
     """
-    link_a: Link
-    link_b: Link
-    position_on_object_a: Optional[List[float]] = None
-    position_on_object_b: Optional[List[float]] = None
-    normal_on_b: Optional[List[float]] = None  # the contact normal vector on object b pointing towards object a
+    body_a: PhysicalBody
+    body_b: PhysicalBody
+    position_on_body_a: Optional[List[float]] = None
+    position_on_body_b: Optional[List[float]] = None
+    normal_on_body_b: Optional[List[float]] = None  # the contact normal vector on object b pointing towards object a
     distance: Optional[float] = None  # distance between the two objects (+ve for separation, -ve for penetration)
     normal_force: Optional[List[float]] = None  # normal force applied during last step simulation
     lateral_friction_1: Optional[LateralFriction] = None
     lateral_friction_2: Optional[LateralFriction] = None
 
+    @property
+    def normal(self) -> List[float]:
+        return self.normal_on_body_b
+
     def __str__(self):
-        return f"ContactPoint: {self.link_a.object.name} - {self.link_b.object.name}"
+        return f"ContactPoint: {self.body_a.name} - {self.body_b.name}"
 
     def __repr__(self):
         return self.__str__()
@@ -670,24 +731,24 @@ class ContactPointsList(list):
     A list of contact points.
     """
 
-    def get_links_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Link]:
+    def get_bodies_that_got_removed(self, previous_points: ContactPointsList) -> List[PhysicalBody]:
         """
-        Return the links that are not in the current points list but were in the initial points list.
+        Return the bodies that are not in the current points list but were in the initial points list.
 
         :param previous_points: The initial points list.
-        :return: A list of Link instances that represent the links that got removed.
+        :return: A list of bodies that got removed.
         """
-        initial_links_in_contact = previous_points.get_links_in_contact()
-        current_links_in_contact = self.get_links_in_contact()
-        return [link for link in initial_links_in_contact if link not in current_links_in_contact]
+        initial_bodies_in_contact = previous_points.get_bodies_in_contact()
+        current_bodies_in_contact = self.get_bodies_in_contact()
+        return [body for body in initial_bodies_in_contact if body not in current_bodies_in_contact]
 
-    def get_links_in_contact(self) -> List[Link]:
+    def get_bodies_in_contact(self) -> List[PhysicalBody]:
         """
-        Get the links in contact.
+        Get the bodies in contact.
 
-        :return: A list of Link instances that represent the links in contact.
+        :return: A list of bodies that are in contact.
         """
-        return [point.link_b for point in self]
+        return [point.body_b for point in self]
 
     def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> bool:
         """
@@ -697,8 +758,21 @@ def check_if_two_objects_are_in_contact(self, obj_a: Object, obj_b: Object) -> b
         :param obj_b: An instance of the Object class that represents the second object.
         :return: True if the objects are in contact, False otherwise.
         """
-        return (any([point.link_b.object == obj_b and point.link_a.object == obj_a for point in self]) or
-                any([point.link_a.object == obj_b and point.link_b.object == obj_a for point in self]))
+        return (any([self.is_body_in_object(point.body_b, obj_b)
+                     and self.is_body_in_object(point.body_a, obj_a) for point in self]) or
+                any([self.is_body_in_object(point.body_a, obj_b)
+                     and self.is_body_in_object(point.body_b, obj_a) for point in self]))
+
+    @staticmethod
+    def is_body_in_object(body: PhysicalBody, obj: Object) -> bool:
+        """
+        Check if the body belongs to the object.
+
+        :param body: The body.
+        :param obj: The object.
+        :return: True if the body belongs to the object, False otherwise.
+        """
+        return body in list(obj.links.values()) or body == obj
 
     def get_normals_of_object(self, obj: Object) -> List[List[float]]:
         """
@@ -715,27 +789,45 @@ def get_normals(self) -> List[List[float]]:
 
         :return: A list of float vectors that represent the normals of the contact points.
         """
-        return [point.normal_on_b for point in self]
+        return [point.normal_on_body_b for point in self]
 
-    def get_links_in_contact_of_object(self, obj: Object) -> List[Link]:
+    def get_links_in_contact_of_object(self, obj: Object) -> List[PhysicalBody]:
         """
         Get the links in contact of the object.
 
         :param obj: An instance of the Object class that represents the object.
         :return: A list of Link instances that represent the links in contact of the object.
         """
-        return [point.link_b for point in self if point.link_b.object == obj]
+        return [point.body_b for point in self if point.body_b.parent_entity == obj]
 
-    def get_points_of_object(self, obj: Object) -> 'ContactPointsList':
+    def get_points_of_object(self, obj: Object) -> ContactPointsList:
         """
         Get the points of the object.
 
         :param obj: An instance of the Object class that represents the object that the points are related to.
         :return: A ContactPointsList instance that represents the contact points of the object.
         """
-        return ContactPointsList([point for point in self if point.link_b.object == obj])
+        return ContactPointsList([point for point in self if self.is_body_in_object(point.body_b, obj)])
 
-    def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') -> List[Object]:
+    def get_points_of_link(self, link: Link) -> ContactPointsList:
+        """
+        Get the points of the link.
+
+        :param link: An instance of the Link class that represents the link that the points are related to.
+        :return: A ContactPointsList instance that represents the contact points of the link.
+        """
+        return self.get_points_of_body(link)
+
+    def get_points_of_body(self, body: PhysicalBody) -> ContactPointsList:
+        """
+        Get the points of the body.
+
+        :param body: An instance of the PhysicalBody class that represents the body that the points are related to.
+        :return: A ContactPointsList instance that represents the contact points of the body.
+        """
+        return ContactPointsList([point for point in self if body == point.body_b])
+
+    def get_objects_that_got_removed(self, previous_points: ContactPointsList) -> List[Object]:
         """
         Return the object that is not in the current points list but was in the initial points list.
 
@@ -746,7 +838,7 @@ def get_objects_that_got_removed(self, previous_points: 'ContactPointsList') ->
         current_objects_in_contact = self.get_objects_that_have_points()
         return [obj for obj in initial_objects_in_contact if obj not in current_objects_in_contact]
 
-    def get_new_objects(self, previous_points: 'ContactPointsList') -> List[Object]:
+    def get_new_objects(self, previous_points: ContactPointsList) -> List[Object]:
         """
         Return the object that is not in the initial points list but is in the current points list.
 
@@ -780,7 +872,7 @@ def get_objects_that_have_points(self) -> List[Object]:
 
         :return: A list of Object instances that represent the objects that have points in the list.
         """
-        return list({point.link_b.object for point in self})
+        return list({point.body_b.parent_entity for point in self})
 
     def __str__(self):
         return f"ContactPointsList: {', '.join([point.__str__() for point in self])}"
@@ -833,6 +925,7 @@ class VirtualMobileBaseJoints:
     """
     Dataclass for storing the names, types and axes of the virtual mobile base joints of a mobile robot.
     """
+
     translation_x: Optional[VirtualJoint] = VirtualJoint(VirtualMobileBaseJointName.LINEAR_X.value,
                                                          JointType.PRISMATIC,
                                                          Point(1, 0, 0))
diff --git a/src/pycram/datastructures/mixins.py b/src/pycram/datastructures/mixins.py
index 5ff4dc555..eef06e1cb 100644
--- a/src/pycram/datastructures/mixins.py
+++ b/src/pycram/datastructures/mixins.py
@@ -19,4 +19,4 @@ class HasConcept:
     """
 
     def __init__(self):
-        self.ontology_individual = self.ontology_concept()
\ No newline at end of file
+        self.ontology_individual = self.ontology_concept()
diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py
index ed5978cd0..13d2a9a9e 100644
--- a/src/pycram/datastructures/world.py
+++ b/src/pycram/datastructures/world.py
@@ -10,7 +10,7 @@
 import numpy as np
 from geometry_msgs.msg import Point
 from trimesh.parent import Geometry3D
-from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type
+from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type, deprecated
 
 import pycrap
 from pycrap import PhysicalObject, Floor, Apartment, Robot
@@ -24,15 +24,13 @@
                                           ContactPointsList, VirtualMobileBaseJoints, RotatedBoundingBox)
 from ..datastructures.enums import JointType, WorldMode, Arms
 from ..datastructures.pose import Pose, Transform
-from ..datastructures.world_entity import StateEntity
+from ..datastructures.world_entity import StateEntity, PhysicalBody, WorldEntity
 from ..failures import ProspectionObjectNotFound, WorldObjectNotFound
 from ..local_transformer import LocalTransformer
 from ..robot_description import RobotDescription
 from ..ros.data_types import Time
 from ..ros.logging import logwarn
-from ..validation.goal_validator import (MultiPoseGoalValidator,
-                                         PoseGoalValidator, JointPositionGoalValidator,
-                                         MultiJointPositionGoalValidator, GoalValidator,
+from ..validation.goal_validator import (GoalValidator,
                                          validate_joint_position, validate_multiple_joint_positions,
                                          validate_object_pose, validate_multiple_object_poses)
 from ..world_concepts.constraints import Constraint
@@ -44,7 +42,7 @@
     from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
 
 
-class World(StateEntity, ABC):
+class World(WorldEntity, ABC):
     """
     The World Class represents the physics Simulation and belief state, it is the main interface for reasoning about
     the World. This is implemented as a singleton, the current World can be accessed via the static variable
@@ -80,8 +78,8 @@ class World(StateEntity, ABC):
     The ontology of this world.
     """
 
-    def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False,
-                 clear_cache: bool = False):
+    def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = False, clear_cache: bool = False,
+                 id_: int = -1):
         """
         Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the
         background. There can only be one rendered simulation.
@@ -89,14 +87,13 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
 
         :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default parameter is
          "GUI"
-        :param is_prospection_world: For internal usage, decides if this World should be used as a prospection world.
+        :param is_prospection: For internal usage, decides if this World should be used as a prospection world.
         :param clear_cache: Whether to clear the cache directory.
+        :param id_: The unique id of the world.
         """
 
-        StateEntity.__init__(self)
-
+        WorldEntity.__init__(self, id_, self)
         self.ontology = pycrap.Ontology()
-
         self.latest_state_id: Optional[int] = None
 
         if clear_cache or (self.conf.clear_cache_at_start and not self.cache_manager.cache_cleared):
@@ -109,15 +106,12 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
 
         self.object_lock: threading.Lock = threading.Lock()
 
-        self.id: Optional[int] = -1
-        # This is used to connect to the physics server (allows multiple clients)
-
         self._init_world(mode)
 
         self.objects: List[Object] = []
         # List of all Objects in the World
 
-        self.is_prospection_world: bool = is_prospection_world
+        self.is_prospection_world: bool = is_prospection
         self._init_and_sync_prospection_world()
 
         self.local_transformer = LocalTransformer()
@@ -131,27 +125,28 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
 
         self._current_state: Optional[WorldState] = None
 
-        self._init_goal_validators()
-
         self.original_state_id = self.save_state()
 
         self.on_add_object_callbacks: List[Callable[[Object], None]] = []
 
-    def get_object_convex_hull(self, obj: Object) -> Geometry3D:
+    @property
+    def parent_entity(self) -> Optional[WorldEntity]:
         """
-        Get the convex hull of an object.
-
-        :param obj: The pycram object.
-        :return: The convex hull of the object as a list of Points.
+        Return the parent entity of this entity, in this case it is None as the World is the top level entity.
         """
-        raise NotImplementedError
+        return None
 
-    def get_link_convex_hull(self, link: Link) -> Geometry3D:
+    @property
+    def name(self) -> str:
         """
-        Get the convex hull of a link of an articulated object.
+        Return the name of the world, which is the name of the implementation class (e.g. BulletWorld).
+        """
+        return self.__class__.__name__
 
-        :param link: The link as a AbstractLink object.
-        :return: The convex hull of the link as a list of Points.
+    def get_body_convex_hull(self, body: PhysicalBody) -> Geometry3D:
+        """
+        :param body: The body object.
+        :return: The convex hull of the body as a Geometry3D object.
         """
         raise NotImplementedError
 
@@ -232,30 +227,6 @@ def robot_joint_actuators(self) -> Dict[str, str]:
         """
         return self.robot_description.joint_actuators
 
-    def _init_goal_validators(self):
-        """
-        Initialize the goal validators for the World objects' poses, positions, and orientations.
-        """
-
-        # Objects Pose goal validators
-        self.pose_goal_validator = PoseGoalValidator(self.get_object_pose, self.conf.get_pose_tolerance(),
-                                                     self.conf.acceptable_percentage_of_goal)
-        self.multi_pose_goal_validator = MultiPoseGoalValidator(
-            lambda x: list(self.get_multiple_object_poses(x).values()),
-            self.conf.get_pose_tolerance(), self.conf.acceptable_percentage_of_goal)
-
-        # Joint Goal validators
-        self.joint_position_goal_validator = JointPositionGoalValidator(
-            self.get_joint_position,
-            acceptable_revolute_joint_position_error=self.conf.revolute_joint_position_tolerance,
-            acceptable_prismatic_joint_position_error=self.conf.prismatic_joint_position_tolerance,
-            acceptable_percentage_of_goal_achieved=self.conf.acceptable_percentage_of_goal)
-        self.multi_joint_position_goal_validator = MultiJointPositionGoalValidator(
-            lambda x: list(self.get_multiple_joint_positions(x).values()),
-            acceptable_revolute_joint_position_error=self.conf.revolute_joint_position_tolerance,
-            acceptable_prismatic_joint_position_error=self.conf.prismatic_joint_position_tolerance,
-            acceptable_percentage_of_goal_achieved=self.conf.acceptable_percentage_of_goal)
-
     def check_object_exists(self, obj: Object) -> bool:
         """
         Check if the object exists in the simulator.
@@ -302,7 +273,7 @@ def _init_prospection_world(self):
         if self.is_prospection_world:  # then no need to add another prospection world
             self.prospection_world = None
         else:
-            self.prospection_world: World = self.__class__(is_prospection_world=True)
+            self.prospection_world: World = self.__class__(is_prospection=True)
 
     def _sync_prospection_world(self):
         """
@@ -470,7 +441,7 @@ def remove_object_from_original_state(self, obj: Object) -> None:
         :param obj: The object to be removed.
         """
         self.original_state.object_states.pop(obj.name)
-        self.original_state.simulator_state_id = self.save_physics_simulator_state(use_same_id=True)
+        self.update_simulator_state_id_in_original_state(use_same_id=True)
 
     def add_object_to_original_state(self, obj: Object) -> None:
         """
@@ -636,7 +607,7 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False,
             curr_time = Time().now()
             self.step(func)
             for objects, callbacks in self.coll_callbacks.items():
-                contact_points = self.get_contact_points_between_two_objects(objects[0], objects[1])
+                contact_points = self.get_contact_points_between_two_bodies(objects[0], objects[1])
                 if len(contact_points) > 0:
                     callbacks.on_collision_cb()
                 elif callbacks.no_collision_cb is not None:
@@ -645,14 +616,6 @@ def simulate(self, seconds: float, real_time: Optional[bool] = False,
                 loop_time = Time().now() - curr_time
                 time_diff = self.simulation_time_step - loop_time.to_sec()
                 time.sleep(max(0, time_diff))
-        self.update_all_objects_poses()
-
-    def update_all_objects_poses(self) -> None:
-        """
-        Update the positions of all objects in the world.
-        """
-        for obj in self.objects:
-            obj.update_pose()
 
     @abstractmethod
     def get_object_pose(self, obj: Object) -> Pose:
@@ -737,47 +700,66 @@ def perform_collision_detection(self) -> None:
         """
         pass
 
-    @abstractmethod
+    @deprecated("Use get_body_contact_points instead")
     def get_object_contact_points(self, obj: Object) -> ContactPointsList:
         """
-        Return a list of contact points of this Object with all other Objects.
+        Same as :meth:`get_body_contact_points` but with objects instead of any type of bodies.
+        """
+        return self.get_body_contact_points(obj)
 
-        :param obj: The object.
-        :return: A list of all contact points with other objects
+    @abstractmethod
+    def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList:
+        """
+        Return the contact points of a body with all other bodies in the world.
+
+        :param body: The body.
         """
         pass
 
-    @abstractmethod
+    @deprecated("Use get_contact_points_between_two_bodies instead")
     def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList:
         """
-        Return a list of contact points between obj_a and obj_b.
+        Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of any type of bodies.
+        """
+        return self.get_contact_points_between_two_bodies(obj1, obj2)
+
+    @abstractmethod
+    def get_contact_points_between_two_bodies(self, body_1: PhysicalBody, body_2: PhysicalBody) -> ContactPointsList:
+        """
+        Return a list of contact points between two bodies.
 
-        :param obj1: The first object.
-        :param obj2: The second object.
-        :return: A list of all contact points between the two objects.
+        :param body_1: The first body.
+        :param body_2: The second body.
+        :return: A list of all contact points between the two bodies.
         """
         pass
 
-    def get_object_closest_points(self, obj: Object, max_distance: float) -> ClosestPointsList:
+    @deprecated("Use get_contact_points_between_two_bodies instead")
+    def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList:
+        """
+        Same as :meth:`get_contact_points_between_two_bodies` but with objects instead of any type of bodies.
         """
-        Return the closest points of this object with all other objects in the world.
+        return self.get_contact_points_between_two_bodies(obj1, obj2)
 
-        :param obj: The object.
-        :param max_distance: The maximum distance between the points.
+    def get_body_closest_points(self, body: PhysicalBody, max_distance: float) -> ClosestPointsList:
+        """
+        Return the closest points of this body with all other bodies in the world.
+
+        :param body: The body.
+        :param max_distance: The maximum allowed distance between the points.
         :return: A list of the closest points.
         """
-        all_obj_closest_points = [self.get_closest_points_between_objects(obj, other_obj, max_distance) for other_obj in
-                                  self.objects
-                                  if other_obj != obj]
+        all_obj_closest_points = [self.get_closest_points_between_two_bodies(body, other_body, max_distance)
+                                  for other_body in self.objects if other_body != body]
         return ClosestPointsList([point for closest_points in all_obj_closest_points for point in closest_points])
 
-    def get_closest_points_between_objects(self, object_a: Object, object_b: Object, max_distance: float) \
+    def get_closest_points_between_two_bodies(self, body_a: PhysicalBody, body_b: PhysicalBody, max_distance: float) \
             -> ClosestPointsList:
         """
         Return the closest points between two objects.
 
-        :param object_a: The first object.
-        :param object_b: The second object.
+        :param body_a: The first body.
+        :param body_b: The second body.
         :param max_distance: The maximum distance between the points.
         :return: A list of the closest points.
         """
@@ -1299,7 +1281,7 @@ def reset_world(self, remove_saved_states=False) -> None:
         self.restore_state(self.original_state_id)
         if remove_saved_states:
             self.remove_saved_states()
-        self.original_state_id = self.save_state(use_same_id=True)
+            self.original_state_id = self.save_state(use_same_id=True)
 
     def remove_saved_states(self) -> None:
         """
@@ -1684,6 +1666,15 @@ def original_state(self) -> WorldState:
     def __del__(self):
         self.exit()
 
+    def __eq__(self, other: World):
+        if not isinstance(other, self.__class__):
+            return False
+        return (self.is_prospection_world == other.is_prospection_world
+                and self.id == other.id)
+
+    def __hash__(self):
+        return hash((self.__class__.__name__, self.is_prospection_world, self.id))
+
 
 class UseProspectionWorld:
     """
diff --git a/src/pycram/datastructures/world_entity.py b/src/pycram/datastructures/world_entity.py
index 41c317fad..fe9756bf2 100644
--- a/src/pycram/datastructures/world_entity.py
+++ b/src/pycram/datastructures/world_entity.py
@@ -1,13 +1,23 @@
+from __future__ import annotations
+
 import os
 import pickle
 from abc import ABC, abstractmethod
+from copy import copy
 
-from typing_extensions import TYPE_CHECKING, Dict, Optional
+from trimesh.parent import Geometry3D
+from typing_extensions import TYPE_CHECKING, Dict, Optional, List, deprecated, Union, Type
 
-from .dataclasses import State
+from pycrap import PhysicalObject
+from .dataclasses import State, ContactPointsList, ClosestPointsList, Color, PhysicalBodyState, \
+    AxisAlignedBoundingBox, RotatedBoundingBox
+from .mixins import HasConcept
+from ..local_transformer import LocalTransformer
+from ..ros.data_types import Time
 
 if TYPE_CHECKING:
     from ..datastructures.world import World
+    from .pose import Pose, Point, GeoQuaternion as Quaternion, Transform
 
 
 class StateEntity:
@@ -110,10 +120,275 @@ def remove_saved_states(self) -> None:
 
 class WorldEntity(StateEntity, ABC):
     """
-    A data class that represents an entity of the world, such as an object or a link.
+    A class that represents an entity of the world, such as an object or a link.
     """
 
-    def __init__(self, _id: int, world: 'World'):
+    def __init__(self, _id: int, world: World):
         StateEntity.__init__(self)
         self.id = _id
-        self.world: 'World' = world
+        self.world: World = world
+
+    @property
+    @abstractmethod
+    def name(self) -> str:
+        """
+        :return: The name of this body.
+        """
+        ...
+
+    @property
+    @abstractmethod
+    def parent_entity(self) -> Optional[WorldEntity]:
+        """
+        :return: The parent entity of this entity, if it has one.
+        """
+        pass
+
+    def __eq__(self, other: WorldEntity) -> bool:
+        """
+        Check if this body is equal to another body.
+        """
+        if not isinstance(other, WorldEntity):
+            return False
+        return self.id == other.id and self.name == other.name and self.parent_entity == other.parent_entity
+
+    def __hash__(self) -> int:
+        return hash((self.id, self.name, self.parent_entity))
+
+
+class PhysicalBody(WorldEntity, HasConcept, ABC):
+    """
+    A class that represents a physical body in the world that has some related physical properties.
+    """
+
+    ontology_concept: Type[PhysicalObject] = PhysicalObject
+    """
+    The ontology concept of this entity.
+    """
+
+    def __init__(self, body_id: int, world: World):
+        WorldEntity.__init__(self, body_id, world)
+        HasConcept.__init__(self)
+        self.local_transformer = LocalTransformer()
+        self._is_translating: Optional[bool] = None
+        self._is_rotating: Optional[bool] = None
+        self._velocity: Optional[List[float]] = None
+
+    @abstractmethod
+    def get_axis_aligned_bounding_box(self) -> AxisAlignedBoundingBox:
+        """
+        :return: The axis-aligned bounding box of this body.
+        """
+        ...
+
+    @abstractmethod
+    def get_rotated_bounding_box(self) -> RotatedBoundingBox:
+        """
+        :return: The rotated bounding box of this body.
+        """
+        ...
+
+    def _plot_convex_hull(self):
+        """
+        Plot the convex hull of the geometry.
+        """
+        self.get_convex_hull().show()
+
+    @abstractmethod
+    def get_convex_hull(self) -> Geometry3D:
+        """
+        :return: The convex hull of this body.
+        """
+        ...
+
+    @property
+    def body_state(self) -> PhysicalBodyState:
+        return PhysicalBodyState(self.pose.copy(), self.is_translating, self.is_rotating, copy(self.velocity)
+                                 , self.world.conf.get_pose_tolerance())
+
+    @body_state.setter
+    def body_state(self, state: PhysicalBodyState) -> None:
+        if self.body_state != state:
+            self.pose = state.pose
+            self.is_translating = state.is_translating
+            self.is_rotating = state.is_rotating
+            self.velocity = state.velocity
+
+    @property
+    def velocity(self) -> Optional[List[float]]:
+        return self._velocity
+
+    @velocity.setter
+    def velocity(self, velocity: List[float]) -> None:
+        self._velocity = velocity
+
+    @property
+    def is_translating(self) -> Optional[bool]:
+        return self._is_translating
+
+    @is_translating.setter
+    def is_translating(self, is_translating: bool) -> None:
+        self._is_translating = is_translating
+
+    @property
+    def is_rotating(self) -> Optional[bool]:
+        return self._is_rotating
+
+    @is_rotating.setter
+    def is_rotating(self, is_rotating: bool) -> None:
+        self._is_rotating = is_rotating
+
+    @property
+    def position(self) -> Point:
+        """
+        :return: A Point object containing the position of the link relative to the world frame.
+        """
+        return self.pose.position
+
+    @property
+    def position_as_list(self) -> List[float]:
+        """
+        :return: A list containing the position of the link relative to the world frame.
+        """
+        return self.pose.position_as_list()
+
+    @property
+    def orientation(self) -> Quaternion:
+        """
+        :return: A Quaternion object containing the orientation of the link relative to the world frame.
+        """
+        return self.pose.orientation
+
+    @property
+    def orientation_as_list(self) -> List[float]:
+        """
+        :return: A list containing the orientation of the link relative to the world frame.
+        """
+        return self.pose.orientation_as_list()
+
+    @property
+    def pose_as_list(self) -> List[List[float]]:
+        """
+        :return: A list containing the position and orientation of the link relative to the world frame.
+        """
+        return self.pose.to_list()
+
+    @property
+    def transform(self) -> Transform:
+        """
+        The transform of this entity.
+
+        :return: The transform of this entity.
+        """
+        return self.pose.to_transform(self.tf_frame)
+
+    def update_transform(self, transform_time: Optional[Time] = None) -> None:
+        """
+        Update the transformation of this link at the given time.
+
+        :param transform_time: The time at which the transformation should be updated.
+        """
+        self.local_transformer.update_transforms([self.transform], transform_time)
+
+    @property
+    @abstractmethod
+    def pose(self) -> Pose:
+        """
+        :return: A Pose object containing the pose of the link relative to the world frame.
+        """
+        ...
+
+    @pose.setter
+    @abstractmethod
+    def pose(self, pose: Pose) -> None:
+        """
+        Set the pose of this body.
+
+        :param pose: The pose of this body.
+        """
+        ...
+
+    @property
+    @abstractmethod
+    def tf_frame(self) -> str:
+        """
+        The tf frame of this entity.
+
+        :return: The tf frame of this entity.
+        """
+        pass
+
+    @property
+    def contact_points(self) -> ContactPointsList:
+        """
+        :return: The contact points of this body with other physical bodies.
+        """
+        return self.world.get_body_contact_points(self)
+
+    def get_contact_points_with_body(self, body: PhysicalBody) -> ContactPointsList:
+        """
+        :param body: The body to get the contact points with.
+        :return: The contact points of this body with the given body.
+        """
+        return self.world.get_contact_points_between_two_bodies(self, body)
+
+    def closest_points(self, max_distance: float) -> ClosestPointsList:
+        """
+        :param max_distance: The maximum distance to consider a body as close, only points closer than or equal to this
+         distance will be returned.
+        :return: The closest points of this body with other physical bodies within the given maximum distance.
+        """
+        return self.world.get_body_closest_points(self, max_distance)
+
+    def get_closest_points_with_body(self, body: PhysicalBody, max_distance: float) -> ClosestPointsList:
+        """
+        :param body: The body to get the points with.
+        :param max_distance: The maximum distance to consider a body as close, only points closer than or equal to this
+         distance will be returned.
+        :return: The closest points of this body with the given body within the given maximum distance.
+        """
+        return self.world.get_closest_points_between_two_bodies(self, body, max_distance)
+
+    @property
+    def is_moving(self) -> Optional[bool]:
+        """
+        :return: True if this body is moving, False if not, and None if not known.
+        """
+        if self.is_translating is not None or self.is_rotating is not None:
+            return self.is_translating or self.is_rotating
+        else:
+            return None
+
+    @property
+    def is_stationary(self) -> Optional[bool]:
+        """
+        :return: True if this body is stationary, False otherwise.
+        """
+        return None if self.is_moving is None else not self.is_moving
+
+    @property
+    @abstractmethod
+    def color(self) -> Union[Color, Dict[str, Color]]:
+        """
+        :return: A Color object containing the rgba_color of this body or a dictionary if the body is articulated.
+        """
+        ...
+
+    @deprecated("Use color property setter instead")
+    def set_color(self, color: Color) -> None:
+        """
+        Set the color of this body, could be rgb or rgba.
+
+        :param color: The color as a list of floats, either rgb or rgba.
+        """
+        self.color = color
+
+    @color.setter
+    @abstractmethod
+    def color(self, color: Color) -> None:
+        """
+        Set the color of this body, could be rgb or rgba.
+
+        :param color: The color as a list of floats, either rgb or rgba.
+        """
+        ...
diff --git a/src/pycram/description.py b/src/pycram/description.py
index 10257a9ab..ff14c1f91 100644
--- a/src/pycram/description.py
+++ b/src/pycram/description.py
@@ -5,20 +5,19 @@
 import pathlib
 from abc import ABC, abstractmethod
 
-from trimesh.parent import Geometry3D
-
-from .ros.data_types import Time
 import trimesh
-from geometry_msgs.msg import Point, Quaternion
-from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, deprecated
+from geometry_msgs.msg import Point
+from trimesh.parent import Geometry3D
+from typing_extensions import Tuple, Union, Any, List, Optional, Dict, TYPE_CHECKING, Self, Sequence
 
 from .datastructures.dataclasses import JointState, AxisAlignedBoundingBox, Color, LinkState, VisualShape, \
     MeshVisualShape, RotatedBoundingBox
 from .datastructures.enums import JointType
 from .datastructures.pose import Pose, Transform
-from .datastructures.world_entity import WorldEntity
+from .datastructures.world_entity import WorldEntity, PhysicalBody
 from .failures import ObjectDescriptionNotFound, LinkHasNoGeometry, LinkGeometryHasNoMesh
 from .local_transformer import LocalTransformer
+from .ros.logging import logwarn_once
 
 if TYPE_CHECKING:
     from .world_concepts.world_object import Object
@@ -29,6 +28,12 @@ class EntityDescription(ABC):
     A description of an entity. This can be a link, joint or object description.
     """
 
+    def __init__(self, parsed_description: Optional[Any] = None):
+        """
+        :param parsed_description: The parsed description (most likely from a description file) of the entity.
+        """
+        self.parsed_description = parsed_description
+
     @property
     @abstractmethod
     def origin(self) -> Pose:
@@ -74,7 +79,7 @@ def __init__(self, parsed_joint_description: Optional[Any] = None, is_virtual: b
         :param parsed_joint_description: The parsed description of the joint (e.g. from urdf or mjcf file).
         :param is_virtual: True if the joint is virtual (i.e. not a physically existing joint), False otherwise.
         """
-        self.parsed_description = parsed_joint_description
+        super().__init__(parsed_joint_description)
         self.is_virtual: Optional[bool] = is_virtual
 
     @property
@@ -158,14 +163,13 @@ def friction(self) -> float:
         raise NotImplementedError
 
 
-class ObjectEntity(WorldEntity):
+class ObjectEntity:
     """
-    An abstract base class that represents a physical part/entity of an Object.
+    An abstract base class that represents a part of an Object.
     This can be a link or a joint of an Object.
     """
 
-    def __init__(self, _id: int, obj: Object):
-        WorldEntity.__init__(self, _id, obj.world)
+    def __init__(self, obj: Object):
         self.object: Object = obj
 
     @property
@@ -175,33 +179,6 @@ def object_name(self) -> str:
         """
         return self.object.name
 
-    @property
-    @abstractmethod
-    def pose(self) -> Pose:
-        """
-        :return: The pose of this entity relative to the world frame.
-        """
-        pass
-
-    @property
-    def transform(self) -> Transform:
-        """
-        The transform of this entity.
-
-        :return: The transform of this entity.
-        """
-        return self.pose.to_transform(self.tf_frame)
-
-    @property
-    @abstractmethod
-    def tf_frame(self) -> str:
-        """
-        The tf frame of this entity.
-
-        :return: The tf frame of this entity.
-        """
-        pass
-
     @property
     def object_id(self) -> int:
         """
@@ -210,18 +187,32 @@ def object_id(self) -> int:
         return self.object.id
 
 
-class Link(ObjectEntity, LinkDescription, ABC):
+class Link(PhysicalBody, ObjectEntity, LinkDescription, ABC):
     """
     A link of an Object in the World.
     """
 
     def __init__(self, _id: int, link_description: LinkDescription, obj: Object):
-        ObjectEntity.__init__(self, _id, obj)
+        PhysicalBody.__init__(self, _id, obj.world)
+        ObjectEntity.__init__(self, obj)
         LinkDescription.__init__(self, link_description.parsed_description, link_description.mesh_dir)
+        self.description = link_description
         self.local_transformer: LocalTransformer = LocalTransformer()
         self.constraint_ids: Dict[Link, int] = {}
-        self._current_pose: Optional[Pose] = None
-        self.update_pose()
+
+    @property
+    def parent_entity(self) -> Object:
+        """
+        :return: The parent of this link, which is the object.
+        """
+        return self.object
+
+    @property
+    def name(self) -> str:
+        """
+        :return: The name of this link.
+        """
+        return self.description.name
 
     def get_axis_aligned_bounding_box(self, transform_to_link_pose: bool = True) -> AxisAlignedBoundingBox:
         """
@@ -267,7 +258,7 @@ def get_convex_hull(self) -> Geometry3D:
         :return: The convex hull of the link geometry.
         """
         try:
-            return self.world.get_link_convex_hull(self)
+            return self.world.get_body_convex_hull(self)
         except NotImplementedError:
             if isinstance(self.geometry, MeshVisualShape):
                 mesh_path = self.get_mesh_path(self.geometry)
@@ -303,7 +294,7 @@ def get_mesh_filename(self, geometry: MeshVisualShape) -> str:
         else:
             raise LinkGeometryHasNoMesh(self.name, type(geometry).__name__)
 
-    def set_pose(self, pose: Pose) -> None:
+    def set_object_pose_given_link_pose(self, pose: Pose) -> None:
         """
         Set the pose of this link to the given pose.
         NOTE: This will move the entire object such that the link is at the given pose, it will not consider any joints
@@ -345,7 +336,7 @@ def get_transform_to_root_link(self) -> Transform:
 
     @property
     def current_state(self) -> LinkState:
-        return LinkState(self.constraint_ids.copy())
+        return LinkState(self.body_state, self.constraint_ids.copy())
 
     @current_state.setter
     def current_state(self, link_state: LinkState) -> None:
@@ -353,6 +344,7 @@ def current_state(self, link_state: LinkState) -> None:
             if not self.all_constraint_links_belong_to_same_world(link_state):
                 raise ValueError("All constraint links must belong to the same world, since the constraint ids"
                                  "are unique to the world and cannot be transferred between worlds.")
+            self.body_state = link_state.body_state
             self.constraint_ids = link_state.constraint_ids
 
     def all_constraint_links_belong_to_same_world(self, other: LinkState) -> bool:
@@ -406,14 +398,6 @@ def is_root(self) -> bool:
         """
         return self.object.get_root_link_id() == self.id
 
-    def update_transform(self, transform_time: Optional[Time] = None) -> None:
-        """
-        Update the transformation of this link at the given time.
-
-        :param transform_time: The time at which the transformation should be updated.
-        """
-        self.local_transformer.update_transforms([self.transform], transform_time)
-
     def get_transform_to_link(self, link: 'Link') -> Transform:
         """
         :param link: The link to which the transformation should be returned.
@@ -435,61 +419,23 @@ def get_pose_wrt_link(self, link: 'Link') -> Pose:
         """
         return self.local_transformer.transform_pose(self.pose, link.tf_frame)
 
-    @property
-    def position(self) -> Point:
-        """
-        :return: A Point object containing the position of the link relative to the world frame.
-        """
-        return self.pose.position
-
-    @property
-    def position_as_list(self) -> List[float]:
-        """
-        :return: A list containing the position of the link relative to the world frame.
-        """
-        return self.pose.position_as_list()
-
-    @property
-    def orientation(self) -> Quaternion:
-        """
-        :return: A Quaternion object containing the orientation of the link relative to the world frame.
-        """
-        return self.pose.orientation
-
-    @property
-    def orientation_as_list(self) -> List[float]:
-        """
-        :return: A list containing the orientation of the link relative to the world frame.
-        """
-        return self.pose.orientation_as_list()
-
-    def update_pose(self) -> None:
+    def get_origin_transform(self) -> Transform:
         """
-        Update the current pose of this link from the world.
+        :return: the transformation between the link frame and the origin frame of this link.
         """
-        self._current_pose = self.world.get_link_pose(self)
+        return self.origin.to_transform(self.tf_frame)
 
     @property
     def pose(self) -> Pose:
         """
-        :return: A Pose object containing the pose of the link relative to the world frame.
+        :return: The pose of this link.
         """
-        if self.world.conf.update_poses_from_sim_on_get:
-            self.update_pose()
-        return self._current_pose
-
-    @property
-    def pose_as_list(self) -> List[List[float]]:
-        """
-        :return: A list containing the position and orientation of the link relative to the world frame.
-        """
-        return self.pose.to_list()
+        return self.world.get_link_pose(self)
 
-    def get_origin_transform(self) -> Transform:
-        """
-        :return: the transformation between the link frame and the origin frame of this link.
-        """
-        return self.origin.to_transform(self.tf_frame)
+    @pose.setter
+    def pose(self, pose: Pose) -> None:
+        logwarn_once("Setting the pose of a link is not allowed,"
+                     " change object pose and/or joint position to affect the link pose.")
 
     @property
     def color(self) -> Color:
@@ -498,15 +444,6 @@ def color(self) -> Color:
         """
         return self.world.get_link_color(self)
 
-    @deprecated("Use color property setter instead")
-    def set_color(self, color: Color) -> None:
-        """
-        Set the color of this link, could be rgb or rgba.
-
-        :param color: The color as a list of floats, either rgb or rgba.
-        """
-        self.color = color
-
     @color.setter
     def color(self, color: Color) -> None:
         """
@@ -516,13 +453,6 @@ def color(self, color: Color) -> None:
         """
         self.world.set_link_color(self, color)
 
-    @property
-    def origin_transform(self) -> Transform:
-        """
-        :return: The transform from world to origin of entity.
-        """
-        return self.origin.to_transform(self.tf_frame)
-
     @property
     def tf_frame(self) -> str:
         """
@@ -530,14 +460,15 @@ def tf_frame(self) -> str:
         """
         return f"{self.object.tf_frame}/{self.name}"
 
-    def __eq__(self, other):
-        return self.id == other.id and self.object == other.object and self.name == other.name
+    @property
+    def origin_transform(self) -> Transform:
+        """
+        The transformation between the link frame and the origin frame of this link.
+        """
+        return self.origin.to_transform(self.tf_frame)
 
     def __copy__(self):
-        return Link(self.id, self, self.object)
-
-    def __hash__(self):
-        return hash((self.id, self.object, self.name))
+        return Link(self.id, self.description, self.object)
 
 
 class RootLink(Link, ABC):
@@ -547,7 +478,7 @@ class RootLink(Link, ABC):
     """
 
     def __init__(self, obj: Object):
-        super().__init__(obj.get_root_link_id(), obj.get_root_link_description(), obj)
+        Link.__init__(self, obj.get_root_link_id(), obj.get_root_link_description(), obj)
 
     @property
     def tf_frame(self) -> str:
@@ -556,14 +487,18 @@ def tf_frame(self) -> str:
         """
         return self.object.tf_frame
 
-    def update_pose(self) -> None:
-        self._current_pose = self.world.get_object_pose(self.object)
+    @property
+    def pose(self) -> Pose:
+        """
+        :return: The pose of the root link, which is the same as the pose of the object.
+        """
+        return self.object.pose
 
     def __copy__(self):
         return RootLink(self.object)
 
 
-class Joint(ObjectEntity, JointDescription, ABC):
+class Joint(WorldEntity, ObjectEntity, JointDescription, ABC):
     """
     Represent a joint of an Object in the World.
     """
@@ -571,12 +506,28 @@ class Joint(ObjectEntity, JointDescription, ABC):
     def __init__(self, _id: int,
                  joint_description: JointDescription,
                  obj: Object, is_virtual: Optional[bool] = False):
-        ObjectEntity.__init__(self, _id, obj)
+        WorldEntity.__init__(self, _id, obj.world)
+        ObjectEntity.__init__(self, obj)
         JointDescription.__init__(self, joint_description.parsed_description, is_virtual)
+        self.description = joint_description
         self.acceptable_error = (self.world.conf.revolute_joint_position_tolerance if self.type == JointType.REVOLUTE
                                  else self.world.conf.prismatic_joint_position_tolerance)
         self._update_position()
 
+    @property
+    def name(self) -> str:
+        """
+        :return: The name of this joint.
+        """
+        return self.description.name
+
+    @property
+    def parent_entity(self) -> Link:
+        """
+        :return: The parent of this joint, which is the object.
+        """
+        return self.parent_link
+
     @property
     def tf_frame(self) -> str:
         """
@@ -674,13 +625,7 @@ def current_state(self, joint_state: JointState) -> None:
             self.position = joint_state.position
 
     def __copy__(self):
-        return Joint(self.id, self, self.object)
-
-    def __eq__(self, other):
-        return self.id == other.id and self.object == other.object and self.name == other.name
-
-    def __hash__(self):
-        return hash((self.id, self.object, self.name))
+        return Joint(self.id, self.description, self.object, self.is_virtual)
 
 
 class ObjectDescription(EntityDescription):
@@ -706,7 +651,7 @@ def __init__(self, path: Optional[str] = None):
         """
         :param path: The path of the file to update the description data from.
         """
-
+        super().__init__(None)
         self._links: Optional[List[LinkDescription]] = None
         self._joints: Optional[List[JointDescription]] = None
         self._link_map: Optional[Dict[str, Any]] = None
diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py
index 1ca3464b1..9a502f34e 100644
--- a/src/pycram/designators/action_designator.py
+++ b/src/pycram/designators/action_designator.py
@@ -24,6 +24,7 @@
 from ..failures import ObjectUnfetchable, ReachabilityFailure, NavigationGoalNotReachedError, PerceptionObjectNotFound, \
     ObjectNotGraspedError
 from ..robot_description import RobotDescription
+from ..ros.ros_tools import sleep
 from ..tasktree import with_tree
 from ..world_reasoning import contact
 
@@ -279,6 +280,7 @@ def plan(self) -> None:
         robot = World.robot
         # Retrieve object and robot from designators
         object = self.object_designator.world_object
+
         # Get grasp orientation and target pose
         grasp = RobotDescription.current_robot_description.grasps[self.grasp]
         # oTm = Object Pose in Frame map
diff --git a/src/pycram/object_descriptors/urdf.py b/src/pycram/object_descriptors/urdf.py
index bdb48b8f7..bf7c196a8 100644
--- a/src/pycram/object_descriptors/urdf.py
+++ b/src/pycram/object_descriptors/urdf.py
@@ -4,7 +4,7 @@
 
 import numpy as np
 
-from ..ros.logging import logerr
+from ..ros.logging import logerr, logwarn
 from ..ros.ros_tools import create_ros_pack, ResourceNotFound, get_parameter
 from geometry_msgs.msg import Point
 from tf.transformations import quaternion_from_euler, euler_from_quaternion
@@ -82,6 +82,10 @@ def collision(self) -> Union[Collision, List[Collision], None]:
             else:
                 return self.parsed_description.collisions
 
+    @property
+    def all_collisions(self) -> List[Collision]:
+        return self.parsed_description.collisions
+
 
 class JointDescription(AbstractJointDescription):
     urdf_type_map = {'unknown': JointType.UNKNOWN,
diff --git a/src/pycram/pose_generator_and_validator.py b/src/pycram/pose_generator_and_validator.py
index a6d55412c..2dc14f344 100644
--- a/src/pycram/pose_generator_and_validator.py
+++ b/src/pycram/pose_generator_and_validator.py
@@ -191,6 +191,8 @@ def reachability_validator(pose: Pose,
     # TODO Make orientation adhere to grasping orientation
     res = False
     arms = []
+    found_grasps = []
+    original_target = target
     for description in manipulator_descs:
         retract_target_pose = LocalTransformer().transform_pose(target, robot.get_link_tf_frame(
             description.end_effector.tool_frame))
diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py
index 9351b8890..e9909a20f 100644
--- a/src/pycram/process_modules/pr2_process_modules.py
+++ b/src/pycram/process_modules/pr2_process_modules.py
@@ -22,6 +22,11 @@
     if Multiverse is not None:
         logwarn("Import for control_msgs for gripper in Multiverse failed")
 
+try:
+    from ..worlds import Multiverse
+except ImportError:
+    Multiverse = type(None)
+
 try:
     from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2
 except ImportError:
@@ -58,7 +63,7 @@ def feedback_callback(msg):
 
 class Pr2MoveGripperReal(ProcessModule):
     """
-    Opens or closes the gripper of the real robot, gripper uses an action server for this instead of giskard
+    Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard
     """
 
     def _execute(self, designator: MoveGripperMotion):
diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py
index e04f1a38d..654098883 100644
--- a/src/pycram/robot_description.py
+++ b/src/pycram/robot_description.py
@@ -119,10 +119,16 @@ class RobotDescription:
     Virtual mobile base joint names for mobile robots, these joints are not part of the URDF, however they are used to
     move the robot in the simulation (e.g. set_pose for the robot would actually move these joints)
     """
+    gripper_name: Optional[str] = None
+    """
+    Name of the gripper of the robot if it has one, this is used when the gripper is a different Object with its own
+    description file outside the robot description file.
+    """
 
     def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str,
                  virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None,
-                 ignore_joints: Optional[List[str]] = None):
+                 ignore_joints: Optional[List[str]] = None,
+                 gripper_name: Optional[str] = None):
         """
         Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic
         chains.
@@ -135,6 +141,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str,
         :param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots
         :param mjcf_path: Path to the MJCF file of the robot
         :param ignore_joints: List of joint names that are not used.
+        :param gripper_name: Name of the gripper of the robot if it has one and is a separate Object.
         """
         self.name = name
         self.base_link = base_link
@@ -152,6 +159,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str,
         self.links: List[str] = [l.name for l in self.urdf_object.links]
         self.joints: List[str] = [j.name for j in self.urdf_object.joints]
         self.virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = virtual_mobile_base_joints
+        self.gripper_name = gripper_name
 
     @property
     def has_actuators(self):
diff --git a/src/pycram/robot_descriptions/ur5e_controlled_description.py b/src/pycram/robot_descriptions/ur5e_controlled_description.py
index 403c5669a..125ef1495 100644
--- a/src/pycram/robot_descriptions/ur5e_controlled_description.py
+++ b/src/pycram/robot_descriptions/ur5e_controlled_description.py
@@ -20,7 +20,7 @@
     filename = get_robot_urdf_path_from_multiverse(robot_relative_dir, ROBOT_NAME, resources_dir=multiverse_resources)
     mjcf_filename = get_robot_mjcf_path(robot_relative_dir, ROBOT_NAME, multiverse_resources=multiverse_resources)
     ur5_description = RobotDescription(ROBOT_NAME, "base_link", "", "",
-                                       filename, mjcf_path=mjcf_filename)
+                                       filename, mjcf_path=mjcf_filename, gripper_name=GRIPPER_NAME)
 
     ################################## Arm ##################################
     arm = KinematicChainDescription("manipulator", "base_link", "wrist_3_link",
diff --git a/src/pycram/ros/logging.py b/src/pycram/ros/logging.py
index 213d911eb..f284b6598 100644
--- a/src/pycram/ros/logging.py
+++ b/src/pycram/ros/logging.py
@@ -15,6 +15,7 @@
 logger_level_service_caller = LoggerLevelServiceCaller()
 
 
+
 def _get_caller_method_name():
     """
     Get the name of the method that called the function from which this function is called. It is intended as a helper
diff --git a/src/pycram/world_concepts/constraints.py b/src/pycram/world_concepts/constraints.py
index 14449d302..4fadedd5e 100644
--- a/src/pycram/world_concepts/constraints.py
+++ b/src/pycram/world_concepts/constraints.py
@@ -51,7 +51,7 @@ def set_child_link_pose(self):
         """
         Set the target pose of the child object to the current pose of the child object in the parent object frame.
         """
-        self.child_link.set_pose(self.get_child_link_target_pose())
+        self.child_link.set_object_pose_given_link_pose(self.get_child_link_target_pose())
 
     def get_child_link_target_pose(self) -> Pose:
         """
diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py
index 034385126..11a36b26f 100644
--- a/src/pycram/world_concepts/world_object.py
+++ b/src/pycram/world_concepts/world_object.py
@@ -2,6 +2,7 @@
 
 import logging
 import os
+from functools import cached_property
 from pathlib import Path
 
 import numpy as np
@@ -18,13 +19,14 @@
 from ..datastructures.enums import ObjectType, JointType
 from ..datastructures.pose import Pose, Transform
 from ..datastructures.world import World
-from ..datastructures.world_entity import WorldEntity
+from ..datastructures.world_entity import PhysicalBody, WorldEntity
 from ..description import ObjectDescription, LinkDescription, Joint
 from ..failures import ObjectAlreadyExists, WorldMismatchErrorBetweenObjects, UnsupportedFileExtension, \
     ObjectDescriptionUndefined
 from ..local_transformer import LocalTransformer
 from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
 from ..object_descriptors.urdf import ObjectDescription as URDF
+from ..ros.data_types import Time
 from ..ros.logging import logwarn
 
 try:
@@ -39,7 +41,7 @@
 Link = ObjectDescription.Link
 
 
-class Object(WorldEntity, HasConcept):
+class Object(PhysicalBody):
     """
     Represents a spawned Object in the World.
     """
@@ -54,13 +56,11 @@ class Object(WorldEntity, HasConcept):
     A dictionary that maps the file extension to the corresponding ObjectDescription type.
     """
 
-    ontology_concept: Type[PhysicalObject] = PhysicalObject
-
     def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] = None,
                  description: Optional[ObjectDescription] = None,
                  pose: Optional[Pose] = None,
                  world: Optional[World] = None,
-                 color: Color = Color(),
+                 color: Optional[Color] = None,
                  ignore_cached_files: bool = False,
                  scale_mesh: Optional[float] = None,
                  mesh_transform: Optional[Transform] = None):
@@ -95,7 +95,6 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str]
         self.name: str = name
         self.path: Optional[str] = path
 
-        self.color: Color = color
         self._resolve_description(path, description)
         self.cache_manager = self.world.cache_manager
 
@@ -117,18 +116,67 @@ def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str]
 
         self.id = self._spawn_object_and_get_id()
 
-        self.tf_frame = (self.tf_prospection_world_prefix if self.world.is_prospection_world else "") + self.name
-
         self._init_joint_name_and_id_map()
         self._init_link_name_and_id_map()
 
         self._init_links_and_update_transforms()
+
+        if color is not None:
+            self.color: Color = color
+
         self._init_joints()
 
         self.attachments: Dict[Object, Attachment] = {}
 
         self.world.add_object(self)
 
+    @property
+    def tf_frame(self) -> str:
+        """
+        The tf frame of the object.
+        """
+        return (self.tf_prospection_world_prefix if self.world.is_prospection_world else "") + self.name
+
+    @property
+    def color(self) -> Union[Color, Dict[str, Color]]:
+        """
+        Return the rgba_color of this object. The return is either:
+
+            1. A Color object with RGBA values, this is the case if the object only has one link (this
+                happens for example if the object is spawned from a .obj or .stl file)
+            2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object.
+                Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which
+                 the object is spawned.
+
+        :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and
+         the rgba_color as value.
+        """
+        link_to_color_dict = self.links_colors
+
+        if len(link_to_color_dict) == 1:
+            return list(link_to_color_dict.values())[0]
+        else:
+            return link_to_color_dict
+
+    @color.setter
+    def color(self, rgba_color: Union[Color, Dict[str, Color]]) -> None:
+        """
+        Change the color of this object.
+
+        :param rgba_color: The color as Color object with RGBA values between 0 and 1
+        """
+        # Check if there is only one link, this is the case for primitive
+        # forms or if loaded from an .stl or .obj file
+        if self.has_one_link:
+            self.root_link.color = rgba_color
+        else:
+            if isinstance(rgba_color, Color):
+                for link in self.links.values():
+                    link.color = rgba_color
+            else:
+                for link_name, color in rgba_color.items():
+                    self.links[link_name].color = color
+
     def get_mesh_path(self) -> str:
         """
         Get the path to the mesh file of the object.
@@ -294,7 +342,7 @@ def pose(self):
         """
         The current pose of the object.
         """
-        return self.get_pose()
+        return self.world.get_object_pose(self)
 
     @pose.setter
     def pose(self, pose: Pose):
@@ -744,15 +792,14 @@ def get_orientation_as_list(self) -> List[float]:
         """
         return self.get_pose().orientation_as_list()
 
+    @deprecated("Use property 'pose' instead.")
     def get_pose(self) -> Pose:
         """
         Return the position of this object as a list of xyz. Alias for :func:`~Object.get_position`.
 
         :return: The current pose of this object
         """
-        if self.world.conf.update_poses_from_sim_on_get:
-            self.update_pose()
-        return self._current_pose
+        return self.pose
 
     def set_pose(self, pose: Pose, base: bool = False, set_attachments: bool = True) -> None:
         """
@@ -771,25 +818,8 @@ def set_pose(self, pose: Pose, base: bool = False, set_attachments: bool = True)
         if set_attachments:
             self._set_attached_objects_poses()
 
-    def reset_base_pose(self, pose: Pose):
-        if self.world.reset_object_base_pose(self, pose):
-            self.update_pose()
-
-    def update_pose(self):
-        """
-        Update the current pose of this object from the world, and updates the poses of all links.
-        """
-        self._current_pose = self.world.get_object_pose(self)
-        # TODO: Probably not needed, need to test
-        self._update_all_links_poses()
-        self.update_link_transforms()
-
-    def _update_all_links_poses(self):
-        """
-        Update the poses of all links by getting them from the simulator.
-        """
-        for link in self.links.values():
-            link.update_pose()
+    def reset_base_pose(self, pose: Pose) -> bool:
+        return self.world.reset_object_base_pose(self, pose)
 
     def move_base_to_origin_pose(self) -> None:
         """
@@ -798,15 +828,16 @@ def move_base_to_origin_pose(self) -> None:
         """
         self.set_pose(self.get_pose(), base=True)
 
-    def save_state(self, state_id) -> None:
+    def save_state(self, state_id: int, save_dir: Optional[str] = None) -> None:
         """
         Save the state of this object by saving the state of all links and attachments.
 
         :param state_id: The unique id of the state.
+        :param save_dir: The directory in which to save the state.
         """
         self.save_links_states(state_id)
         self.save_joints_states(state_id)
-        super().save_state(state_id)
+        super().save_state(state_id, save_dir)
 
     def save_links_states(self, state_id: int) -> None:
         """
@@ -831,8 +862,8 @@ def current_state(self) -> ObjectState:
         """
         The current state of this object as an ObjectState.
         """
-        return ObjectState(self.get_pose().copy(), self.attachments.copy(), self.link_states.copy(),
-                           self.joint_states.copy(), self.world.conf.get_pose_tolerance())
+        return ObjectState(self.body_state, self.attachments.copy(), self.link_states.copy(),
+                           self.joint_states.copy())
 
     @current_state.setter
     def current_state(self, state: ObjectState) -> None:
@@ -840,7 +871,7 @@ def current_state(self, state: ObjectState) -> None:
         Set the current state of this object to the given state.
         """
         if self.current_state != state:
-            self.set_pose(state.pose, base=False, set_attachments=False)
+            self.body_state = state.body_state
             self.set_attachments(state.attachments)
             self.link_states = state.link_states
             self.joint_states = state.joint_states
@@ -1151,7 +1182,7 @@ def set_joint_position(self, joint_name: str, joint_position: float) -> None:
         """
         self.clip_joint_positions_to_limits({joint_name: joint_position})
         if self.world.reset_joint_position(self.joints[joint_name], joint_position):
-            self._update_on_joint_position_change()
+            self._set_attached_objects_poses()
 
     @deprecated("Use set_multiple_joint_positions instead")
     def set_joint_positions(self, joint_positions: Dict[str, float]) -> None:
@@ -1167,7 +1198,7 @@ def set_multiple_joint_positions(self, joint_positions: Dict[str, float]) -> Non
         joint_positions = {self.joints[joint_name]: joint_position
                            for joint_name, joint_position in joint_positions.items()}
         if self.world.set_multiple_joint_positions(joint_positions):
-            self._update_on_joint_position_change()
+            self._set_attached_objects_poses()
 
     def clip_joint_positions_to_limits(self, joint_positions: Dict[str, float]) -> Dict[str, float]:
         """
@@ -1181,12 +1212,6 @@ def clip_joint_positions_to_limits(self, joint_positions: Dict[str, float]) -> D
                 if self.joints[joint_name].has_limits else joint_position
                 for joint_name, joint_position in joint_positions.items()}
 
-    def _update_on_joint_position_change(self):
-        self.update_pose()
-        self._update_all_links_poses()
-        self.update_link_transforms()
-        self._set_attached_objects_poses()
-
     def get_joint_position(self, joint_name: str) -> float:
         """
         Return the current position of the given joint.
@@ -1322,6 +1347,7 @@ def update_link_transforms(self, transform_time: Optional[Time] = None) -> None:
         for link in self.links.values():
             link.update_transform(transform_time)
 
+    @property
     def contact_points(self) -> ContactPointsList:
         """
         Return a list of contact points of this Object with other Objects.
@@ -1338,7 +1364,7 @@ def contact_points_simulated(self) -> ContactPointsList:
         """
         state_id = self.world.save_state()
         self.world.step()
-        contact_points = self.contact_points()
+        contact_points = self.contact_points
         self.world.restore_state(state_id)
         return contact_points
 
@@ -1349,7 +1375,7 @@ def closest_points(self, max_distance: float) -> ClosestPointsList:
         :param max_distance: The maximum distance between the closest points
         :return: A list of closest points between this Object and other Objects
         """
-        return self.world.get_object_closest_points(self, max_distance)
+        return self.world.get_body_closest_points(self, max_distance)
 
     def closest_points_with_obj(self, other_object: Object, max_distance: float) -> ClosestPointsList:
         """
@@ -1359,42 +1385,15 @@ def closest_points_with_obj(self, other_object: Object, max_distance: float) ->
         :param max_distance: The maximum distance between the closest points
         :return: A list of closest points between this Object and the other Object
         """
-        return self.world.get_closest_points_between_objects(self, other_object, max_distance)
+        return self.world.get_closest_points_between_two_bodies(self, other_object, max_distance)
 
+    @deprecated("Use property setter 'color' instead.")
     def set_color(self, rgba_color: Color) -> None:
-        """
-        Change the color of this object, the color has to be given as a list
-        of RGBA values.
-
-        :param rgba_color: The color as Color object with RGBA values between 0 and 1
-        """
-        # Check if there is only one link, this is the case for primitive
-        # forms or if loaded from an .stl or .obj file
-        if self.links != {}:
-            for link in self.links.values():
-                link.color = rgba_color
-        else:
-            self.root_link.color = rgba_color
+        self.color = rgba_color
 
+    @deprecated("Use property 'color' instead.")
     def get_color(self) -> Union[Color, Dict[str, Color]]:
-        """
-        Return the rgba_color of this object. The return is either:
-
-            1. A Color object with RGBA values, this is the case if the object only has one link (this
-                happens for example if the object is spawned from a .obj or .stl file)
-            2. A dict with the link name as key and the rgba_color as value. The rgba_color is given as a Color Object.
-                Please keep in mind that not every link may have a rgba_color. This is dependent on the URDF from which
-                 the object is spawned.
-
-        :return: The rgba_color as Color object with RGBA values between 0 and 1 or a dict with the link name as key and
-         the rgba_color as value.
-        """
-        link_to_color_dict = self.links_colors
-
-        if len(link_to_color_dict) == 1:
-            return list(link_to_color_dict.values())[0]
-        else:
-            return link_to_color_dict
+        return self.color
 
     @property
     def links_colors(self) -> Dict[str, Color]:
@@ -1435,7 +1434,7 @@ def get_convex_hull(self) -> Geometry3D:
         if self.has_one_link:
             return self.root_link.get_convex_hull()
         else:
-            return self.world.get_object_convex_hull(self)
+            return self.world.get_body_convex_hull(self)
 
     def get_base_origin(self) -> Pose:
         """
@@ -1485,10 +1484,9 @@ def copy_to_world(self, world: World) -> Object:
                      world, self.color)
         return obj
 
-    def __eq__(self, other):
-        return (isinstance(other, Object) and self.id == other.id and self.name == other.name
-                and self.world == other.world)
-
-    def __hash__(self):
-        return hash((self.id, self.name, self.world))
+    def parent_entity(self) -> World:
+        """
+        :return: The parent of this object which is the world.
+        """
+        return self.world
 
diff --git a/src/pycram/world_reasoning.py b/src/pycram/world_reasoning.py
index b87e7a073..df013d945 100644
--- a/src/pycram/world_reasoning.py
+++ b/src/pycram/world_reasoning.py
@@ -53,11 +53,11 @@ def contact(
         prospection_obj1 = World.current_world.get_prospection_object_for_object(object1)
         prospection_obj2 = World.current_world.get_prospection_object_for_object(object2)
         World.current_world.perform_collision_detection()
-        con_points: ContactPointsList = World.current_world.get_contact_points_between_two_objects(prospection_obj1,
-                                                                                                   prospection_obj2)
+        con_points: ContactPointsList = World.current_world.get_contact_points_between_two_bodies(prospection_obj1,
+                                                                                                  prospection_obj2)
         objects_are_in_contact = len(con_points) > 0
         if return_links:
-            contact_links = [(point.link_a, point.link_b) for point in con_points]
+            contact_links = [(point.body_a, point.body_b) for point in con_points]
             return objects_are_in_contact, contact_links
         else:
             return objects_are_in_contact
diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py
index dee26a369..49a26f14e 100755
--- a/src/pycram/worlds/bullet_world.py
+++ b/src/pycram/worlds/bullet_world.py
@@ -16,6 +16,7 @@
 from ..datastructures.enums import ObjectType, WorldMode, JointType
 from ..datastructures.pose import Pose
 from ..datastructures.world import World
+from ..datastructures.world_entity import PhysicalBody
 from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
 from ..object_descriptors.urdf import ObjectDescription
 from ..ros.logging import logwarn, loginfo
@@ -36,7 +37,7 @@ class is the main interface to the Bullet Physics Engine and should be used to s
     manipulate the Bullet World.
     """
 
-    def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: bool = False,
+    def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection: bool = False,
                  use_multiverse_for_real_world_simulation: bool = False):
         """
         Creates a new simulation, the type decides of the simulation should be a rendered window or just run in the
@@ -44,10 +45,10 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
         The BulletWorld object also initializes the Events for attachment, detachment and for manipulating the world.
 
         :param mode: Can either be "GUI" for rendered window or "DIRECT" for non-rendered. The default is "GUI"
-        :param is_prospection_world: For internal usage, decides if this BulletWorld should be used as a shadow world.
+        :param is_prospection: For internal usage, decides if this BulletWorld should be used as a shadow world.
         :param use_multiverse_for_real_world_simulation: Whether to use the Multiverse for real world simulation.
         """
-        super().__init__(mode=mode, is_prospection_world=is_prospection_world)
+        super().__init__(mode=mode, is_prospection=is_prospection)
 
         if use_multiverse_for_real_world_simulation:
             self.add_multiverse_resources()
@@ -63,7 +64,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
         # Some default settings
         self.set_gravity([0, 0, -9.8])
 
-        if not is_prospection_world:
+        if not is_prospection:
             _ = Object("floor", Floor, "plane.urdf",
                        world=self)
 
@@ -191,30 +192,42 @@ def get_object_number_of_links(self, obj: Object) -> int:
     def perform_collision_detection(self) -> None:
         p.performCollisionDetection(physicsClientId=self.id)
 
-    def get_object_contact_points(self, obj: Object) -> ContactPointsList:
-        """
-        Get the contact points of the object with akk other objects in the world. The contact points are returned as a
-        ContactPointsList object.
-
-        :param obj: The object for which the contact points should be returned.
-        :return: The contact points of the object with all other objects in the world.
-        """
+    def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList:
         self.perform_collision_detection()
-        points_list = p.getContactPoints(obj.id, physicsClientId=self.id)
+        body_data = self.get_body_and_link_id(body, index='A')
+        points_list = p.getContactPoints(**body_data, physicsClientId=self.id)
         return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list
                                   if len(point) > 0])
 
-    def get_contact_points_between_two_objects(self, obj_a: Object, obj_b: Object) -> ContactPointsList:
+    def get_contact_points_between_two_bodies(self, obj_a: PhysicalBody, obj_b: PhysicalBody) -> ContactPointsList:
         self.perform_collision_detection()
-        points_list = p.getContactPoints(obj_a.id, obj_b.id, physicsClientId=self.id)
+        body_a_data = self.get_body_and_link_id(obj_a, index='A')
+        body_b_data = self.get_body_and_link_id(obj_b, index='B')
+        points_list = p.getContactPoints(**body_a_data, **body_b_data, physicsClientId=self.id)
         return ContactPointsList([ContactPoint(**self.parse_points_list_to_args(point)) for point in points_list
                                   if len(point) > 0])
 
-    def get_closest_points_between_objects(self, obj_a: Object, obj_b: Object, distance: float) -> ClosestPointsList:
-        points_list = p.getClosestPoints(obj_a.id, obj_b.id, distance, physicsClientId=self.id)
+    def get_body_closest_points(self, body: PhysicalBody, max_distance: float) -> ClosestPointsList:
+        all_obj_closest_points = [self.get_closest_points_between_two_bodies(body, other_body, max_distance)
+                                  for other_body in self.objects if other_body != body]
+        return ClosestPointsList([point for closest_points in all_obj_closest_points for point in closest_points])
+
+    def get_closest_points_between_two_bodies(self, obj_a: PhysicalBody, obj_b: PhysicalBody,
+                                              max_distance: float) -> ClosestPointsList:
+        body_a_data = self.get_body_and_link_id(obj_a, index='A')
+        body_b_data = self.get_body_and_link_id(obj_b, index='B')
+        points_list = p.getClosestPoints(**body_a_data, **body_b_data, distance=max_distance, physicsClientId=self.id)
         return ClosestPointsList([ClosestPoint(**self.parse_points_list_to_args(point)) for point in points_list
                                   if len(point) > 0])
 
+    @staticmethod
+    def get_body_and_link_id(body: PhysicalBody, index='') -> Dict[str, int]:
+        body_id, link_id = (body.object_id, body.id) if isinstance(body, Link) else (body.id, None)
+        values_dict = {f"body{index}": body_id}
+        if link_id is not None:
+            values_dict.update({f"linkIndex{index}": link_id})
+        return values_dict
+
     def parse_points_list_to_args(self, point: List) -> Dict:
         """
         Parses the list of points to a list of dictionaries with the keys as the names of the arguments of the
@@ -222,11 +235,11 @@ def parse_points_list_to_args(self, point: List) -> Dict:
 
         :param point: The list of points.
         """
-        return {"link_a": self.get_object_by_id(point[1]).get_link_by_id(point[3]),
-                "link_b": self.get_object_by_id(point[2]).get_link_by_id(point[4]),
-                "position_on_object_a": point[5],
-                "position_on_object_b": point[6],
-                "normal_on_b": point[7],
+        return {"body_a": self.get_object_by_id(point[1]).get_link_by_id(point[3]),
+                "body_b": self.get_object_by_id(point[2]).get_link_by_id(point[4]),
+                "position_on_body_a": point[5],
+                "position_on_body_b": point[6],
+                "normal_on_body_b": point[7],
                 "distance": point[8],
                 "normal_force": point[9],
                 "lateral_friction_1": LateralFriction(point[10], point[11]),
@@ -472,7 +485,7 @@ def __init__(self, world: World, mode: WorldMode):
         self.mode: WorldMode = mode
 
         # Checks if there is a display connected to the system. If not, the simulation will be run in direct mode.
-        if not "DISPLAY" in os.environ:
+        if "DISPLAY" not in os.environ:
             loginfo("No display detected. Running the simulation in direct mode.")
             self.mode = WorldMode.DIRECT
 
diff --git a/src/pycram/worlds/multiverse.py b/src/pycram/worlds/multiverse.py
index 5e17c4899..4cb72f5be 100644
--- a/src/pycram/worlds/multiverse.py
+++ b/src/pycram/worlds/multiverse.py
@@ -18,6 +18,7 @@
     MultiverseJointCMD
 from ..datastructures.pose import Pose
 from ..datastructures.world import World
+from ..datastructures.world_entity import PhysicalBody
 from ..description import Link, Joint
 from ..object_descriptors.generic import ObjectDescription as GenericObjectDescription
 from ..object_descriptors.mjcf import ObjectDescription as MJCF, PrimitiveObjectFactory
@@ -55,12 +56,12 @@ class Multiverse(World):
     Add the MJCF description extension to the extension to description type mapping for the objects.
     """
 
-    def __init__(self, is_prospection_world: Optional[bool] = False,
+    def __init__(self, is_prospection: Optional[bool] = False,
                  clear_cache: bool = False):
         """
         Initialize the Multiverse Socket and the PyCram World.
 
-        :param is_prospection_world: Whether the world is prospection or not.
+        :param is_prospection: Whether the world is prospection or not.
         :param clear_cache: Whether to clear the cache or not.
         """
 
@@ -68,11 +69,11 @@ def __init__(self, is_prospection_world: Optional[bool] = False,
         self.saved_simulator_states: Dict = {}
         self.make_sure_multiverse_resources_are_added(clear_cache=clear_cache)
 
-        self.simulation = self.conf.prospection_world_prefix if is_prospection_world else "belief_state"
+        self.simulation = self.conf.prospection_world_prefix if is_prospection else "belief_state"
         self.client_manager = MultiverseClientManager(self.conf.simulation_wait_time_factor)
-        self._init_clients(is_prospection=is_prospection_world)
+        self._init_clients(is_prospection=is_prospection)
 
-        World.__init__(self, WorldMode.DIRECT, is_prospection_world)
+        World.__init__(self, mode=WorldMode.DIRECT, is_prospection=is_prospection)
 
         self._init_constraint_and_object_id_name_map_collections()
 
@@ -544,6 +545,20 @@ def remove_constraint(self, constraint_id) -> None:
     def perform_collision_detection(self) -> None:
         ...
 
+    def get_body_contact_points(self, body: PhysicalBody) -> ContactPointsList:
+        if isinstance(body, Object):
+            return self.get_object_contact_points(body)
+        else:
+            multiverse_contact_points = self.api_requester.get_contact_points(body.name)
+            contact_points = ContactPointsList([])
+            for mcp in multiverse_contact_points:
+                body_object, body_link = self.get_object_with_body_name(mcp.body_2)
+                obj, obj_link = self.get_object_with_body_name(mcp.body_1)
+                contact_points.append(ContactPoint(obj_link, body_link))
+                contact_points[-1].normal_on_body_b = mcp.normal
+                contact_points[-1].position_on_b = mcp.position
+            return contact_points
+
     def get_object_contact_points(self, obj: Object, ignore_attached_objects: bool = True) -> ContactPointsList:
         """
         Note: Currently Multiverse only gets one contact point per contact objects.
@@ -564,7 +579,7 @@ def get_object_contact_points(self, obj: Object, ignore_attached_objects: bool =
             body_object, body_link = self.get_object_with_body_name(mcp.body_2)
             obj_link = obj.links[mcp.body_1] if mcp.body_1 in obj.links.keys() else obj.root_link
             contact_points.append(ContactPoint(obj_link, body_link))
-            contact_points[-1].normal_on_b = mcp.normal
+            contact_points[-1].normal_on_body_b = mcp.normal
             contact_points[-1].position_on_b = mcp.position
         return contact_points
 
@@ -607,7 +622,7 @@ def _get_normal_force_on_object_from_contact_force(obj: Object, contact_force: L
         contact_force_array = obj_rot_matrix @ np.array(contact_force).reshape(3, 1)
         return contact_force_array.flatten().tolist()[2]
 
-    def get_contact_points_between_two_objects(self, obj1: Object, obj2: Object) -> ContactPointsList:
+    def get_contact_points_between_two_bodies(self, obj1: Object, obj2: Object) -> ContactPointsList:
         obj1_contact_points = self.get_object_contact_points(obj1)
         return obj1_contact_points.get_points_of_object(obj2)
 
diff --git a/src/pycrap/base.py b/src/pycrap/base.py
index 0c263f15d..d9e6e2090 100644
--- a/src/pycrap/base.py
+++ b/src/pycrap/base.py
@@ -6,6 +6,7 @@
 default_pycrap_ontology_file = tempfile.NamedTemporaryFile()
 default_pycrap_ontology = owlready2.get_ontology("file://" + default_pycrap_ontology_file.name).load()
 
+
 class Base(Thing):
     comment = __doc__
     namespace = default_pycrap_ontology
@@ -17,5 +18,7 @@ def set_comment_to_docstring(cls):
 
 class PhysicalObject(Base):
     """
-    Any Object that has a proper space region. The prototypical physical object has also an associated mass, but the nature of its mass can greatly vary based on the epistemological status of the object (scientifically measured, subjectively possible, imaginary).
+    Any Object that has a proper space region. The prototypical physical object has also an associated mass,
+     but the nature of its mass can greatly vary based on the epistemological status of the object (scientifically
+      measured, subjectively possible, imaginary).
     """
\ No newline at end of file
diff --git a/src/pycrap/objects.py b/src/pycrap/objects.py
index ab49ed20c..e78e33051 100644
--- a/src/pycrap/objects.py
+++ b/src/pycrap/objects.py
@@ -96,7 +96,13 @@ class Cereal(Food):
     """
 
 
-class Floor(PhysicalObject):
+class Supporter(PhysicalObject):
+    """
+    An object that supports another object.
+    """
+
+
+class Floor(Supporter):
     """
     The lower surface of a room.
     """
diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py
index f749a7b01..a609de66f 100644
--- a/test/test_bullet_world.py
+++ b/test/test_bullet_world.py
@@ -68,9 +68,9 @@ def test_get_joint_position(self):
         self.assertAlmostEqual(self.robot.get_joint_position("head_pan_joint"), 0.0, delta=0.01)
 
     def test_get_object_contact_points(self):
-        self.assertEqual(len(self.robot.contact_points()), 0)
+        self.assertEqual(len(self.robot.contact_points), 0)  # 8 because of robot wheels with floor
         self.milk.set_position(self.robot.get_position())
-        self.assertTrue(len(self.robot.contact_points()) > 0)
+        self.assertTrue(len(self.robot.contact_points) > 0)
 
     def test_enable_joint_force_torque_sensor(self):
         self.world.enable_joint_force_torque_sensor(self.robot, self.robot.get_joint_id("head_pan_joint"))
diff --git a/test/test_multiverse.py b/test/test_multiverse.py
index 8bb3cf711..54541f2cc 100644
--- a/test/test_multiverse.py
+++ b/test/test_multiverse.py
@@ -124,7 +124,7 @@ def test_spawn_mesh_object(self):
         milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1, 1, 0.1]))
         self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1]))
         self.multiverse.simulate(0.2)
-        contact_points = milk.contact_points()
+        contact_points = milk.contact_points
         self.assertTrue(len(contact_points) > 0)
 
     def test_parse_mjcf_actuators(self):
@@ -211,7 +211,6 @@ def test_set_position(self):
 
     def test_update_position(self):
         milk = self.spawn_milk([1, 1, 0.1])
-        milk.update_pose()
         milk_position = milk.get_position_as_list()
         self.assert_list_is_equal(milk_position[:2], [1, 1], delta=self.multiverse.conf.position_tolerance)
 
@@ -371,7 +370,7 @@ def test_get_object_contact_points(self):
             self.assertIsInstance(contact_points, ContactPointsList)
             self.assertTrue(len(contact_points) >= 1)
             self.assertIsInstance(contact_points[0], ContactPoint)
-            self.assertTrue(contact_points[0].link_b.object, self.multiverse.floor)
+            self.assertTrue(contact_points[0].body_b.object, self.multiverse.floor)
             cup = self.spawn_cup([1, 1, 0.12])
             # This is needed because the cup is spawned in the air, so it needs to fall
             # to get in contact with the milk
@@ -380,7 +379,7 @@ def test_get_object_contact_points(self):
             self.assertIsInstance(contact_points, ContactPointsList)
             self.assertTrue(len(contact_points) >= 1)
             self.assertIsInstance(contact_points[0], ContactPoint)
-            self.assertTrue(contact_points[0].link_b.object, milk)
+            self.assertTrue(contact_points[0].body_b.object, milk)
             self.tearDown()
 
     def test_get_robot_contact_points(self):
@@ -388,7 +387,7 @@ def test_get_robot_contact_points(self):
                                  quaternion_from_euler(0, 0, 2.26).tolist(),
                                  robot_name="pr2")
         apartment = self.spawn_apartment()
-        contact_points = self.multiverse.get_contact_points_between_two_objects(robot, apartment)
+        contact_points = self.multiverse.get_contact_points_between_two_bodies(robot, apartment)
         self.assertTrue(len(contact_points) > 0)
 
     def test_get_contact_points_between_two_objects(self):
@@ -398,12 +397,12 @@ def test_get_contact_points_between_two_objects(self):
             # This is needed because the cup is spawned in the air so it needs to fall
             # to get in contact with the milk
             self.multiverse.simulate(0.4)
-            contact_points = self.multiverse.get_contact_points_between_two_objects(milk, cup)
+            contact_points = self.multiverse.get_contact_points_between_two_bodies(milk, cup)
             self.assertIsInstance(contact_points, ContactPointsList)
             self.assertTrue(len(contact_points) >= 1)
             self.assertIsInstance(contact_points[0], ContactPoint)
-            self.assertTrue(contact_points[0].link_a.object, milk)
-            self.assertTrue(contact_points[0].link_b.object, cup)
+            self.assertTrue(contact_points[0].body_a.object, milk)
+            self.assertTrue(contact_points[0].body_b.object, cup)
             self.tearDown()
 
     def test_get_one_ray(self):