Skip to content

feat(manipulation): depth search slides failing grasps to a depth that holds the object - #4530

Draft
mustafab0 wants to merge 4 commits into
mustafa/feat/single-accurate-checkfrom
mustafa/feat/grasp-depth-search
Draft

mustafab0 wants to merge 4 commits into
mustafa/feat/single-accurate-checkfrom
mustafa/feat/grasp-depth-search

Conversation

@mustafab0

Copy link
Copy Markdown
Contributor
  • grasp_depth_search (metres, 0 = off): a grasp that fails where its source put it may slide along its approach to the nearest depth that passes, where a pad presses the object and holds enough of it.
  • Moved grasps are used only when no grasp passes unmoved.
  • The table is solid: a grasp whose open gripper reaches below the table top is rejected (it used to pass when no table point was near it).
  • Speed-ups that do not change any verdict: those depths skip the shape tests, the closing poses are only tested for grasps that pass open, and the search stops once max_grasp_attempts grasps pass.

Evidence: perfect perception, grasped 80 vs 73 (p = 0.17), "no grasp left" 13 → 1, scissors 6/7 vs 0/7; camera, held after carry 39 vs 29 (p = 0.064). Earlier versions that moved every grasp lost on boxes and cans; the fallback rule fixes that. Off by default.

Eval setup for the numbers below: SceneReplica, 100 cases, xArm7 in sim2 (MuJoCo), scripted pick (no LLM), paired on the same cases. "Perfect perception" means the object's true mesh from the sim (the grasp lab); "camera" means the wrist camera's scene registration.

Part 12 of 13 in the closed-loop grasp (M1) stack; based on mustafa/feat/single-accurate-check, so this PR shows only its own change.

… holds the object

A grasp source picks its depth for its own gripper model. GraspGenX centres
the object in the open xArm fingers, but they swing about 1.3 cm further as
they close, so on flat objects every grasp put the fingertips into the table
and the pick had none left.

grasp_depth_search (metres, 0 = off) tries each grasp at depths along its
approach (grasp_depth_step apart) with the gripper's modelled close. A depth
passes when the check is clear and a finger pad presses the object with at
least grasp_min_gap_points object points between the pads where the fingers
stop; the grasp is kept at the passing depth that holds the most. Grasps are
taken best first, 50 at a time, and the search stops once
max_grasp_attempts pass.

The table is now solid for the check: scene_obstacles also gives the table's
height, and a grasp with any gripper part's centre below it touches "floor"
(the table was a sheet of surface points, so a gripper wholly under it passed).
Grasps below the floor skip the shape tests.
…dict

The depth search runs the full check at every depth, and on a busy machine a
three-try pick ran past the 120 s pick time limit. Two exact savings:

- The floor test uses the open gripper's outermost points (each link's convex
  hull vertices) instead of each part's box centre. A gripper reaching below
  the table top always touches it, so those depths skip the shape tests; this
  is half the depths on a mug and 95% on flat objects.
- The closing poses are tested only for grasps that touch nothing at the open
  pose; a grasp already touching something cannot pass.

On recorded mug, marker, scissors and pudding box checks the depth search picks
the same depth for every grasp as before, 2.5-6x faster.
…ce put them

Moving every grasp to the depth that holds the most of the object put grasps
at the edge of what the check allows (the palm just over a box, fingertips
just over the table), and the arm lands a few millimetres off its target: in
the grasp lab, 17 of 25 cases grasped against 23 without the search, and the
fingers closed on nothing 5 times.

A grasp that passes where its source put it now stays there. Only a failing
grasp moves, to the nearest depth that passes (backing off before going
deeper) where a finger pad presses the object and holds at least
min_gap_points of it. A grasp too deep for a flat object backs off only until
it clears the table, so its fingers still meet the object's sides.
In the grasp lab, moved grasps were tried in their source's rank order next
to grasps that passed where they were, and on boxes, cans and the drill they
often closed on nothing (13 first tries against 1 without the search). Moved
grasps are now kept only when no grasp passes where its source put it, so a
pick with such grasps behaves as without the search.

@github-actions github-actions Bot left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

go2 replay realtime (arm64)

Details
Benchmark suite Current: 16b49ee Previous: 1e7b491 Ratio
peak memory 1604.43 MB 1569.52 MB 1.02
peak threads 349 threads 352 threads 0.99
network (transport) 2724.689 MB 2722.016 MB 1.00
disk write 1.492 MB 1.492 MB 1
instructions 115.633 G 115.634 G 1.00

This comment was automatically generated by workflow using github-action-benchmark.

@codecov

codecov Bot commented Oct 11, 2026

Copy link
Copy Markdown

❌ 2 Tests Failed:

Tests completed Failed Passed Skipped
6296 2 6294 85
View the top 2 failed test(s) by shortest run time
dimos.manipulation.grasping.test_heuristic_grasp::test_with_aim_pads_off_the_grasps_are_the_tool_tip_at_mid_height_as_before
Stack Traces | 0.18s run time
def test_with_aim_pads_off_the_grasps_are_the_tool_tip_at_mid_height_as_before() -> None:
        config = HeuristicGraspConfig(
            heuristic_aim_pads=False,
            pad_behind_tcp=0.0167,
            fingertip_reach=0.0031,
            palm_behind_tcp=0.05,
            **_EVERY_KIND,
        )
    
        proposals = heuristic_grasps(_cloud(_STEPPED_BAR), config)
    
        # Each grasp's position, orientation (x, y, z, w) and score, as computed
        # before the pads were aimed.
        y, down = 3.469446951953614e-18, (-1.0, -3.8738279251049335e-09, 2.3720354844636946e-25)
        w = 6.123233995736766e-17
        turned = (-0.7071067784473375, -0.7071067839257575, 4.3297802979502905e-17)
>       assert [_values(c) for c in proposals.candidates] == [
            (0.017500000074505806, y, 0.03750000149011612, *down, w, 1.0),
            (0.027499999850988388, y, 0.03999999910593033, *down, w, 0.8),
            (-0.027499999850988388, y, 0.014999999664723873, *down, w, 0.7),
            (0.017500000074505806, y, 0.03750000149011612, *turned, 4.3297802644046424e-17, 0.5),
        ]
E       assert [(0.017500000...703e-17, ...)] == [(0.017500000...905e-17, ...)]
E         
E         At index 0 diff: #x1B[0m(#x1B[94m0.017500000074505806#x1B[39;49;00m, #x1B[94m3.469446951953614e-18#x1B[39;49;00m, #x1B[94m0.03750000149011612#x1B[39;49;00m, -#x1B[94m1.0#x1B[39;49;00m, -#x1B[94m1.4872725273562537e-09#x1B[39;49;00m, #x1B[94m9.106917700433152e-26#x1B[39;49;00m, #x1B[94m6.123233995736766e-17#x1B[39;49;00m, #x1B[94m1.0#x1B[39;49;00m)#x1B[90m#x1B[39;49;00m != #x1B[0m(#x1B[94m0.017500000074505806#x1B[39;49;00m, #x1B[94m3.469446951953614e-18#x1B[39;49;00m, #x1B[94m0.03750000149011612#x1B[39;49;00m, -#x1B[94m1.0#x1B[39;49;00m, -#x1B[94m3.8738279251049335e-09#x1B[39;49;00m, #x1B[94m2.3720354844636946e-25#x1B[39;49;00m, #x1B[94m6.123233995736766e-17#x1B[39;49;00m, #x1B[94m1.0#x1B[39;49;00m)#x1B[90m#x1B[39;49;00m
E         
E         Full diff:
E         #x1B[0m#x1B[90m #x1B[39;49;00m [#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m     (#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.017500000074505806,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         3.469446951953614e-18,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.03750000149011612,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         -1.0,#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         -3.8738279251049335e-09,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         2.3720354844636946e-25,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         -1.4872725273562537e-09,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         9.106917700433152e-26,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         6.123233995736766e-17,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         1.0,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m     ),#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m     (#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.027499999850988388,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         3.469446951953614e-18,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.03999999910593033,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         -1.0,#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         -3.8738279251049335e-09,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         2.3720354844636946e-25,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         -1.4872725273562537e-09,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         9.106917700433152e-26,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         6.123233995736766e-17,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.8,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m     ),#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m     (#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         -0.027499999850988388,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         3.469446951953614e-18,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.014999999664723873,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         -1.0,#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         -3.8738279251049335e-09,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         2.3720354844636946e-25,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         -1.4872725273562537e-09,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         9.106917700433152e-26,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         6.123233995736766e-17,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.7,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m     ),#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m     (#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.017500000074505806,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         3.469446951953614e-18,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.03750000149011612,#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         -0.7071067784473375,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         -0.7071067839257575,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         ?                     ^^^ ---#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         -0.707106780134887,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         ?                    ++ ^^^#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         -0.707106782238208,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         4.32978028761703e-17,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         4.3297802979502905e-17,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         ?                  ^  - ^^^^#x1B[90m#x1B[39;49;00m
E         #x1B[92m+         4.329780274737903e-17,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         ?                  ^^^^   ^#x1B[90m#x1B[39;49;00m
E         #x1B[91m-         4.3297802644046424e-17,#x1B[39;49;00m#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m         0.5,#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m     ),#x1B[90m#x1B[39;49;00m
E         #x1B[90m #x1B[39;49;00m ]#x1B[90m#x1B[39;49;00m

config     = HeuristicGraspConfig(max_grasp_width=0.25, jaw_clearance=0.01, min_split_length=0.08, heuristic_aim_pads=False, pad_behind_tcp=0.0167, fingertip_reach=0.0031, palm_behind_tcp=0.05, gripper_clearance=0.005)
down       = (-1.0, -3.8738279251049335e-09, 2.3720354844636946e-25)
proposals  = <dimos.msgs.manipulation_msgs.GraspCandidateArray.GraspCandidateArray object at 0xffa8b4c4cc80>
turned     = (-0.7071067784473375, -0.7071067839257575, 4.3297802979502905e-17)
w          = 6.123233995736766e-17
y          = 3.469446951953614e-18

.../manipulation/grasping/test_heuristic_grasp.py:353: AssertionError
dimos.codebase_checks.test_inline_heavy_imports::test_heavy_imports_are_inline
Stack Traces | 2.79s run time
def test_heavy_imports_are_inline() -> None:
        """Fail if any file imports cv2/open3d/rerun at module level."""
        hits = find_eager_heavy_imports()
        if hits:
            listing = "\n".join(
                f"  - dimos/{f}:{line}: `{module}`"
                for f, lines in sorted(hits.items())
                for line, module in lines
            )
>           raise AssertionError(
                f"Found module-level import(s) of {'/'.join(HEAVY_MODULES)}:\n{listing}\n\n"
                "These libraries load large native extensions into every process that "
                "transitively imports the module. Import them inside the function or "
                "method that uses them; imports needed only for type annotations go "
                "under `if TYPE_CHECKING:`."
            )
E           AssertionError: Found module-level import(s) of cv2/open3d/rerun:
E             - .../manipulation/planning/gripper_contact.py:37: `open3d`
E           
E           These libraries load large native extensions into every process that transitively imports the module. Import them inside the function or method that uses them; imports needed only for type annotations go under `if TYPE_CHECKING:`.

hits       = {'manipulation/planning/gripper_contact.py': [(37, 'open3d')]}
listing    = '  - .../manipulation/planning/gripper_contact.py:37: `open3d`'

dimos/codebase_checks/test_inline_heavy_imports.py:92: AssertionError

To view more test analytics, go to the Test Analytics Dashboard
📋 Got 3 mins? Take this short survey to help us improve Test Analytics.

@mustafab0
mustafab0 added this pull request to stack #4532 October 11, 2026 06:49

This branch has not been deployed

No deployments
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant