Support PickUp on articulated Rubik's cube asset - #632
Conversation
The rubiks_cube_001 asset is an articulation, not a rigid body: a top_turn
revolute joint couples top_layer to lower_two_layers. Add a tutorial that
picks it up with the joint locked, plus focused tests and documentation of
the articulated-target path.
Three constraints drove the implementation, each verified in simulation:
* The USD parser rejects the asset as a rigid object ("Expected exactly one
rigid object ... found 0"), because both bodies sit under an
ArticulationRoot. It is spawned through ArticulationCfg.
* ArticulationCfg fixes roots to the world by default, which welds a
graspable target in place. Left at the default, plan_success still reports
success and the whole trajectory replays while the cube never moves.
* make_clear_dynamics_callback takes a RigidObject; applying it to this
floating-base articulation clears the root velocity as the lift begins and
breaks the grasp. The articulation needs no freeze.
Articulation exposes no mesh accessors, so antipodal sampling is unavailable
and the grasp pose is supplied explicitly, reusing the robot's current
end-effector orientation.
The asset also carries a Scene Engine canonicalization defect: its
bottom-center translate is authored on Y while stages are Z-up, so the cube
sits half an edge off in Y and straddles z=0. The tutorial compensates via
CUBE_BOTTOM_CENTER_OFFSET; the generator bug is tracked separately.
Verified on RTX 4090: cube settles on the table, lifts +0.147 m, and
|top_turn| stays under 1e-5 rad throughout the replay.
Co-Authored-By: Claude <noreply@anthropic.com>
 |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| offset = torch.tensor( | ||
| CUBE_BOTTOM_CENTER_OFFSET, dtype=pose.dtype, device=pose.device | ||
| ) | ||
| pose[:, :3, 3] = pose[:, :3, 3] + offset |
There was a problem hiding this comment.
CUBE_BOTTOM_CENTER_OFFSET is defined in the root frame, but this line adds it directly to the world translation. Because the floating articulation advances through physics before its pose is sampled, a rotated root produces an incorrect grasp center and can cause an off-center or failed grasp. Transform the offset by the root rotation before adding it.
Knowledge Base Used: Simulation lab
Prompt To Fix With AI
This is a comment left during a code review.
Path: scripts/tutorials/atomic_action/pickup_rubiks_cube.py
Line: 191
Comment:
**Center offset ignores rotation**
`CUBE_BOTTOM_CENTER_OFFSET` is defined in the root frame, but this line adds it directly to the world translation. Because the floating articulation advances through physics before its pose is sampled, a rotated root produces an incorrect grasp center and can cause an off-center or failed grasp. Transform the offset by the root rotation before adding it.
**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.
| from pathlib import Path | ||
| from types import ModuleType | ||
|
|
||
| import pytest |
There was a problem hiding this comment.
Marking this feature test no_sim means its fakes only confirm that configuration calls were issued. The tests cannot detect the physical regressions this change addresses, including a fixed root, an ineffective joint lock, a broken grasp, or zero lift. This violates the repository directive that new features include focused tests proving their behavior, so a real-simulation test measuring post-replay elevation and bounding top_turn motion is required before merging.
Context Used: CLAUDE.md (source)
Knowledge Base Used: Simulation lab
Prompt To Fix With AI
This is a comment left during a code review.
Path: tests/sim/test_pickup_rubiks_cube_tutorial.py
Line: 23
Comment:
**Tests skip physical behavior**
Marking this feature test `no_sim` means its fakes only confirm that configuration calls were issued. The tests cannot detect the physical regressions this change addresses, including a fixed root, an ineffective joint lock, a broken grasp, or zero lift. This violates the repository directive that new features include focused tests proving their behavior, so a real-simulation test measuring post-replay elevation and bounding `top_turn` motion is required before merging.
**Context Used:** CLAUDE.md ([source](https://github.com/dexforce/embodichain/blob/main/CLAUDE.md))
**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.
| entity_id=cube.uid, | ||
| ) | ||
| center_pose = cube_center_pose(cube) | ||
| grasp_pose = robot.compute_fk( |
There was a problem hiding this comment.
We should use grasp pose generator, similar as pickup example.
| @@ -0,0 +1,196 @@ | |||
| # ---------------------------------------------------------------------------- | |||
There was a problem hiding this comment.
Currently not necessary to add this unit test.
| ) | ||
| center_pose = cube_center_pose(cube) | ||
| grasp_pose = robot.compute_fk( | ||
| qpos=robot.get_qpos()[:, robot.get_joint_ids(name="arm")], |
There was a problem hiding this comment.
When we use grasp pose generator for articulated object, the affordance (grasp pose) should be computed from a specified link. So we may also need to modified the Afforfance term to support sample grasp poses from link shape under this context.
Address review: use the shared parallel-jaw grasp pose generator instead of an explicit end-effector pose, and drop the tutorial unit test. Articulation has no whole-body get_vertices(); it publishes geometry per link through get_link_vert_face(). Feeding the lower_two_layers mesh into AntipodalAffordance lets the existing generator sample grasps exactly as it does for rigid objects, which is the same source slide.py uses for its handle affordance. No change to affordance.py was needed: AntipodalAffordance already accepts arbitrary mesh vertices and triangles, and OpenDoorAffordance and SlideAffordance already carry link meshes as its subclasses. lower_two_layers is the top_turn joint's parent and carries two of the three layers, so its link frame is the stable grasp reference while the joint is held at zero. Verified on RTX 4090 by instrumenting the shipped main() rather than a replica: cube lifts +0.14647 m and |top_turn| stays at 5e-6 rad through the replay. Demo GIF regenerated from that run. Co-Authored-By: Claude <noreply@anthropic.com>
|
Thanks @yuecideng — all three addressed in 833820f. 1. Use the grasp pose generator. Done. The tutorial now passes 2. Drop the unit test. Removed. 3. Affordance from a specified link. Implemented, and it turned out not to need a change to
vertices, triangles = cube.get_link_vert_face("lower_two_layers")
ObjectSemantics(
label="rubiks_cube",
geometry={},
affordance=AntipodalAffordance(
mesh_vertices=torch.as_tensor(vertices),
mesh_triangles=torch.as_tensor(triangles),
),
entity_id=cube.uid,
)This is the same source I picked Verification. Measured by instrumenting the shipped The demo GIF is regenerated from that same run. One thing worth flagging for this skill: |
|
Thanks for the detailed follow-up. I agree that this case does not require a new affordance type, but I think the frame contract needs to be made explicit before this becomes a reusable articulation path. The supported abstraction should be:
The current implementation reads the mesh with I suggest the following implementation:
Conceptually, the integration API could look like: semantics = create_rigidized_articulation_antipodal_semantics(
cube,
grasp_link="lower_two_layers",
locked_qpos={"top_turn": 0.0},
label="rubiks_cube",
)The important part is not the helper name, but that its output mesh is in the articulation-root frame. Please also add a focused unit test where the selected link has a non-identity rotation and translation relative to the root. That test should verify the transformed vertices or resulting world grasp pose. An identity-transform root-link case alone would not catch the current frame mismatch. Invalid link, malformed mesh, and unlocked or changing-joint cases should also be covered proportionally. This boundary is also friendlier to Task Program. The program can continue to express For this cube, |
A locked articulation is one compound rigid body: the articulation root carries object identity and pose, while one selected link is only the source of grasp geometry. get_link_vert_face() returns vertices in the link frame, but ObjectSemantics.entity_id names the articulation root, so grounding applies the root pose to link-frame geometry. That is correct only where T_root_link is identity. Locking the joint prevents future relative motion; it does not remove a non-identity root-to-link transform. Add create_rigidized_articulation_antipodal_semantics(), which reads the link mesh, evaluates the root-to-link transform at the declared locked configuration, and transforms the vertices into the articulation-root frame before constructing the affordance. AntipodalAffordance stays simulator-independent and still owns only object-local geometry; no articulation handle or link name reaches it. The transform comes from named forward kinematics where a kinematic chain exists. USD-backed articulations never build one (articulation.py only builds pk_chain for URDF sources), so there the transform is read from the link pose reported while the joints hold the declared configuration. Both routes describe the same frame because the same root pose grounds the entity. Validate the compound-rigid assumption instead of trusting it: every joint must be declared in locked_qpos and must actually be held there, otherwise the call is rejected. A joint still free to move would leave the mesh describing a pose the object no longer has, which fails as a missed grasp rather than as an error. Joint locking, initial positions, fixed_base and drive parameters remain owned by ArticulationCfg or the physical environment. Generalize the public scene_entities type from Sequence[RigidObject] to the SceneEntity protocol, which declares the uid and get_local_pose() that grounding actually requires. Articulation support becomes part of the declared API rather than incidental duck typing. Measured on the Rubik's cube asset: lower_two_layers has an exactly identity root-to-link transform, so the previous code was correct for this asset by coincidence. Rotating the joint to pi/2 and sourcing top_layer moves a vertex by 28.3 mm on a 57.6 mm cube. The regression test uses a non-identity rotation and translation, and fails if the transform is removed; an identity-only case does not. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
|
You were right, and the measurement makes the case sharper than the code review alone does. Measured on this asset. Implemented as specified:
One deviation, with a reason. You asked for Tests —
On the Task Program boundary — agreed, and that was the deciding argument for me. Keeping |
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
create_rigidized_articulation_antipodal_semantics publishes one immutable mesh for the whole batch, but validated only arena 0 and only the joint's current position. Two failures were therefore silent: * An arena whose joint sat elsewhere passed validation and received grasp geometry that does not describe its own link pose, because the USD route also derived the shared root-to-link transform from row 0. * A passive or zero-stiffness joint resting at the declared value passed, then moves under load while the immutable mesh keeps the old frame. The lock check now covers every arena and every joint, and requires the joint to be held there rather than merely resting: a position drive with non-zero stiffness commanded to the declared value, or coincident position limits that pin it. The live root-to-link route evaluates every arena and refuses a batch whose arenas disagree, so one mesh is never published on arena 0's authority. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
|
@yuecideng Rebased onto Branch stateMerged Fixes in 8f3f55cBoth findings were the same root cause: one immutable mesh is published for the whole batch, while validation looked at a single row and a single quantity. 1. Validation covered only arena 0. 2. Being at the declared position was treated as being locked. A passive or zero-stiffness joint can rest there and move under load while the mesh keeps the old frame. Every joint must now also be held, by either:
I accepted the limit route deliberately — pinning through limits is a stronger lock than a drive, so requiring a drive would falsely reject it. The check reads stiffness, target and limits rather than The tutorial is unchanged:
ValidationFour of the six new tests were checked in the negative direction: reverting Still openThe two remaining greptile comments on the tutorial are about the asset rather than the adapter, and I'd rather settle them with you than guess:
|
2 participants
Description
Adds
PickUpsupport for therubiks_cube_001asset and documents the articulated-target path.The asset is an articulation, not a rigid body: a
top_turnrevolute joint (Z axis, ±90°) couplestop_layertolower_two_layers. Per the agreed scope the joint is locked with a stiff position drive so the cube grasps as one body; turning it via the existingTwistskill is out of scope here.Three constraints drove the implementation. Each was found by running the simulation, not by reading code:
Expected exactly one rigid object ... found 0, because both bodies sit under anArticulationRoot.MeshCfgis not viable; the cube is spawned throughArticulationCfg.ArticulationCfgfixes roots to the world by default. That suits drawers and doors but welds a graspable target in place. Left at the default,plan_successstill reports success and the entire trajectory replays while the cube never moves — the first run measuredlifted by = +0.00000 m. Fixed withroot_props=ArticulationRootPropertiesCfg(fixed_base=False), and pinned by a test, since a regression here fails silently.make_clear_dynamics_callbacktakes aRigidObject. Applying it to this floating-base articulation clears the root velocity exactly as the lift begins: the grasp breaks and the cube drops back after rising ~3 mm. The articulation needs no freeze.Articulationexposes noget_vertices()/get_triangles(), soAntipodalAffordancesampling is unavailable. The grasp pose is supplied explicitly, reusing the robot's current end-effector orientation and replacing only the translation — the same idiom asstack_blocks_two.py.Known asset defect, compensated here
The asset carries a Scene Engine canonicalization bug:
_canonicalize_articulated_usdc_bottom_centercomputes bottom-center withminimum[1], hardcoding Y as the up axis, while EmbodiChain stages are Z-up (this asset declaresupAxis: Zand gravity(0, 0, -1)). The cube therefore sits half an edge off in Y and straddlesz = 0instead of resting on it. The tutorial compensates viaCUBE_BOTTOM_CENTER_OFFSET, which lands the bottom face atz = 0.000.This affects every articulated USDC the pipeline produces, not just this asset. It is a generator fix and is deliberately not bundled into this PR.
Dependencies: none.
Type of change
Screenshots
PickUp on the articulated Rubik's cube,
top_turnlocked. Grasp sampled from thelower_two_layerslink by the parallel-jaw generator (recorded from the run measured below):Checklist
black .command to format the code base (black==26.3.1, clean)builtin_actions.md: new "Picking articulated targets" subsection underPickUp)python docs/scripts/check_api_docs.py-> 2069/2069; no public API added)tests/sim/test_pickup_rubiks_cube_tutorial.py, 6 passed)Validation
Measured on an RTX 4090,
--headless:Note on verification:
plan_successis not sufficient evidence for this skill. Both the fixed-root and theclear_dynamicsfailures above planned and replayed cleanly while the object stayed on the table. The numbers above come from measuring the cube's pose before and after the replay, and the recorded GIF is the same run.Asset availability
rubiks_cube_001.usdcis not in the download registry —embodichain/data/assets/entries are remotezip+ MD5 datasets and cannot reference a local file. The tutorial defaults to$EMBODICHAIN_DEFAULT_DATA_ROOT/RubiksCube/rubiks_cube_001.usdcand accepts--asset_path. Publishing the asset to the DexForce index is a separate step.🤖 Generated with Claude Code