programmatically infer grasping points

omnigibson/robots/a1.py

@@ -175,8 +175,8 @@ def _default_joint_pos(self):
         return self._default_robot_model_joint_pos
 
     @property
-    def finger_lengths(self):
-        return {self.default_arm: 0.087}
+    def eef_to_fingertip_lengths(self):
+        return {arm: {name: 0.087 for name in names} for arm, names in self.finger_link_names.items()}
 
     @cached_property
     def arm_link_names(self):
@@ -215,12 +215,12 @@ def teleop_rotation_offset(self):
         return {self.default_arm: self._teleop_rotation_offset}
 
     @property
-    def assisted_grasp_start_points(self):
+    def _assisted_grasp_start_points(self):
         return {self.default_arm: self._ag_start_points}
 
     @property
-    def assisted_grasp_end_points(self):
-        return {self.default_arm: self._ag_start_points}
+    def _assisted_grasp_end_points(self):
+        return {self.default_arm: self._ag_end_points}
 
     @property
     def disabled_collision_pairs(self):

omnigibson/robots/behavior_robot.py

@@ -404,7 +404,7 @@ def set_position_orientation(
                 )
 
     @property
-    def assisted_grasp_start_points(self):
+    def _assisted_grasp_start_points(self):
         side_coefficients = {"left": th.tensor([1, -1, 1]), "right": th.tensor([1, 1, 1])}
         return {
             arm: [
@@ -419,7 +419,7 @@ def assisted_grasp_start_points(self):
         }
 
     @property
-    def assisted_grasp_end_points(self):
+    def _assisted_grasp_end_points(self):
         side_coefficients = {"left": th.tensor([1, -1, 1]), "right": th.tensor([1, 1, 1])}
         return {
             arm: [

omnigibson/robots/fetch.py

@@ -214,28 +214,6 @@ def wheel_radius(self):
     def wheel_axle_length(self):
         return 0.372
 
-    @property
-    def finger_lengths(self):
-        return {self.default_arm: 0.1}
-
-    @property
-    def assisted_grasp_start_points(self):
-        return {
-            self.default_arm: [
-                GraspingPoint(link_name="r_gripper_finger_link", position=th.tensor([0.025, -0.012, 0.0])),
-                GraspingPoint(link_name="r_gripper_finger_link", position=th.tensor([-0.025, -0.012, 0.0])),
-            ]
-        }
-
-    @property
-    def assisted_grasp_end_points(self):
-        return {
-            self.default_arm: [
-                GraspingPoint(link_name="l_gripper_finger_link", position=th.tensor([0.025, 0.012, 0.0])),
-                GraspingPoint(link_name="l_gripper_finger_link", position=th.tensor([-0.025, 0.012, 0.0])),
-            ]
-        }
-
     @cached_property
     def base_joint_names(self):
         return ["l_wheel_joint", "r_wheel_joint"]

omnigibson/robots/franka.py

@@ -238,8 +238,8 @@ def _default_joint_pos(self):
         return self._default_robot_model_joint_pos
 
     @property
-    def finger_lengths(self):
-        return {self.default_arm: 0.1}
+    def eef_to_fingertip_lengths(self):
+        return {arm: {name: 0.1 for name in names} for arm, names in self.finger_link_names.items()}
 
     @cached_property
     def arm_link_names(self):
@@ -286,11 +286,11 @@ def teleop_rotation_offset(self):
         return {self.default_arm: self._teleop_rotation_offset}
 
     @property
-    def assisted_grasp_start_points(self):
+    def _assisted_grasp_start_points(self):
         return {self.default_arm: self._ag_start_points}
 
     @property
-    def assisted_grasp_end_points(self):
+    def _assisted_grasp_end_points(self):
         return {self.default_arm: self._ag_start_points}
 
     @property

omnigibson/robots/franka_mounted.py

@@ -24,8 +24,8 @@ def _default_controllers(self):
         return controllers
 
     @property
-    def finger_lengths(self):
-        return {self.default_arm: 0.15}
+    def eef_to_fingertip_lengths(self):
+        return {arm: {name: 0.15 for name in names} for arm, names in self.finger_link_names.items()}
 
     @property
     def usd_path(self):
@@ -40,15 +40,15 @@ def curobo_path(self):
         return os.path.join(gm.ASSET_PATH, "models/franka/franka_mounted_description_curobo.yaml")
 
     @property
-    def assisted_grasp_start_points(self):
+    def _assisted_grasp_start_points(self):
         return {
             self.default_arm: [
                 GraspingPoint(link_name="panda_rightfinger", position=th.tensor([0.0, 0.001, 0.045])),
             ]
         }
 
     @property
-    def assisted_grasp_end_points(self):
+    def _assisted_grasp_end_points(self):
         return {
             self.default_arm: [
                 GraspingPoint(link_name="panda_leftfinger", position=th.tensor([0.0, 0.001, 0.045])),

omnigibson/robots/manipulation_robot.py

@@ -22,13 +22,19 @@
 )
 from omnigibson.macros import create_module_macros, gm
 from omnigibson.object_states import ContactBodies
+from omnigibson.prims.geom_prim import VisualGeomPrim
 from omnigibson.robots.robot_base import BaseRobot
 from omnigibson.utils.backend_utils import _compute_backend as cb
 from omnigibson.utils.constants import JointType, PrimType
 from omnigibson.utils.geometry_utils import generate_points_in_volume_checker_function
 from omnigibson.utils.python_utils import assert_valid_key, classproperty
 from omnigibson.utils.sampling_utils import raytest_batch
-from omnigibson.utils.usd_utils import ControllableObjectViewAPI, GripperRigidContactAPI, create_joint
+from omnigibson.utils.usd_utils import ControllableObjectViewAPI, GripperRigidContactAPI, create_joint, \
+    create_primitive_mesh, absolute_prim_path_to_scene_relative
+from omnigibson.utils.ui_utils import create_module_logger
+
+# Create module logger
+log = create_module_logger(module_name=__name__)
 
 # Create settings for this module
 m = create_module_macros(module_path=__file__)
@@ -157,6 +163,9 @@ def __init__(
         self._grasping_direction = grasping_direction
         self._disable_grasp_handling = disable_grasp_handling
 
+        # Other variables filled in at runtime
+        self._eef_to_fingertip_lengths = None      # dict mapping arm name to finger name to offset
+
         # Initialize other variables used for assistive grasping
         self._ag_freeze_joint_pos = {
             arm: {} for arm in self.arm_names
@@ -214,6 +223,14 @@ def _validate_configuration(self):
     def _initialize(self):
         super()._initialize()
 
+        # Infer relevant link properties, e.g.: fingertip location, AG grasping points
+        # We use a try / except to maintain backwards-compatibility with robots that do not follow our
+        # OG-specified convention
+        try:
+            self._infer_finger_properties()
+        except AssertionError as e:
+            log.warning(f"Could not infer relevant finger link properties because:\n\n{e}")
+
         if gm.AG_CLOTH:
             for arm in self.arm_names:
                 self._ag_check_in_volume[arm], self._ag_calculate_volume[arm] = (
@@ -222,6 +239,136 @@ def _initialize(self):
                     )
                 )
 
+    def _infer_finger_properties(self):
+        """
+        Infers relevant finger properties based on the given finger meshes of the robot
+
+        NOTE: This assumes the given EEF convention for parallel jaw grippers -- i.e.:
+        z points in the direction of the fingers, y points in the direction of grasp articulation, and x
+        is then inferred automatically
+        """
+        # Calculate and cache fingertip to eef frame offsets, as well as AG grasping points
+        self._eef_to_fingertip_lengths = dict()
+        self._default_ag_start_points = dict()
+        self._default_ag_end_points = dict()
+        for arm, finger_links in self.finger_links.items():
+            self._eef_to_fingertip_lengths[arm] = dict()
+            eef_link = self.eef_links[arm]
+            world_to_eef_tf = T.pose2mat(eef_link.get_position_orientation())
+            eef_to_world_tf = T.pose_inv(world_to_eef_tf)
+
+            # Infer parent link for this finger
+            finger_parent_link, finger_parent_max_z = None, None
+            is_parallel_jaw = len(finger_links) == 2
+            if not is_parallel_jaw:
+                self._default_ag_start_points[arm] = None
+                self._default_ag_end_points[arm] = None
+            for i, finger_link in enumerate(finger_links):
+                # Find parent, and make sure one exists
+                parent_prim_path, parent_link = None, None
+                for joint in self.joints.values():
+                    if finger_link.prim_path == joint.body1:
+                        parent_prim_path = joint.body0
+                        break
+                assert parent_prim_path is not None, f"Expected articulated parent joint for finger link {finger_link.name} but found none!"
+                for link in self.links.values():
+                    if parent_prim_path == link.prim_path:
+                        parent_link = link
+                        break
+                assert parent_link is not None, f"Expected parent link located at {parent_prim_path} but found none!"
+                # Make sure all fingers share the same parent
+                if finger_parent_link is None:
+                    finger_parent_link = parent_link
+                    finger_parent_pts = finger_parent_link.collision_boundary_points_world
+                    assert finger_parent_pts is not None, f"Expected finger parent points to be defined for parent link {finger_parent_link.name}, but got None!"
+                    # Convert from world frame -> eef frame
+                    finger_parent_pts = th.concatenate([finger_parent_pts, th.ones(len(finger_parent_pts), 1)], dim=-1)
+                    finger_parent_pts = (finger_parent_pts @ eef_to_world_tf.T)[:, :3]
+                    finger_parent_max_z = finger_parent_pts[:, 2].max().item()
+                else:
+                    assert finger_parent_link == parent_link, \
+                        f"Expected all fingers to have same parent link, but found multiple parents at {finger_parent_link.prim_path} and {parent_link.prim_path}"
+
+                # Calculate this finger's collision boundary points in the world frame
+                finger_pts = finger_link.collision_boundary_points_world
+                assert finger_pts is not None, f"Expected finger points to be defined for link {finger_link.name}, but got None!"
+                # Convert from world frame -> eef frame
+                finger_pts = th.concatenate([finger_pts, th.ones(len(finger_pts), 1)], dim=-1)
+                finger_pts = (finger_pts @ eef_to_world_tf.T)[:, :3]
+
+                # Since we know the EEF frame always points with z outwards towards the fingers, the outer-most point /
+                # fingertip is the maximum z value
+                finger_max_z = finger_pts[:, 2].max().item()
+                assert finger_max_z > 0, f"Expected positive fingertip to eef frame offset for link {finger_link.name}, but got: {finger_max_z}. Does the EEF frame z-axis point in the direction of the fingers?"
+                self._eef_to_fingertip_lengths[arm][finger_link.name] = finger_max_z
+
+                # Now, only keep points that are above the parent max z by 20% for inferring y values
+                finger_range = finger_max_z - finger_parent_max_z
+                valid_idxs = th.where(finger_pts[:, 2] > (finger_parent_max_z + finger_range * 0.2))[0]
+                finger_pts = finger_pts[valid_idxs]
+                # Make sure all points lie on a single side of the EEF's y-axis
+                y_signs = th.sign(finger_pts[:, 1])
+                assert th.all(y_signs == y_signs[0]).item(), "Expected all finger points to lie on single side of EEF y-axis!"
+                y_sign = y_signs[0].item()
+                y_abs_min = th.abs(finger_pts[:, 1]).min().item()
+
+                # Compute the default grasping points for this finger
+                # For now, we only have a strong heuristic defined for parallel jaw grippers, which assumes that
+                # there are exactly 2 fingers
+                # In this case, this is defined as the x2 (x,y,z) tuples where:
+                # z - the 20% and 80% length between the range from [finger_parent_max_z, finger_max_z]
+                #       This is synonymous to inferring the length of the finger (lower bounded by the gripper base,
+                #       assumed to be the parent link), and then taking the values 20% and 80% along its length
+                # y - the value closest to the edge of the finger surface in the direction of +/- EEF y-axis.
+                #       This assumes that each individual finger lies completely on one side of the EEF y-axis
+                # x - 0. This assumes that the EEF axis evenly splits each finger symmetrically on each side
+                if is_parallel_jaw:
+                    # (x,y,z,1) -- homogenous form for efficient transforming into finger link frame
+                    grasp_pts = th.tensor([
+                        [0, (y_abs_min - 0.002) * y_sign, finger_parent_max_z + finger_range * 0.2, 1],
+                        [0, (y_abs_min - 0.002) * y_sign, finger_parent_max_z + finger_range * 0.8, 1],
+                    ])
+                    finger_to_world_tf = T.pose_inv(T.pose2mat(finger_link.get_position_orientation()))
+                    finger_to_eef_tf = finger_to_world_tf @ world_to_eef_tf
+                    grasp_pts = (grasp_pts @ finger_to_eef_tf.T)[:, :3]
+                    grasping_points = [GraspingPoint(link_name=finger_link.body_name, position=grasp_pt) for grasp_pt in grasp_pts]
+                    if i == 0:
+                        # Start point
+                        self._default_ag_start_points[arm] = grasping_points
+                    else:
+                        # End point
+                        self._default_ag_end_points[arm] = grasping_points
+
+        # For each grasping point, if we're in DEBUG mode, visualize with spheres
+        if gm.DEBUG:
+            for ag_points in (self.assisted_grasp_start_points, self.assisted_grasp_end_points):
+                for arm_ag_points in ag_points.values():
+                    # Skip if None exist
+                    if arm_ag_points is None:
+                        continue
+                    # For each ag point, generate a small sphere at that point
+                    for i, arm_ag_point in enumerate(arm_ag_points):
+                        link = self.links[arm_ag_point.link_name]
+                        local_pos = arm_ag_point.position
+                        vis_mesh_prim_path = f"{link.prim_path}/ag_point_{i}"
+                        vis_mesh = create_primitive_mesh(
+                            prim_path=vis_mesh_prim_path,
+                            extents=0.005,
+                            primitive_type="Sphere",
+                        )
+                        vis_geom = VisualGeomPrim(
+                            relative_prim_path=absolute_prim_path_to_scene_relative(
+                                scene=self.scene,
+                                absolute_prim_path=vis_mesh_prim_path,
+                            ),
+                            name=f"ag_point_{i}",
+                        )
+                        vis_geom.load(self.scene)
+                        vis_geom.set_position_orientation(
+                            position=local_pos,
+                            frame="parent",
+                        )
+
     def is_grasping(self, arm="default", candidate_obj=None):
         """
         Returns True if the robot is grasping the target option @candidate_obj or any object if @candidate_obj is None.
@@ -657,6 +804,23 @@ def finger_joints(self):
         """
         return {arm: [self._joints[joint] for joint in self.finger_joint_names[arm]] for arm in self.arm_names}
 
+    @property
+    def _assisted_grasp_start_points(self):
+        """
+        Returns:
+            dict: Dictionary mapping individual arm appendage names to array of GraspingPoint tuples,
+                composed of (link_name, position) values specifying valid grasping start points located at
+                cartesian (x,y,z) coordinates specified in link_name's local coordinate frame.
+                These values will be used in conjunction with
+                @self.assisted_grasp_end_points to trigger assisted grasps, where objects that intersect
+                with any ray starting at any point in @self.assisted_grasp_start_points and terminating at any point in
+                @self.assisted_grasp_end_points will trigger an assisted grasp (calculated individually for each gripper
+                appendage). By default, each entry returns None, and must be implemented by any robot subclass that
+                wishes to use assisted grasping.
+        """
+        # Should be optionally implemented by subclass
+        return None
+
     @property
     def assisted_grasp_start_points(self):
         """
@@ -671,7 +835,24 @@ def assisted_grasp_start_points(self):
                 appendage). By default, each entry returns None, and must be implemented by any robot subclass that
                 wishes to use assisted grasping.
         """
-        return {arm: None for arm in self.arm_names}
+        return self._assisted_grasp_start_points if self._assisted_grasp_start_points is not None else self._default_ag_start_points
+
+    @property
+    def _assisted_grasp_end_points(self):
+        """
+        Returns:
+            dict: Dictionary mapping individual arm appendage names to array of GraspingPoint tuples,
+                composed of (link_name, position) values specifying valid grasping end points located at
+                cartesian (x,y,z) coordinates specified in link_name's local coordinate frame.
+                These values will be used in conjunction with
+                @self.assisted_grasp_start_points to trigger assisted grasps, where objects that intersect
+                with any ray starting at any point in @self.assisted_grasp_start_points and terminating at any point in
+                @self.assisted_grasp_end_points will trigger an assisted grasp (calculated individually for each gripper
+                appendage). By default, each entry returns None, and must be implemented by any robot subclass that
+                wishes to use assisted grasping.
+        """
+        # Should be optionally implemented by subclass
+        return None
 
     @property
     def assisted_grasp_end_points(self):
@@ -687,16 +868,16 @@ def assisted_grasp_end_points(self):
                 appendage). By default, each entry returns None, and must be implemented by any robot subclass that
                 wishes to use assisted grasping.
         """
-        return {arm: None for arm in self.arm_names}
+        return self._assisted_grasp_end_points if self._assisted_grasp_end_points is not None else self._default_ag_end_points
 
     @property
-    def finger_lengths(self):
+    def eef_to_fingertip_lengths(self):
         """
         Returns:
-            dict: Dictionary mapping arm appendage name to corresponding length of the fingers in that
-                hand defined from the palm (assuming all fingers in one hand are equally long)
+            dict: Dictionary mapping arm appendage name to per-finger corresponding z-distance between EEF and each
+                respective fingertip
         """
-        raise NotImplementedError
+        return self._eef_to_fingertip_lengths
 
     @property
     def arm_workspace_range(self):