Add rigid object constraint support (attach/detach)#337
Open
yuecideng wants to merge 17 commits into
Open
Conversation
Design for attaching two RigidObjects via a fixed physics constraint and removing it, with a standalone sim-layer API (SimulationManager + RigidConstraint) and an on-demand event functor in events.py. Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
Skips gracefully when the test asset is absent or CUDA is unavailable. On a machine with the asset + GPU, asserts the fixed constraint holds the relative transform under physics and that detaching lets objects separate. Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
With identity local frames the dexsim fixed constraint pulls the two body origins together, which is wrong for the grasp/attach use case. When local_frame_b is None, compute it per env as inv(pose_B) @ pose_A so the constraint welds the objects at their current relative pose. local_frame_a None still defaults to identity. Explicit local_frame_b is used verbatim. Updates the spec, adds two unit tests, and makes the integration test's detach assertion robust (relative-pose drift instead of absolute fall). Co-Authored-By: Claude <noreply@anthropic.com>
Two-cube tutorial demonstrating create_rigid_constraint / remove via the SimulationManager API, with a runnable script (CubeCfg, no asset file needed) registered in the tutorial index. Prints the bodies' relative z while attached (held constant) and after removal (free to drift). Co-Authored-By: Claude <noreply@anthropic.com>
Co-Authored-By: Claude <noreply@anthropic.com>
Contributor
There was a problem hiding this comment.
Pull request overview
Adds first-class “attach/detach” support for RigidObject ↔ RigidObject via fixed physics constraints, exposed both through the sim-layer SimulationManager API and through gym event functors, with tests and a tutorial to demonstrate usage.
Changes:
- Introduces
RigidConstraintCfg+RigidConstraintwrapper and wires a_constraintsregistry intoSimulationManagerwith create/remove/get APIs. - Adds gym event functors (
create_rigid_constraint/remove_rigid_constraint) to trigger attach/detach on demand from tasks. - Adds unit tests, an integration smoke test, and new tutorial docs + example script.
Reviewed changes
Copilot reviewed 13 out of 13 changed files in this pull request and generated 12 comments.
Show a summary per file
| File | Description |
|---|---|
embodichain/lab/sim/cfg.py |
Adds RigidConstraintCfg config object for constraints. |
embodichain/lab/sim/objects/constraint.py |
Adds RigidConstraint batched wrapper over per-arena dexsim handles. |
embodichain/lab/sim/objects/__init__.py |
Exports RigidConstraint from the sim objects package. |
embodichain/lab/sim/sim_manager.py |
Adds constraint registry + create/remove/get_rigid_constraint APIs and cleanup wiring. |
embodichain/lab/gym/envs/managers/events.py |
Adds attach/detach event functors that delegate into the sim API. |
tests/sim/objects/test_rigid_constraint.py |
Adds mock-based unit tests for cfg/wrapper and sim-manager constraint plumbing. |
tests/gym/envs/managers/test_event_rigid_constraint.py |
Adds unit tests for new rigid-constraint event functors and EventManager custom mode application. |
tests/sim/test_rigid_constraint_integration.py |
Adds real-sim smoke test validating attach holds relative pose and detach allows separation. |
scripts/tutorials/sim/create_rigid_constraint.py |
Adds runnable tutorial script demonstrating attach/detach with two cubes. |
docs/source/tutorial/rigid_constraint.rst |
Adds tutorial page documenting the workflow and referencing the script. |
docs/source/tutorial/index.rst |
Adds the new rigid-constraint tutorial to the tutorial index/toctree. |
docs/superpowers/specs/2026-06-29-rigid-constraint-design.md |
Adds design spec for the feature. |
docs/superpowers/plans/2026-06-29-rigid-constraint.md |
Adds implementation plan document. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+654
to
+670
| obj_a = env.sim.get_asset(obj_a_cfg.uid) | ||
| obj_b = env.sim.get_asset(obj_b_cfg.uid) | ||
| if not isinstance(obj_a, RigidObject) or not isinstance(obj_b, RigidObject): | ||
| logger.log_error( | ||
| f"Constraint '{name}' requires two RigidObjects, but got " | ||
| f"{type(obj_a).__name__} and {type(obj_b).__name__}." | ||
| ) | ||
| env.sim.create_rigid_constraint( | ||
| cfg=RigidConstraintCfg( | ||
| name=name, | ||
| rigid_object_a_uid=obj_a_cfg.uid, | ||
| rigid_object_b_uid=obj_b_cfg.uid, | ||
| local_frame_a=local_frame_a, | ||
| local_frame_b=local_frame_b, | ||
| ), | ||
| env_ids=env_ids, | ||
| ) |
| env_ids: Target environment indices. None -> all envs. | ||
| name: Base constraint name to remove. | ||
| """ | ||
| env.sim.remove_rigid_constraint(name, env_ids=env_ids) |
Comment on lines
+1012
to
+1017
| local_frame_a: 4x4 joint frame in object A's local coordinates. | ||
| ``None`` attaches at the objects' current relative pose (identity). | ||
| Accepts a single ``(4, 4)`` matrix (shared by all envs) or an | ||
| ``(N, 4, 4)`` array (one frame per env). Defaults to None. | ||
| local_frame_b: As :attr:`local_frame_a`, for object B. Defaults to None. | ||
| constraint_type: Reserved for future typed constraints (prismatic, |
Comment on lines
+1034
to
+1039
| local_frame_a: np.ndarray | None = None | ||
| """Local joint frame on object A. None -> identity (current relative pose).""" | ||
|
|
||
| local_frame_b: np.ndarray | None = None | ||
| """Local joint frame on object B. None -> identity (current relative pose).""" | ||
|
|
Comment on lines
+647
to
+650
| local_frame_a: Local joint frame on object A. None attaches at the | ||
| objects' current relative pose. Accepts (4,4) or (N,4,4). | ||
| local_frame_b: Local joint frame on object B. None -> identity. | ||
|
|
| assert all(h is None for h in constraint.constraint_handles) | ||
|
|
||
|
|
||
| from embodichain.lab.sim.sim_manager import SimulationManager |
Comment on lines
+146
to
+148
| from embodichain.lab.gym.envs.managers.event_manager import EventManager | ||
| from embodichain.lab.gym.envs.managers.cfg import EventCfg | ||
| from embodichain.utils import configclass |
Comment on lines
+76
to
+78
| duck_path = get_data_path(DUCK_PATH) | ||
| # Two dynamic ducks at different heights, welded at identity frames. | ||
| attrs_a = RigidBodyAttributesCfg() |
Comment on lines
+101
to
+105
| if sim_device == "cuda" and getattr(self.sim, "is_use_gpu_physics", False): | ||
| self.sim.init_gpu_physics() | ||
| self.sim.enable_physics(True) | ||
|
|
||
| def test_fixed_constraint_holds_relative_pose(self): |
- Normalize env_ids (None / tensor / sequence) to list[int] at the sim
layer via _normalize_env_ids, used in create_rigid_constraint and
remove_rigid_constraint. Widens the sim API to accept torch.Tensor
(as passed by EventManager) so the per-arena dexsim names are clean
('weld_0' not a tensor stringification) and create/remove agree.
Adds tensor-env_ids regression tests.
- Correct local_frame_b docstrings/comments across RigidConstraintCfg,
the create functor, the tutorial script, and the integration test:
None -> inv(pose_B) @ pose_A (weld at current relative pose), not
identity. local_frame_a None stays identity.
- Make RigidConstraint.rigid_object_a/b optional in the type hint
(RigidObject | None) to match their None defaults.
- Move mid-file test imports to module top (E402); drop an unused
RigidObject import.
- Add teardown_method to the integration base test (destroy + flush) to
avoid leaking dexsim scenes between tests.
Co-Authored-By: Claude <noreply@anthropic.com>
…mbodiChain into feat/rigid-constraint
Comment on lines
424
to
442
| def asset_uids(self) -> List[str]: | ||
| """Get all assets uid in the simulation. | ||
|
|
||
| The assets include lights, sensors, robots, rigid objects and articulations. | ||
|
|
||
| Returns: | ||
| List[str]: list of all assets uid. | ||
| """ | ||
| uid_list = ["default_plane"] | ||
| uid_list.extend(list(self._lights.keys())) | ||
| uid_list.extend(list(self._sensors.keys())) | ||
| uid_list.extend(list(self._robots.keys())) | ||
| uid_list.extend(list(self._rigid_objects.keys())) | ||
| uid_list.extend(list(self._rigid_object_groups.keys())) | ||
| uid_list.extend(list(self._soft_objects.keys())) | ||
| uid_list.extend(list(self._cloth_objects.keys())) | ||
| uid_list.extend(list(self._articulations.keys())) | ||
| uid_list.extend(list(self._constraints.keys())) | ||
| return uid_list |
Comment on lines
+1178
to
+1182
| if handle is None: | ||
| logger.log_error( | ||
| f"Failed to create constraint '{name_i}' in arena {env_id}." | ||
| ) | ||
| handles[env_id] = handle |
Comment on lines
+63
to
+68
| sim_cfg = SimulationManagerCfg( | ||
| width=1920, | ||
| height=1080, | ||
| headless=True, | ||
| physics_dt=1.0 / 100.0, # Physics timestep (100 Hz) | ||
| sim_device=args.device, |
| import argparse | ||
| import sys | ||
|
|
||
| import numpy as np |
Comment on lines
+131
to
+133
| sim.remove_rigid_constraint("cube_weld") | ||
| assert sim.get_rigid_constraint("cube_weld") is None | ||
| print("\n[INFO]: Removed constraint 'cube_weld'. cube_a and cube_b are now free.") |
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
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.
Description
This PR adds support for attaching two
RigidObjects via a fixed physics constraint and removing it, exposed both as a standaloneSimulationManagerAPI (usable outside the gym) and as on-demand event functors triggered from a task environment. It also includes a two-cube tutorial.What's added:
RigidConstraintCfg(embodichain/lab/sim/cfg.py) — names the constraint, points at two object UIDs, optional local joint frames, and a reservedconstraint_typefield (fixed-only for v1; prismatic/revolute/spherical/d6 can land later without API change).RigidConstraintwrapper (embodichain/lab/sim/objects/constraint.py) — a batch wrapper mirroringRigidObject's per-arena pattern: one dexsimFixedConstrainthandle per arena, withNonewhere inactive so arena-index == list-index. Exposesget_relative_transform,get_local_pose,is_valid,destroy(allenv_ids-aware).SimulationManagerAPI (sim_manager.py) —create_rigid_constraint/remove_rigid_constraint/get_rigid_constraint+ a_constraintsregistry, wired intoasset_uidsand_deferred_destroy.env_ids-aware, so a vectorized task can attach/detach a subset of arenas.managers/events.py) —create_rigid_constraint/remove_rigid_constraintthin adapters (resolveSceneEntityCfg→RigidObject→ sim API), triggered via custom modes (event_manager.apply(mode="attach"/"detach", env_ids)).scripts/tutorials/sim/create_rigid_constraint.py+docs/source/tutorial/rigid_constraint.rst(two cubes viaCubeCfg, no asset file needed; prints the bodies' relative z while attached vs after removal).Design choice worth highlighting: with default (
None) local frames, the constraint welds the two objects at their current relative pose —local_frame_adefaults to identity andlocal_frame_bis computed per env asinv(pose_B) @ pose_A. This matches the grasp/attach use case ("attach where the object is") rather than pulling the two origins together. (Confirmed by a real headless run: relative z held constant at −0.2 while attached, drifts to −0.16 after removal.)Motivation: EmbodiChain had no physics constraint between rigid bodies. Constraints are needed for grasping (weld a held object to the gripper) and assembly (temporarily join two parts), and for reset-safe episode logic (detach before reset).
Dependencies: None new. Uses the existing dexsim
create_fixed_constraint/remove_constraintAPI (bound onArena).Design spec:
docs/superpowers/specs/2026-06-29-rigid-constraint-design.md; plan:docs/superpowers/plans/2026-06-29-rigid-constraint.md.Type of change
Screenshots
Headless tutorial run (relative z = cube_b.z − cube_a.z, env 0):
Checklist
black .command to format the code base.🤖 Generated with Claude Code