Add rigid object constraint support (attach/detach)

docs/source/tutorial/index.rst

@@ -13,23 +13,24 @@ Follow the tutorials in this order for the best learning experience:
 1. :doc:`create_scene` — Set up a simulation, add objects, and run the render loop. **Start here.**
 2. :doc:`create_softbody` and :doc:`create_cloth` — Add deformable bodies to your scenes.
 3. :doc:`rigid_object_group` — Manage collections of rigid objects efficiently.
-4. :doc:`robot` — Load and control a robot in simulation.
-5. :doc:`sensor` — Add cameras and capture RGB/depth/segmentation data.
-6. :doc:`solver` — Configure IK solvers for end-effector control.
-7. :doc:`motion_gen` — Generate smooth trajectories with motion planners.
-8. :doc:`atomic_actions` — Use built-in action primitives (move, move joints, pick, move held object, place).
-9. :doc:`gizmo` — Interactively control robots with on-screen gizmos.
+4. :doc:`rigid_constraint` — Attach and detach two rigid objects via a fixed constraint.
+5. :doc:`robot` — Load and control a robot in simulation.
+6. :doc:`sensor` — Add cameras and capture RGB/depth/segmentation data.
+7. :doc:`solver` — Configure IK solvers for end-effector control.
+8. :doc:`motion_gen` — Generate smooth trajectories with motion planners.
+9. :doc:`atomic_actions` — Use built-in action primitives (move, move joints, pick, move held object, place).
+10. :doc:`gizmo` — Interactively control robots with on-screen gizmos.
 
 **Phase 2: Environments**
 
-10. :doc:`basic_env` — Create a simple Gymnasium environment with ``BaseEnv``. Prerequisite: Phase 1 basics.
-11. :doc:`modular_env` — Build a config-driven environment with ``EmbodiedEnv``, managers, and randomization. Prerequisite: :doc:`basic_env`.
-12. :doc:`data_generation` — Generate expert demonstration datasets for imitation learning. Prerequisite: :doc:`modular_env`.
-13. :doc:`rl` — Train RL agents with PPO or GRPO. Prerequisite: :doc:`basic_env`.
+11. :doc:`basic_env` — Create a simple Gymnasium environment with ``BaseEnv``. Prerequisite: Phase 1 basics.
+12. :doc:`modular_env` — Build a config-driven environment with ``EmbodiedEnv``, managers, and randomization. Prerequisite: :doc:`basic_env`.
+13. :doc:`data_generation` — Generate expert demonstration datasets for imitation learning. Prerequisite: :doc:`modular_env`.
+14. :doc:`rl` — Train RL agents with PPO or GRPO. Prerequisite: :doc:`basic_env`.
 
 **Phase 3: Extending the Framework**
 
-14. :doc:`add_robot` — Add a new robot model to EmbodiChain.
+15. :doc:`add_robot` — Add a new robot model to EmbodiChain.
 
 .. toctree::
    :maxdepth: 1
@@ -39,6 +40,7 @@ Follow the tutorials in this order for the best learning experience:
    create_softbody
    create_cloth
    rigid_object_group
+   rigid_constraint
    robot
    add_robot
    solver

docs/source/tutorial/rigid_constraint.rst

@@ -0,0 +1,158 @@
+Rigid constraint tutorial
+==========================
+
+.. currentmodule:: embodichain.lab.sim
+
+This tutorial shows how to attach two rigid objects via a fixed physics
+constraint, observe the constraint holding their relative pose, and then remove
+it. It follows the style used in the :doc:`rigid_object_group` tutorial and
+references the example script located in
+``scripts/tutorials/sim/create_rigid_constraint.py``.
+
+A *fixed constraint* (a weld) binds two dynamic bodies so that their relative
+pose is held constant by the physics solver — they move as a single rigid
+assembly until the constraint is removed. This is useful for grasping and
+assembly tasks, where an object must be "held" to a gripper or two parts must
+be joined temporarily.
+
+.. tip::
+
+   Constraints are created and removed through the
+   :class:`SimulationManager`, which owns one constraint handle per arena. The
+   same API is exposed as on-demand event functors (``create_rigid_constraint``
+   / ``remove_rigid_constraint`` in ``embodichain.lab.gym.envs.managers.events``)
+   so a task environment can attach/detach mid-episode via
+   ``env.event_manager.apply(mode="attach", env_ids=...)``.
+
+The Code
+~~~~~~~~
+
+The tutorial corresponds to the ``create_rigid_constraint.py`` script in the
+``scripts/tutorials/sim`` directory.
+
+.. dropdown:: Code for create_rigid_constraint.py
+	:icon: code
+
+	.. literalinclude:: ../../../scripts/tutorials/sim/create_rigid_constraint.py
+		:language: python
+		:linenos:
+
+
+The Code Explained
+~~~~~~~~~~~~~~~~~~
+
+
+Adding two cubes
+----------------
+
+Two dynamic cubes are added with :meth:`SimulationManager.add_rigid_object`.
+Each uses a :class:`CubeCfg` shape (a primitive cube, so no mesh asset file is
+needed) and a :class:`RigidBodyAttributesCfg` for mass and friction. ``cube_a``
+is placed slightly higher than ``cube_b`` so that, once detached, the lower
+cube lands first and the relative pose visibly changes.
+
+.. literalinclude:: ../../../scripts/tutorials/sim/create_rigid_constraint.py
+	:language: python
+	:start-at: cube_a = sim.add_rigid_object(
+	:end-at: print("[INFO]: Scene setup complete with two cubes (cube_a, cube_b).")
+
+
+Attaching the cubes
+-------------------
+
+The two cubes are welded with :meth:`SimulationManager.create_rigid_constraint`.
+A :class:`RigidConstraintCfg` names the constraint and points at the two object
+UIDs. ``local_frame_a`` / ``local_frame_b`` default to ``None``, so the
+constraint welds the cubes at their *current* relative pose: ``local_frame_a``
+defaults to identity (object A's origin) and ``local_frame_b`` is computed from
+the objects' current poses so that the offset is preserved rather than the two
+origins being pulled together. Pass explicit ``(4, 4)`` matrices — or an
+``(N, 4, 4)`` array for one frame per arena — to define a specific joint frame
+instead.
+
+.. literalinclude:: ../../../scripts/tutorials/sim/create_rigid_constraint.py
+	:language: python
+	:start-at: constraint = sim.create_rigid_constraint(
+	:end-at: print("[INFO]: Created constraint 'cube_weld' between cube_a and cube_b.")
+
+While attached, the cubes' relative pose stays essentially constant across
+physics steps because the solver enforces the constraint. (``constraint.get_relative_transform()``
+returns the constraint-frame transform, which is ~0 while the constraint is
+satisfied; the tutorial instead prints the bodies' relative z, ``cube_b.z -
+cube_a.z``, to make the held offset visible.)
+
+
+Removing the constraint
+-----------------------
+
+The constraint is removed by name with
+:meth:`SimulationManager.remove_rigid_constraint`. After removal,
+:meth:`SimulationManager.get_rigid_constraint` returns ``None``, and the two
+cubes are independent again — their relative pose is no longer enforced and
+will drift as they interact with gravity and the ground.
+
+.. literalinclude:: ../../../scripts/tutorials/sim/create_rigid_constraint.py
+	:language: python
+	:start-at: sim.remove_rigid_constraint("cube_weld")
+	:end-at: print("\n[INFO]: Removed constraint 'cube_weld'. cube_a and cube_b are now free.")
+
+.. attention::
+
+   ``remove_rigid_constraint`` accepts an ``env_ids`` argument, so in a
+   vectorized simulation you can detach a subset of arenas while leaving the
+   rest attached. Likewise, ``create_rigid_constraint`` accepts ``env_ids`` to
+   attach only specific arenas.
+
+
+Using the constraint from a task environment
+--------------------------------------------
+
+Inside a Gym environment the same operations are triggered on demand through
+event functors registered under custom modes. A task wires up the attach and
+detach functors, then calls ``event_manager.apply`` when its own logic decides
+(for example, when a gripper closes or opens):
+
+.. code-block:: python
+
+    from embodichain.lab.gym.envs.managers.cfg import EventCfg, SceneEntityCfg
+    from embodichain.lab.gym.envs.managers.events import (
+        create_rigid_constraint,
+        remove_rigid_constraint,
+    )
+    from embodichain.utils import configclass
+
+    @configclass
+    class MyTaskEventsCfg:
+        attach_objects: EventCfg = EventCfg(
+            func=create_rigid_constraint,
+            mode="attach",
+            params={
+                "obj_a_cfg": SceneEntityCfg(uid="cube_a"),
+                "obj_b_cfg": SceneEntityCfg(uid="cube_b"),
+                "name": "cube_weld",
+            },
+        )
+        detach_objects: EventCfg = EventCfg(
+            func=remove_rigid_constraint,
+            mode="detach",
+            params={"name": "cube_weld"},
+        )
+
+    # Triggered from the task's own step / reset logic:
+    self.event_manager.apply(mode="attach", env_ids=gripping_env_ids)
+    self.event_manager.apply(mode="detach", env_ids=released_env_ids)
+
+
+Running the tutorial
+~~~~~~~~~~~~~~~~~~~~
+
+To run the script from the repository root:
+
+.. code-block:: bash
+
+	python scripts/tutorials/sim/create_rigid_constraint.py
+
+You can pass flags such as ``--headless``, ``--num_envs <n>``, and
+``--device <cpu|cuda>`` to customize the run. With the default settings the
+script prints the cubes' relative z-position every 20 steps, first while
+attached (held constant) and then after removal (free to drift).

embodichain/lab/gym/envs/managers/events.py

@@ -20,6 +20,8 @@
 import os
 import random
 
+import numpy as np
+
 from copy import deepcopy
 from typing import TYPE_CHECKING, List, Tuple, Dict
 
@@ -30,7 +32,7 @@
     Articulation,
     Robot,
 )
-from embodichain.lab.sim.cfg import RigidObjectCfg, ArticulationCfg
+from embodichain.lab.sim.cfg import RigidObjectCfg, ArticulationCfg, RigidConstraintCfg
 from embodichain.lab.sim.shapes import MeshCfg
 from embodichain.lab.gym.envs.managers.cfg import SceneEntityCfg
 from embodichain.lab.gym.envs.managers import Functor, FunctorCfg
@@ -619,3 +621,70 @@ def set_detached_uids_for_env_reset(
     """
 
     env.add_detached_uids_for_reset(uids=uids)
+
+
+def create_rigid_constraint(
+    env: EmbodiedEnv,
+    env_ids: torch.Tensor | None,
+    obj_a_cfg: SceneEntityCfg,
+    obj_b_cfg: SceneEntityCfg,
+    name: str,
+    local_frame_a: np.ndarray | None = None,
+    local_frame_b: np.ndarray | None = None,
+) -> None:
+    """Attach two rigid objects via a fixed constraint for the given env_ids.
+
+    Registered under a custom event mode (e.g. ``"attach"``); the task triggers it
+    with ``env.event_manager.apply(mode="attach", env_ids=...)``. Delegates to
+    :meth:`SimulationManager.create_rigid_constraint`.
+
+    Args:
+        env: The environment instance.
+        env_ids: Target environment indices. None -> all envs.
+        obj_a_cfg: SceneEntityCfg pointing at the first RigidObject.
+        obj_b_cfg: SceneEntityCfg pointing at the second RigidObject.
+        name: Base constraint name; per-arena names derived by the sim layer.
+        local_frame_a: Local joint frame on object A. None -> identity (object
+            A's origin). Accepts (4,4) or (N,4,4).
+        local_frame_b: Local joint frame on object B. None -> computed per env as
+            ``inv(pose_B) @ pose_A`` so the constraint welds the objects at their
+            current relative pose. Accepts (4,4) or (N,4,4).
+
+    Raises:
+        RuntimeError: If either entity is not a RigidObject.
+    """
+    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,
+    )
+
+
+def remove_rigid_constraint(
+    env: EmbodiedEnv,
+    env_ids: torch.Tensor | None,
+    name: str,
+) -> None:
+    """Remove the named constraint for the given env_ids.
+
+    Delegates to :meth:`SimulationManager.remove_rigid_constraint`. Idempotent:
+    warns (via the sim layer) if the constraint is not found.
+
+    Args:
+        env: The environment instance.
+        env_ids: Target environment indices. None -> all envs.
+        name: Base constraint name to remove.
+    """
+    env.sim.remove_rigid_constraint(name, env_ids=env_ids)

embodichain/lab/sim/cfg.py

@@ -997,6 +997,56 @@ def from_dict(cls, init_dict: Dict[str, Any]) -> RigidObjectGroupCfg:
         return cfg
 
 
+@configclass
+class RigidConstraintCfg:
+    """Configuration for a fixed constraint between two RigidObjects.
+
+    The constraint binds rigid_object_a's entity[i] to rigid_object_b's entity[i]
+    within arena[i] (one constraint per arena).
+
+    Args:
+        name: Base constraint name. Per-arena names are derived as ``f"{name}"``
+            (single env) or ``f"{name}_{i}"`` (multi env).
+        rigid_object_a_uid: UID of the first RigidObject (must exist in the sim).
+        rigid_object_b_uid: UID of the second RigidObject (must exist in the sim).
+        local_frame_a: 4x4 joint frame in object A's local coordinates.
+            ``None`` -> identity (object A's origin). 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: 4x4 joint frame in object B's local coordinates.
+            ``None`` -> the frame is computed per env as ``inv(pose_B) @ pose_A``
+            from the objects' current poses, so the constraint welds the objects
+            at their *current* relative pose (rather than pulling their origins
+            together). An explicit ``(4, 4)`` or ``(N, 4, 4)`` value is used
+            verbatim. Defaults to None.
+        constraint_type: Reserved for future typed constraints (prismatic,
+            revolute, spherical, d6). Only ``"fixed"`` is supported in v1.
+
+    .. attention::
+        Both objects must be :class:`RigidObject` instances and must share the
+        same number of arenas.
+    """
+
+    name: str = MISSING
+    """Base name of the constraint (per-arena names are derived from this)."""
+
+    rigid_object_a_uid: str = MISSING
+    """UID of the first RigidObject."""
+
+    rigid_object_b_uid: str = MISSING
+    """UID of the second RigidObject."""
+
+    local_frame_a: np.ndarray | None = None
+    """Local joint frame on object A. None -> identity (object A's origin)."""
+
+    local_frame_b: np.ndarray | None = None
+    """Local joint frame on object B. None -> ``inv(pose_B) @ pose_A`` per env
+    (weld at the objects' current relative pose)."""
+
+    constraint_type: Literal["fixed"] = "fixed"
+    """Constraint type. Only ``"fixed"`` is supported in v1."""
+
+
 @configclass
 class URDFCfg:
     """Standalone configuration class for URDF assembly."""

embodichain/lab/sim/objects/__init__.py

@@ -27,6 +27,7 @@
 from .robot import Robot, RobotCfg
 from .light import Light, LightCfg
 from .gizmo import Gizmo
+from .constraint import RigidConstraint
 
 
 from dexsim.engine import RenderBody