Fix: factory_sim bin picking fails when brackets settle near the workspace boundary#754
Merged
Merged
Conversation
|
No actionable comments were generated in the recent review. 🎉 ℹ️ Recent review info⚙️ Run configurationConfiguration used: Organization UI Review profile: CHILL Plan: Pro Plus Run ID: 📒 Files selected for processing (3)
🚧 Files skipped from review as they are similar to previous changes (3)
📝 WalkthroughSummary by CodeRabbit
WalkthroughThis PR updates three factory_sim behavior tree XML objectives: it changes drop-pose framing for the jig drop step, expands vacuum-pick planning to multiple rotated approach candidates, and adds early-stop control plus an unpickable branch to the left-bin pick-and-place loop. ChangesPick and place planning fixes
Possibly related issues
Suggested reviewers: 🚥 Pre-merge checks | ✅ 4✅ Passed checks (4 passed)
Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/factory_sim/objectives/pick_and_place_brackets_from_left_bin.xml`:
- Around line 122-126: The LogMessage in the detect-and-pick fallback is too
specific for a branch that can be reached by multiple failures, not just
grasp-yaw planning. Update the message in
pick_and_place_brackets_from_left_bin.xml near the else path so it reflects the
actual failure scope from the surrounding DetectAndPick sequence, and avoid
attributing the cause to “any sampled grasp yaw” unless that node is the only
possible failure. Keep the wording generic enough to help identify the failing
node from logs, or make it conditional on the specific child node that failed.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro Plus
Run ID: 4620e185-d1b4-4a4e-a2d4-10ae2844252e
📒 Files selected for processing (3)
src/factory_sim/objectives/drop_bracket_part_on_jig_subtree.xmlsrc/factory_sim/objectives/pick_and_place_brackets_from_left_bin.xmlsrc/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml
|
…in picking The left bin deliberately straddles the LR Mate 200iD's reachable workspace, and brackets settle with arbitrary yaw, so the single hardcoded pick orientation intermittently has no IK solution (pose IK 0/1) or fails the 55mm approach push. A suction grasp on the flat bracket face is yaw-free, so the pick now batches 8 approach candidates at 45-degree yaw increments about the part face normal via SetupMTCBatchPoseIK. The jig drop targets the attached part frame instead of tool0, so any grasp yaw still lands the alignment holes on the jig posts. If no sampled yaw plans, the objective logs INFO and ends cleanly instead of aborting with PLANNING_FAILED. Also migrates the deprecated Repeat decorator to RepeatUnlessFailureWithinTick. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
griswaldbrooks
force-pushed
the
fix/20020-factory-sim-unreachable-brackets
branch
from
a5c691e to
6a5d8b2
Compare
|
Member
|
/backport v9.4 |
|
Successfully created backport PR for |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
[written by AI]
Fixes PickNikRobotics/moveit_pro#20020
Problem
The left bin in
factory_simdeliberately straddles the LR Mate 200iD's reachable workspace (measured boundary ≈0.70–0.75 m planar at pick height, orientation-dependent). Brackets settle with arbitrary yaw aligned to the rotated bin, which swings the hardcoded pick offset toward the bin's far side.Pick and Place Brackets from Left Bininherits the settled yaw exactly, so depending on where the physics settles each launch, the first pick either has no IK solution (pose IK (0/1)inpick_part) or plans but fails the 55 mm approach push (Cartesian Motion: failed to move full distance). Machine load shifts the settle, which is why the failure reproduces 100% on some machines and intermittently on others.Approach
A suction grasp on the flat bracket face is yaw-free about the face normal, so:
vacuum_pick_bracket_part_subtree.xml— keeps the same pick point but builds 8 approach candidates at 45° yaw increments about the part's settled face normal (respecting settled pitch/roll), and replaces the single-poseSetupMTCPlanToPosewithSetupMTCConnectWithProRRT+SetupMTCBatchPoseIK, letting MTC plan through whichever yaw solves. This converts most of the bin's far side from unpickable to pickable (86% of the bin interior is reachable tool-down at some yaw vs. only 10% at every yaw).drop_bracket_part_on_jig_subtree.xml— the jig drop target is re-expressed for the attached part's own frame (ik_frame="{part_name}") instead oftool0. The attached body carries the true part-to-gripper transform (the MuJoCo weld freezes the grasp-time relative orientation), so the alignment holes land on the jig posts regardless of which grasp yaw was chosen. The part-frame pose was derived by composing the nominal grasp transform out of the previous tool0 numbers.pick_and_place_brackets_from_left_bin.xml— if no sampled yaw plans, the objective deactivates the vacuum, logs INFO ("Could not plan a pick … skipping the remaining bracket picks"), and ends cleanly instead of aborting withPLANNING_FAILED. This matches the sim's teaching purpose (reachable vs. unreachable areas). Also migrates the deprecatedRepeatdecorator toRepeatUnlessFailureWithinTick.The bins intentionally stay where they are — reachable and unreachable regions are the point of this sim.
Manual verification
pose_ikbehavior), and the full run ledger are attached to the linked issue.Open question for reviewers
On an unpickable part this gives up on all remaining picks rather than re-detecting, because registration would find the same far bracket again (the ICP guess pose biases to the bin's far side). If retry-per-remaining-part is preferred, it is a small policy change in the top-level tree.
Release notes
factory_sim'sPick and Place Brackets from Left Binobjective intermittently failing with a planning failure by sampling vacuum-grasp yaws at the pick point, compensating the jig placement for the chosen grasp, and skipping parts that settle outside the robot's reachable workspace with an informative log message instead of aborting.🤖 Generated with Claude Code