From 3bbd33936d06cc47aceeb08ac054e907d69d4012 Mon Sep 17 00:00:00 2001 From: A_Bassiouny Date: Mon, 16 Dec 2024 16:36:42 +0100 Subject: [PATCH] [ObjectTracker] Placing and picking are not working properly. --- src/episode_segmenter/episode_segmenter.py | 38 +++---- src/episode_segmenter/event_detectors.py | 119 ++++++++++++++------- src/episode_segmenter/object_tracker.py | 25 +++-- src/episode_segmenter/utils.py | 37 ++++++- 4 files changed, 147 insertions(+), 72 deletions(-) diff --git a/src/episode_segmenter/episode_segmenter.py b/src/episode_segmenter/episode_segmenter.py index e0118b4..c13e642 100644 --- a/src/episode_segmenter/episode_segmenter.py +++ b/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) diff --git a/src/episode_segmenter/event_detectors.py b/src/episode_segmenter/event_detectors.py index 9ab8acb..1c38cd9 100644 --- a/src/episode_segmenter/event_detectors.py +++ b/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,7 +666,7 @@ 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(): @@ -656,12 +674,13 @@ def detect_events(self) -> List[EventUnion]: 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,8 +846,8 @@ 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 @@ -831,12 +855,25 @@ 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]: diff --git a/src/episode_segmenter/object_tracker.py b/src/episode_segmenter/object_tracker.py index e138132..47908cb 100644 --- a/src/episode_segmenter/object_tracker.py +++ b/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: diff --git a/src/episode_segmenter/utils.py b/src/episode_segmenter/utils.py index 6a78fd0..49b7bf8 100644 --- a/src/episode_segmenter/utils.py +++ b/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