From 5f111d797cc64f3160d676c79948e745d8137dda Mon Sep 17 00:00:00 2001
From: aclegg3 <alexanderwclegg@gmail.com>
Date: Fri, 6 Sep 2024 12:16:42 -0700
Subject: [PATCH] update the default_link code to align with new lab util

---
 .../utils/namespace/hsim_physics.py           | 30 +++++++++++++------
 1 file changed, 21 insertions(+), 9 deletions(-)

diff --git a/src_python/habitat_sim/utils/namespace/hsim_physics.py b/src_python/habitat_sim/utils/namespace/hsim_physics.py
index d740701347..b2c817567b 100644
--- a/src_python/habitat_sim/utils/namespace/hsim_physics.py
+++ b/src_python/habitat_sim/utils/namespace/hsim_physics.py
@@ -198,16 +198,18 @@ def get_ao_default_link(
     """
     Get the "default" link index for a ManagedArticulatedObject.
     The "default" link is the one link which should be used if only one joint can be actuated. For example, the largest or most accessible drawer or door.
+
+    :param ao: The ManagedArticulatedObject instance.
+    :param compute_if_not_found: If true, try to compute the default link if it isn't found.
+    :return: The default link index or None if not found. Cannot be base link (-1).
+
     The default link is determined by:
+
         - must be "prismatic" or "revolute" joint type
         - first look in the metadata Configuration for an annotated link.
         - (if compute_if_not_found) - if not annotated, it is programmatically computed from a heuristic.
 
     Default link heuristic: the link with the lowest Y value in the bounding box with appropriate joint type.
-
-    :param compute_if_not_found: If true, try to compute the default link if it isn't found.
-
-    :return: The default link index or None if not found. Cannot be base link (-1).
     """
 
     # first look in metadata
@@ -218,7 +220,7 @@ def get_ao_default_link(
             habitat_sim.physics.JointType.Revolute,
             habitat_sim.physics.JointType.Prismatic,
         ]
-        lowest_link = None
+        lowest_links: List[int] = None
         lowest_y: int = None
         # compute the default link
         for link_id in ao.get_link_ids():
@@ -228,11 +230,21 @@ def get_ao_default_link(
                     get_articulated_link_global_keypoints(ao, link_id),
                     key=lambda x: x[1],
                 )[1]
-                if lowest_y is None or link_lowest_y < lowest_y:
+                if (
+                    lowest_y is None
+                    or link_lowest_y < lowest_y
+                    or abs(lowest_y - link_lowest_y) < 0.01
+                ):
+                    if lowest_y is not None and abs(lowest_y - link_lowest_y) < 0.01:
+                        # 1 cm or less difference
+                        lowest_links.append(link_id)
+                    else:
+                        lowest_links = [link_id]
                     lowest_y = link_lowest_y
-                    lowest_link = link_id
-        if lowest_link is not None:
-            default_link = lowest_link
+
+        if lowest_links is not None:
+            # if multiple links could be confused as the default, use the lowest link index of the set
+            default_link = min(lowest_links)
             # if found, set in metadata for next time
             ao.user_attributes.set("default_link", default_link)