[BUG FIX] Restore the carried object's pose when plan_path fails#2938
[BUG FIX] Restore the carried object's pose when plan_path fails#2938HaozheZhang6 wants to merge 2 commits into
Conversation
![chatgpt-codex-connector[bot]](https://avatars.githubusercontent.com/in/1144995?s=60&v=4){kind=small}
There was a problem hiding this comment.
💡 Codex Review
Here are some automated review suggestions for this pull request.
Reviewed commit: ab7d96156d
ℹ️ About Codex in GitHub
Codex has been enabled to automatically review pull requests in this repo. Reviews are triggered when you
- Open a pull request for review
- Mark a draft as ready
- Comment "@codex review".
If Codex has suggestions, it will comment; otherwise it will react with 👍.
When you sign up for Codex through ChatGPT, Codex can also answer questions or update the PR, like "@codex address that feedback".
| if is_plan_with_obj: | ||
| self.update_object(ee_link_idx, obj_link_idx, _pos, _quat, envs_idx) |
There was a problem hiding this comment.
When callers pass a qpos_start that differs from the robot's current pose, get_exclude_geom_pairs(...) above has already moved the robot to qpos_start before _pos/_quat are captured, so this new restore applies a start-relative grasp transform after setting the robot back to qpos_cur. In that scenario a failed plan_path(with_entity=...) still teleports the attached object instead of preserving its original world pose; the same pattern was added in the RRTConnect failure branch, so the regression test only covers the qpos_start=None case.
Useful? React with 👍 / 👎.
…urrent qpos_start The grasp transform is captured relative to qpos_start (the robot is moved there by get_exclude_geom_pairs before capture), so re-deriving the object pose at qpos_cur on failure teleported it whenever qpos_start differed from the current pose. Snapshot the object's world pose up front and restore it directly; extend the regression test with a custom-qpos_start case that fails on the old restore.
1 participant
Description
plan_path(..., with_entity=obj)mutates the scene whenever a planning attempt fails. BothRRT.planandRRTConnect.planrestore the robot'sqposin the failure early-return (if is_invalid.all():), but only the success path also callsupdate_object(...)to put the carried object back. So a failed attempt leaves the object at whatever configuration the planner last explored. Because planning is retried (max_retry), a failed attempt also corrupts the relative grasp offset that the next retry captures, so even an eventually-successful plan can return with the object displaced.The fix mirrors the success path: restore the object via
update_object(...)in the failure branch of both planners (+2 lines each).Related Issue
Resolves #2715
Motivation and Context
plan_pathis a query and should not change the scene. Today, calling it withwith_entity(e.g. a grasped cube) and hitting a failed attempt teleports the object, which corrupts downstream control and makes retried planning unsound.How Has This Been / Can This Be Tested?
Issue repro (Franka + cube), headless, recording the cube pose immediately before/after
plan_path(with_entity=cube)with no trajectory executed:plan_path[0.399, -0.021, 1.078][0.300, 0.132, 0.233]Isolated single failed attempt (
max_retry=0, in-collision goal so the attempt fails deterministically): before → the cube is dragged 1.12 m; after → preserved (~3e-8).Added a regression test that reproduces this without depending on the planner's RNG (an in-collision goal makes the single attempt fail deterministically), then asserts the attached object's pose is unchanged:
It passes with the fix and fails without it (the object moves ~1.16 m).
ruff check/ruff formatare clean on both files. I ran the new test on the CPU backend; I was not able to run the full suite on my box (its CUDA driver is too old for the installed torch), so I'm relying on CI for the rest.Checklist:
Submitting Code Changessection of CONTRIBUTING document.