enhance atom action - #559
Conversation
Greptile SummaryThe PR combines the approach and follow-up portions of PickUp and Place into one planner request, then splits and resamples the resulting joint trajectory around the grasp or release pose.
Confidence Score: 4/5The PR is not yet safe to merge because TOPP-RA-backed PickUp and Place actions still receive an invalid default quantity and fail planning. The attempted sampling fix clears the requested count before the configured TOPP-RA planner is invoked; TOPP-RA consequently uses its quantity default of 0.01, rejects it as below two, and returns a failed plan. Files Needing Attention: embodichain/lab/sim/atomic_actions/primitives/pick_up.py and embodichain/lab/sim/atomic_actions/primitives/place.py
|
| Filename | Overview |
|---|---|
| embodichain/lab/sim/atomic_actions/primitives/pick_up.py | Combines approach and lift planning, but the strategy-based sample-count override leaves the previously reported TOPP-RA failure outstanding. |
| embodichain/lab/sim/atomic_actions/primitives/place.py | Combines descent and retraction planning and repeats the outstanding TOPP-RA sample-count failure. |
| embodichain/lab/sim/atomic_actions/primitives/_helpers.py | Adds batched FK-based trajectory splitting and distance resampling around a target pose. |
| embodichain/lab/sim/atomic_actions/engine.py | Allows callers to provide an invocation tracking policy while preserving joint-position tracking as the default. |
Reviews (3): Last reviewed commit: "update" | Re-trigger Greptile
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| if motion_policy.strategy == "motion_gen": | ||
| motion_options.sample_count = None |
There was a problem hiding this comment.
When PickUp or Place uses the TOPP-RA motion_gen backend with default plan options, clearing sample_count leaves quantity sampling at 0.01; TOPP-RA rejects quantities below two and returns an empty failed plan, so the action cannot be planned.
Knowledge Base Used: Simulation lab
Prompt To Fix With AI
This is a comment left during a code review.
Path: embodichain/lab/sim/atomic_actions/primitives/pick_up.py
Line: 317-318
Comment:
**TOPP-RA sampling becomes invalid**
When PickUp or Place uses the TOPP-RA `motion_gen` backend with default plan options, clearing `sample_count` leaves quantity sampling at `0.01`; TOPP-RA rejects quantities below two and returns an empty failed plan, so the action cannot be planned.
**Knowledge Base Used:** [Simulation lab](https://app.greptile.com/dexforce/-/custom-context/knowledge-base/dexforce/embodichain/-/docs/simulation-lab.md)
---
For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.
There was a problem hiding this comment.
Pull request overview
This PR refactors the PickUp and Place atomic actions to reduce per-action motion-planner calls by generating a single combined joint trajectory and then splitting it back into logical segments for hand command timing and segment bookkeeping.
Changes:
- Combine multi-stage motion planning into a single
motion_generator.generate(...)call forPickUpandPlace, then split the resulting joint trajectory at a target pose. - Add
split_joint_trajectory_at_pose(...)to split + resample joint trajectories using per-environment FK-based boundary detection. - Update tests and the Place tutorial script to reflect the new planning/splitting behavior.
Reviewed changes
Copilot reviewed 6 out of 6 changed files in this pull request and generated 2 comments.
Show a summary per file
| File | Description |
|---|---|
| tests/sim/atomic_actions/test_trajectory_ops.py | Adds coverage for the new split/resample helper, including per-environment boundary selection. |
| tests/sim/atomic_actions/test_actions.py | Updates motion-generator monkeypatching and asserts single-call planning + option semantics. |
| scripts/tutorials/atomic_action/place.py | Provides an explicit SceneSnapshot when creating the initial planning context for the demo. |
| embodichain/lab/sim/atomic_actions/primitives/place.py | Plans approach+retract in one motion-gen call and splits at the place pose. |
| embodichain/lab/sim/atomic_actions/primitives/pick_up.py | Plans approach+lift in one motion-gen call and splits at the grasp pose. |
| embodichain/lab/sim/atomic_actions/primitives/_helpers.py | Introduces split_joint_trajectory_at_pose(...) and exports it. |
💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.
| expected_index = round( | ||
| (trajectory.shape[1] - 1) | ||
| * first_sample_count | ||
| / (first_sample_count + second_sample_count) | ||
| ) |
| trajectory_xpos = robot.compute_batch_fk( | ||
| qpos=trajectory, | ||
| name=control_part, | ||
| to_matrix=True, | ||
| ) |
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 11 out of 11 changed files in this pull request and generated 1 comment.
Suppressed comments (1)
embodichain/lab/sim/atomic_actions/primitives/_helpers.py:188
split_joint_trajectory_at_poseassumesrobot.compute_batch_fk(...)returns a pose tensor, butRobot.compute_batch_fkcan returnNoneon solver/shape errors. That would currently fail later with a confusingTypeErrorduring tensor indexing/gather. Add an explicit return-type/shape check here and raise a clear error early.
trajectory_xpos = robot.compute_batch_fk(
qpos=trajectory,
name=control_part,
to_matrix=True,
)
| failed = [ | ||
| f"{plan.skill_id}: {plan.diagnostics.messages or ('planning failed',)}" | ||
| for plan in compiled.action_plans | ||
| if not plan.plan_success.all() | ||
| ] |
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 15 out of 15 changed files in this pull request and generated 2 comments.
Suppressed comments (1)
scripts/tutorials/atomic_action/tutorial_utils.py:1059
create_ur10_robotiq_robot_cfgaccepts**kwargsbut does not consume them. This makes it easy for callers to pass unsupported parameters without noticing. Consider validatingkwargs(or removing it).
def create_ur10_robotiq_robot_cfg(
init_pos: Sequence[float] = (0.0, 0.0, 0.0),
init_qpos: Sequence[float] | None = None,
**kwargs,
) -> RobotCfg:
| def create_franka_panda_robot_cfg( | ||
| init_pos: Sequence[float] = (0.0, 0.0, 0.0), | ||
| init_qpos: Sequence[float] | None = None, | ||
| **kwargs, | ||
| ) -> RobotCfg: |
| if robot_type == "ur5": | ||
| return create_ur5_gripper_robot_cfg( | ||
| init_pos=init_pos, | ||
| init_qpos=init_qpos, | ||
| **kwargs, | ||
| ) |
Description
TODO:
motion_generator.generatein one atom action.Type of change
Checklist
black .command to format the code base.python docs/scripts/check_api_docs.py), if applicable