[ObjectTracker] Placing and picking are not working properly.

src/episode_segmenter/episode_segmenter.py

@@ -20,7 +20,7 @@
 from .events import ContactEvent, Event, AgentContactEvent, PickUpEvent, EventUnion, StopMotionEvent, MotionEvent, \
     NewObjectEvent, RotationEvent, StopRotationEvent, PlacingEvent, HasTrackedObject
 from .object_tracker import ObjectTracker
-from .utils import check_if_object_is_supported, add_imaginary_support_for_object
+from .utils import check_if_object_is_supported, add_imaginary_support_for_object, adjust_imaginary_support_for_object
 
 
 class EpisodeSegmenter(ABC):
@@ -209,6 +209,8 @@ def start_detector_thread_for_starter_event(self, starter_event: EventUnion,
         :param detector_type: The type of the detector.
         """
         if not self.is_detector_redundant(detector_type, starter_event):
+            if detector_type == PlacingDetector:
+                print(f"new placing detector for object {starter_event.tracked_object.name}")
             self.start_and_add_detector_thread(detector_type, starter_event=starter_event)
 
     def is_detector_redundant(self, detector_type: TypeEventDetectorUnion, starter_event: EventUnion) -> bool:
@@ -319,33 +321,19 @@ def run_initial_event_detectors(self) -> None:
         """
         Start the motion detection threads for the objects in the world.
         """
+        set_of_objects = set()
         for obj in World.current_world.objects:
             if not obj.is_an_environment and (obj not in self.objects_to_avoid):
-                self.start_motion_detection_threads_for_object(obj)
+                set_of_objects.add(obj)
                 try:
-                    self.detect_missing_support_for_object(obj)
+                    if not check_if_object_is_supported(obj):
+                        if World.current_world.get_object_by_name('imagined_support') is not None:
+                            adjust_imaginary_support_for_object(obj)
+                        else:
+                            add_imaginary_support_for_object(obj)
                 except NotImplementedError:
                     logwarn("Support detection is not implemented for this simulator.")
+        for obj in set_of_objects:
+            self.start_motion_detection_threads_for_object(obj)
+            self.start_contact_threads_for_object(obj)
         self.episode_player.resume()
-
-    def detect_missing_support_for_object(self, obj: Object) -> None:
-        """
-        Detect if the object is not supported by any other object.
-
-        :param obj: The object to check if it is supported.
-        """
-        support_name = f"imagined_support"
-        support_obj = World.current_world.get_object_by_name(support_name)
-        support_thickness = 0.005
-        supported = check_if_object_is_supported(obj)
-        if supported:
-            return
-        obj_base_position = obj.get_base_position_as_list()
-        if support_obj is None:
-            support_obj = add_imaginary_support_for_object(obj, support_name, support_thickness)
-            self.start_contact_threads_for_object(support_obj)
-        else:
-            support_position = support_obj.get_position_as_list()
-            if obj_base_position[2] <= support_position[2]:
-                support_position[2] = obj_base_position[2] - support_thickness * 0.5
-                support_obj.set_position(support_position)

src/episode_segmenter/event_detectors.py

@@ -15,6 +15,7 @@
 from pycram import World
 from pycram.datastructures.dataclasses import ContactPointsList
 from pycram.datastructures.pose import Pose
+from pycram.datastructures.world import UseProspectionWorld
 from pycram.ros.logging import logdebug
 from pycram.world_concepts.world_object import Object
 from pycrap import PhysicalObject
@@ -352,7 +353,8 @@ def trigger_events(self, contact_points: ContactPointsList) -> List[LossOfSurfac
         objects_that_lost_contact = self.get_objects_that_lost_contact(contact_points)
         if len(objects_that_lost_contact) == 0:
             return []
-        supporting_surface = check_for_supporting_surface(objects_that_lost_contact, self.latest_contact_points)
+        supporting_surface = check_for_supporting_surface(self.tracked_object,
+                                                          objects_that_lost_contact)
         if supporting_surface is None:
             return []
         return [LossOfSurfaceEvent(contact_points, self.latest_contact_points, of_object=self.tracked_object,
@@ -539,6 +541,22 @@ def start_condition_checker(cls, event: Event) -> bool:
         """
         pass
 
+    def check_for_event_pre_starter_event(self, event_type: Type[Event],
+                                          time_tolerance: timedelta) -> Optional[EventUnion]:
+        """
+        Check if the tracked_object was involved in an event before the starter event.
+
+        :param event_type: The event type to check for.
+        :param time_tolerance: The time tolerance to consider the event as before the starter event.
+        """
+        event = self.object_tracker.get_first_event_of_type_before_event(event_type,
+                                                                         self.starter_event)
+        if event is None or event.timestamp < self.start_timestamp - time_tolerance.total_seconds():
+            logdebug(f"{event_type.__name__} found no event before {self.start_timestamp} with object :"
+                     f" {self.tracked_object.name}")
+            return None
+        return event
+
     def check_for_event_post_starter_event(self, event_type: Type[Event]) -> Optional[EventUnion]:
         """
         Check if the tracked_object was involved in an event after the starter event.
@@ -648,20 +666,21 @@ def detect_events(self) -> List[EventUnion]:
 
         :return: An instance of the interaction event if the tracked_object was interacted with, else None.
         """
-
+        event = None
         while not self.kill_event.is_set():
 
             if not self.interaction_checks():
                 time.sleep(0.01)
                 continue
 
             self.interaction_event.end_timestamp = self.end_timestamp
+            event = self.interaction_event
 
             break
-
-        rospy.loginfo(f"{self.__class__.__name__} detected an interaction with: {self.tracked_object.name}")
-
-        return [self.interaction_event]
+        if event:
+            rospy.loginfo(f"{self.__class__.__name__} detected an interaction with: {self.tracked_object.name}")
+            return [event]
+        return []
 
     @abstractmethod
     def interaction_checks(self) -> bool:
@@ -785,15 +804,20 @@ def interaction_checks(self) -> bool:
         """
         Check for upward motion after the object lost contact with the surface.
         """
-        latest_event = self.check_for_event_near_starter_event(TranslationEvent, timedelta(milliseconds=1000))
+        if check_for_supporting_surface(self.tracked_object,
+                                        self.starter_event.latest_objects_that_got_removed) is not None:
+            # wait for the object to be lifted TODO: Should be replaced with a wait on a lifting event
+            dt = timedelta(milliseconds=300)
+            time.sleep(dt.total_seconds())
+            latest_event = self.check_for_event_near_starter_event(TranslationEvent, dt)
 
-        if not latest_event:
-            return False
+            if not latest_event:
+                return False
 
-        z_motion = latest_event.current_pose.position.z - latest_event.start_pose.position.z
-        if z_motion > 0.001:
-            self.end_timestamp = max(latest_event.timestamp, self.start_timestamp)
-            return True
+            z_motion = latest_event.current_pose.position.z - latest_event.start_pose.position.z
+            if z_motion >= 0.005:
+                self.end_timestamp = max(latest_event.timestamp, self.start_timestamp)
+                return True
 
         return False
 
@@ -822,21 +846,34 @@ def start_condition_checker(cls, event: Event) -> bool:
 
         :param event: The ContactEvent instance that represents the contact event.
         """
-        if isinstance(event, ContactEvent) and any(select_transportable_objects([event.tracked_object])):
-            print('new placing detector for object:', event.tracked_object.name)
+        if (isinstance(event, ContactEvent)
+                and any(select_transportable_objects([event.tracked_object]))):
             return True
         return False
 
     def initial_interaction_checkers(self) -> bool:
         """
         Perform initial checks to determine if the object was placed.
         """
-        stop_motion_event = self.check_for_event_near_starter_event(StopMotionEvent, timedelta(milliseconds=1000))
-        if stop_motion_event and self.check_if_contact_event_is_with_surface(self.starter_event):
+        dt = timedelta(milliseconds=1000)
+        time.sleep(dt.total_seconds())
+        event = self.check_for_event_near_starter_event(StopTranslationEvent, dt)
+        if event is not None:
+            print(f"found translation event: {event} for {self.tracked_object.name}")
+            print(f"object with history: {self.object_tracker.get_event_history()}")
+            if (event.current_pose.position.z - event.start_pose.position.z) > -0.001:
+                self.kill_event.set()
+                return False
+            # wait for the object to stop moving
+            dt = timedelta(milliseconds=500)
+            time.sleep(dt.total_seconds())
+            if not check_if_object_is_supported(self.tracked_object, 0.01):
+                self.kill_event.set()
+                return False
             self.end_timestamp = self.start_timestamp
             print(f"end_timestamp: {self.end_timestamp}")
             return True
-
+        self.kill_event.set()
         return False
 
     def check_if_contact_event_is_with_surface(self, contact_event: ContactEvent) -> bool:
@@ -847,30 +884,41 @@ def check_if_contact_event_is_with_surface(self, contact_event: ContactEvent) ->
         """
         return check_if_object_is_supported_by_another_object(self.tracked_object, contact_event.with_object,
                                                               contact_event.contact_points)
+        # return check_if_object_is_supported(self.tracked_object)
 
 
-def check_for_supporting_surface(objects_that_lost_contact: List[Object],
-                                 initial_contact_points: ContactPointsList) -> Optional[Object]:
+def check_for_supporting_surface(tracked_object: Object,
+                                 possible_surfaces: List[Object]) -> Optional[Object]:
     """
-    Check if any of the objects that lost contact are supporting surfaces.
+    Check if any of the possible surfaces are supporting the tracked_object.
 
-    :param objects_that_lost_contact: An instance of the Object class that represents the tracked_object to check.
-    :param initial_contact_points: A list of ContactPoint instances that represent the contact points of the
-     tracked_object before it lost contact.
+    :param tracked_object: An instance of the Object class that represents the tracked_object to check.
+    :param possible_surfaces: A list of Object instances that represent the possible surfaces.
     :return: An instance of the Object class that represents the supporting surface if found, else None.
     """
+    with UseProspectionWorld():
+        dt = 0.1
+        World.current_world.simulate(dt)
+        prospection_obj = World.current_world.get_prospection_object_for_object(tracked_object)
+        contact_points = prospection_obj.contact_points
+        contacted_bodies = contact_points.get_objects_that_have_points()
+        contacted_body_names = [body.name for body in contacted_bodies]
+        contacted_bodies = dict(zip(contacted_body_names, contacted_bodies))
+        possible_surface_names = [obj.name for obj in possible_surfaces]
+        possible_surface_names = list(set(contacted_body_names).intersection(possible_surface_names))
     supporting_surface = None
     opposite_gravity = [0, 0, 1]
-    smallest_angle = np.pi / 4
-    for obj in objects_that_lost_contact:
-        normals = initial_contact_points.get_normals_of_object(obj)
-        for normal in normals:
-            # check if normal is pointing upwards opposite to gravity by finding the angle between the normal
-            # and gravity vector.
-            angle = get_angle_between_vectors(normal, opposite_gravity)
-            if angle < smallest_angle:
-                smallest_angle = angle
-                supporting_surface = obj
+    smallest_angle = np.pi / 8
+    for obj_name in possible_surface_names:
+        obj = World.current_world.get_object_by_name(obj_name)
+        normals = contact_points.get_normals_of_object(contacted_bodies[obj_name])
+        normal = np.mean(normals, axis=0)
+        angle = get_angle_between_vectors(normal, opposite_gravity)
+        if 0 <= angle <= smallest_angle:
+            smallest_angle = angle
+            supporting_surface = obj
+    if supporting_surface is not None:
+        print("found surface ", supporting_surface.name)
     return supporting_surface
 
 
@@ -929,8 +977,7 @@ def select_transportable_objects_from_loss_of_contact_event(event: Union[LossOfC
     """
     Select the objects that can be transported from the loss of contact event.
     """
-    objects_that_lost_contact = event.latest_objects_that_got_removed
-    return select_transportable_objects(objects_that_lost_contact + [event.tracked_object])
+    return select_transportable_objects([event.tracked_object])
 
 
 def select_transportable_objects(objects: List[Object]) -> List[Object]:

src/episode_segmenter/object_tracker.py

@@ -74,13 +74,15 @@ def get_nearest_event_of_type_to_timestamp(self, timestamp: float, event_type: T
             if len(valid_indices) > 0:
                 time_stamps = time_stamps[valid_indices]
                 nearest_event_index = self._get_nearest_index(time_stamps, timestamp, tolerance)
-                return self._event_history[valid_indices[nearest_event_index]]
+                if nearest_event_index is not None:
+                    return self._event_history[valid_indices[nearest_event_index]]
 
     def get_nearest_event_to(self, timestamp: float, tolerance: Optional[timedelta] = None) -> Optional[Event]:
         with self._lock:
             time_stamps = self.time_stamps_array
             nearest_event_index = self._get_nearest_index(time_stamps, timestamp, tolerance)
-            return self._event_history[nearest_event_index]
+            if nearest_event_index is not None:
+                return self._event_history[nearest_event_index]
 
     def _get_nearest_index(self, time_stamps: np.ndarray,
                            timestamp: float, tolerance: Optional[timedelta] = None) -> Optional[int]:
@@ -96,10 +98,21 @@ def get_first_event_of_type_after_event(self, event_type: Type[Event], event: Ev
     def get_first_event_of_type_after_timestamp(self, event_type: Type[Event], timestamp: float) -> Optional[Event]:
         with self._lock:
             start_index = self.get_index_of_first_event_after(timestamp)
-            for event in self._event_history[start_index:]:
-                if isinstance(event, event_type):
-                    return event
-            return None
+            if start_index is not None:
+                for event in self._event_history[start_index:]:
+                    if isinstance(event, event_type):
+                        return event
+
+    def get_first_event_of_type_before_event(self, event_type: Type[Event], event: Event) -> Optional[EventUnion]:
+        return self.get_first_event_of_type_before_timestamp(event_type, event.timestamp)
+
+    def get_first_event_of_type_before_timestamp(self, event_type: Type[Event], timestamp: float) -> Optional[Event]:
+        with self._lock:
+            start_index = self.get_index_of_first_event_before(timestamp)
+            if start_index is not None:
+                for event in reversed(self._event_history[:start_index]):
+                    if isinstance(event, event_type):
+                        return event
 
     def get_index_of_first_event_after(self, timestamp: float) -> Optional[int]:
         with self._lock:

src/episode_segmenter/utils.py

@@ -1,3 +1,5 @@
+import math
+
 import numpy as np
 from tf.transformations import quaternion_inverse, quaternion_multiply
 from typing_extensions import List, Optional
@@ -11,20 +13,23 @@
 from pycram.object_descriptors.generic import ObjectDescription as GenericObjectDescription
 
 
-def check_if_object_is_supported(obj: Object) -> bool:
+def check_if_object_is_supported(obj: Object, distance: Optional[float] = 0.03) -> bool:
     """
     Check if the object is supported by any other object.
 
     :param obj: The object to check if it is supported.
+    :param distance: The distance to check if the object is supported.
     :return: True if the object is supported, False otherwise.
     """
     supported = True
     with UseProspectionWorld():
         prospection_obj = World.current_world.get_prospection_object_for_object(obj)
         current_position = prospection_obj.get_position_as_list()
-        World.current_world.simulate(1)
+        dt = math.sqrt(2 * distance / 9.81) + 0.01  # time to fall distance
+        World.current_world.simulate(dt)
         new_position = prospection_obj.get_position_as_list()
-        if current_position[2] - new_position[2] >= 0.2:
+        print("change in position", current_position[2] - new_position[2])
+        if current_position[2] - new_position[2] > distance:
             logdebug(f"Object {obj.name} is not supported")
             supported = False
     return supported
@@ -71,6 +76,27 @@ def is_vector_opposite_to_gravity(vector: List[float], gravity_vector: Optional[
     return np.dot(vector, gravity_vector) < 0
 
 
+def adjust_imaginary_support_for_object(obj: Object,
+                                        support_name: Optional[str] = f"imagined_support",
+                                        support_thickness: Optional[float] = 0.005) -> None:
+    """
+    Adjust the imaginary support for the object such that it is at the base of the object.
+
+    :param obj: The object to check if it is supported.
+    :param support_name: The name of the support object.
+    :param support_thickness: The thickness of the support.
+    """
+    support_obj = World.current_world.get_object_by_name(support_name)
+    floor = World.current_world.get_object_by_name('floor')
+    obj_base_position = obj.get_base_position_as_list()
+    support_position = support_obj.get_position_as_list()
+    if obj_base_position[2] <= (support_position[2] + support_thickness * 0.5):
+        floor.detach(support_obj)
+        support_position[2] = obj_base_position[2] - support_thickness * 0.5
+        support_obj.set_position(support_position)
+        floor.attach(support_obj)
+
+
 def add_imaginary_support_for_object(obj: Object,
                                      support_name: Optional[str] = f"imagined_support",
                                      support_thickness: Optional[float] = 0.005) -> Object:
@@ -83,11 +109,12 @@ def add_imaginary_support_for_object(obj: Object,
     :return: The support object.
     """
     obj_base_position = obj.get_base_position_as_list()
-    support = GenericObjectDescription(support_name, [0, 0, 0], [1, 1, obj_base_position[2]*0.5])
+    support = GenericObjectDescription(support_name, [0, 0, 0], [1, 1, support_thickness*0.5])
     support_obj = Object(support_name, pycrap.Genobj, None, support)
     support_position = obj_base_position.copy()
-    support_position[2] = obj_base_position[2] * 0.5
+    support_position[2] -= support_thickness
     support_obj.set_position(support_position)
+    World.current_world.get_object_by_name('floor').attach(support_obj)
     return support_obj