Pre-planning cup grabbing#232
Open
Yao Chen (yao-stuy) wants to merge 18 commits into
Open
Conversation
… cleaning sequence
…defined in expresso.go. Added "requiredPoses" function that creates a list of the required poses for the module. Added "validateConfiguredPoses" function that checks if all the required poses are configured in the module. It checks for the existence of the requrired poses and checks that the pose is not all 0s.
Removed seen map since there won't be duplicate poses.
…t unnecessarily clean a clean filter.
Dynamic cup/glass pickup discovered unreachable candidates by physically
reaching for them: tryGrab moved to the approach pose, then found the
grab descent unplannable, then retreated — wasting a full reach-and-back-out
per unreachable candidate before trying the next one.
Split moveToRawPose into planRawPose (plan only, no motion) + executePlan,
keeping moveToRawPose as a thin wrapper so all existing callers are
unchanged. tryGrab now plans the approach AND the grab descent up front
without moving; an unreachable candidate returns errMotionPlanning with the
arm still parked at the observe pose, so the loop skips it with zero motion.
Only a candidate whose legs both plan is grabbed, and the validated plans are
executed directly rather than re-planned.
The grab sequence is otherwise identical — approach, open, descend, verify
holding, attach geometry, retreat — and grabAndVerifyHolding remains the
backstop that a cup was actually picked up. The retreat is still planned
fresh after the grab, since the held-item geometry changes the collision set.
Also:
- planRawPose takes a start config (descent plans from the approach plan's
end) overlaid onto the full current inputs, so the start state stays
complete and a non-world goal resolves against the right configuration.
- planRawPose takes a label, so pre-plan ("grab") plans are distinguishable
from generic ("move") plans in save_motion_requests_dir.
- An operator cancel (which PlanMotion wraps as errMotionPlanning) is caught
before being misread as "unreachable".
- An approach-execution failure now recovers to the observe pose before
aborting, matching the descent/verify failure paths.
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Nicolas Palpacuer (NickPPC)
left a comment
Nicolas Palpacuer (NickPPC)
left a comment
Member
There was a problem hiding this comment.
Can you clean up the comments a little? claude added a lot of comments.
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.
Dynamic cup/glass pickup used to discover unreachable candidates by physically
reaching for them — approach, find the grab descent unplannable, retreat — wasting
a full reach-and-back-out per bad candidate.
tryGrabnow plans both the approach and grab descent up front without moving. Anunreachable candidate is skipped with the arm still parked at the observe pose;
only a candidate whose legs both plan is grabbed, and the validated plans are
executed directly instead of re-planned.
The grab sequence is otherwise unchanged (approach → open → descend → verify →
attach → retreat), and
grabAndVerifyHoldingremains the backstop that a cup wasactually picked up. Implemented by splitting
moveToRawPoseintoplanRawPose+executePlan; existing callers are unaffected.🤖 Generated with Claude Code